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

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

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

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

其中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