基于倍矩陣的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 串聯機械手的逆運動學可以用統一的方法建模與代數求解.盡管基于四元數法也可對該問題實現統一建模和代數求解，但是所提方法屬于矩陣建模，不需要掌握四元數的運算法則等，易于編程實現，更適合工程師掌握和使用.