基于单纯形法的机械手位姿逆解算法

下面简要介绍一下单纯形法的计算方法与计算步骤：针对目标函数 J = f (p),p ∈ Rn ，
3.1 n 维单纯形初始化：
（1）、根据迭代的初始点生成由 n+1 个 n 维向量表示的顶点（ p(i),i = 1, 2,..., n +1）组成的
单纯形，同时给定镜像系数αm > 0 ，扩展系数αe > 1、压缩系数αc ∈ (0,1) 和迭代中止条件，如： J ≤ ε ，或 Jk − Jk−1 ≤ δ ,δ > 0
2、问题描述
机械手位姿逆解问题是指在给定工具坐标系中的目标位姿和当前位姿的信息后，求取由 当前位姿到达目标位姿所需的关节转角，即根据给出或推导得到机械手的位姿正解映射矩阵
516
gst (θ) 和目标位姿矩阵 ggoal ，求取满足（1）的向量 θ ：
Π gst (θ) = n eξˆiθi • gst (0) = ggoal i =1
基于单纯形法的机械手位姿逆解算法
蔡骅
胡维礼 李 胜
南京理工大学自动化系，南京 210094 摘要：本文提出了一种基于单纯形方法的机械手位姿逆解的分步求解方法。这种方法充分利用了单纯形法
大范围收敛和计算简单的特性，在不计算目标函数一阶导数的情况下，确定极值点的查找方向与步长，多
次迭代，直至目标函数满足所给条件。最后，用一个六自由度的肘机器人验证了该求解方法的有效性。 关键字：单纯形法、运动逆解、机械手
Π e 6 ξˆiθi
i =1
⋅
gst (0)
其中：
⎡cosθ1 − sinθ1 0 0⎤
eξˆ1θ1
=
⎢ ⎢
sin
θ1
cosθ1
0 0⎥⎥
⎢0
0 1 0⎥
⎢ ⎣
0
0 0 1⎥⎦
⎡1 eξˆ3θ3 = ⎢⎢0
⎢0 ⎢⎣0
0 cosθ3 − sinθ3
0
0 sin θ3 cosθ3
0
0
⎤
l1
(1
−
cos
（1）
其中： gst : Q → SE(3) ；n 为机械手的关节数；θi 为第 i 个关节的转角（针对纯转动关节）
或位移（针对纯移动关节），ξˆi 为各关节的运动旋量矩阵。 gst (0) 为给定当前位姿矩阵信息，
ggoal ∈ SE(3) 为给定的目标位姿矩阵。
SE(3) 是 机 械 手 位 置 空 间 p ∈ R3 与 姿 态 空 间 SO(3) 的 组 合 ， 即 ：
3.2 计算查找方向：
（2）、计算 n+1 个顶点的函数值，即计算 J (i) = f (p(i) ) ，
（3）、找出使 J 为最大的最高点 p(max) ，使 J 为最小的最低点 p(min) ，使 J 为次高的次 高点 p(sub) ，其中：max,min,sub ∈[1, 2,...n +1] ；判断 J (min) 是否满足中止条件，如成立，则 p(min)
向量的函数。
i =1
从（2）可推得：
⎡Se ⎢⎣ 0
pe 1
⎤ ⎥⎦
−
F(θ)
⎡ ⎢⎣
S0 0
p0 1
⎤ ⎥⎦
=
0
（3）
取目标函数 f (θ) 为：
f (θ) =
⎡Se ⎢⎣ 0
pe 1
⎤ ⎥⎦
−
F(θ)
⎡ ⎢⎣
S0 0
p0 ⎤ 1 ⎥⎦ F
（4）
其中： • 表示求矩阵的 Frobenius 范数。 F
本文尝试利用单纯形算法解决机器人位姿逆解的问题，这种算法具有大范围收敛的特 性，而且计算简单，不需要计算目标函数的一阶导数，并针对多自由度的机械手提出了一种 分步求解的方法，以降低每次求解的维数，大大提高了多自由度机械手的求解精度和求解速 度。 本文内容主要分为三个部分：（1）问题描述：简要介绍了机械手位姿逆解问题的提出 和优化方法在其中的应用；（2）单纯形算法：简要介绍了经典单纯形法的计算步骤；（3）示 例与仿真结果：主要通过一个 6DOF 机械手说明了分步单纯形方法在其中的应用，通过与共 轭梯度法比较，说明了该方法在计算时间上的优越性。另外，通过与不采用分步方法的单纯 形算法的结果进行比较，说明了在高自由度机械手中采取分步方法的必要性。
如果 f (p(min) ) ≤ f (p(mirror) ) ≤ f (p(sub) ) ，则 p(max) = p(mirror ) ；转至（3）；
如果 f (p(mirror) ) ≤ f (p(min) ) ，则转至（7）；
如果
f (p(sub) ) ≤
f (p(mirror ) ) ≤
，则 f (p(max) )
An algorithm of Inverse Kinematics for Manipulators based on Simplex
Cai Hua Hu Weili Li Sheng
The department of automation, Nanjing University of Science and Technology, 210094
为所求向量，否则，继续（4）。
∑ （4）、计算除最高点外的 n 个顶点组成的几何体的中心： p(mid) = 1 ( n+1 p(i) ) n i=1,i≠max （5）、确定查找方向： D = p(mid ) − p(max)
3.3 计算查找步长：
（6）、计算镜像点及其函数值： p(mirror ) = p(mid ) + αm (p(mid ) − p(max) ), f (p(mirror ) )
p(compress0 )
= p(mirror) ，转至（8）；
如果
f (p(max) ) ≤
，则 f (p(mirror) )
p(compress0 )
= p(max) ，转至（8）；
（7）、扩展计算： p(expand ) = p(mid ) + αe (p(mirror ) − p(mid ) ), f (p(expand ) )
⎡1 eξˆ2θ2 = ⎢⎢0
⎢0 ⎢⎣0
0 cosθ2 − sinθ2
0
0 sin θ 2 cosθ2
0
0⎤
−l0 sinθ2
⎥ ⎥
l0
(1
−
cos 1
θ2
)
⎥ ⎥ ⎦
⎡cosθ4 − sinθ4 0 (l1 + l2 ) sinθ4 ⎤
eξˆ4θ4
=
⎢ ⎢
sin
θ
4
cosθ4
0 (l1 + l2 )(1− cosθ4 )⎥⎥
如果 f (p(expand ) ) ≤ f (p(mid ) ) ，则扩展成功： p(max) = p(expand ) ；转至（3）； 如果 f (p(expand ) ) > f (p(mid ) ) ，则扩展失败； p(max) = p(mirror) ；转至（3）；
（8）、压缩计算： p(compress)
对于简单结构的机械手，其封闭解经过数学推导很容易得到，但随着机械手的灵活性增 加，其结构也越来越复杂，普通方法很难得到它的封闭解公式，即使利用分解消元法[1]可以 得到，结果也很复杂，不适合实际应用，因此对于结构复杂的机械手通常采取数值解法来得 到位姿逆解。目前，求取位姿逆解的数值解的方法主要有牛顿法、最速下降法、信任域法、 遗传算法等[2,3]，这些方法大多数都要用到目标函数的导数信息，包括一阶偏导和二阶导数 的Hessian矩阵，计算繁琐。
517
单纯形法在这两点上克服了其他算法的不足，它在保证大范围收敛的特性的同时，只要求目 标函数连续即可，在实际计算时不需要求取目标函数的一阶导数。
单纯形法的基本思想[4]就是针对n维向量给出n+1 个点，将选取的n+1 个点作为n维单纯 形的n+1 个顶点，求取n+1 个顶点上的函数值，找出其中带有最大函数值的顶点（最高点）、 最小函数值的顶点（最低点），仅低于最高点的顶点（次高点）。通过反射、扩展和压缩求取 一个新的较好的顶点代替最高点，形成一个新的单纯形，或者通过向最低点收缩形成一个新 的单纯形，已逐步逼近函数的极小值点。
Abstract: In this paper, it is proposed that a substep algorithm of inverse kinematics for manipulators based on simplex. This method utilized the feature of simplex: large-scale convergence and simple compute. This algorithm doesn’t need to computer the first derivative of objective function and determine the searching direction and step. After iterations, the algorithm can converse the solution of problem. At last, a case of manipulators with 6 DOF is given and the advantages of this algorithm are shown. Keywords: Simplex Inverse Kinematics Manipulators
g
st
(0)
=
⎡ ⎢ ⎣
Rcurrent 0
⎡1 0 0 0⎤
⎢0
01
0
⎥
⎢ ⎣
0
00
1
⎥ ⎦
⎡ cosθ6 0 − sinθ6 l0 sinθ6 ⎤