使用Mathematica梳理《机器人导论》中三轴相交的Pieper解法

刚刚接触串联机器人的新生，写这篇文章一方面是记录自己的学习成果（阶段性的）另一方面是希望能通过Mathematica的推导更加透彻的理解Pieper解法，同时可以练习Mathematica（针不错），其他便不敢多求。虽然可能会有很多错误但是会一直保持学习和进步。

Pieper解法在课本（《机器人导论》-John J.Craig）的79页，感兴趣的同学可以去仔细研究一下。

Mathematica整个程序就用到了下面四个函数

Clear[]

Print[]

MatrixForm[]

Simplify[]

首先描述一下我对Pieper解法的一个理解。Pieper解法是六自由度机械臂运动学逆解的一个特殊解法，特殊就特殊在后面三个关节轴相交于一点。在这个解法中主要分为两大部分：

第一部分是前三个关节角（cita1，cita2，cita3）的求解。

第二部分是后三个关节角（cita4，cita5，cita6）的求解。

复杂的是第一部分，下面的的思维导图中红色的平行四边形和黄色的正方形都是第一部分中的两大步
直接上代码：思维导图中红色平行四边形部分

Clear["Global`*"];
a0 = 0;alph0 = 0; d1 = 0;                             (*这些参数在书上的推导是默认为0的，这里提前赋值为0*)
(*
首先我们的目的是求出P04org（列向量）
因为这样一个等式：
  
P04org={{x}，{y}，{z}，{1}}

*需要说明的是：
等号右边的列向量是机械臂末端（腕关节坐标系W）的原点的位置（在坐标系0中表示）
在逆解的背景下{{x}，{y}，{z}，{1}}全为已知量
而P04org是用机械臂连杆参数（DH参数）表示的
*要注意的是：
这里默认的是六自由度机械臂，且最后三个关节轴相交因此只对机械臂末端的姿态产生影响
而机械臂末端的位置由前三个关节轴确定
这是P04org={{x}，{y}，{z}，{1}}这个等式能够成立前提条件

综上所述通过上面的等式就可以建立工作空间与关节空间的映射从而实现机械臂逆解
*)
(*******************************************************************************************
*)(*
为了目的利用如下公式
P04org = T01.T12.T23.P34org
（T矩阵都有公式直接带入参数即可）
分成三步求解
一：
  P24org = T23.P34org;  
二：
  T02 = T01.T12;
三：
   P04org = T02.P24org
在过程中将会用到大量的变量替换以化简公式
*)
P34org = {{a3},{-d4*Sin[alph3]},{d4*Cos[alph3]},{1}};(*坐标4的原点在坐标3里面表示{套用公式获得的}，仅仅是位置*)
Print["下面是坐标系4（关节4）在坐标系3中位置描述（列向量的形式）他是齐次矩阵T34的最后一列"];
Print["将DH参数直接带公式即可"];
Print["这个向量中没有变量"];
Print["*****P34org*****"];
MatrixForm[P34org]
(*
**第一步**
*)
T23 = {{Cos [cita3],-Sin[cita3],0,a2},{Sin[cita3]*Cos[alph2],Cos[cita3]*Cos[alph2],-Sin[alph2],-Sin[alph2]*d3},{Sin[cita3]*Sin[alph2],Cos[cita3]*Sin[alph2],Cos[alph2],Cos[alph2]*d3},{0,0,0,1}};
P24org = T23.P34org;                  (*在坐标2中表示4的原点*)
Print["坐标系4的原点位置在坐标系2中表示，同样以列向量的形式"];
Print["可以看到矩阵元素已经开始变得非常复杂了"];
Print["*****P24org*****"];
MatrixForm[P24org]
(*
**第二步**
*)
T01 = {{Cos [cita1],-Sin[cita1],0,a0},{Sin[cita1]*Cos[alph0],Cos[cita1]*Cos[alph0],-Sin[alph0],-Sin[alph0]*d1},{Sin[cita1]*Sin[alph0],Cos[cita1]*Sin[alph0],Cos[alph0],Cos[alph0]*d1},{0,0,0,1}};
T12 = {{Cos [cita2],-Sin[cita2],0,a1},{Sin[cita2]*Cos[alph1],Cos[cita2]*Cos[alph1],-Sin[alph1],-Sin[alph1]*d2},{Sin[cita2]*Sin[alph1],Cos[cita2]*Sin[alph1],Cos[alph1],Cos[alph1]*d2},{0,0,0,1}};
T02 = T01.T12;
Print["前两个坐标系（2到0）的映射，注意这是齐次矩阵！"];
Print["******T02******"];
MatrixForm[T02]

