基于幾何法的碼垛機械臂逆向求解

李照祥 胡杰

摘要：逆向解算是機械臂抓取目標物的前提，是實現機械臂軌跡規劃、動態導航避障的基礎。常見的機械臂運動學解算方法有D-H法、解析法、POE法和人工智能法。此處，以物流、工業應用場景中常見的四軸碼垛機械臂為研究對象，使用幾何方法，針對其機械結構和幾何特征推導出求解公式，實現碼垛機械臂的定點逆向求解功能。然后，將上述逆向求解得出的結果代入D-H正向解算方法，求出的此時的末端執行器的坐標，并與目標點進行對比驗證。最后，將逆向求解方法應用到實際的四軸碼垛機械臂中，觀察其實際運行時的執行效果。碼垛機械臂的逆向求解方法的實現為機械臂開發和實景應用的設計提供了理論基礎。

關鍵詞：碼垛機械臂;運動學分析;D-H建模;自動抓取

中圖分類號 TP391? ? 文獻標識碼：A

文章編號：1009-3044（2021）11-0216-04

Reverse Solution of Palletizing Manipulator Based on Geometric Method

LI Zhao-xiang， HU Jie

（School of Computer Science， Yangtze University，? Jingzhou 434000， China）

Abstract： Reverse calculation is the prerequisite for the robot arm to grasp the target， and it is the basis for the robot arm trajectory planning and dynamic navigation obstacle avoidance. The common kinematics calculation methods of mechanical arm include D-H method， analytical method， POE method and artificial intelligence method. Here， taking the common four-axis palletizing manipulator arm in logistics and industrial application scenarios as the research object， using geometric methods， the solution formula is derived for its mechanical structure and geometric characteristics， and the fixed-point reverse solution function of the palletizing manipulator arm is realized. Then， substitute the result obtained by the above-mentioned reverse solution into the D-H forward solution method to obtain the coordinates of the end effector at this time， and compare and verify with the target point. Finally， the inverse solution method is applied to the actual four-axis palletizing manipulator to observe its execution effect during actual operation. The realization of the inverse solution method of the palletizing manipulator provides a theoretical basis for the development of the manipulator and the design of real-world applications.

Key words： palletizing manipulator; kinematics analysis; D-H modeling; automatic grasping

1引言（Introduction）

物流行業的快速發展，傳統的人工分揀方式已不再能滿足物流對速度和成本的要求;工業上的機械自動化不僅能提升了生產效率，而且能降低工人勞動強度。傳統上生產環節中簡單又重復的分揀、搬運動作終將被機械臂取代[1]。其中，四自由度的碼垛機械臂可像人工一樣靈活地完成從一個平面抓起目標物，放置到另一個平面的執行動作流程，并且可以根據實際需求，選擇設定固定執行序列或是動態實時響應。為了完成上述的抓取放置功能以及如何規劃設計對應的生產線布局，碼垛機械臂的逆向求解方法將是兩者的前提基礎。在已知目標物相對于機械臂基座的空間坐標位置的前提下，本文基于幾何方法對四軸碼垛機械臂進行分析，推導出了能自動計算碼垛機械臂逆向求解方法。并使用較為熟悉的D-H方法對其進行正向運動學驗證。

2機械臂模型的簡化（Simplified Robotic Arm Model）

如圖1所示，碼垛機械臂可分為基座、大臂、小臂、末端夾具。分別在每個關節處建立笛卡爾坐標系。其中底座的電機（M1）可控制整個機械臂繞Z軸旋轉，大臂和基座之間的電機（M2）可控制整個大臂繞Y軸旋轉，小臂和大臂之間的電機（M3）可控制小臂繞Y軸旋轉。末端夾具和小臂之間的電機（M4）可控制末端繞Y軸旋轉。大臂、小臂、末端夾具的長度分別為：L1、L2、L3。

根據圖1，可將其簡化為如圖2所示的三維空間及線段。簡化流程是：可在基座中心處建立三維空間的笛卡爾坐標系，大臂、小臂、末端L型夾具的長度也可簡化成三條長度為L1、L2、L3的線段。藍色線段是機械臂的初始位置狀態，紅色（X0，Y0，Z0）表示待抓取點G的坐標，黑色虛線表示機械臂運動到G處的位置時，各個連桿所處的位置狀態。因為機械結構的限制，小臂和大臂之間的夾角小于900，所以其抓取時的位置狀態是唯一的。為了便于陳述，將三維模型圖2分解為如圖3和圖4兩部分。

當基座電機（M1）旋轉r弧度后，大臂和目標點G將會處在同樣一個平面內，此時就變成了如圖3所示的共面情形了。如圖4所示，目標點G在基座平面的投影為點G，原點和G的連線與機械臂初始位置所在平面的夾角為γ。

3逆向運動學解算（Inverse kinematics Solution）

通常，機械臂運動學求解方法有Denavit-Hartenberg法[2]（簡稱D-H法）、解析法和POE法[3]、基于多智能體的強化學習方法[4]、搜索方法[5]。在這里使用一種根據機械結構和本體的幾何特征提取推導出的求解方法。

