基于幾何法的碼垛機(jī)械臂逆向求解

李照祥 胡杰

摘要：逆向解算是機(jī)械臂抓取目標(biāo)物的前提，是實(shí)現(xiàn)機(jī)械臂軌跡規(guī)劃、動(dòng)態(tài)導(dǎo)航避障的基礎(chǔ)。常見(jiàn)的機(jī)械臂運(yùn)動(dòng)學(xué)解算方法有D-H法、解析法、POE法和人工智能法。此處，以物流、工業(yè)應(yīng)用場(chǎng)景中常見(jiàn)的四軸碼垛機(jī)械臂為研究對(duì)象，使用幾何方法，針對(duì)其機(jī)械結(jié)構(gòu)和幾何特征推導(dǎo)出求解公式，實(shí)現(xiàn)碼垛機(jī)械臂的定點(diǎn)逆向求解功能。然后，將上述逆向求解得出的結(jié)果代入D-H正向解算方法，求出的此時(shí)的末端執(zhí)行器的坐標(biāo)，并與目標(biāo)點(diǎn)進(jìn)行對(duì)比驗(yàn)證。最后，將逆向求解方法應(yīng)用到實(shí)際的四軸碼垛機(jī)械臂中，觀察其實(shí)際運(yùn)行時(shí)的執(zhí)行效果。碼垛機(jī)械臂的逆向求解方法的實(shí)現(xiàn)為機(jī)械臂開(kāi)發(fā)和實(shí)景應(yīng)用的設(shè)計(jì)提供了理論基礎(chǔ)。

關(guān)鍵詞：碼垛機(jī)械臂;運(yùn)動(dòng)學(xué)分析;D-H建模;自動(dòng)抓取

中圖分類(lèi)號(hào) TP391? ? 文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：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）

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

2機(jī)械臂模型的簡(jiǎn)化（Simplified Robotic Arm Model）

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

根據(jù)圖1，可將其簡(jiǎn)化為如圖2所示的三維空間及線段。簡(jiǎn)化流程是：可在基座中心處建立三維空間的笛卡爾坐標(biāo)系，大臂、小臂、末端L型夾具的長(zhǎng)度也可簡(jiǎn)化成三條長(zhǎng)度為L(zhǎng)1、L2、L3的線段。藍(lán)色線段是機(jī)械臂的初始位置狀態(tài)，紅色（X0，Y0，Z0）表示待抓取點(diǎn)G的坐標(biāo)，黑色虛線表示機(jī)械臂運(yùn)動(dòng)到G處的位置時(shí)，各個(gè)連桿所處的位置狀態(tài)。因?yàn)闄C(jī)械結(jié)構(gòu)的限制，小臂和大臂之間的夾角小于900，所以其抓取時(shí)的位置狀態(tài)是唯一的。為了便于陳述，將三維模型圖2分解為如圖3和圖4兩部分。

當(dāng)基座電機(jī)（M1）旋轉(zhuǎn)r弧度后，大臂和目標(biāo)點(diǎn)G將會(huì)處在同樣一個(gè)平面內(nèi)，此時(shí)就變成了如圖3所示的共面情形了。如圖4所示，目標(biāo)點(diǎn)G在基座平面的投影為點(diǎn)G，原點(diǎn)和G的連線與機(jī)械臂初始位置所在平面的夾角為γ。

3逆向運(yùn)動(dòng)學(xué)解算（Inverse kinematics Solution）

通常，機(jī)械臂運(yùn)動(dòng)學(xué)求解方法有Denavit-Hartenberg法[2]（簡(jiǎn)稱(chēng)D-H法）、解析法和POE法[3]、基于多智能體的強(qiáng)化學(xué)習(xí)方法[4]、搜索方法[5]。在這里使用一種根據(jù)機(jī)械結(jié)構(gòu)和本體的幾何特征提取推導(dǎo)出的求解方法。

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

