物流裝卸機器人運動學(xué)分析及碰撞檢測研究*

魏 鵬

（泉州華中科技大學(xué)智能制造研究院，福建泉州 362000）

0 引言

物流系統(tǒng)是生產(chǎn)系統(tǒng)中至關(guān)重要的組成部分。為了提高物流效率、減輕工人的勞動強度以及便于管理，物流機器人在物流行業(yè)得到了廣泛應(yīng)用。這些物流機器人包括AGV 小車、碼垛機器人、分揀機器人等，它們被用于實現(xiàn)搬運、裝卸、分揀等任務(wù)的自動化[1-5]。但是，當(dāng)面對從集裝箱中裝卸箱包這種任務(wù)時，常見的六軸機器人、SCARA 機器人和Delta 機器人會存在剛性不足、效率不高和經(jīng)濟(jì)性差等問題?；诖?，本文設(shè)計一種適用于從集裝箱內(nèi)裝卸箱包的物流裝卸機器人。

物流裝卸機器人被部署在貨物裝卸區(qū)域，其主要任務(wù)是將貨物從地面裝入集裝箱或?qū)⒇浳飶募b箱內(nèi)搬出。這個工作場景通常發(fā)生在物流中心、港口、貨運站等地方，其中集裝箱是一種常見的貨物運輸容器。當(dāng)貨車司機駕駛集裝箱貨車將車輛調(diào)整到適當(dāng)?shù)慕嵌群臀恢煤?，車輛往往不再頻繁移動，而是停留在指定的裝卸位置上。因此，集裝箱的相對位置相對固定，物流裝卸機器人可以利用視覺或雷達(dá)設(shè)備獲取集裝箱的位姿信息，以便進(jìn)行后續(xù)的操作和碰撞檢測。

近年來，也有許多企業(yè)工程師和學(xué)者對物流裝卸機器人進(jìn)行研究。顧綺芳[6]針對貨柜車廂的作業(yè)環(huán)境，設(shè)計空間七連桿機構(gòu)、二轉(zhuǎn)動自由度差動機構(gòu)實現(xiàn)機械臂在行程范圍內(nèi)能達(dá)到貨箱中任意位置，并通過空間軌跡視覺引導(dǎo)實現(xiàn)快速精確移動定位。韓祥東[7]以井下礦用坑道鉆機的鉆桿裝卸為應(yīng)用背景，進(jìn)行運動學(xué)動力學(xué)建模，使用人工蜂群算法進(jìn)行最優(yōu)沖擊軌跡優(yōu)化，并設(shè)計了裝卸機器人的控制系統(tǒng)以及操作界面。

本文主要完成物流裝卸機器人的運動學(xué)建模、逆解公式推導(dǎo)、仿真驗證、可達(dá)空間分析和碰撞檢測研究。

1 物流裝卸機器人結(jié)構(gòu)介紹

物流裝卸機器人結(jié)構(gòu)如圖1 所示。機器人整體可分為兩大部分，一部分是可移動底座，另一部分是由旋轉(zhuǎn)平臺、旋轉(zhuǎn)臂、支撐臂等零部件組成的五軸機械臂，當(dāng)橫向旋轉(zhuǎn)臂移動到集裝箱內(nèi)指定位姿后，可將貨物經(jīng)由旋轉(zhuǎn)臂上的傳送帶運送至集裝箱內(nèi)。

圖1 物流裝卸機器人結(jié)構(gòu)簡圖

五軸機械臂部分是一種五關(guān)節(jié)的串聯(lián)機構(gòu)，5 個關(guān)節(jié)均為轉(zhuǎn)動副，J1軸的旋轉(zhuǎn)運動可實現(xiàn)旋轉(zhuǎn)平臺與可移動底座之間繞Z軸的旋轉(zhuǎn)；J2、J3和J4軸的旋轉(zhuǎn)運動可實現(xiàn)相鄰連桿之間繞X軸的旋轉(zhuǎn)；J5軸的旋轉(zhuǎn)運動可實現(xiàn)橫向旋轉(zhuǎn)臂與縱向旋轉(zhuǎn)臂3 之間繞Z軸的旋轉(zhuǎn)。其中J1、J4和J5由電機直接驅(qū)動，J2、J3由支撐臂間接驅(qū)動，支撐臂上安裝有伺服液壓缸，可以提供更好的支撐和穩(wěn)定性。θi為連桿i的轉(zhuǎn)角，Li為連桿i的長度，S1、S2、S3和S4為支撐臂在底座和連桿上的安裝尺寸。

2 機械臂運動學(xué)建模與求逆

2.1 運動學(xué)建模

