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

魏 鵬

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

0 引言

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

物流裝卸機器人被部署在貨物裝卸區域，其主要任務是將貨物從地面裝入集裝箱或將貨物從集裝箱內搬出。這個工作場景通常發生在物流中心、港口、貨運站等地方，其中集裝箱是一種常見的貨物運輸容器。當貨車司機駕駛集裝箱貨車將車輛調整到適當的角度和位置后，車輛往往不再頻繁移動，而是停留在指定的裝卸位置上。因此，集裝箱的相對位置相對固定，物流裝卸機器人可以利用視覺或雷達設備獲取集裝箱的位姿信息，以便進行后續的操作和碰撞檢測。

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

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

1 物流裝卸機器人結構介紹

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

圖1 物流裝卸機器人結構簡圖

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

2 機械臂運動學建模與求逆

2.1 運動學建模

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

圖2 各軸坐標系示意

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

由式（1）便可以實現正向運動學求解。

2.2 求運動學逆解

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

該機器人共有5 個關節軸，沒有冗余的關節，所以可以求出解析解。本文采用幾何推導的方式求出其解析解。在求逆的過程中，默認控制點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平面內的投影關系可知，θ1由xJd5與yJd5決定，θ1的求解公式如下：

（4）縱向旋轉臂1、縱向旋轉臂2 與縱向旋轉臂3 共同構建了一個平面，將其命名為縱向旋轉面，

求出該平面在{B}坐標系下的單位法向量BVver-lon-plane的表達式：

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

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

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

（9）在第二關節軸靜坐標系{Js2}內，求解θ2、θ3、θ4。在{Js2}內，縱向旋轉臂1、縱向旋轉臂2 與縱向旋轉臂3 構成了一個典型的SCARA 機器人結構，相關的研究非常豐富，本文不再重復推導，直接列出如下公式：

2.2.2 求解D1、D2

利用三角函數關系，把θ2、θ3轉換成D1、D2。角度與長度轉換關系如圖3 所示。旋轉角θ1、θ4、θ5可由安裝在關節上的電機旋轉實現角度調整，而θ2、θ3所處的位置等效轉動慣量較大，若直接用電機在關節上進行驅動，穩定性難以保證，所以采用伺服液壓缸間接控制θ2、θ3。轉換公式如式（11）所示。

圖3 角度與長度轉換關系

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

3.1 數值仿真

使用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]，取插補數ns=100，分別對末端位置和方向進行平均插補，得到期望軌跡上各插補點位置[pex pey pez]與歸一化方向向量[vex vey vez]。根據本文所提出的逆運動學解法得到各關節轉角，再進行正運動學求解獲取各插補點實際位置[prx pry prz]與歸一化方向向量[vrx vry vrz]。末端執行器軌跡如圖4所示。

圖4 末端執行器軌跡

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

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

3.2 可達工作空間分析

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

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

圖5 工作空間仿真

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

4 碰撞檢測研究

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

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

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

圖6 包絡線示意

考慮到集裝箱的厚度較小且幾何形狀接近長方體的外表面，使用線段集對集裝箱進行碰撞檢測建模。具體方法如下：將集裝箱長方體的4 個棱進行數學表達，并選擇與棱平行的其他線段作為參考線段。確保各線段之間的間距不大于L，其中L是縱向旋轉臂2、縱向旋轉臂3 和橫向旋轉臂所形成的包絡圓柱體的最小半徑。

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

5 結束語

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

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