柑橘采摘機械臂控制算法研究

郝光鵬，寧志剛

（南華大學 電氣工程學院，衡陽 421001）

0 引言

柑橘采摘機械臂運動的控制是采摘機器人研究相關領域中不可或缺的重要部分，它包含了機械臂的運動學建模、動力學建模及運動學逆解算法優化等諸多方面[1-5]。采用仿真方法可以在研究機械臂整體控制系統的基礎上，節省大量時間，提前發現問題，大大降低制造過程中的成本。

本文對柑橘采摘機械臂的控制算法進行了研究與優化。調用OpenGL（Open Graphics Library）搭建仿真平臺，通過對機械臂進行運動學、動力學方面的建模以及相應算法的實現，完成柑橘采摘機械臂運動控制的仿真[6]。

1 系統流程

首先搭建基于OpenGL的機械臂仿真平臺，然后進行運動學方面的建模，并使用MATLAB對逆解結果進行驗證，達到預期指標后，進行動力學方面建模與仿真，驗證控制算法，以實現對機械臂運動進行準確的控制。具體的流程如圖1所示，仿真效果如圖2所示。

圖1 系統流程圖Fig.1 System flow diagram

圖2 仿真效果圖Fig.2 Simulation effect diagram

2 運動學建模

2.1 正運動學

本項目的機械臂由3段連桿與基座組成，全部為旋轉關節，共有3個自由度。在考慮基座以及末端執行器的前提下，通過DH參數法，為機械臂各關節賦予相應的坐標系，并列出DH參數表。建立的坐標系如圖3所示。其中，O表示所建立坐標系原點，X、Z分別表示所建立坐標系的X軸與Z軸。根據其標號依次為基坐標系1、底座與連桿1之間的坐標系1.5、關節坐標系2、關節坐標系3、機械臂末端執行器坐標系H。

圖3 DH坐標系示意圖Fig.3 Schematic diagram of DH coordinate system

根據賦予的坐標系得出相應的DH參數，如表1所示。其中，ai表示關節軸之間法線的距離，αi表示關節軸夾角，di表示公共法線在同一軸上的距離，θi表示公共法線在同一軸上的夾角。

表1 DH參數表Table.1 DH parameter table

通過計算得到相鄰坐標系坐標變換矩陣T：

矩陣中s和c分別代表sin和cos。根據坐標變換關系可求出從基坐標系到末端執行器坐標系的變換矩陣：

式中 aa表示l2cosθ1+ l3cos (θ1+θ2)，bb表示l2sinθ1+l3sin(θ1+θ2)。

同時，由于柑橘樹高度、果實分布等自然特性，在機械臂連桿長度設置時應保證滿足柑橘采摘的要求，機械臂的運動空間代表了機械臂的工作范圍，使用MATLAB對機械臂運動空間進行模擬。其工作空間如圖4—6所示。

圖4 工作空間仿真圖Fig.4 workspace simulation

圖5 YOZ空間投影圖Fig.5 YOZ space projection

圖6 XOY空間投影圖Fig.6 XOY space projection

由圖4可以看出機械臂的工作高度最高可達3 m，水平范圍為半徑0～2 m的圓。圖5為工作空間在YOZ面上的投影，為2個半徑為1 m的圓與1個半徑為2 m的圓組成的心形圖。圖6為工作空間在XOY面上的投影，為1個半徑為1.5 m的圓，此外，工作空間在XOZ面上的投影與圖5相同，由此可得，機械臂運動范圍基本覆蓋柑橘生長空間，滿足柑橘采摘的要求。

2.2 逆運動學

已知機械臂末端位姿求各關節參數的過程稱為逆運動學。由于自由度較少，本方案采用雅克比矩陣求逆法進行逆運動學計算。雅克比矩陣作為從關節空間到操作空間的映射，可得如下關系式：

式中 x為操作空間；q為關節空間。

計算流程如圖7所示。

圖7 逆解框圖Fig.7 Block diagram of inverse solution

由圖7可得如下表達式：

式中 下標d表示期望值；c表示當前值。

首先，設置末端執行器期望的位置xd，并與當前末端執行器位姿比較，將誤差與雅克比矩陣的逆相乘，并將結果送至各關節控制器中，確定各關節位置，然后使用當前關節位置，利用正運動學求出此時末端執行器位姿，與期望位姿進行比較，求得它們之間的差值，然后進行迭代運算，直到dx的值滿足要求。

