基于倍矩阵的1P5R串联机械手逆运动学分析

　　摘要: 将倍矩阵引入到串联机械手运动学分析的建模中，结合四维旋转矩阵，提出了一种空间 1P5R 串联机械手逆运动学分析的新方法.基于倍矩阵和四维旋转矩阵对空间 1P5R 串联机械手进行运动学建模，直接得到 14 个逆运动学约束方程;将 14 个方程通过线性消元和 Sylvester 结式消元，得到只含 1 个关节变量的一元 16 次方程.通过数字实例求解和 SolidWorks 软件仿真验证了所提方法的正确性.所提方法最大的优势在于可直接获得 14 个逆运动学约束方程，不需要进行矢量运算或者投影等，为空间其他串联机械手的逆运动学建模提供了一种新思路.

　　关 键 词: 1P5R 串联机械手;逆运动学分析;倍矩阵;四维旋转矩阵;Sylvester 结式消元

　　张英; 黄起能; 魏世民; 廖启征 北京邮电大学学报 2021-12-05

　　串联机械手因具有工作空间大、运动灵活、非冗余等特点，被广泛地应用于工业和航天等多个领域中，因此，对串联机械手的研究显得十分重要 [1] .其中，串联机械手的逆运动学问题一直是机器人领域学者密切关注的难点问题.串联机械手的逆运动学通过给定机械手末端执行器的位姿(位置和姿态)计算各关节的运动变量.对于一般串联机械手的运动学问题，经典 的 建模方法有 D-H ( Denavit-Hartenberg ) 矩阵法 [2] 和 指 数 积 方 法 [3-4].其中，D-H 矩阵法在串联机械手的每个连杆上固定一个坐标系，然后用 4 4 的齐次变换矩阵描述相邻连杆的空间关系，从而推导出运动学方程.此外，串联机械手的运动学建模还常使用对偶矩阵法 [5-6]、对偶四元数法[7]、倍四元数法[8]、运动映射法 [9]和几何代数法[10]等数学方法，其中，前 3 种方法也是通过描述连杆坐标系之间的坐标变换来建立运动学方程;对偶矩阵法和 D-H 矩阵法类似，属于矩阵建模方法，而对偶四元数法和倍四元数法属于四元数建模方法，其运算时需要运用对应的四元数运算法则.串联机械手逆运动学的约束方程组通常具有高维、非线性的特点，求解复杂且不易求出.目前常用的求解方法有数值解法、封闭解法和人工智能法 [1]，这些方法各有优缺点.数值解法通用性高，但是其缺点在于依赖初始值选取，往往无法得到全部解.封闭解法的优点在于可以求得全部解，但是因为其运算量大，涉及符号运算、求解过程复杂等特性，对工程人员的数学基础有一定的要求.人工智能算法相比于数值解法和封闭解法属于新兴算法，在求解机械手逆运动学上取得了一定的成果，但是其求解精度方面仍然需要一定的提升.

　　自 20 世纪 80 年代以来，众多学者[6-13]在分析串联机械手的逆运动学问题方面做出了巨大贡献.廖启征[6]采用酉交矩阵的方法解决所有空间单自由度单环串联机构的运动学分析方法.Raghavan 等[11]采用 D-H矩阵法和矢量方法解决所有 6自由度串联机器人机构逆运动学的分析方法.Konli 等[12] 采用和文献[11]一样的建模方法对 6R 和 1P5R 串联机械手的逆运动学进行建模，但是采用不同的求解方法，最终将求解简化为特征向量的问题，得到没有增根的 16 次多项式.于艳秋等[13]对一种特殊结构的医用 1P5R 机械手的逆运动学进行求解，得到了该问题的 12 组解.提出了一种基于倍矩阵新理论[14]和四维旋转矩阵的建模新方法，使用该方法对空间 1P5R 串联机械手的逆运动学问题进行建模，直接得到用于消元求解的 14 个运动约束方程，然后采用线性消元和 Sylvester结式消元结合的方法得到该问题的一元 16 次方程，最后通过数值实例和 SolidWorks 软件仿真验证了所提方法的正确性.

　　1 倍矩阵和四维旋转矩阵表示将倍数引入机器人机构运动学的建模中，类比对偶代数在三维空间机器人机构运动建模的表示，通过研究对偶四元数和倍四元数在三维空间运动中的关联关系，基于对偶矩阵理论，黄起能[14]提出了倍矩阵的新理论.倍矩阵和倍四元数相似，通过 G 部与 H 部共同描述三维空间刚体变换.因此，使用倍矩阵理论描述空间串联机构运动学时，需要分别建立空间串联机构的 G 部与 H 部数学模型.

