郝光鵬，寧志剛

（南華大學(xué) 電氣工程學(xué)院，衡陽 421001）

0 引言

柑橘采摘機(jī)械臂運(yùn)動的控制是采摘機(jī)器人研究相關(guān)領(lǐng)域中不可或缺的重要部分，它包含了機(jī)械臂的運(yùn)動學(xué)建模、動力學(xué)建模及運(yùn)動學(xué)逆解算法優(yōu)化等諸多方面[1-5]。采用仿真方法可以在研究機(jī)械臂整體控制系統(tǒng)的基礎(chǔ)上，節(jié)省大量時間，提前發(fā)現(xiàn)問題，大大降低制造過程中的成本。

本文對柑橘采摘機(jī)械臂的控制算法進(jìn)行了研究與優(yōu)化。調(diào)用OpenGL（Open Graphics Library）搭建仿真平臺，通過對機(jī)械臂進(jìn)行運(yùn)動學(xué)、動力學(xué)方面的建模以及相應(yīng)算法的實(shí)現(xiàn)，完成柑橘采摘機(jī)械臂運(yùn)動控制的仿真[6]。

1 系統(tǒng)流程

首先搭建基于OpenGL的機(jī)械臂仿真平臺，然后進(jìn)行運(yùn)動學(xué)方面的建模，并使用MATLAB對逆解結(jié)果進(jìn)行驗(yàn)證，達(dá)到預(yù)期指標(biāo)后，進(jìn)行動力學(xué)方面建模與仿真，驗(yàn)證控制算法，以實(shí)現(xiàn)對機(jī)械臂運(yùn)動進(jìn)行準(zhǔn)確的控制。具體的流程如圖1所示，仿真效果如圖2所示。

圖1 系統(tǒng)流程圖Fig.1 System flow diagram

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

2 運(yùn)動學(xué)建模

2.1 正運(yùn)動學(xué)

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

圖3 DH坐標(biāo)系示意圖Fig.3 Schematic diagram of DH coordinate system

根據(jù)賦予的坐標(biāo)系得出相應(yīng)的DH參數(shù)，如表1所示。其中，ai表示關(guān)節(jié)軸之間法線的距離，αi表示關(guān)節(jié)軸夾角，di表示公共法線在同一軸上的距離，θi表示公共法線在同一軸上的夾角。

表1 DH參數(shù)表Table.1 DH parameter table

通過計(jì)算得到相鄰坐標(biāo)系坐標(biāo)變換矩陣T：

矩陣中s和c分別代表sin和cos。根據(jù)坐標(biāo)變換關(guān)系可求出從基坐標(biāo)系到末端執(zhí)行器坐標(biāo)系的變換矩陣：

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

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

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

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

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

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

2.2 逆運(yùn)動學(xué)

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

式中 x為操作空間；q為關(guān)節(jié)空間。

計(jì)算流程如圖7所示。

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

由圖7可得如下表達(dá)式：

式中 下標(biāo)d表示期望值；c表示當(dāng)前值。

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

運(yùn)動學(xué)逆解建模完成后，驗(yàn)證OpenGL中的運(yùn)動學(xué)逆解結(jié)果。驗(yàn)證效果如圖8所示。

圖8 MATLAB驗(yàn)證圖Fig.8 MATLAB diagram

OpenGL實(shí)際末端執(zhí)行器位姿與MATLAB計(jì)算所得的誤差約為0.05 m，可以達(dá)到柑橘采摘的要求。各關(guān)節(jié)角度、角速度、角加速度的變化如圖9—11所示。

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

圖9 關(guān)節(jié)角度隨時間變化圖Fig.9 Change of joint angle with time

圖10 關(guān)節(jié)角速度隨時間變化圖Fig.10 Change of joint angular velocity with time

圖11 關(guān)節(jié)角加速度隨時間變化圖Fig.11 Change of joint angular acceleration with time

3 動力學(xué)建模

由拉格朗日法推導(dǎo)得到的機(jī)械臂動力學(xué)方程為：

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

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

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

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

圖12 關(guān)節(jié)力矩隨時間度變化圖Fig.12 Variation of joint torque with time

圖13 關(guān)節(jié)力矩隨角度變化圖Fig.13 Change of joint torque with angle

圖14 關(guān)節(jié)力矩隨角速度變化圖Fig.14 Variation of joint torque with angular velocity

圖15 關(guān)節(jié)力矩隨角加速度變化圖Fig15.Variation of joint torque with angular acceleration

4 結(jié)束語

本文根據(jù)柑橘采摘機(jī)械臂結(jié)構(gòu)的特點(diǎn)，在OpenGL平臺上建立了3D仿真模型，進(jìn)行運(yùn)動學(xué)以及動力學(xué)建模計(jì)算，對末端執(zhí)行器的運(yùn)動路徑進(jìn)行了規(guī)劃，并利用MATLAB對仿真結(jié)果進(jìn)行了驗(yàn)證。結(jié)果表明，該算法控制效果良好，能滿足柑橘采摘機(jī)械臂運(yùn)動控制的要求。