正逆运动学解（二维）

提要：我是为了研究机械狗的步态算法而去学习的正逆运动学算法，实际上可以将机械狗的每一条足理解成是一个机械臂，所以我就用机械臂来举例了。
一、正运动学
这里所谓的运动学解其实就是舵机的转动角度与足尖的坐标值相互转化的数学运算方法。
要想进行计算首先需要一个坐标轴。

在这里我只先研究二维的运算。
首先我们可以很轻易地算出C点的X坐标X=L2COS(α)+L3COS(β)
同样的我们也可以算出C点的Z坐标Z=L2SIN(α)-L3SIN(β)
这样我们就有了C点坐标的计算公式，下面我们要将它转换成我们的算法。

#include
#include
float Alpha,Beta,L2,L3;
struct ForwardKinematics
{
	float x;
	float z;
}h;
void main()
{
	h.x=L2*cos(Alpha)+L3*cos(Beta);
	h.z=L2*sin(Alpha)-L3*sin(Beta);
}

这个算法的作用就是输入舵机的转动角度然后就能知道尖端C点所在的坐标。
以上就是正运动学的原理。
二、逆运动学
上面讲了正运动学是求C点的坐标而逆运动学就是反过来输入C点的坐标求出α和β的角度。
我们还用上面建立的坐标轴。

我们还是只在二维坐标下求解。
刚才我们的已知量是α和β，而现在我们的已知量是X和Z坐标。
所以我们的式子从数学的角度是和上面一样的，但是我们在算法实现的时候就不能这么写了。
这里有两种求解方法
1、直接用数学公式求出这个方程式最后结果如下：图片里的y其实就是坐标里的z（笔误了）


那么程序实现如下：

#include
#include
float x,z,L2,L3;
struct ForwardKinematics
{
	float Alpha;
	float Beta;
}h;
void main()
{
	 B=acos((-(L2*L2-x*x-z*z-L3*L3)/2*L3)/sqrt(x*x+z*z))-acos(x/sqrt(x*x+z*z))
	 h.Beta=B*180/PI;                       //注意弧度制和角度制的转换
	 h.Alpha=acos((x-L3*cos(B*180/PI))/L2)*180/PI;
}

2、通过几何推导出α和β的值

转换到程序中：

#include
#include
float x,z,L2,L3;
struct ForwardKinematics
{
	float Alpha;
	float Beta;
}h;
void main()
{
	A=atan(z/x)*180/PI+acos((L2*L2+z*z+x*x-L3*L3)/(2*L2*sqrt(z*z+x*x)))*180/PI;
	h.Alpha=A;
	h.Beta=180-acos((L2*L2+L3*L3-z*z-x*x)/(2*L2*L3))*180/PI-A;
}

以上就是在二维直角坐标系下正逆运动学解的程序化算法，根据其原理同样可以推广到三维上。同样的也可以推广到球坐标系等其它各种坐标系上。具体要根据实际用途来确定，如果以后我用到其它坐标系的话，我会回来继续更新的。