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

魏 鵬

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

0 引言

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

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

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

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

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

物流裝卸機器人結(jié)構(gòu)如圖1 所示。機器人整體可分為兩大部分，一部分是可移動底座，另一部分是由旋轉(zhuǎn)平臺、旋轉(zhuǎn)臂、支撐臂等零部件組成的五軸機械臂，當橫向旋轉(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 機械臂運動學建模與求逆

2.1 運動學建模

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

圖2 各軸坐標系示意

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

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

2.2 求運動學逆解

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

該機器人共有5 個關(guān)節(jié)軸，沒有冗余的關(guān)節(jié)，所以可以求出解析解。本文采用幾何推導的方式求出其解析解。在求逆的過程中，默認控制點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進行單位化處理，使得其向量長度為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}坐標系下的單位法向量BVver-lon-plane的表達式：

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

θ5的求解還需要進行符號判斷，當BV的x方向分量大于BVver-lon-plane的x方向分量時，θ5的符號為負，反之為正。

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

（9）在第二關(guān)節(jié)軸靜坐標系{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)的研究非常豐富，本文不再重復推導，直接列出如下公式：

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é)上進行驅(qū)動，穩(wěn)定性難以保證，所以采用伺服液壓缸間接控制θ2、θ3。轉(zhuǎn)換公式如式（11）所示。

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

3 仿真驗證與可達空間分析

3.1 數(shù)值仿真

使用MATLAB 對機器人逆運動學準確性進行驗證。給定機械臂長度，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，分別對末端位置和方向進行平均插補，得到期望軌跡上各插補點位置[pex pey pez]與歸一化方向向量[vex vey vez]。根據(jù)本文所提出的逆運動學解法得到各關(guān)節(jié)轉(zhuǎn)角，再進行正運動學求解獲取各插補點實際位置[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ù)值上很小，使用該運動學逆解方法可實現(xiàn)末端軌跡的準確規(guī)劃。

3.2 可達工作空間分析

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

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

圖5 工作空間仿真

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

4 碰撞檢測研究

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

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

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

圖6 包絡(luò)線示意

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

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

5 結(jié)束語

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

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