五連桿機(jī)器手臂攔截問題的最優(yōu)化控制策略

湛志勇， 李學(xué)文

（1.東莞市清溪科技創(chuàng)新服務(wù)有限公司，廣東 東莞 523000；2.東莞市三潤田自動(dòng)化設(shè)備有限公司，廣東 東莞 523640）

0 引言

機(jī)器手臂軌跡的最優(yōu)化控制一直以來是研究的重要方向，同時(shí)也是工業(yè)應(yīng)用中研究的重要課題，為有效地應(yīng)對(duì)機(jī)器手臂的重復(fù)性任務(wù)，以及能源緊張的情況，提高能耗利用效率，降低機(jī)器手臂運(yùn)動(dòng)能耗是目前十分重要的問題，有著非常大的現(xiàn)實(shí)意義。本文涉及到的DCNLP法，是將最優(yōu)控制問題轉(zhuǎn)化為非線性規(guī)劃問題進(jìn)行求解，建立起能耗方程，以獲得機(jī)器手臂完成所規(guī)定任務(wù)的最小能耗，經(jīng)仿真結(jié)果分析發(fā)現(xiàn)，采用DCNLP法進(jìn)行求解，具有良好的魯棒性與適應(yīng)性。

1 五連桿機(jī)器手臂及機(jī)器人動(dòng)力學(xué)研究方法

如今，機(jī)器手臂已經(jīng)大量應(yīng)用于工廠的生產(chǎn)線，隨著我國逐漸進(jìn)入老齡化，使用機(jī)器手臂來對(duì)老人進(jìn)行照顧或者輔助患者進(jìn)行復(fù)健也已成為了新的研究方向與發(fā)展趨勢，如上肢康復(fù)機(jī)器人等。上肢康復(fù)機(jī)器人包括了最基本的、最重要的5個(gè)自由度，即肩部、肘部、腕部的屈與伸，肩部的外展與內(nèi)收，以及腕部的旋內(nèi)與旋外。機(jī)器手臂模擬人體上肢的運(yùn)動(dòng)，安裝在高度可調(diào)的支架上，以適應(yīng)不同患者的康復(fù)訓(xùn)練要求，完成患者的坐姿與站姿的訓(xùn)練，同時(shí)，由于機(jī)器手臂的上臂、前壁與位姿能夠進(jìn)行調(diào)節(jié)，因此能夠?qū)崿F(xiàn)左右的穿戴，滿足患者的一定需求。在機(jī)器手臂的研究領(lǐng)域中，其動(dòng)力學(xué)研究方法有多種，包括拉格朗日方程法、牛頓-歐拉方程法、凱恩方程法等。

拉格朗日方程：在系統(tǒng)的動(dòng)力學(xué)方程建立中，因?yàn)槎鄤傮w系統(tǒng)較為復(fù)雜，因此采用系統(tǒng)的、獨(dú)立的拉格朗日坐標(biāo)也就十分困難，而采用笛卡爾廣義坐標(biāo)方便簡單。對(duì)于具有多余坐標(biāo)的完整或非完整約束相同，可采用帶乘子的拉氏方程來進(jìn)行規(guī)范處理。牛頓-歐拉方程：剛體在空間中的運(yùn)動(dòng)通常可分解為隨其上某點(diǎn)的平動(dòng)和繞該點(diǎn)的轉(zhuǎn)動(dòng)，所以可分別地采用牛頓定律和歐拉方程進(jìn)行處理。但是通過牛頓-歐拉方法導(dǎo)出的動(dòng)力學(xué)方程包含了大量的、不需要的未知約束反力，所以在牛頓-歐拉方法的運(yùn)用中需要對(duì)約束反力進(jìn)行重點(diǎn)消除。由于該方法需要消去內(nèi)力，因此推導(dǎo)起來較為麻煩。凱恩方程法：該方法提供了分析復(fù)雜機(jī)械系統(tǒng)動(dòng)力學(xué)性能的統(tǒng)一方法，不用動(dòng)力學(xué)函數(shù)，也無需求導(dǎo)計(jì)算，只需要對(duì)矢量點(diǎn)積、叉積進(jìn)行計(jì)算即可。但是該方法并沒有給出一個(gè)具有通用性的適合于任意多剛體系統(tǒng)的動(dòng)力學(xué)方程，而在廣義速度的選擇上也需要經(jīng)驗(yàn)和技巧。三種方法中，前兩者為主要的動(dòng)力學(xué)建模方式。

