当前位置:   article > 正文

三轴机械臂逆运动学解算(附代码)_机械臂姿态解算

机械臂姿态解算

三轴机械臂逆运动学解算(附代码)

机械臂运动位姿的求解有两种方式

一、正运动学

通过控制已知的连轴(舵机或电机)的旋转角度,求出机械臂终端的空间坐标

二、逆运动学

通过已知的抓取点的空间坐标,求解出三个舵机所需要转动的角度,这里主要讲解逆运动学解法

在这里插入图片描述
​​

此处θ1 ,θ2, θ3是三个舵机所需转动的角度 ,γ是杆3相对于x轴的夹角,根据刚体旋转,逆运动学求解,会得到两个解,即有两种姿态,相对于前一个杆逆时针旋转的夹角为正,顺时针为负。如上图第一种姿态(上折线),θ1<0,θ2<0,θ3>0

三、求解

(这里就第一种姿态求解)

①求B点坐标

已知空间点A坐标x,y。(因为三杆必然处于同于平面,若涉及z坐标,则机械臂下方云台的解算应考虑在内。

Bx = x - L3*cos(γ);

By = y - L3*sin(γ);

②求出θ1

cosβ = (Bx^2+ By^2 + L1^2 -L2^2) / (2* L1 *sqrt(Bx^2 +By^2));

β = acos(cosβ)*180/pi;

α = atan2(By,Bx)*180/pi;

θ1=-(pi/2-α-β); //数学上θ1为正,但应用上应为负值

③求出θ2

cosθ2 = (Bx^2 + By^2 - L1^2 - L2^2)/(2* L1 *L2);

θ2 = - acos(cosθ2 )*180/pi; //同理θ2改为负值

④求出θ3(这里的γ需预先定好)

通过γ = θ1 + θ2 + θ3

求出θ3 = γ- θ1- θ2

至此三个角度结算完毕

四、代码实现

#include “stdio.h”
#include “math.h”
float theta[3];
#define pi 3.1415
void caculate(float x, float y, float z, float * ptheta)
{
float L1 = 0.14; //杆长
float L2 = 0.11;
float L3 = 0.04;
Bx=x-L3; //这里设置γ为-90
By=y;
lp=Bx* Bx+By* By;
if (sqrt(lp)>=L1+L2 || sqrt(lp)<=fabs(L1-L2))
printf(“unreached”);
alpha = atan2(By,Bx);
beta = acos((L1* L1+lp-L2* L2)/(2* L1* sqrt(lp))); //这里使用弧度制
ptheta[0] = -(pi/2.0-alpha-beta);
ptheta[1] = acos((L1* L1+L2* L2-lp)/(2* L1 *L2))-pi;
ptheta[2] = -ptheta[0] -ptheta[1]- pi/2;
}

caculate(0.14, 0.09, 0.1, theta);

ROS moveit仿真效果

在这里插入图片描述
参考博客
(https://blog.csdn.net/xinglucao/article/details/86534402?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522164432010116780261924975%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fblog.%2522%257D&request_id=164432010116780261924975&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2blogfirst_rank_ecpm_v1~hot_rank-1-86534402.nonecase&utm_term=%E4%B8%89%E8%BD%B4%E6%9C%BA%E6%A2%B0%E8%87%82%E6%8E%A7%E5%88%B6%E7%AE%97%E6%B3%95&spm=1018.2226.3001.4450)

​修改:代码中部分 * 没有打出来

void caculate(float x, float y, float z, float *ptheta)
{
	float L1 = 0.14; //杆长
	float L2 = 0.11;
	float L3 = 0.04;
	float alpha, beta, lp, Bx, By;
	Bx=x-L3; //这里设置γ为-90
	By=y;
	lp=Bx*Bx+By*By;
	if (sqrt(lp)>=L1+L2 || sqrt(lp)<=fabs(L1-L2))
		return 1;
	alpha = atan2(By,Bx);
	beta = acos((L1*L1+lp-L2*L2)/(2*L1*sqrt(lp))); //这里使用弧度制
	ptheta[0] = -(pi/2.0-alpha-beta);
	ptheta[1] = acos((L1*L1+L2*L2-lp)/(2*L1*L2))-pi;
	ptheta[2] = -ptheta[0] -ptheta[1]- pi/2.0;
	return 0;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/人工智能uu/article/detail/904395
推荐阅读
相关标签
  

闽ICP备14008679号