(*
**第三步**
这三个是推到过程中的变量，用于简化表达式，这里先不赋值，因为赋值后编译器会全部显示表达式，从而起不到简化的效果
我们令
ff1 = P24org[[1]];第一个元素
ff2 = P24org[[2]];第二个元素
ff3 = P24org[[3]];第三个元素
最后：
P24org = {{ff1},{ff2},{ff3},{1}}
*)
Print["使用变量替换，将P24org表示为更简洁的的形式"];
Print["其中：ff1 = a2+a3 Cos[cita3]+d4 Sin[alph3] Sin[cita3]"];
Print["其中：ff2 = -d3 Sin[alph2]-d4 Cos[alph3] Sin[alph2]-d4 Cos[alph2] Cos[cita3] Sin[alph3]+a3 Cos[alph2] Sin[cita3]"];
Print["其中：ff3 = d3 Cos[alph2]+d4 Cos[alph2] Cos[alph3]-d4 Cos[cita3] Sin[alph2] Sin[alph3]+a3 Sin[alph2] Sin[cita3]"];
MatrixForm[{{ff1},{ff2},{ff3},{1}}]   
P04org = T02.{{ff1},{ff2},{ff3},{1}};(*到这里已经将坐标系4的原点的位置描述表示出来了，这是一个4个元素的列向量*)
P04org = Simplify[P04org];            (*整理并显示*)
Print["通过P04org = T02.P24org"];
Print["得到坐标系4到0的映射"];
Print["*****P04org*****"];
MatrixForm[P04org]                    (*显示P04，他是列向量*)
Print["至此上文提到的目标已将实现"];

程序运行结果：

黄色长方形部分：

Print["接下来的目标是通过P04org={{x}，{y}，{z}，{1}}求解关节角三个cita"];
Print["令："];
Print["g1 = a1+ff1 Cos[cita2]-ff2 Sin[cita2] "];
Print["g2 = (d2+ff3) Sin[alph1]-Cos[alph1] (ff2 Cos[cita2]+ff1 Sin[cita2]) "];
Print["g3 = d1+(d2+ff3) Cos[alph1]+ff2 Cos[cita2] Sin[alph1]+ff1 Sin[alph1] Sin[cita2]"];
Print["化简后的P04org表示为："];
MatrixForm[{{Cos[cita1]*g1-Sin[cita1]*g2},{Sin[cita1]*g1+Cos[cita1]*g2},{g3},{1}}]
Print["求解方程的关键就是找到相等关系"];
Print["相等关系就是两个坐标原点（4和0）间的直线距离也就是r"];

(*
用一些变量替换多项式和课本相同，以简化公式
但是实际在系统计算时不会用到这个替换，这里
的替换只是为了方便观察
其中：
g1 = a1+ff1 Cos[cita2]-ff2 Sin[cita2]
g2 = (d2+ff3) Sin[alph1]-Cos[alph1] (ff2 Cos[cita2]+ff1 Sin[cita2])
g3 = d1+(d2+ff3) Cos[alph1]+ff2 Cos[cita2] Sin[alph1]+ff1 Sin[alph1] Sin[cita2]
化简后的P04org表示为：
P04org = {{Cos[cita1]*g1-Sin[cita1]*g2},{Sin[cita1]*g1+Cos[cita1]*g2},{g3},{1}}
注意符号的变换*)
(**)

r =P04org[[1]]^2+P04org[[2]]^2+P04org[[3]]^2 ;(*这里r是坐标4原点到0坐标系原点的直线距离*)
z = P04org[[3]];

