基于旋量理论的7自由度机械臂运动学建模与分析

李冠琦，武建德，李瑞琴

(中北大学 机械工程学院，山西 太原 030051)

0 引言

机械臂是机器人技术领域中应用最为广泛的自动化装置，在工业制造、医疗康复等领域都有其应用实例，拟人化机械臂技术也逐渐成熟[1-4]。匹兹堡大学生物医学团队在BCI机械臂假肢里引入实时触觉反馈，使完成任务的时间减少一半。波士顿动力开发Stretch移动式7R机械臂已应用于仓库运输。

20世纪80年代学者们开始讨论将旋转轴从数学中引入机器人研究的可行性，旋量理论[5-9]日益成熟，然而对其的应用介绍却鲜见报道。本文建模7自由度串联机械臂，着重对螺旋轴这一单位矢量在运动学的应用加以详细描述。

6自由度逆解有通用公式，7R逆解包含32个实根，出现虚空间或自运动歧。7R逆解可结合Newton-Raphson数值迭代法求解[10]。

使用Matlab进行正解中的化简与直接展开等运算过程的比较，并且同时使用开源Python对结果进行比较。使用基于空间(space简称s)坐标系的空间雅可比矩阵，找出数值迭代法求逆解的适用范围，基于正解的前提下，验证逆解的准确性。

1 旋量理论

螺旋(旋量——旋转向量)理论在正运动学中的应用。

1.1 M矩阵(0位置、起始位置)

M矩阵为当所有的关节转角都为0的时候，操作空间坐标系(body简称b)在s坐标系中的位置和姿态矩阵。

1.2 旋转向量与指数积公式

(1)

(2)

T07=eS7θ7M

(3)

特殊正交群是所有有效的3×3旋转矩阵的集合群SO(3)：包括R、ω、eωθ。特殊的欧几里得集合群或刚体运动群或R3中的同质变换矩阵Se(3)表示位姿：包括M、eSθ、T07。

(4)

式(4)中，可利用特性ω3=-ω来化简，且eωθ有Rodrigues公式：

f(θ)=Iθ+(1-cosθ)ω+(θ-sinθ)ω2

(5)

eωθ=I+sinθω+(1-cosθ)ω2

(6)

依次解锁一个角度，往前代值，得到

T07=eS1θ1…eS7θ7M

(7)

2 数值法逆解

使用非线性寻根的Newton-Raphson方法，有寻根、不存在根时寻找近似解、存在多个解时寻找最优解的优势。给定一个初始值，然后代入迭代式求解直到出现误差范围内的解。

设正向运动学函数为f(θd)，末端执行器的位置向量为ν，非线性寻根Newton-Raphson是找到目标函数的解。几何视角如图1所示；式(8)是解析视角。式(9)-式(11)是计算雅可比矩阵。

图1 迭代法几何过程

Δθ=J-1(θ0)[vd-f(θi)]

(8)

(9)

(10)

(11)

式中*为其伴随矩阵。

对这种算法的进一步改进：

1)末端执行器的位置描述f(θd)变更为正运动学计算出的矩阵T07。

3)引进伪逆矩阵J†避免求解奇异时无解的情况。在Matlab里编程为pinv(J)。

当前基于b坐标系，变换为基于s坐标系：

(12)

(13)

式中：Tsb是坐标转移矩阵；*为求其伴随矩阵。

改进算法的流程图如图2所示。

图2 改进的数值迭代法流程图

3 验证正逆解

3.1 正运动学方程

(14)

(15)

图3 三维模型图及7R示意图

根据式(3)，使用矩阵分块后化简的运算过程，编程并计算结果。

(16)

根据式(3)，发现将矩阵指数直接泰勒展开(采用了3种计算方法：Pada法、特征值法、6次的泰勒展开)得出的结果虽然一致，但是它是虚数形式，增加了计算量。结果：

(17)

使用先进行矩阵分块然后利用特性化简的运算方法，该结果更准确、后期的运算量更小。代入式(7)，使用该方法分别在Matlab和Python中运算，得出的结果一致：

r1=150s1234-5-1 350s12347-300s123-675s123457-6-

675s123467-5-150s12345-1 350s1234-7+600s1234+300s12+

675s12345-6-7+675s12346-5-7-300s1+750s12345-6-750s12346-5

r2=150c1234-5+1 350c12347+300c123+675c123457-6+

675c123467-5-150c1234-5+1 350c1234-7-600c1234-300c12-

675c12345-6-7-675c12346-5-7+300c1-750c12345-6+750c12346-5

r3=2 700s7c5s6-s5c6-1 500s5s6-c5(1 500c6-1 500)-

1 200c5+1 200

3.2 逆运动学方程

1)第一次验证

设初始位置为正解的0位置，转动角度为θlist。

[3.142;1.571;1.047;0.785;0.628;0.524;0.449]

给逆解的初始值[3,1.5,1,0.6,0.5,0.4,0.3]。解得[3.161,1.563,1.059,0.762,0.628,0.523,0.449]。

2)第二次验证

因发现误差较大：1)改用角度制；2)迭代次数增加到1 000次；3)精度调整为eω<0.000 1，ev<0.000 1。经过多组数据实验发现，相差3°以内，位置完全重现；相差10°以内，第一角度有0.1°的偏差；相差15°以上的逆运算，第一个角度有1°以上的偏差。所以，该方法求逆解有完全重现的适用范围，需要把初始解猜测在真实解的附近15°以内(表1)。

表1 正逆解互相验证 单位：(°)

4 结语

本文重点介绍了如何使用形如螺旋楼梯的转向量计算正逆解。结合旋量与Newton-Raphson数值法求逆解。为涵盖转动机构可能出现的问题，采用目前串联机构中最复杂的7R机构并且使机构尽可能复杂，但是设计中没有涵盖helical螺旋和cylindrical圆筒等机构。在比较了不同的运算方法后，计算正解，得出先将矩阵分块、再利用特性化简的方法更好的结论。在计算逆解的过程中，发现当初始猜测的第一个角度超过真实解20°，Newton-Raphson数值法不会重现正解。验证结果表明旋量形式美观，建模快捷，编程明了，适用于机械臂中的串联机构、并联机构、转动关节、平动关节、螺旋关节等的运动学、动力学特性建模和分析。