机械臂运动学逆解（Analytical solution）

计算机器人运动学逆解首先要考虑可解性(solvability)，即考虑无解、多解等情况。在机器人工作空间外的目标点显然是无解的。对于多解的情况从下面的例子可以看出平面二杆机械臂（两个关节可以360°旋转）在工作空间内存在两个解：

 ​

　　如果逆运动学有多个解，那么控制程序在运行时就必须选择其中一个解，然后发给驱动器驱动机器人关节旋转或平移。如何选择合适的解有许多不同的准则，其中一种比较合理的方法就是选择“最近”的解（the closest solution）。如下图所示，如果机器人在A点，并期望运动到B点，合理的解是关节运动量最小的那一个（A good choice would be the solution that minimizes the amount that each joint is required to move）。因此在不存在障碍物的情况下，上面的虚线构型会被选为逆解。在计算逆解时我们可以考虑将当前位置作为输入参数，这样我们就可以选择关节空间中离当前位置最近的解。

　　这个“最近”有多种定义方式。比如对于典型的6自由度关节型机器人来说，其前三个关节较大，后三个关节较小。因此在定义关节空间内的距离远近时要考虑给不同关节赋予不同的权重，比如前三个关节设置大权重，后三个关节设置小权重。那么在选择解的时候会优先考虑移动较小的关节而非移动大关节。而当存在障碍物时，“最近”的解的运动路径会与其发生碰撞，这时就要选择另一个运动距离较远的解（"farther" solution）。因此在考虑碰撞、路径规划等问题时我们需要计算出可能存在的全部解。

​

　　逆解个数取决于机器人关节数目（the number of joints）、机器人的构型（link parameters）以及关节运动范围(the allowable ranges of motion of the joints)。决定机器人构型的D-H参数表中的非零值越多，就有越多的解存在。对于通用型6轴转动关节的机械臂来说，最多可能存在16个不同的解。下图展示了最大解的数量与非零值的连杆长度参数aa （两关节转轴之间的最短距离，即两轴线之间公垂线的长度）的数量之间的关系：

​

　　另外机器人逆运动学求解也有多种方法，一般分为两类：封闭解（closed-form solutions）和数值解（numerical solutions）。不同学者对同一机器人的运动学逆解也提出不同的解法。应该从计算方法的计算效率、计算精度等要求出发，选择较好的解法。通常来说数值迭代解法比计算封闭解的解析表达式更慢、更耗时，因此在设计机器人的构型时就要考虑封闭解的存在性。

 　　求解逆运动学方程的解析解（给出解的具体函数形式，从解的表达式中就可以算出任何对应值）时主要采用代数法（Algebraic solution）或几何法（Geometric solution）。下面我们先用代数法来计算平面二连杆机械臂的运动学逆解（不考虑末端关节的旋转）。正向运动学很容易得到：

x=l1c1+l2c12y=l1s1+l2s12x=l1c1+l2c12y=l1s1+l2s12

　　将上面方程两边取平方再相加得到x2+y2=l21+l22+2l1l2c2x2+y2=l12+l22+2l1l2c2 ，消除θ1θ1 。这里用到了三角函数的和差角公式：

c12=c1c2−s1s2s12=s1c2+c1s2c12=c1c2−s1s2s12=s1c2+c1s2

　　可以求得cosθ2cos⁡θ2 ：

c2=x2+y2−l21−l222l1l2c2=x2+y2−l12−l222l1l2

　　为了使解存在，上式的值必须在-1~1之间，因为余弦函数cosxcos⁡x 的取值范围就是[−1,1][−1,1] 。在计算逆解时需要检查这一条件，当不满足时说明目标位置已经位于工作空间之外（the goal point is too far away for the manipulator to reach）。当目标位置(x,y)(x,y) 位于工作空间内时可以求得sinθ2sin⁡θ2 ：

s2=±1−c22−−−−−√s2=±1−c22

　　为了计算θ2θ2 ，可以使用Atan2函数，即：

