5DOF机械臂逆运动学求解

---

一、模型简化

---

二、模型逆运动学求解

用户给定笛卡尔末端工具坐标系的位姿为X, Y, Z, roll, pitch, yaw；由图上可知，云台角度（控制末端工具坐标的yaw）：
$${\theta }_0=atan2(Y,X)$$
转关节角度（控制机械臂工具坐标系的roll）：
$${\theta }_4=roll$$
将坐标转换到中间三个关节的平面：
$$x=\surd (X^2+Y^2)$$

$$y=Z$$
根据上面的图示右下角，可以列出如下方程：
$$x=l_{0}cos{\theta }_1+l_{1}cos({\theta }_1+{\theta }_2)+l_{2}cos({\theta }_1+{\theta }_2+{\theta }_3)$$
$$y=l_{0}sin{\theta }_1+l_{1}sin({\theta }_1+{\theta }_2)+l_{2}sin({\theta }_1+{\theta }_2+{\theta }_3)$$

末端工具坐标系的俯仰角pitch：
$$pitch={\theta }_1+{\theta }_2+{\theta }_3$$
下面对上述方程组进行化简，把式pitch代入式(6)(7)，得：
$$x=l_{0}cos{\theta }_1+l_{1}cos({\theta }_1+{\theta }_2)+l_{2}cos(pitch)$$
$$y=l_{0}sin{\theta }_1+l_{1}sin({\theta }_1+{\theta }_2)+l_{2}sin(pitch)$$
为了方便计算，令：
$$m=l_{2}cos(pitch)-x$$
$$n=l_{2}sin(pitch)-y$$
化简(8)得：
$$l_1=(l_{0}cos{\theta }_1+m{)}^2+(l_{0}sin{\theta }_1+n{)}^2$$
可将该方程化简成超越方程：
$$acos\theta +bsin\theta =c$$
查阅《机器人导论》书的附录C可求得${\theta }_1$两组解：
$${\theta }_1=Atan2(b,a)\pm Atan2(\surd (a^2+b^2-c^2),c)$$
同理，可以求出${\theta }_1+{\theta }_2$。

两组解怎么选择？
两组解示意图如下：

我用一个想法将它们区分开来, 因为$l_1$、$l_2$的长度不相等，所以会有：
$${\theta }_1+{\theta }_2>{\theta }_1^{\prime }+{\theta }_2^{\prime }$$
注意${\theta }_2^{\prime }$是小于0的！再加上：
$${\theta }_1^{\prime }>{\theta }_1$$
就可以判断出组合从而求得：${\theta }_2$、${\theta }_2^{\prime }$；最后加上机械臂关节角度限制选择一组合适的解即可，至此${\theta }_0$ 、${\theta }_1$ 、${\theta }_2$ 、${\theta }_3$ 、${\theta }_4$五个关节角度就都求出来了，最后${\theta }_5$是控制夹具夹取的。

---

三、C++代码

