柑橘采摘機(jī)械臂控制算法研究

郝光鵬，寧志剛

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

0 引言

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

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

1 系統(tǒng)流程

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

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

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

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

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

本項(xiàng)目的機(jī)械臂由3段連桿與基座組成，全部為旋轉(zhuǎn)關(guān)節(jié)，共有3個(gè)自由度。在考慮基座以及末端執(zhí)行器的前提下，通過(guò)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)號(hà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

通過(guò)計(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í)，由于柑橘樹(shù)高度、果實(shí)分布等自然特性，在機(jī)械臂連桿長(zhǎng)度設(shè)置時(shí)應(yīng)保證滿足柑橘采摘的要求，機(jī)械臂的運(yùn)動(dòng)空間代表了機(jī)械臂的工作范圍，使用MATLAB對(duì)機(jī)械臂運(yùn)動(dòng)空間進(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個(gè)半徑為1 m的圓與1個(gè)半徑為2 m的圓組成的心形圖。圖6為工作空間在XOY面上的投影，為1個(gè)半徑為1.5 m的圓，此外，工作空間在XOZ面上的投影與圖5相同，由此可得，機(jī)械臂運(yùn)動(dòng)范圍基本覆蓋柑橘生長(zhǎng)空間，滿足柑橘采摘的要求。

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

已知機(jī)械臂末端位姿求各關(guān)節(jié)參數(shù)的過(guò)程稱為逆運(yùn)動(dòng)學(xué)。由于自由度較少，本方案采用雅克比矩陣求逆法進(jìn)行逆運(yùn)動(dòng)學(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)動(dòng)學(xué)求出此時(shí)末端執(zhí)行器位姿，與期望位姿進(jìn)行比較，求得它們之間的差值，然后進(jìn)行迭代運(yùn)算，直到dx的值滿足要求。

運(yùn)動(dòng)學(xué)逆解建模完成后，驗(yàn)證OpenGL中的運(yùn)動(dòng)學(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)，由此說(shuō)明了逆運(yùn)動(dòng)學(xué)求解的正確性。機(jī)械臂在運(yùn)動(dòng)過(guò)程中，如果速度和加速度突然發(fā)生變化，就會(huì)導(dǎo)致機(jī)械臂抖動(dòng)。由圖10—11可知，角速度和角加速度曲線光滑，在啟動(dòng)與停止時(shí)機(jī)械臂加速度為0。這表明機(jī)械臂運(yùn)動(dòng)整體過(guò)程中符合連續(xù)性要求，運(yùn)行平穩(wěn)，達(dá)到了期望效果。結(jié)果表明，OpenGL使用的運(yùn)動(dòng)學(xué)逆解可以滿足柑橘采摘的要求。

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

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

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

3 動(dòng)力學(xué)建模

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

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

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

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

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

圖12 關(guān)節(jié)力矩隨時(shí)間度變化圖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é)束語(yǔ)

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