如圖2所示，假設：末端夾具的垂直高度是h，并且末端抓手始終與基座所處的平面保持平行;機械臂位于初始位置時，大臂與基座保持垂直，小臂與大臂保持垂直，并且大臂、小臂的長度已知;電機M1、M2、M3、M4所需轉動的角度分別為：（γ、α、β、θ）。

規定：γ>0表示M1左轉;α>0表示M2繞坐標軸+Y2順時針旋轉、β>0表示M3繞坐標軸+Y3旋轉順時針旋轉;θ>0表示M4繞坐標+Y4軸旋轉。

已知目標點、大臂、小臂參數，那么一定可以構成如圖3中紅色虛線部分所示的等腰三角形。若目標點為：G（X0，Y0，Z0），設定γ、為M1所需轉動的角度大小：

[γ‘'=arctanY0X0]? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（1）

已知末端夾具的長度和高度分別為：L3、h;可求解紅色虛線三角形邊長L4：

[L'4‘=X02+Y02-L3'2+Z0+h2]? ? ? ? ? ? ? （2）

至此，圖3中的等腰三角形三邊長均已知，且：L1=L1'、L2=L2'、L3=L3'。可根據余弦定理和正切三角函數關系求解α、β：

[α1=arccosL1'2+L4'2-L2'22·L1'·L4'α2=arcsinHL4'β1=L1'2+L2'2-L4'22L1'·L2']? ? ? ? ? ? ? ? ? ?（3）

故驅動大臂和小臂轉動的電機角度求解公式為：

[α=π2-α1+α2? β=π2-β1]? ? ? ? ? ? ? ? ? ? ? ? ? ? （4）

根據機械臂初始位置時，末端夾具和基座所在平面保持平行關系，那么末端夾具與小臂連接處關節需轉動的角度θ為：

[θ=-α+β]? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? （5）

需要注意的是，M1旋轉角度γ不僅與式（1）有關，還與待抓取點G所在的XOY平面投影點所在的象限有關。如圖4所示，將坐標系分為四個象限，當G投影位于一、二象限時，電機M1為向左正轉，反之三、四象限為反轉;故γ滿足如下關系式：

[][γ=γ'X0>0π-γ'X0≤0，Y0>0-π2+γ'X0≤0，Y0<0]? ? ? ? ? ? ? ? ? ? ?（6）

綜上，可求出機械臂抓取到待抓取物時各個關節相對于初始位置時的旋轉角度：（γ、α、β、θ）。

4正向運動學解算（Forward kinematics Solution）

上述中描述了基于幾何法的碼垛機械臂逆求解公式推導過程，并給出了計算各個關節旋轉角度的計算公式。為了驗證上述各個關節角度求解的正確性，現在將使用由Denavit和Hartenberg于1955年提出的對機器人進行表示和建模D-H方法，參照改方法對該型號的碼垛機械臂進行建模，求解相鄰連桿之間的位姿變換矩陣，最后推導出正向運動學方程。

4.1 D-H參數坐標系的建立

D-H參數坐標系如圖5所示，參照第5篇文獻簡述的方法[6]，其步驟如下：

結合實際的機械臂的構造，上述坐標系0、1、2的坐標原點重合，故L0=0;機械臂在如圖5的狀態下，此時各個關節的旋轉角度θi應該為：

[θ0T=0-π2π2π20T]? ? ? ? ? ? ? ? ?（7）

根據建立的坐標系求解D-H參數，如下表1：

4.2 求解坐標系之間的變換矩陣和公式

[i-1iT=cosθi-sinθi0ai-1sinθicosαi-1cosθicosαi-1-sinai-1-disinai-1sinθisinai-1cosθisinai-1cosai-1dicosai-10001]? （8）

根據表1和式（8）可得5個變換矩陣：

[01T=cosθ1-sinθ100sinθ1cosθ10000100001]? [12T=cosθ2-sinθ2000010-sinθ2-cosθ2000001]

[23T=cosθ3-sinθ30L1sinθ3cosθ30000100001] [34T=cosθ4-sinθ40L2sinθ4cosθ40000100001]

根據碼垛機械臂的特點，末端夾具始終和地面保持平行的狀態，并且X5和X4軸之間的方向永遠保持不變，即：θ5=0。

[45T=100h00-1-L301000001]

由上述的矩陣，可求得機械臂末端夾具末端與基坐標系之間得變換關系[7]為：

[05T=01T12T23T34T45T]? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（9）

帶入上述各式后得：

[05T=t11t12t13t14t21t22t23t24t31t32t33t34t41t42t43t44]（10）

其中t14，t24，t34，為夾具末端（x5，y5，z5）在基坐標系下的坐標，其表達式如下：

[t14=c1c2c3-c1s2s3（c4h+s4L3+L2）-c1c2s3+c1s2c3s4h-s4L3+c1c2L1t34=s1c2c3-s1s2s3（c4h+s4L3+L2）-（s1c2s3+s1s2c3）s4h-s4L3+s1c2L1t34=-（s2c3+c2s3）（c4h+s4L3+L2）+（s2s3-c2c3）s4h-s4L3-s2L1]? （11）

該表達式的意義為：若已知碼垛機械臂各個關節的旋轉角度，那么就可以求解出夾具末端相對于基坐標系的坐標值，這個過程也稱為正向解算。除此之外，還有改進的Modified-DH模型方法[8]。

5 數值算例（Numerical Verification）

為了說明本碼垛機械臂的逆向解算方法的準確性，接下來將給出相對于基坐標系的待抓取目標點坐標G（x，y，z），利用逆向求解方法求得每個關節需要旋轉的角度;然后再根據D-H正向解算方法求得的表達式計算出夾具末端相對于基坐標系的坐標值，最后和待抓取點坐標進行比較。

需要注意的是，第2節中的逆向解算方法求出的角度（γ、α、β、θ）表示的是相對于圖1中機械臂所示各個關節轉動的相對變化角度。在帶入式（11）時，需要結合D-H參數法中θi的幾何意義對其轉換，轉換表達式為：

[θ1=γθ2=-π2-αθ3=π2+βθ4=π2-θ2+θ3]? ? ? ? ? ? ? ? ? ? ? （12）

驗證方法是在Linux系統下使用C++語言編寫與前面對應的正逆向求解函數，對逆向求解結果帶入正向D-H函數，對結果進行比較。同時將計算出的關節旋轉角度通過串口通信方式發送給機械臂控制器（MCU），在不考慮機械結構靜態誤差的情況下，觀察機械臂末端是否能夠到達指定的坐標點。實驗采用的實體機械臂由鋁合金材料和步進電機組成，如圖7所示，機械臂末端夾爪正運動到某一個待抓取目標物附近。

為了驗證該逆向求解方法的正確性，在Z軸正方向所在的空間分別選擇了四個待抓取目標點，分別所處四個不同象限。為了觀察直觀，表中Gi和Ti的單位為米（m），角度單位為度。表2為根據幾何法求解機械臂末端到達Gi點所需各個關節所需旋轉的角度;表3由表2經過式（12）轉換得來。

首先基于幾何法的碼垛機械臂逆向求解方法得出的各個關節應旋轉的角度（γ、α、β、θ）如表2所示;根據式（12）所示的轉換規則得出如表3所示的角度θi，最后由D-H方法推導出的式（11）可正向求解出末端相對于基坐標系的坐標值Ti。經過編碼測試[9]，基于幾何法的碼垛機械臂逆向求解方法得出的角度帶入D-H正向解算公式的結果完全等于待抓取點Gi。

6 結論（Conclusion）

（1）根據碼垛機械臂的連桿式機械結構，將其簡化為電機位于機械臂的各個關節處，并直接驅動機械臂運動，然后在對其進行運動學分析。

（2）根據上述簡化的機械臂運動模型，提出了基于幾何法的碼垛機械臂逆向解算方法，并推導出了機械臂末端由起始位置到達待抓取點求所需的各個關節旋轉角度求解公式。

（3）為了驗證該方法的正確性，根據D-H方法，對碼垛機械臂進行了坐標系建模，提取出了D-H參數，并推導出了基于各個關節旋轉角度求解機械臂末端相對于基坐標系坐標值的公式。

最后，根據第三節提出的幾何法求解Gi位置的關節角度和式（11）的計算結果Ti進行比較，驗證了基于幾何法的碼垛機械臂逆向求解方法的正確性，與此同時，實體機械臂末端也能移動到待抓取物體所在位置處。因此，若已知待抓取目標點，利用該方法能求解各個關節所應轉動的角度，能讓碼垛機械臂的末端準確地到達待抓取點所處的位置，完成抓取和放置物體的功能。

參考文獻：

[1] 《中國機器人產業發展報告2019》發布[J].傳感器世界，2019，25（9）：42.

[2] Hartenburg R.S.，Denavit J.，Freudenstein F..Kinematic Synthesis of Linkages[J]. Journal of Applied Mechanics，1965，32（2）.

[3] 冷舒，吳克，居鶴華.機械臂運動學建模及解算方法綜述[J].宇航學報，2019，40（11）：1262-1273.

[4] 趙毓，管公順，郭繼峰，等.基于多智能體強化學習的空間機械臂軌跡規劃[J/OL].航空學報：1-11[2020-11-11].http：//kns.cnki.net/kcms/detail/11.1929.V.20200713.1501.004.html.

[5] 馮晨旭，胡三寶.基于改進天牛須搜索的機械臂運動學逆解[J/OL].武漢大學學報（工學版）：1-8[2020-11-11].https：//doi.org/10.14188/j.1671-8844.2020-12-012.

[6] Tony Owen. Introduction to Robotics[J]. Robotica，1991，9（4）.

[7] 朱訓棟，徐海港，趙棟杰.碼垛機械臂運動學位置正逆解分析[J].農業工程，2017，7（4）：155-158.

[8] 彭光宇，董洪波，馬斌.兩種DH模型的機器人運動學建模對比研究[J].機械研究與應用，2019，32（6）：62-65.

[9] 潘磊，錢煒，張志艷，等.四自由度機械臂運動學分析及Matlab仿真[J].機械科學與技術，2013，32（3）：421-425.

【通聯編輯：梁書】