//逆运动学
//只需要输入五个全局参数pose[5]：X Y Z roll pitch
//对于视觉：传入XY    z，roll固定   pitch给定    
void inverse_Kinematic_Y(float pose[5], vector<float>& v_k)
{
	float x_1, y_1, beta_1, alpha_1,roll;
	y_1 = pose[2];//Z
	x_1 = sqrt(pose[0] * pose[0] + pose[1] * pose[1]);
	beta_1 = atan2(pose[1],pose[0])*180/PI;//yaw，有X Y算出
	cout <<"beta:"<< beta_1<<endl;
	roll = pose[3]*180/PI;//roll
	alpha_1 = pose[4];//pitch
	float *mmm;
	mmm = Kinematic_Analysis(y_1, x_1, beta_1, alpha_1);
	//得到六个关节角，最后一暂时还没控制
	v_k = { beta_1,*mmm,*(mmm + 1), *(mmm + 2),roll,0 };
}
float* Kinematic_Analysis(float x, float y, float Beta, float Alpha)
{
	cout << "111" << cos(Alpha) << endl;
	float m, n, k, a, b, c, theta1_1, theta1_2, theta2_1, theta2_2, theta3, s1ps2_1, s1ps2_2, theta1, theta2, s1ps2;
	m = l2 * cos(Alpha) - x;   //中间变量
	n = l2 * sin(Alpha) - y;   //中间变量
	k = (l1 * l1 - l0 * l0 - m * m - n * n) / 2 / l0;//中间变量
	a = m * m + n * n;             //解一元二次方程
	b = -2 * n * k;
	c = k * k - m * m;

	if (b * b - 4 * a * c < 0)
	{
		cout << "该点无法到达！！！" << endl;
		system("pause");
		exit(0);
		return 0; //防止出现非实数解
	}
	//theta1 = (-b- sqrt(b * b - 4 * a * c)) / 2 / a;  //得到一元二次方程的解，只取其中一个，另外一个解是(-b+sqrt(b*b-4*a*c))/2/a
	//theta1 = asin(theta1) * 180 / PI;       //弧度换成角度

	theta1_1 = atan2(n, m) + atan2(sqrt(m * m + n * n - k * k), k);
	theta1_1 = theta1_1 * 180 / PI;
	theta1_2 = atan2(n, m) - atan2(sqrt(m * m + n * n - k * k), k);
	theta1_2 = theta1_2 * 180 / PI;
	if (theta1_1 > 90)theta1_1 = theta1_1 - 360;
	if (theta1_1 < -90)theta1_1 = theta1_1 + 360;
	if (theta1_2 > 90)theta1_2 = theta1_2 - 360;
	if (theta1_2 < -90)theta1_2 = theta1_2 + 360;

	//if (theta1 > 90)theta1 = 90;           //控制舵机的最大角度±90°
	//if (theta1 < -90)theta1 = -90;

	k = (l0 * l0 - l1 * l1 - m * m - n * n) / 2 / l1;     //过程系数
	a = m * m + n * n;                        //解一元二次方程
	b = -2 * n * k;
	c = k * k - m * m;

	if (b * b - 4 * a * c < 0)  return 0;         //防止出现非实数解

	//s1ps2 = (-b + sqrt(b * b - 4 * a * c)) / 2 / a;      //得到一元二次方程的解，只取其中一个，另外一个解是(-b+sqrt(b*b-4*a*c))/2/a
	//s1ps2 = asin(s1ps2) * 180 / PI;            //弧度换成角度

	s1ps2_1 = atan2(n, m) + atan2(sqrt(m * m + n * n - k * k), k);
	s1ps2_1 = s1ps2_1 * 180 / PI;
	s1ps2_2 = atan2(n, m) - atan2(sqrt(m * m + n * n - k * k), k);
	s1ps2_2 = s1ps2_2 * 180 / PI;
	if (s1ps2_1 > 90)s1ps2_1 = s1ps2_1 - 360;
	if (s1ps2_1 < -90)s1ps2_1 = s1ps2_1 + 360;
	if (s1ps2_2 > 90)s1ps2_2 = s1ps2_2 - 360;
	if (s1ps2_2 < -90)s1ps2_2 = s1ps2_2 + 360;

	float bj[4] = { theta1_1 ,theta1_2,s1ps2_1,s1ps2_2 };
	cout << bj[0] << bj[1] << bj[2] << bj[3] << endl;
	for (int j = 1; j < 4; j++)
	{
		for (int g = 0; g < j; g++)
		{
			if (bj[j] > bj[g])
			{
				int temp;
				temp = bj[j];
				bj[j] = bj[g];
				bj[g] = temp;
			}
		}
	}
	cout << bj[0] << bj[1] << bj[2] << bj[3] << endl;
	theta1_1 = bj[1];//大theata1角
	theta2_1 = bj[3] - bj[1];
	theta1_2 = bj[2];//小theata1角
	theta2_2 = bj[0] - bj[2];

	if (theta1_2 < -90 || theta1_2 > 90)
	{
		theta1 = theta1_1;
		theta2 = theta2_1;
	}
	else if (theta1_1 < -90 || theta1_1 > 90)
	{
		theta1 = theta1_2;
		theta2 = theta2_2;
	}
	else if (theta1_1 <= 0 && theta1_1 >= -90)
	{
		theta1 = theta1_2;
		theta2 = theta2_2;
	}
	else
	{
		theta1 = theta1_1;
		theta2 = theta2_1;
	}

	//if (s1ps2 > 90)theta2 = 90;
	//if (s1ps2 < -90)theta2 = -90;

	//theta2 = s1ps2 - theta1;
	//if (theta2 > 90)theta2 = 90;      //控制舵机的最大角度±90°
	//if (theta2 < -90)theta2 = -90;    //控制舵机的最大角度±90°

	theta3 = Alpha * 180 / PI - theta1 - theta2;   //求关节3角度
	if (theta3 > 90)theta3 = 90;
	if (theta3 < -90)theta3 = -90;	    //控制舵机的最大角度±90°

	cout << theta1 << "   " << theta2 << "   " << theta3 << endl;
	float pp[3] = { theta1 ,theta2,theta3 };

	static float u[3];
	for (int i = 0; i < 3; ++i)
	{
		u[i] = pp[i];
		cout << i << ":" << u[i] << endl;
	}
	return u;
}