　　绕 z 轴旋转角并沿 z 轴平移 d ，其倍矩阵的 G 部和 H 部[14]分别表示为       cos sin 0 ( , ) sin cos 0 0 0 1 z                      G (1)        cos sin 0 ( , ) sin cos 0 0 0 1 z                      H (2) 其中:   tan /  d R ， R 为四维球体的半径，一般取 R L  /  ，L 为机械手机构工作空间的最大边界值，是期望的近似精度.绕 x 轴旋转并沿 x 轴平移 a ，其倍矩阵的 G 部和 H 部[14]分别为       1 0 0 ( , ) 0 cos sin 0 sin cos x                      G (3)        1 0 0 ( , ) 0 cos sin 0 sin cos x                      H (4) 其中:   tan /  a R .根据式(1)~式(4)，两个相邻连杆坐标系 j 1和 j之间变换关系的倍矩阵为    1 , , j j z j j x j j     G G G  (5)     1 , , j j z j j x j j     H H H  (6) 基于倍矩阵理论对空间串联机械手建模时，需要结合三维空间刚体运动的四维旋转矩阵表示.接下来给出三维空间刚体运动的四维旋转矩阵表示.绕 z 轴转动并沿 z 轴平移 d 的四维旋转矩阵[14]为  cos sin 0 0 sin cos 0 0 , 0 0 cos sin 0 0 sin cos                 Z (7) 绕 x 轴转动并沿 x 轴平移 a 的四维旋转矩阵 [14]为  cos 0 0 sin 0 cos sin 0 , 0 sin cos 0 sin 0 0 cos                X (8)

　　因此，根据式(5)和式(6)，两个相邻连杆坐标系 j 1和 j之间变换关系的四维旋转矩阵为    1 4 , , j j j j j j     T Z X  (9) 根据式(5)、式(6)和式(9)，可以对空间 1P5R 串联机械手的逆运动学问题进行直接建模和代数求解.其运动学模型所需要的 14 个实数方程可以直接获得，不像传统矩阵建模方法，如 D-H 矩阵法或对偶矩阵法等，建模时需要进行矢量运算或者投影等.

　　2 1P5R 串联机械手的运动约束方程

　　空间 1P5R 串联机械手，其运动链如图 1 所示，其中 , , , j j j j   d a 为 D-H 参数，分别表示连杆 j 的关节角、偏距、连杆长度和扭转角.根据 D-H 参数和图 1 所建立的坐标系，使用式(5)、式(6)和式(9)，建立基于倍矩阵和四维旋转矩阵表示的一般 1P5R 串联机械手运动学方程如下: 6 0 0 1 2 3 4 5 6 1 2 3 4 5 6 1 ( , ) ( , ) z j j x j j j     G G G G G G G G G   (10) 6 0 0 1 2 3 4 5 6 1 2 3 4 5 6 1 ( , ( , ) z j j x j j j     H H H H H H H H )H   (11)     6 0 0 1 2 3 4 5 6 4 1 4 2 4 3 4 4 4 5 4 6 4 1 , , j j j j j     T T T T T T T Z X   (12) 其中: 0 6G 和 0 6H 为三维单位正交矩阵， 0 6 4 T 为四维单位正交矩阵.根据 P 副所在的关节位置，空间 1P5R 串联机械手可以分为 6 种情况，分别对应 P 副在第 1，2， 3，4，5 和 6 关节位置处.由于倍矩阵和四维旋转矩阵均将三维空间刚体的平移近似替换为四维空间刚体的旋转，因此，6 类 1P5R 串联机械手逆运动学的建模和代数求解过程是相似的，因此，接下来仅以第 6 个关节位置为 P 副的 1P5R 串联机械手(即RRRRRP 串联机械手)为例具体说明其建模和代数求解过程.

　　3 1P5R 串联机械手逆运动学的建模和代数求解

　　3.1 1P5R 串联机械手逆运动学的建模