θ2=Atan2(s2,c2)θ2=Atan2(s2,c2)

　　注意对于tan(θ) = y / x ，两种反正切函数的区别是：θ = ATan(y / x)求出的θ取值范围是(−π2,π2)(−π2,π2) ；θ = ATan2(y, x)求出的θ取值范围是(−π,π](−π,π] 。

• 当 (x, y) 在第一象限, 0 < θ < PI/2

• 当 (x, y) 在第二象限 PI/2 < θ≤PI

• 当 (x, y) 在第三象限, -PI < θ < -PI/2

• 当 (x, y) 在第四象限, -PI/2 < θ < 0

　　s2s2 的符号有两种选择，对应的我们可以选择"elbow-up"或"elbow-down"两种不同构型。求出θ2θ2 后我们可以根据正解方程再计算出θ1θ1 。将正解方程改写为

x=k1c1−k2s1y=k1s1+k2c1x=k1c1−k2s1y=k1s1+k2c1

欢迎点击链接加入群聊【程序代写-接单群】共同致富：https://jq.qq.com/?_wv=1027&k=5WxihsL 

群号：733065427

　　其中

k1=l1+l2c2k2=l2s2k1=l1+l2c2k2=l2s2

　　为了求解方程对k1k1 、k2k2 进行变量替换：

k1=rcosγk2=rsinγk1=rcos⁡γk2=rsin⁡γ

　　其中r=k21+k22−−−−−−√r=k12+k22 ，γ=Atan2(k2,k1)γ=Atan2(k2,k1)

　　于是正解方程可写为：

xr=cosγcosθ1−sinγsinθ1yr=cosγsinθ1+sinγcosθ1xr=cos⁡γcos⁡θ1−sin⁡γsin⁡θ1yr=cos⁡γsin⁡θ1+sin⁡γcos⁡θ1

　　因此有：

cos(γ+θ1)=xrsin(γ+θ1)=yrcos⁡(γ+θ1)=xrsin⁡(γ+θ1)=yr

　　使用Atan2函数可得到：γ+θ1=Atan2(y,x)γ+θ1=Atan2(y,x)

　　于是第一个关节的转角θ1θ1 为：

θ1=Atan2(y,x)−Atan2(k2,k1)=Atan2(y,x)−Atan2(l2s2,l1+l2c2)θ1=Atan2(y,x)−Atan2(k2,k1)=Atan2(y,x)−Atan2(l2s2,l1+l2c2)

　　注意之前在求解θ2θ2 时对s2s2 的符号进行了选择，这会引起k2k2 符号的变化，并影响θ1θ1 的求解。另外当x=y=0时，函数Atan2是未定义的状态，这种情况下θ1θ1 可以任意取值。

• Geometric solution

　　根据机构平面图，由L1L1 、L2L2 以及原点与末端之间的连线构成的三角形的余弦定理可求得θ2θ2 ：

x2+y2=l21+l22−2l1l2cos(180∘+θ2)x2+y2=l12+l22−2l1l2cos⁡(180∘+θ2)

​

　　由于cos(180∘+θ2)=−cos(θ2)cos⁡(180∘+θ2)=−cos⁡(θ2) ，可解得：

c2=x2+y2−l21−l222l1l2c2=x2+y2−l12−l222l1l2

　　为了保证三角形存在（三角形两边之和大于第三边），即x2+y2−−−−−−√x2+y2 必须小于或等于连杆长度之和l1+l2l1+l2 。在求逆解时需要验证是否满足这一条件，判断解的存在性。另一个可能的解（虚线所示）与之对称，θ′2=−θ2θ2′=−θ2

　　为了计算θ1θ1 ，先求出图中的ββ 和ψψ 角。ββ 可能位于坐标系四象限中的任一象限，取决于xx 和yy 的符号，因此使用Atan2函数来求解：β=Atan2(y,x)β=Atan2(y,x)

　　对ψψ 用余弦定理来计算：