如圖2 所示，基于物流裝卸機器人的機械臂三維結(jié)構(gòu)建立其連桿坐標(biāo)系，分別命名為機器人基坐標(biāo)系{B}、第一關(guān)節(jié)軸靜坐標(biāo)系{Js1}、第一關(guān)節(jié)軸動坐標(biāo)系{Jd1}、第二關(guān)節(jié)軸靜坐標(biāo)系{Js2}、第二關(guān)節(jié)軸動坐標(biāo)系{Jd2}、第三關(guān)節(jié)軸靜坐標(biāo)系{Js3}、第三關(guān)節(jié)軸動坐標(biāo)系{Jd3}、第四關(guān)節(jié)軸靜坐標(biāo)系{Js4}、第四關(guān)節(jié)軸動坐標(biāo)系{Jd4}、第五關(guān)節(jié)軸靜坐標(biāo)系{Js5}、第五關(guān)節(jié)軸動坐標(biāo)系{Jd5}。

圖2 各軸坐標(biāo)系示意

建立各坐標(biāo)系之間的轉(zhuǎn)換矩陣。T表示轉(zhuǎn)換矩陣，具體為從左上標(biāo)坐標(biāo)系到左下標(biāo)坐標(biāo)系的轉(zhuǎn)換矩陣。依次建立。由于{B}的原點位置可以任意選取，令{B}與{Js1}重合，則為單位矩陣。設(shè)矩陣運算符號如下：

由式（1）便可以實現(xiàn)正向運動學(xué)求解。

2.2 求運動學(xué)逆解

常見的逆運動學(xué)求解方法有幾何法、解析法、數(shù)值法，近年來隨著人工智能熱潮，一些人工智能的方法也得到了應(yīng)用[8]。國內(nèi)外學(xué)者對機器人逆運動學(xué)方法進(jìn)行了深入的研究。馬建偉等[9]針對六自由度串聯(lián)機械臂，在Levenberg-Marquardt（LM）迭代法求值的基礎(chǔ)上，針對迭代參數(shù)因子和步長，提出改進(jìn)的逆運動學(xué)問題數(shù)值解法。劉曰濤等[10]針對KUKA 機器人，使用反變換法計算機器人逆運動學(xué)解，并通過算法篩選和條件限制確定唯一解。劉國平等[11]針對一種前3 關(guān)節(jié)軸線交于一點的六自由度機械臂，提出一種旋量理論結(jié)合已知pandenkahan 子問題的逆解方法，將旋量、幾何法、歐拉角相結(jié)合，簡化了傳統(tǒng)旋量求解方法。

該機器人共有5 個關(guān)節(jié)軸，沒有冗余的關(guān)節(jié)，所以可以求出解析解。本文采用幾何推導(dǎo)的方式求出其解析解。在求逆的過程中，默認(rèn)控制點BP=[x y z1]T和控制方向BV=[i j k0]T已知，且連桿的長度尺寸Li已知。

2.2.1 求θ1、θ2、θ3、θ4、θ5

（1）對 控制 向量BV=[i j k0]T進(jìn)行單位化處理，使得其向量長度為1。單位化公式為：

用BVnorm=[inormjnormknorm0]T表示單位化后的控制向量BV。

（3）分析機械臂在oxy平面內(nèi)的投影關(guān)系可知，θ1由xJd5與yJd5決定，θ1的求解公式如下：

（4）縱向旋轉(zhuǎn)臂1、縱向旋轉(zhuǎn)臂2 與縱向旋轉(zhuǎn)臂3 共同構(gòu)建了一個平面，將其命名為縱向旋轉(zhuǎn)面，

求出該平面在{B}坐標(biāo)系下的單位法向量BVver-lon-plane的表達(dá)式：

（5）θ5是控制向量BV=[i j k0]T與縱向旋轉(zhuǎn)面之間的夾角，縱向旋轉(zhuǎn)面的法向量BVver-lon-plane已知，公式如下：

θ5的求解還需要進(jìn)行符號判斷，當(dāng)BV的x方向分量大于BVver-lon-plane的x方向分量時，θ5的符號為負(fù)，反之為正。

（6）求出控制方向BV=[i j k0]T在縱向旋轉(zhuǎn)面內(nèi)的投影向量BVp-lon-plane，公式如下：

（9）在第二關(guān)節(jié)軸靜坐標(biāo)系{Js2}內(nèi)，求解θ2、θ3、θ4。在{Js2}內(nèi)，縱向旋轉(zhuǎn)臂1、縱向旋轉(zhuǎn)臂2 與縱向旋轉(zhuǎn)臂3 構(gòu)成了一個典型的SCARA 機器人結(jié)構(gòu)，相關(guān)的研究非常豐富，本文不再重復(fù)推導(dǎo)，直接列出如下公式：