規(guī)定：γ>0表示M1左轉(zhuǎn);α>0表示M2繞坐標(biāo)軸+Y2順時(shí)針旋轉(zhuǎn)、β>0表示M3繞坐標(biāo)軸+Y3旋轉(zhuǎn)順時(shí)針旋轉(zhuǎn);θ>0表示M4繞坐標(biāo)+Y4軸旋轉(zhuǎn)。

已知目標(biāo)點(diǎn)、大臂、小臂參數(shù)，那么一定可以構(gòu)成如圖3中紅色虛線部分所示的等腰三角形。若目標(biāo)點(diǎn)為：G（X0，Y0，Z0），設(shè)定γ、為M1所需轉(zhuǎn)動(dòng)的角度大?。?/p>

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

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

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

至此，圖3中的等腰三角形三邊長(zhǎng)均已知，且：L1=L1'、L2=L2'、L3=L3'?？筛鶕?jù)余弦定理和正切三角函數(shù)關(guān)系求解α、β：

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

故驅(qū)動(dòng)大臂和小臂轉(zhuǎn)動(dòng)的電機(jī)角度求解公式為：

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

根據(jù)機(jī)械臂初始位置時(shí)，末端夾具和基座所在平面保持平行關(guān)系，那么末端夾具與小臂連接處關(guān)節(jié)需轉(zhuǎn)動(dòng)的角度θ為：

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

需要注意的是，M1旋轉(zhuǎn)角度γ不僅與式（1）有關(guān)，還與待抓取點(diǎn)G所在的XOY平面投影點(diǎn)所在的象限有關(guān)。如圖4所示，將坐標(biāo)系分為四個(gè)象限，當(dāng)G投影位于一、二象限時(shí)，電機(jī)M1為向左正轉(zhuǎn)，反之三、四象限為反轉(zhuǎn);故γ滿(mǎn)足如下關(guān)系式：

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

綜上，可求出機(jī)械臂抓取到待抓取物時(shí)各個(gè)關(guān)節(jié)相對(duì)于初始位置時(shí)的旋轉(zhuǎn)角度：（γ、α、β、θ）。

4正向運(yùn)動(dòng)學(xué)解算（Forward kinematics Solution）

上述中描述了基于幾何法的碼垛機(jī)械臂逆求解公式推導(dǎo)過(guò)程，并給出了計(jì)算各個(gè)關(guān)節(jié)旋轉(zhuǎn)角度的計(jì)算公式。為了驗(yàn)證上述各個(gè)關(guān)節(jié)角度求解的正確性，現(xiàn)在將使用由Denavit和Hartenberg于1955年提出的對(duì)機(jī)器人進(jìn)行表示和建模D-H方法，參照改方法對(duì)該型號(hào)的碼垛機(jī)械臂進(jìn)行建模，求解相鄰連桿之間的位姿變換矩陣，最后推導(dǎo)出正向運(yùn)動(dòng)學(xué)方程。

4.1 D-H參數(shù)坐標(biāo)系的建立

D-H參數(shù)坐標(biāo)系如圖5所示，參照第5篇文獻(xiàn)簡(jiǎn)述的方法[6]，其步驟如下：

結(jié)合實(shí)際的機(jī)械臂的構(gòu)造，上述坐標(biāo)系0、1、2的坐標(biāo)原點(diǎn)重合，故L0=0;機(jī)械臂在如圖5的狀態(tài)下，此時(shí)各個(gè)關(guān)節(jié)的旋轉(zhuǎn)角度θi應(yīng)該為：

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

根據(jù)建立的坐標(biāo)系求解D-H參數(shù)，如下表1：

4.2 求解坐標(biāo)系之間的變換矩陣和公式

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

根據(jù)表1和式（8）可得5個(gè)變換矩陣：

[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]

根據(jù)碼垛機(jī)械臂的特點(diǎn)，末端夾具始終和地面保持平行的狀態(tài)，并且X5和X4軸之間的方向永遠(yuǎn)保持不變，即：θ5=0。

[45T=100h00-1-L301000001]