　　将式(10)~式(12)移项，并分离变量，得到: 2 3 4 1 1 0 1 0 5 1 3 4 5 2 1 6 6    G G G G G G G  (13) 2 3 4 1 1 0 1 0 5 1 3 4 5 2 1 6 6    H H H H H H H  (14) 2 3 4 1 1 0 1 0 5 1 3 4 4 4 5 4 2 4 1 4 6 4 6 4    T T T T T T T  (15) 展开式(13)或式(14)得到:                       ( ) ( ) ( ) 1 3 4 5 1 3 4 5 1 3 4 5 ( ) ( ) ( ) 1 3 4 5 1 3 4 5 1 3 4 5 ( ) ( ) ( ) 1 4 5 1 4 5 1 4 5 ( ) ( ) ( ) 2 1 2 6 2 1 2 6 2 1 2 ( ) ( 2 1 2 6 2 , , , , , , , , , , , , , , , , , , , , , , x x x y y y z z z x x x y y J K N J K N J K N J K N J K                                   G H G H G H G H G H G H G H G H G H G H G H G H G H G H          ) ( ) 1 2 6 2 1 2 ( ) ( ) ( ) 2 1 2 6 2 1 2 6 2 1 2 , , , , , , , , y z z z N J K N                G H G H G H G H (16) 其中      J K N       j j j j j j , , , , ,   G H G H G H 是关于 j 和 j 的函数.由式(16)可知，式(13)或式(14)等号右边的矩阵第 3 列中 3 个元素均不含关节变量 6 ，因此，提取式(13)和式(14)等号两边矩阵第 3 列的 3 个元素，组成不含关节变量 6 的 6 个等式: N N x x 1 3 4 5 2 1 2      , , = ,    G G (17) N N y y 1 3 4 5 2 1 2      , , = ,    G G (18) N N z z 1 4 5 2 1 2     , = ,    G G (19) N N x x 1 3 4 5 2 1 2      , , = ,    H H (20) N N y y 1 3 4 5 2 1 2      , , = ,    H H (21) N N z z 1 4 5 2 1 2     , = ,    H H (22) 展开式(15)得到: 其中 J K N P         j j j j j j j j , , , , , , ,        T T T T 是关于 j 和 j 的函数.由式(23)可知，式(15)等号右边矩阵的第 1 和第 2 列的所有元素均不含关节变量 6  ，因此，提取等号两边矩阵第 1 列和第 2 列的全部 8 个元素，组成不含关节变量 6 的 8 个等式: J J x x 1 3 4 5 2 1 2      , , = ,    T T (24) J J y y 1 3 4 5 2 1 2      , , = ,    T T (25) J J z z 1 4 5 2 1 2     , = ,    T T (26) J J m m 1 4 5 2 1 2     , = ,    T T (27) K K x x 1 3 4 5 2 1 2      , , = ,    T T (28) K K y y 1 3 4 5 2 1 2      , , = ,    T T (29) K K z z 1 4 5 2 1 2     , = ,    T T (30) K K m m 1 4 5 2 1 2     , = ,    T T (31) 式(17)~式(22)和式(24)~式(31)就是一般 1P5R 串联机械手逆运动学分析的 14 个基本运动学封闭方程，14 个方程只含有 5 个关节变量 1 2 3 4     , , , 和 5 .

　　3.2 1P5R 串联机械手逆运动学的代数求解