2 機(jī)器手臂軌跡最優(yōu)化相關(guān)研究

隨著科技的發(fā)展，在現(xiàn)階段機(jī)械手臂在工業(yè)界已經(jīng)獲得了較為廣泛的應(yīng)用，而其手臂軌跡的最優(yōu)化一直以來都是重要的研究對(duì)象。機(jī)器手臂的最優(yōu)化軌跡即是要實(shí)現(xiàn)機(jī)器手臂的運(yùn)動(dòng)耗時(shí)最短、能耗最少。先前學(xué)者的有關(guān)研究中較多的通過不同的算法來實(shí)現(xiàn)對(duì)時(shí)間的最優(yōu)化，如李東潔等提出了一種基于遺傳算法的軌跡優(yōu)化控制方法，對(duì)時(shí)間進(jìn)行最優(yōu)化，而羅熊等則提出混合進(jìn)化計(jì)算模擬退火求解算法來實(shí)現(xiàn)對(duì)時(shí)間的最優(yōu)化。當(dāng)然也有學(xué)者針對(duì)能耗最優(yōu)化的研究，如朱心科等就基于兩步鏈?zhǔn)絃-K算法針對(duì)水下機(jī)器人進(jìn)行了延長其水下工作時(shí)間的研究，而徐海黎等則對(duì)工業(yè)機(jī)器人提出基因環(huán)境雙演化免疫克隆算法，進(jìn)行了能耗的優(yōu)化。黨培等提出了一種基于梯形升降速法對(duì)機(jī)器手臂運(yùn)動(dòng)軌跡進(jìn)行了優(yōu)化。潘秀強(qiáng)等則提出直接配置非線性規(guī)劃方法（DCNLP）對(duì)機(jī)器人手臂運(yùn)動(dòng)軌跡進(jìn)行能耗的最優(yōu)化。其中，DCNLP法將能耗的最優(yōu)問題轉(zhuǎn)換成為了非線性的規(guī)劃問題來求解，進(jìn)而實(shí)現(xiàn)機(jī)器手臂在完成既定工作下，能耗最小。

直接配置非線性規(guī)劃方法（Direct Collocation with Nonlinear Programming，DCNLP），可將所需的各個(gè)關(guān)節(jié)的驅(qū)動(dòng)器輸出轉(zhuǎn)矩進(jìn)行計(jì)算，確定出最優(yōu)化的指標(biāo)為“最低能耗”，并獲得各個(gè)關(guān)節(jié)相應(yīng)的位移與角速度。在G-H公約所描述的及其手臂幾何結(jié)構(gòu)下，可應(yīng)用Mathematica軟件符號(hào)語言來推導(dǎo)出機(jī)器手臂的動(dòng)力學(xué)方程EOM，并采用Euler必要條件為最優(yōu)化的理論基礎(chǔ)將最優(yōu)化過程中的兩點(diǎn)邊值問題（TPBVP），轉(zhuǎn)化成一般的線性規(guī)劃問題，并進(jìn)行數(shù)學(xué)求解。DCNLP法針對(duì)五連桿機(jī)器手臂在醫(yī)療看護(hù)中的基本動(dòng)作“攔截”與“對(duì)接”進(jìn)行了最優(yōu)化研究。

3 基于DCNLP法在五連桿機(jī)器手臂攔截問題的最優(yōu)化控制

3.1 五連桿機(jī)器手臂的建模

五連桿機(jī)器手臂包括5個(gè)關(guān)節(jié)，關(guān)節(jié)2、關(guān)節(jié)4負(fù)責(zé)上下轉(zhuǎn)動(dòng)，關(guān)節(jié)3和關(guān)節(jié)5負(fù)責(zé)左右轉(zhuǎn)動(dòng)，設(shè)機(jī)器手臂的關(guān)節(jié)轉(zhuǎn)動(dòng)角度為θ1（t）、θ2（t）、θ3（t）、θ4（t）、θ5（t），然后根據(jù)DH公約建立起五連桿機(jī)器手臂模型。

3.2 動(dòng)力學(xué)方程