由上述的矩陣，可求得機(jī)械臂末端夾具末端與基坐標(biāo)系之間得變換關(guān)系[7]為：

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

帶入上述各式后得：

[05T=t11t12t13t14t21t22t23t24t31t32t33t34t41t42t43t44]（10）

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

[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）

該表達(dá)式的意義為：若已知碼垛機(jī)械臂各個(gè)關(guān)節(jié)的旋轉(zhuǎn)角度，那么就可以求解出夾具末端相對(duì)于基坐標(biāo)系的坐標(biāo)值，這個(gè)過(guò)程也稱(chēng)為正向解算。除此之外，還有改進(jìn)的Modified-DH模型方法[8]。

5 數(shù)值算例（Numerical Verification）

為了說(shuō)明本碼垛機(jī)械臂的逆向解算方法的準(zhǔn)確性，接下來(lái)將給出相對(duì)于基坐標(biāo)系的待抓取目標(biāo)點(diǎn)坐標(biāo)G（x，y，z），利用逆向求解方法求得每個(gè)關(guān)節(jié)需要旋轉(zhuǎn)的角度;然后再根據(jù)D-H正向解算方法求得的表達(dá)式計(jì)算出夾具末端相對(duì)于基坐標(biāo)系的坐標(biāo)值，最后和待抓取點(diǎn)坐標(biāo)進(jìn)行比較。

需要注意的是，第2節(jié)中的逆向解算方法求出的角度（γ、α、β、θ）表示的是相對(duì)于圖1中機(jī)械臂所示各個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)的相對(duì)變化角度。在帶入式（11）時(shí)，需要結(jié)合D-H參數(shù)法中θi的幾何意義對(duì)其轉(zhuǎn)換，轉(zhuǎn)換表達(dá)式為：

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

驗(yàn)證方法是在Linux系統(tǒng)下使用C++語(yǔ)言編寫(xiě)與前面對(duì)應(yīng)的正逆向求解函數(shù)，對(duì)逆向求解結(jié)果帶入正向D-H函數(shù)，對(duì)結(jié)果進(jìn)行比較。同時(shí)將計(jì)算出的關(guān)節(jié)旋轉(zhuǎn)角度通過(guò)串口通信方式發(fā)送給機(jī)械臂控制器（MCU），在不考慮機(jī)械結(jié)構(gòu)靜態(tài)誤差的情況下，觀察機(jī)械臂末端是否能夠到達(dá)指定的坐標(biāo)點(diǎn)。實(shí)驗(yàn)采用的實(shí)體機(jī)械臂由鋁合金材料和步進(jìn)電機(jī)組成，如圖7所示，機(jī)械臂末端夾爪正運(yùn)動(dòng)到某一個(gè)待抓取目標(biāo)物附近。

為了驗(yàn)證該逆向求解方法的正確性，在Z軸正方向所在的空間分別選擇了四個(gè)待抓取目標(biāo)點(diǎn)，分別所處四個(gè)不同象限。為了觀察直觀，表中Gi和Ti的單位為米（m），角度單位為度。表2為根據(jù)幾何法求解機(jī)械臂末端到達(dá)Gi點(diǎn)所需各個(gè)關(guān)節(jié)所需旋轉(zhuǎn)的角度;表3由表2經(jīng)過(guò)式（12）轉(zhuǎn)換得來(lái)。

首先基于幾何法的碼垛機(jī)械臂逆向求解方法得出的各個(gè)關(guān)節(jié)應(yīng)旋轉(zhuǎn)的角度（γ、α、β、θ）如表2所示;根據(jù)式（12）所示的轉(zhuǎn)換規(guī)則得出如表3所示的角度θi，最后由D-H方法推導(dǎo)出的式（11）可正向求解出末端相對(duì)于基坐標(biāo)系的坐標(biāo)值Ti。經(jīng)過(guò)編碼測(cè)試[9]，基于幾何法的碼垛機(jī)械臂逆向求解方法得出的角度帶入D-H正向解算公式的結(jié)果完全等于待抓取點(diǎn)Gi。

