[机械臂运动学系列]机械臂及其笛卡尔坐标正逆解算（四）

[四]机械手臂的逆运动学解

正运动学分析是已知每个关节的姿态的前提下，解算出末端执行器的姿态。而逆运动学研究的问题是，要求控制末端执行器到达某一位置时，各关节应处于什么姿态。

逆运动学最基本的思路，是从正运动学反过来，对于我们的机械臂而言，也就是已知末端执行器的位置和朝向，求每个关节的角度。我们的机械臂的三维运动是比较复杂的，这里为了简化模型更加便于大家的理解，我们对模型进行精简，先去掉下方云台的旋转关节，这样就可以在二维的平面上进行运动学分析了。

其中0, 1, 2 是各个关节的角度，未知量。 P(x, y, ) 是末端执行器的位姿表示，x 和 y 是在 OXY 平面的坐标， 是末端执行器的朝向。一般我们看的文章，计算运动学分析那都是一大堆的矩阵变换，实际上那些算法在单片机上执行是比较费时的，甚至可以说是无法满足实时控制的。适合在 matlab 上面运算。所以，我们需要寻找一种高效的方案，以满足我们实时控制的需要。下面我们将使用几何法进行分析。

根据上面的图示，很容易列出如下方程：


实际上，这也是运动学正解的一种表达方式，既然可以如此简单的表达，为什么要之前要学习 DH 模型呢？主要是我们需要通过这种机构掌握一种通用的方法，后面解决更复杂的模型分析的时候能够游刃有余。毕竟几何法不具备通用性，每个模型都需要单独的分析，一些超出我们感性认识的复杂模型更是无能为力。

下面对上述方程组进行化简，把式(3)代入式(2)和(1)中，得：

通过同样的方法，可求得2，这样就完成了逆运动学的计算。显然，最终有2个正确的解，这个根据图 1 的虚线部分可以看出。一般我们选取的是虚线部分的解，这样每个关节的受力可以小一点。
C代码：

/**************************************************************************
函数功能：数学模型
入口参数：末端执行器位姿态
返回  值：无
**************************************************************************/
u8 Kinematic_Analysis(float x,float y,float Beta,float Alpha)
{  
	      float m,n,k,a,b,c,theta1,theta2,theta3,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)  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;       //弧度换成角度

	      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;            //弧度换成角度
	
				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°

        Target1   = 750-(Beta)*Ratio;   //作用到输出
        Target2   = 789+(theta1-90)*Ratio;
	      Target3   = 717-(theta2)*Ratio;
	      Target4   = 702-(theta3)*Ratio;  
				return 0;
}