2.2.2 求解D1、D2

利用三角函數(shù)關(guān)系，把θ2、θ3轉(zhuǎn)換成D1、D2。角度與長度轉(zhuǎn)換關(guān)系如圖3 所示。旋轉(zhuǎn)角θ1、θ4、θ5可由安裝在關(guān)節(jié)上的電機旋轉(zhuǎn)實現(xiàn)角度調(diào)整，而θ2、θ3所處的位置等效轉(zhuǎn)動慣量較大，若直接用電機在關(guān)節(jié)上進(jìn)行驅(qū)動，穩(wěn)定性難以保證，所以采用伺服液壓缸間接控制θ2、θ3。轉(zhuǎn)換公式如式（11）所示。

圖3 角度與長度轉(zhuǎn)換關(guān)系

3 仿真驗證與可達(dá)空間分析

3.1 數(shù)值仿真

使用MATLAB 對機器人逆運動學(xué)準(zhǔn)確性進(jìn)行驗證。給定機械臂長度，L1～L5分別為400、850、5 104、175、1 660 mm。選取機器人末端起始點位置[-300 7 000 200]，方向向量[0 1-0.5]，終止點位置[300 7 000 200]，方向向量[0 1-0.5]，取插補數(shù)ns=100，分別對末端位置和方向進(jìn)行平均插補，得到期望軌跡上各插補點位置[pex pey pez]與歸一化方向向量[vex vey vez]。根據(jù)本文所提出的逆運動學(xué)解法得到各關(guān)節(jié)轉(zhuǎn)角，再進(jìn)行正運動學(xué)求解獲取各插補點實際位置[prx pry prz]與歸一化方向向量[vrx vry vrz]。末端執(zhí)行器軌跡如圖4所示。

圖4 末端執(zhí)行器軌跡

計算期望軌跡與實際軌跡在各插補點的位置誤差Δp與方向誤差Δv，其中：

由數(shù)值計算結(jié)果可知，在該軌跡中位置誤差最大值Δpmax=2.365 5×10-10，方向誤差最大值Δvmax=1.425 0×10-13。算法引起的誤差在數(shù)值上很小，使用該運動學(xué)逆解方法可實現(xiàn)末端軌跡的準(zhǔn)確規(guī)劃。

3.2 可達(dá)工作空間分析

機器人可達(dá)空間的求解方法主要有幾何法、解析法以及數(shù)值法三類[12]。幾何法利用幾何繪圖的原理確定工作空間的邊界，從而表示機器人工作空間。解析法主要通過求解工作空間邊界的包絡(luò)問題，得到邊界方程，從而進(jìn)行工作分析。于英紅等[13]從構(gòu)成并聯(lián)機構(gòu)的基本單元出發(fā)，研究了其位置正解與反解的分析方法，以3-RPS并聯(lián)機構(gòu)進(jìn)行分析，得到其工作空間分析的解析方法。而數(shù)值法計算機器人工作空間時，通過運動學(xué)正解得到一系列末端位置，從而得到工作空間的邊界曲面與曲線。數(shù)值法中應(yīng)用較為廣泛的是蒙特卡洛法，蒙特卡羅法是一種以概率統(tǒng)計為指導(dǎo)的數(shù)值計算方法，通過大量的正解模擬出機器人的工作空間。楊永鯤等[14]為進(jìn)行焊接機器人工作空間分析，使用蒙特卡洛法，在關(guān)節(jié)限位內(nèi)對關(guān)節(jié)變量進(jìn)行隨機賦值，獲取焊接末端點。許如賓等[15]在電子設(shè)備電磁兼容檢測近場成像的六軸機械臂應(yīng)用中，給定末端天線姿態(tài)，使用蒙特卡洛法求解其末端執(zhí)行器工作空間，并證明該空間內(nèi)存在一個不可達(dá)球域。

根據(jù)該機器人尺寸參數(shù)，并考慮機器人關(guān)節(jié)以和液壓缸的實際約束條件，在MATLAB中進(jìn)行機器人正運動學(xué)求解，取計算點數(shù)nc=10 000進(jìn)行遍歷，得到機器人工作空間云圖，將其沿各坐標(biāo)軸進(jìn)行二維投影，結(jié)果如圖5所示。

圖5 工作空間仿真