6 結(jié)論（Conclusion）

（1）根據(jù)碼垛機(jī)械臂的連桿式機(jī)械結(jié)構(gòu)，將其簡(jiǎn)化為電機(jī)位于機(jī)械臂的各個(gè)關(guān)節(jié)處，并直接驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)，然后在對(duì)其進(jìn)行運(yùn)動(dòng)學(xué)分析。

（2）根據(jù)上述簡(jiǎn)化的機(jī)械臂運(yùn)動(dòng)模型，提出了基于幾何法的碼垛機(jī)械臂逆向解算方法，并推導(dǎo)出了機(jī)械臂末端由起始位置到達(dá)待抓取點(diǎn)求所需的各個(gè)關(guān)節(jié)旋轉(zhuǎn)角度求解公式。

（3）為了驗(yàn)證該方法的正確性，根據(jù)D-H方法，對(duì)碼垛機(jī)械臂進(jìn)行了坐標(biāo)系建模，提取出了D-H參數(shù)，并推導(dǎo)出了基于各個(gè)關(guān)節(jié)旋轉(zhuǎn)角度求解機(jī)械臂末端相對(duì)于基坐標(biāo)系坐標(biāo)值的公式。

最后，根據(jù)第三節(jié)提出的幾何法求解Gi位置的關(guān)節(jié)角度和式（11）的計(jì)算結(jié)果Ti進(jìn)行比較，驗(yàn)證了基于幾何法的碼垛機(jī)械臂逆向求解方法的正確性，與此同時(shí)，實(shí)體機(jī)械臂末端也能移動(dòng)到待抓取物體所在位置處。因此，若已知待抓取目標(biāo)點(diǎn)，利用該方法能求解各個(gè)關(guān)節(jié)所應(yīng)轉(zhuǎn)動(dòng)的角度，能讓碼垛機(jī)械臂的末端準(zhǔn)確地到達(dá)待抓取點(diǎn)所處的位置，完成抓取和放置物體的功能。

參考文獻(xiàn)：

[1] 《中國(guó)機(jī)器人產(chǎn)業(yè)發(fā)展報(bào)告2019》發(fā)布[J].傳感器世界，2019，25（9）：42.

[2] Hartenburg R.S.，Denavit J.，F(xiàn)reudenstein F..Kinematic Synthesis of Linkages[J]. Journal of Applied Mechanics，1965，32（2）.

[3] 冷舒，吳克，居鶴華.機(jī)械臂運(yùn)動(dòng)學(xué)建模及解算方法綜述[J].宇航學(xué)報(bào)，2019，40（11）：1262-1273.

[4] 趙毓，管公順，郭繼峰，等.基于多智能體強(qiáng)化學(xué)習(xí)的空間機(jī)械臂軌跡規(guī)劃[J/OL].航空學(xué)報(bào)：1-11[2020-11-11].http：//kns.cnki.net/kcms/detail/11.1929.V.20200713.1501.004.html.

[5] 馮晨旭，胡三寶.基于改進(jìn)天牛須搜索的機(jī)械臂運(yùn)動(dòng)學(xué)逆解[J/OL].武漢大學(xué)學(xué)報(bào)（工學(xué)版）：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] 朱訓(xùn)棟，徐海港，趙棟杰.碼垛機(jī)械臂運(yùn)動(dòng)學(xué)位置正逆解分析[J].農(nóng)業(yè)工程，2017，7（4）：155-158.

[8] 彭光宇，董洪波，馬斌.兩種DH模型的機(jī)器人運(yùn)動(dòng)學(xué)建模對(duì)比研究[J].機(jī)械研究與應(yīng)用，2019，32（6）：62-65.

[9] 潘磊，錢(qián)煒，張志艷，等.四自由度機(jī)械臂運(yùn)動(dòng)學(xué)分析及Matlab仿真[J].機(jī)械科學(xué)與技術(shù)，2013，32（3）：421-425.

【通聯(lián)編輯：梁書(shū)】