r=Simplify[r];
z=Simplify[z];
Print["用P04org求出r与末端的坐标求出的r相等这就是等量关系"];
Print["再加上z轴坐标相等这个等量关系"];
Print["形成两个方程组"];
Print["r = \!\(\*SuperscriptBox[\(a1\), \(2\)]\)+\!\(\*SuperscriptBox[\(d2\), \(2\)]\)+\!\(\*SuperscriptBox[\(ff1\), \(2\)]\)+\!\(\*SuperscriptBox[\(ff2\), \(2\)]\)+2 d2 ff3+\!\(\*SuperscriptBox[\(ff3\), \(2\)]\)+2 a1 ff1 Cos[cita2]-2 a1 ff2 Sin[cita2]"]
Print["z =(d2+ff3) Cos[alph1]+Sin[alph1] (ff2 Cos[cita2]+ff1 Sin[cita2])"]

(*
将下面两个等式结合可以消去cos（cita2）和sin（cita2）
这个结合方法自己动脑子计算
其中：
k1 = ff1
k2 = -ff2
k3 = a1^2+d2^2+ff1^2+ff2^2+2 d2 ff3+ff3^2
k4 = (d2+ff3) Cos[alph1]*)

Print["为了简化形式再一次进行变量替换"];
Print["k1 = ff1"];
Print["k2 = -ff2"];
Print["k3 = \!\(\*SuperscriptBox[\(a1\), \(2\)]\)+\!\(\*SuperscriptBox[\(d2\), \(2\)]\)+\!\(\*SuperscriptBox[\(ff1\), \(2\)]\)+\!\(\*SuperscriptBox[\(ff2\), \(2\)]\)+2 d2 ff3+\!\(\*SuperscriptBox[\(ff3\), \(2\)]\)"];
Print["k4 = (d2+ff3) Cos[alph1]"];
Print["下面是通过P04org计算出的r和z轴坐标值"];
r = 2*a1*(k1*Cos[cita2]+k2*Sin[cita2])+k3
z = Sin[alph1]*(k1*Sin[cita2]-k2*Cos[cita2])+k4
Print["最后"];
Print["r = 2*a1*(k1*Cos[cita2]+k2*Sin[cita2])+k3"];
Print["z = Sin[alph1]*(k1*Sin[cita2]-k2*Cos[cita2])+k4"];
Print["使用上述方程组中没有cita1，这是便捷之处"];
Print["要解上述方程组需要分类讨论"];
Print["case1：a1 = 0"];
Print["根据式1可直接解出cita3"];
Print["case2：sin（alph1） = 0"];
Print["同样可以直接解出cita3"];
Print["再将cita3带入原方程组即可分别解出cita1和cita2"];
Print["case3：sin（alph1） ！= 0&&a1 ！= 0"];
Print["这一步化简人工操作会更简单"];
Print["两式联立消去sin（cita2）和cos（cita2）"];
Print["得到：[（r-k3）^2/4a1^2]+[(z-k4)^2/sin(alph1)^2]=k1^2+k2^2"];
Print["式中只有cita3一个变量，使用半角正切变换得出要给4次方程，在解方程式最好将常量值带入进行计算，若继续使用符号会极为不便"];

运行结果：

其中最重要的一个方程组：

r = 2*a1*(k1*Cos[cita2]+k2*Sin[cita2])+k3
z = Sin[alph1]*(k1*Sin[cita2]-k2*Cos[cita2])+k4

前三个关节角全是由这个方程组得到的

课本上是这么说的：它很有用，因为它消去因变量cita1，并且因变量cita2的关系式简单。

最后也就是思维导图中绿色椭圆的部分，课本用的如下公式；

这公式的意思是，把后三个关节的旋转看作是绕Z_Y_Z欧拉角旋转，而这个旋转可以用一个旋转矩阵表示，这矩阵就是公式中的 [公式] ，有一点比较迷惑的是cita4=0，如果cita4不为0，公式则为：

这没试过只是推测。

至于如何通过旋转矩阵求出欧拉角，这在课本29页做了详细的说明，而且完全可以手算。

完了，整个过程算是粗略的捋了一遍，希望这篇文章能作为一个开始鼓励我继续学下去，加油！