運動學逆解建模完成后，驗證OpenGL中的運動學逆解結果。驗證效果如圖8所示。

圖8 MATLAB驗證圖Fig.8 MATLAB diagram

OpenGL實際末端執行器位姿與MATLAB計算所得的誤差約為0.05 m，可以達到柑橘采摘的要求。各關節角度、角速度、角加速度的變化如圖9—11所示。

由圖9可知，各關節角從（0，0.785 398 2，-0.785 398 2）變化到（0.785 398 2，-0.785 398 2，0.785 398 2），達到了期望的設計目標，由此說明了逆運動學求解的正確性。機械臂在運動過程中，如果速度和加速度突然發生變化，就會導致機械臂抖動。由圖10—11可知，角速度和角加速度曲線光滑，在啟動與停止時機械臂加速度為0。這表明機械臂運動整體過程中符合連續性要求，運行平穩，達到了期望效果。結果表明，OpenGL使用的運動學逆解可以滿足柑橘采摘的要求。

圖9 關節角度隨時間變化圖Fig.9 Change of joint angle with time

圖10 關節角速度隨時間變化圖Fig.10 Change of joint angular velocity with time

圖11 關節角加速度隨時間變化圖Fig.11 Change of joint angular acceleration with time

3 動力學建模

由拉格朗日法推導得到的機械臂動力學方程為：

式中 M為質量矩陣，由每個關節自身加速需要的扭矩與其他關節加速對該關節的影響疊加而成；v由科里奧利力和離心力組成，表示關節勻速運動對每個關節所需要施加力矩的影響；g由系統勢能對關節位置的偏導求得。對于每一行為，每個關節需要克服其之后所有連桿的重力在該關節上的分量；q為機械臂末端執行器位置；τ為力矩。同時在末端執行器受力分析中需要考慮柑橘自身的重量對動力學所建立模型帶來的影響，將其轉化為關節扭矩，假設柑橘的平均質量為80 g，重力加速度為9.8 m/s2，代入動力學方程左邊：

首先，編寫程序讀取文本文檔中已提供的機械臂的長度、質心位置、質量、轉動慣量、初始關節角度等參數，以獲得質量矩陣。一個n×n的質量矩陣的第i個對角項，表示第i個關節加速運動時需要克服其它所有連桿的質量或慣量。質量矩陣為對稱矩陣。一個關節的加速運動同樣會造成第二個關節需要施加額外的力矩，以維持其運動狀態，這種現象稱為動力學耦合。以此可得到如下質量矩陣：

其中 I表示各機械臂轉動慣量，θ由DH參數法求得。根據定義代入已給出的機械臂相關參數，得到v及g，從而獲得機械臂動力學方程。等式右邊的τ是被控量，表示根據建立的動力學模型，讓關節按照設定好的軌跡運動時，每個關節驅動器所需要施加的力矩。根據所建立的動力學模型，在MATLAB中使用rne函數可對模型進行驗證，其關節力矩與時間、角度、角速度、角加速度關系如圖12—15所示。

通過MATLAB對動力學模型的驗證，得到圖12—15的關節動力學特性曲線。由這些圖可知，在機械臂運動的過程中各關節力矩均為線性變化，過渡過程比較平穩，符合OpenGL中機械臂運動狀態，表明關節能夠按照所期望的速度及加速度運行，同時也進一步表明所規劃的軌跡較為合理。

圖12 關節力矩隨時間度變化圖Fig.12 Variation of joint torque with time

圖13 關節力矩隨角度變化圖Fig.13 Change of joint torque with angle

圖14 關節力矩隨角速度變化圖Fig.14 Variation of joint torque with angular velocity

圖15 關節力矩隨角加速度變化圖Fig15.Variation of joint torque with angular acceleration

4 結束語

本文根據柑橘采摘機械臂結構的特點，在OpenGL平臺上建立了3D仿真模型，進行運動學以及動力學建模計算，對末端執行器的運動路徑進行了規劃，并利用MATLAB對仿真結果進行了驗證。結果表明，該算法控制效果良好，能滿足柑橘采摘機械臂運動控制的要求。