cosψ=x2+y2+l21−l222l1x2+y2−−−−−−√cos⁡ψ=x2+y2+l12−l222l1x2+y2

　　于是

θ1=β±ψθ1=β±ψ

　　当θ2<0θ2<0 时取正号，θ2>0θ2>0 时取负号

---

 　　编写Mathematica代码进行平面二杆机械臂运动模拟，使用Locator来改变工作空间中机械臂末端的目标位置。

​

 ​

​

• Pieper's solution when three axes intersect

 　　机器人运动学中的Pieper准则是：机器人的三个相邻关节轴交于一点或三轴线平行。

　　对于6自由度的机器人来说，运动学反解非常复杂，一般没有封闭解。在应用D-H法建立运动学方程的基础上，进行一定的解析计算后发现，位置反解往往有很多个，不能得到有效地封闭解。Pieper方法就是在此基础上进行研究发现，如果机器人满足两个充分条件中的一个，就会得到封闭解，这两个条件是：
　　（1）三个相邻关节轴相交于一点
　　（2）三个相邻关节轴相互平行（在无限远处交于一点）
　　现在的大多数商品化机器人都满足封闭解的两个充分条件之一。如PUMA和STANFORD机器人满足第一条件，而ASEA和MINIMOVER机器人满足第二条件。以PUMA560机器人为例，它的最后3个关节轴相交于一点。

 　　下面考虑6自由度转动关节机器人最后三根轴交于一点的情况。根据D-H参数坐标系建立方法，这种情况下杆件坐标系{4}、{5}、{6}的原点将位于三根轴线的交汇点。该点在机器人基座标系中的齐次坐标可表示为：

0P4ORG=01T⋅12T⋅23T⋅3P4ORG=⎡⎣⎢⎢⎢xyz1⎤⎦⎥⎥⎥(1)(1)0P4ORG=10T⋅21T⋅32T⋅3P4ORG=[xyz1]

　　或者根据相邻杆件坐标系之间的变换关系：

i−1iT=⎡⎣⎢⎢⎢cθisθicαi−1sθisαi−10−sθicθicαi−1cθisαi−100−sαi−1cαi−10ai−1−sαi−1dicαi−1di1⎤⎦⎥⎥⎥(2)(2)ii−1T=[cθi−sθi0ai−1sθicαi−1cθicαi−1−sαi−1−sαi−1disθisαi−1cθisαi−1cαi−1cαi−1di0001]

　　可以得到：

0P4ORG=01T⋅12T⋅23T⎡⎣⎢⎢⎢a3−d4sα3d4cα31⎤⎦⎥⎥⎥(3)(3)0P4ORG=10T⋅21T⋅32T[a3−d4sα3d4cα31]

　　或：

0P4ORG=01T⋅12T⎡⎣⎢⎢⎢⎢f1(θ3)f2(θ3)f3(θ3)1⎤⎦⎥⎥⎥⎥(4)(4)0P4ORG=10T⋅21T[f1(θ3)f2(θ3)f3(θ3)1]

　　其中：

⎡⎣⎢⎢⎢f1f2f31⎤⎦⎥⎥⎥=23T⎡⎣⎢⎢⎢a3−d4sα3d4cα31⎤⎦⎥⎥⎥(5)(5)[f1f2f31]=32T[a3−d4sα3d4cα31]

　　将参数带入i−1iTii−1T 矩阵得到23T32T ，可以列出f1f1 、f2f2 、f3f3 的表达式：

f1f2f3=a3c3+d4sα3s3+a2=a3cα2s3−d4sα3cα2c3−d4sα2cα3−d3sα2=a3sα2s3−d4sα3sα2c3+d4cα2cα3+d3cα2(6)(6)f1=a3c3+d4sα3s3+a2f2=a3cα2s3−d4sα3cα2c3−d4sα2cα3−d3sα2f3=a3sα2s3−d4sα3sα2c3+d4cα2cα3+d3cα2

　　再根据 01T10T 和 12T21T 我们可以得到