　　将上述 14 个逆运动学方程整理为:    1 T 4 5 4 5 4 5 4 5 4 4 5 5 2 1 T 1 2 1 2 1 2 1 2 1 1 2 2 2 , , , , , , , ,1 , , , , , , , c s c c s s s c c s s c s s s c c s c c s c s c           L L R R (32) 其中: sin j j s   ， cos j j c  ; L1 为 8 9 维矩阵， L2 为 6 9 维矩阵，它们的元素均是 3 3 s c, ,1 的线性组合; R1 为 8 8 维矩阵， R2 为 6 8 维矩阵，它们的元素全是常数.假定矩阵 R1 是非奇异的，线性求解式(32)其中的 8 个方程组，并将其结果代入余下的 6 个方程，得到: L = O c s c c s s s c c s s c , , , , , , , ,1 (33) 其中: 1 3 2 2 1 1  L L R R L   ，为 6 9 维矩阵，它的元素是 3 3 s c, ,1 的线性组合; O 为 6 1维的零矩阵.接下来，对 3 、  4 和 5 使用欧拉变换 s t t j j j    1/ / 2i,  c t t j j j j     1/ / 2 3,4,5  ，，其中， t j j j   exp i 3,4 5   ， ，，并将式(33)通分，提取分子得到: T 2 2 2 2 2 2 4 4 5 4 5 4 4 5 4 5 4 5 5   t t t t t t t t t t t t , , , , , , , 1   L O ， (34) 其中: L4 为 6 9 维矩阵，它的元素均是 3 3 s c, ,1 的线性组合.将式(34)乘以 4 t ，得到 6 个新方程，但只产生 3 个新的单项式 3 2 3 3 4 5 4 5 4 t t t t t , , ，将其与式(34)组合， 12 个方程可表示为 4 5 12 4        L 0 L X X O 0 L (35) 其 中 : X  T 3 2 3 3 2 2 2 2 2 2 4 5 4 5 4 4 5 4 5 4 4 5 4 5 4 5 5   t t t t t t t t t t t t t t t t t , , , , , , , , , , 1   ， ; 4 5 4 0 0       L L L 为 X 的系数矩阵，是一个 12 12 维的方阵，其元素都是 3 t 的二次表达式; O12 为 12 1维的零矩阵.根据克莱姆法则可知，式(35)有解的必要条件就是其系数矩阵 L5 的行列式为 0，即  4 5 4 0 det det 0 0              L L L (36) 展开式(36)，可以得到一个关于变量 3 t 的一元 16 次方程.求解式(36)得到变量 3 x ，进而根据 3 3 3 1 2arctan(i ) 1 t t ，求得角度 3 .将求解得到的 3 t 代入式(35)，从式(35)中选出 11 个方程，将 3 2 3 3 2 2 2 2 2 4 5 4 5 4 4 5 4 5 4 4 5 4 5 t t t t t t t t t t t t t t , , , , , , , , 2 4 5 5 t t t , , 当作假想变量求解该线性方程组，得到变量 4 5 t t, 的 相 应 解 ， 进 而 根 据 1 arctan(i ) 1 j j j t j t  ， 4,5 ，求得角度 4 和 5 .将 3 4 5    , , 的结果带入式(32)的前 8 个方程，并线性求解，得到 1 2  , 的正、余弦值 , ( 1,2) j j c s j  ，根据 arctan( / ) 1,2 j j j    s c j ， ，求得  1 2 和 .在式(13)~式(15)中任取 2 个包含未知数 6 的等式，将 1 2 3 4 5      , , , , 值代入，联立求解得到 6 的正、余弦值 6 6 c s, ，根据 6 6 6   arctan( / ) s c ，求得 6 ，接着根据 6 6 d R  求得关节 P 副的位移.至此，RRRRRP 串联机械手的 6 个关节未知数全部求得.对于 P 副位于其他关节位置的 1P5R 串联机械手，它们的逆运动学算法与 RRRRRP 串联机械手的相似，区别主要在于从四维旋转矩阵建立的运动学方程中如何选取 8 个用于消元求解的方程.P 副位置的不同不影响从基于倍矩阵建立的运动学方程选取的 6 个用于消元求解方程，即式(13)或式(14)的展开式(16)没有变化，因为 j 和 j 在式(1)~ 式(4)的位置是相同的.如果第 6 个关节不是 P 副，那么式(15)的展开式会有所不同，此时式(15)及其展开式的右边矩阵是第 3 和第 4 列的所有元素均不含关节变量 6 ，而非 3.1 节中提到的第 1 和第 2 列的所有元素不含关节变量 6 ，此时，选取 8 个用于消元求解的方程是第3和第4列的所有元素.从四维旋转矩阵建立的运动学方程中选取方程不同的原因在于 j 和 j 在式(7)中的位置是不同的， j 位于式(7)的前两行两列，而 j 位于式(7)的后两行两列，因此，展开时导致其位置也不同.除了上述提及的主要区别，其他过程都是类似的，不同仅在于未知数有可能是 j ，而非 j .与现有 D-H 矩阵法相比，不需要更改式(32)~式(35)中的单项式表示，3.2 节中的单项式表示全部是角度的正余弦表示，而不会出现位移表示，因此具有统一建模的特性.

　　4 数值实例

　　为了验证基于倍矩阵对 1P5R 串联机械手逆运动学问题进行建模和求解的正确性，采用文献[12] 的结构参数，如表 1 所示.将参数代入式(24)~ 式(36)，得到 RRRRRP 串联机械手逆运动学的全部 16 组解，表 2 所示为其中 4 组实数解.将表 2 的结果与文献[12]的结果进行对比，在误差范围内结果相同，验证了方法的正确性.由表 2 可知，表 2 中的第 2 组解与表 1 中输入参数一致，也验证了该解的正确性. 接下来，使用基于 SolidWorks 软件中对表 2 里的 4 组实数解建立 RRRRRP 串联机械手的 3D 模型.为了直观体现 4 组解连杆的相对位置关系，将 4 组 3D 模型呈现在同一个模型内，如图 2 所示.由图 2 可知，4 组 1P5R 串联机械手的末端位姿重合在一起，说明了通过所提方法得到的 4 组解的一致性和正确性

　　5 结束语

　　提出了一种基于倍矩阵和四维旋转矩阵的 1P5R 串联机械手逆运动学问题的新方法.所提方法是基于矩阵建模并求解，相较于传统的矩阵方法，如 D-H 矩阵法或对偶矩阵法等，所提方法主要有以下 3 个优势: (1)基于倍矩阵和四维旋转矩阵建模，能够直接得到 1P5R 串联机械手逆运动学问题的 14 个运动约束方程，不需要进行矢量运算或投影运算等;(2)所提方法由于将三维空间刚体的平移位移近似替代为四维空间刚体的旋转位移，即把三维空间的平移和转动统一起来，从而使得 6 类 1P5R 串联机械手的逆运动学可以用统一的方法建模与代数求解.尽管基于四元数法也可对该问题实现统一建模和代数求解，但是所提方法属于矩阵建模，不需要掌握四元数的运算法则等，易于编程实现，更适合工程师掌握和使用.