引入Euler-Lagrange（拉格朗日-歐拉）公式來對(duì)機(jī)器的系統(tǒng)運(yùn)動(dòng)學(xué)與控制輸入間的關(guān)系進(jìn)行描述。求出5個(gè)關(guān)節(jié)驅(qū)動(dòng)器產(chǎn)生力矩的矢量和τ→，由于機(jī)器手臂連桿為細(xì)長的剛性連桿，因此求得其轉(zhuǎn)動(dòng)慣量矩陣Ji。為了解決拉格朗日方程推導(dǎo)的困難，采用符號(hào)編程方法來解決，求得 τ（t）、θ¨（t）。

3.3 最優(yōu)化控制與直接配置

求出θ¨后，采用DC直接配置來對(duì)TPBVP兩點(diǎn)邊值問題進(jìn)行解決，并利用最優(yōu)控制方法實(shí)現(xiàn)機(jī)器手臂運(yùn)動(dòng)的能耗最小化控制。

第二步，進(jìn)行直接配置非線性規(guī)劃。DCNLP法將傳統(tǒng)的TPBVP問題轉(zhuǎn)化為一般的非線性規(guī)劃問題，求出x˙（t），λ˙兩者的解也就能夠找到最優(yōu)的解決方案。但是，xi（t）、xj（tf）和λk（tf）在最初與最后時(shí)刻的端點(diǎn)值缺乏，也就造成了x˙（t），λ˙差分方程迭代過程變得較為復(fù)雜。DCNLP 法將連續(xù)的TPBVP問題分隔為n段，因此也就將時(shí)間節(jié)點(diǎn)劃分為了n+1個(gè)，節(jié)點(diǎn)上的值則設(shè)為Xi，采用Hermit插值方式對(duì)兩節(jié)點(diǎn)間進(jìn)行三次多項(xiàng)式的定義，該三次多項(xiàng)式既包含了Xi值，同時(shí)也包含了Xi+1的值。通過DCNLP計(jì)算出兩節(jié)點(diǎn)間的中心點(diǎn)斜率X′c，進(jìn)而與fc逼近系統(tǒng)方程實(shí)現(xiàn)最終的吻合。經(jīng)多次迭代后，插值斜率與其中心點(diǎn)系統(tǒng)差分方程相一致。θ¨（t）中包含了10個(gè)狀態(tài)差分方程，根據(jù)時(shí)間歷程將其劃分為512份，并由此獲得限制方程△5120個(gè)，期望收斂為0。

第三步，進(jìn)行能耗方程的設(shè)計(jì)。根據(jù)機(jī)器手臂動(dòng)作，以達(dá)到最小能耗為重點(diǎn)設(shè)計(jì)能耗方程J。

4 仿真與分析

設(shè)機(jī)器手臂初始狀態(tài)的轉(zhuǎn)動(dòng)角度與角速度分別為0，設(shè)定機(jī)器手臂取自由落體的球，球的初始狀態(tài)為0，經(jīng)求解，機(jī)器手臂成功地接到了目標(biāo)球，時(shí)間為0.8512，能耗為15.9329。對(duì)機(jī)器手臂接球軌跡進(jìn)行Matlab仿真，則該軌跡也就是能耗得到最優(yōu)控制的接球軌跡。

5 結(jié)語

DCNLP法將TPBVP問題成功地轉(zhuǎn)化為非線性規(guī)劃問題，使得機(jī)器手臂能耗最優(yōu)化問題，通過性能指標(biāo)和邊值問題的構(gòu)建，能耗方程的設(shè)計(jì)獲得了最優(yōu)化的解。通過仿真結(jié)果也證實(shí)，DCNLP法能夠?yàn)闄C(jī)器手臂運(yùn)動(dòng)控制提供高質(zhì)量的解。

［1］ 潘修強(qiáng)，梅成才，陳軍杰.五連桿機(jī)器手臂攔截問題的最優(yōu)化控制研究［J］.系統(tǒng)仿真學(xué)報(bào)，2012（9）：1772-1776.

［2］ 梅成才，潘修強(qiáng)，陳軍杰.應(yīng)用DCNLP法求解五連桿機(jī)器手臂對(duì)接問題［J］.小型微型計(jì)算機(jī)系統(tǒng)，2012（12）：2749-2752.

［3］ 潘修強(qiáng)，梅成才，陳軍杰.基于DCNLP的機(jī)器手臂最優(yōu)化控制及拉格朗日因子研究［J］.計(jì)算機(jī)應(yīng)用與軟件，2013（5）：140-144.

［4］ 朱雪楓.五自由度上肢康復(fù)機(jī)器人數(shù)學(xué)建模與仿真［D］.沈陽：東北大學(xué)，2011：36.