0P4ORG=⎡⎣⎢⎢⎢c1g1−s1g2s1g1+c1g2g31⎤⎦⎥⎥⎥(7)(7)0P4ORG=[c1g1−s1g2s1g1+c1g2g31]

　　其中

g1g2g3=c2f1−s2f2+a1=s2cα1f1+c2cα1f2−sα1f3−d2sα1=s2sα1f1+c2sα1f2+cα1f3+d2cα1(8)(8)g1=c2f1−s2f2+a1g2=s2cα1f1+c2cα1f2−sα1f3−d2sα1g3=s2sα1f1+c2sα1f2+cα1f3+d2cα1

　　接下来我们计算0P4ORG0P4ORG 坐标的平方和：

r=x2+y2+z2=g21+g22+g23(9)(9)r=x2+y2+z2=g12+g22+g32

　　根据方程(8)有：

r=f21+f22+f23+a21+d22+2d2f3+2a1(c2f1−s2f2)(10)(10)r=f12+f22+f32+a12+d22+2d2f3+2a1(c2f1−s2f2)

　　再接着进行变量替换消除对关节转角θ1θ1 的依赖，令：

rz=(k1c2+k2s2)2a1+k3=(k1s2−k2c2)sα1+k4(11)(11)r=(k1c2+k2s2)2a1+k3z=(k1s2−k2c2)sα1+k4

　　其中：

k1k2k3k4=f1=−f2=f21+f22+f23+a21+d22+2d2f3=f3cα1+d2cα1(12)(12)k1=f1k2=−f2k3=f12+f22+f32+a12+d22+2d2f3k4=f3cα1+d2cα1

　　下面考虑根据方程(11)求解θ3θ3 ，分下面3种情况：

　　1. 如果a1=0a1=0 ，则r=k3r=k3 ，由于rr 是已知量，等式右边的k3k3 是θ3θ3 的函数。用三角函数万能公式进行变量替换后可求解θ3θ3 　　

　　2. 如果sα1=0sα1=0 ，则z=k4z=k4 ，由于zz 已知，进行变量替换后可求出θ3θ3

　　3. 如果条件1、2不成立，则从公式(11)中消除s2s2 和c2c2 ，得到：

(r−k3)24a21+(z−k4)2s2α1=k21+k22(13)(13)(r−k3)24a12+(z−k4)2s2α1=k12+k22

　　对方程(13)进行变量替换后可求解θ3θ3 ，之后可根据方程(11)求解θ2θ2 ，根据方程(7)求解θ1θ1 。

　　最后还需要求解θ4θ4 、θ5θ5 、θ6θ6 。机械臂最后三个关节的轴线交于一点，这三个关节的转角会影响末端姿态，可以从代表末端姿态的旋转矩阵06R60R 中求解出θ4θ4 、θ5θ5 、θ6θ6 。在之前求解得到θ1θ1 、θ2θ2 、θ3θ3 后我们可以计算出矩阵04R|θ4=040R|θ4=0 ，表示θ4=0θ4=0 时杆件坐标系{4}相对于基坐标系的姿态。从坐标系{4}到末端坐标系{6}的姿态变化由最后三根轴决定，写成矩阵变换的形式如下：

46R|θ4=0=04R−1|θ4=0⋅06R(14)(14)64R|θ4=0=40R−1|θ4=0⋅60R

　　已知46R|θ4=064R|θ4=0 后最后三个关节转角可根据欧拉角与旋转矩阵之间的关系计算出来。

参考：

Introduction to Robotics - Mechanics and Control. Chapter 4 Inverse manipulator kinematics

Forward and Inverse Kinematics for Two-Link Arm

V-rep学习笔记：机器人路径规划2

V-rep学习笔记：机器人逆运动学数值解法（The Jacobian Transpose Method）

V-rep学习笔记：机器人逆运动学数值解法（The Pseudo Inverse Method）

V-rep学习笔记：机器人逆运动学数值解法（Damped Least Squares / Levenberg-Marquardt Method）