總體上，可達(dá)工作空間在Z方向兩端收縮，如圖5（a）所示。在圖5（b）中，由于機器人關(guān)節(jié)1 和5 旋轉(zhuǎn)角的限制，且機器人旋轉(zhuǎn)臂1、2、3 始終構(gòu)成一縱向平面，使xoy平面投影的扇形圖像限定在一定角度范圍內(nèi)。當(dāng)機器人末端趨于可達(dá)工作空間兩端時，橫向旋轉(zhuǎn)臂轉(zhuǎn)角θ5趨于0，使末端的x方向可移動距離減小，但由于末端繞z軸轉(zhuǎn)動由θ1和θ5共同作用，且θ1的改變不影響末端z坐標(biāo)值大小，因此在工作空間z方向兩端θ1可取范圍內(nèi)的任意值，使得扇形圖像的下端收縮并保持一定轉(zhuǎn)角，從而產(chǎn)生圖5（b）。當(dāng)機器人末端遠(yuǎn)離可達(dá)空間兩端時，θ5的取值范圍放寬，使機器人在x方向的可移動距離變大，所以產(chǎn)生了如圖5（c）所示的鼓形圖像。

4 碰撞檢測研究

由于該物流機器人需要在貨物大小不一、空間有限的貨箱中完成裝卸任務(wù)，對機器人進(jìn)行碰撞檢測，合理規(guī)劃其運動軌跡是有必要的，以避免造成對貨物、貨箱乃至機器人本體的破壞。

目前，已有眾多學(xué)者進(jìn)行了機器人碰撞檢測方向的研究，主要的實現(xiàn)方法有基于幾何建模方法、基于外部傳感器和視覺的方法、數(shù)字孿生方法等[16-18]。朱戰(zhàn)霞等[19]針對障礙環(huán)境中自由漂浮模式空間冗余機械臂進(jìn)行了球形包圍盒和空間疊加思想研究，簡化描述障礙物和空間機械臂的占位關(guān)系，使用直線段和球體判斷碰撞。王張飛等[20]提出了一種基于深度投影的點云目標(biāo)實時分割方法，并使用該分割后的點云目標(biāo)構(gòu)造混合層次包圍盒進(jìn)行碰撞檢測。江雪梅等[21]將數(shù)字孿生引入重型機床碰撞檢測，構(gòu)建了感知-演化預(yù)測-反饋的碰撞檢測框架，動態(tài)感知工件刀具等加工要素，對潛在的干涉現(xiàn)象進(jìn)行預(yù)測。

對于機器人的碰撞檢測，本文采用圓柱包絡(luò)法。由于旋轉(zhuǎn)平臺、縱向旋轉(zhuǎn)臂1 和支撐臂都位于靠近移動平臺的位置，并且在各自的可達(dá)范圍內(nèi)都不會發(fā)生碰撞，因此只需要對縱向旋轉(zhuǎn)臂2、縱向旋轉(zhuǎn)臂3和橫向旋轉(zhuǎn)臂進(jìn)行包絡(luò)處理。包絡(luò)線如圖6所示。

圖6 包絡(luò)線示意

考慮到集裝箱的厚度較小且?guī)缀涡螤罱咏L方體的外表面，使用線段集對集裝箱進(jìn)行碰撞檢測建模。具體方法如下：將集裝箱長方體的4 個棱進(jìn)行數(shù)學(xué)表達(dá)，并選擇與棱平行的其他線段作為參考線段。確保各線段之間的間距不大于L，其中L是縱向旋轉(zhuǎn)臂2、縱向旋轉(zhuǎn)臂3 和橫向旋轉(zhuǎn)臂所形成的包絡(luò)圓柱體的最小半徑。

包絡(luò)完成后，逐個求解圓柱中心線線段和箱體線段之間的距離，判斷該距離是否小于圓柱半徑即可，若小于，則發(fā)生碰撞，若不小于，則不發(fā)生碰撞。通過這種方法，可以有效地檢測機器人在操作過程中與集裝箱之間的碰撞情況，并采取相應(yīng)的措施以確保安全運行。

5 結(jié)束語

本文基于物流裝卸機器人的結(jié)構(gòu)，建立了機器人的運動學(xué)模型，推導(dǎo)了機器人的逆運動學(xué)求解方法。該方法簡潔可靠，并具有快速求解的特點，適用于實時控制系統(tǒng)。通過計算機仿真，驗證了逆運動學(xué)的有效性，并用數(shù)值方法求解了機器人的可達(dá)空間。所得結(jié)果為機器人的結(jié)構(gòu)改進(jìn)提供了重要的參考，可以用于優(yōu)化機器人的尺寸參數(shù)。同時，研究了機器人的防碰撞問題。針對機器人與障礙物之間的碰撞風(fēng)險，進(jìn)行了包絡(luò)建模研究。

下一步可在本文研究的基礎(chǔ)上，以視覺拍攝結(jié)果為輸入，結(jié)合碰撞檢測結(jié)果規(guī)劃末端執(zhí)行器的運動路徑，使其能夠高效且安全地完成裝卸任務(wù)。