魏麗君 ，吳海波，劉海龍

(1.中南大學(xué) 信息科學(xué)與工程學(xué)院，湖南 長(zhǎng)沙 410001；2.湖南鐵道職業(yè)技術(shù)學(xué)院，湖南 株洲 412001)

隨著應(yīng)用領(lǐng)域的拓展，機(jī)械臂作業(yè)環(huán)境更為復(fù)雜，其運(yùn)動(dòng)時(shí)可能會(huì)與環(huán)境中的障礙物發(fā)生碰撞，導(dǎo)致任務(wù)無法完成，極大地制約了機(jī)械臂的應(yīng)用。移動(dòng)機(jī)械臂由移動(dòng)平臺(tái)和機(jī)械臂組成，通常具有冗余自由度。該特性使移動(dòng)機(jī)械臂可以較好地解決上述問題[1-3]。冗余自由度機(jī)器人末端的某一特定位姿對(duì)應(yīng)的機(jī)器人關(guān)節(jié)位形有無限多個(gè)，因此研究相應(yīng)的軌跡規(guī)劃問題是十分必要的。

許多學(xué)者對(duì)冗余自由度機(jī)器人的軌跡規(guī)劃問題進(jìn)行了深入研究。研究主要限定在固定環(huán)境下運(yùn)動(dòng)控制任務(wù)中，典型的研究結(jié)果包括面向笛卡爾空間或關(guān)節(jié)空間的多項(xiàng)式插值[4-7]、投影梯度法[8-10]、幾何模型法[11-13]、人工勢(shì)場(chǎng)法[14-16]、生物智能算法[17-19]等。近年來，一些研究開始將未知或動(dòng)態(tài)環(huán)境的感知加入到軌跡規(guī)劃的任務(wù)目標(biāo)中，但這些成果往往也需要以上述研究成果作為基礎(chǔ)，其中梯度投影法是提出較早、應(yīng)用較廣的一類方法[20-22]。

梯度投影法中的比例因子k的選擇直接影響著目標(biāo)函數(shù)優(yōu)化效果。如果比例因子(絕對(duì)值)選擇太小，關(guān)節(jié)空間自運(yùn)動(dòng)速度慢，運(yùn)動(dòng)優(yōu)化效果不明顯；如果比例因子選擇太大，機(jī)器人關(guān)節(jié)空間自運(yùn)動(dòng)的速度可能很高而引起關(guān)節(jié)速度越限[23-27]。因此合理選擇比例因子是梯度投影法中最為關(guān)鍵的一步。

目前關(guān)于選擇比例因子的研究主要可以分為三類：① 取比例因子為常值，但這需要經(jīng)過反復(fù)試值才能確定，應(yīng)用煩瑣；② 引入稱為均衡比例因子的系數(shù)，保證特解和齊次解在一個(gè)數(shù)量級(jí)上，防止特解或齊次解被對(duì)方掩蓋；③ 極值法：通過最優(yōu)化方法求解，將比例因子作為目標(biāo)函數(shù)，并結(jié)合一定的約束條件如關(guān)節(jié)速度極限、可優(yōu)化度等。

極值法的應(yīng)用較為廣泛，其基本思想是找到比例因子的允許取值范圍，然后取其為極值，以達(dá)到最速優(yōu)化的目的；用這種方法取得的值能起到最速優(yōu)化的效果,但由于缺少對(duì)機(jī)器人運(yùn)動(dòng)可優(yōu)化能力的度量工具,常導(dǎo)致機(jī)器人關(guān)節(jié)角振蕩。Xin等[5]提出了運(yùn)動(dòng)可優(yōu)化度，建立了次最優(yōu)化的放大系數(shù),此種方法解決了放大系數(shù)的實(shí)時(shí)、連續(xù)、理想選取問題,使得關(guān)節(jié)速度具有較好的連續(xù)性,但沒有討論自運(yùn)動(dòng)限制因子的取值問題,從而忽略了任務(wù)時(shí)間段內(nèi)各關(guān)節(jié)角速度的大小。楊三永等[8]討論了自運(yùn)動(dòng)限制因子的取值，通過引入量化的優(yōu)化力度函數(shù)用于單目標(biāo)運(yùn)動(dòng)學(xué)優(yōu)化控制，實(shí)現(xiàn)人為地控制任務(wù)時(shí)間段內(nèi)關(guān)節(jié)角速度的大小。

筆者基于極值法求比例因子，引入可優(yōu)化度和自運(yùn)動(dòng)限制因子來對(duì)其進(jìn)行描述。在此基礎(chǔ)上，針對(duì)極值法存在可能使關(guān)節(jié)加速度超限的問題，提出一種考慮關(guān)節(jié)加速度約束的新算法。

1 梯度投影法

一般地，機(jī)器人運(yùn)動(dòng)學(xué)方程可以表示為[28-29]

x=f(θ)

(1)

式中：x為機(jī)器人末端在笛卡兒空間的位姿，x∈Rm，對(duì)于平面問題m=3，空間問題m=6；θ為機(jī)器人關(guān)節(jié)空間的坐標(biāo)，θ∈Rn，n為機(jī)器的關(guān)節(jié)數(shù)。當(dāng)m

(2)

(3)

式中：J+為雅可比矩陣J的Moor-Penrose廣義逆或偽逆。

(4)

把式(3)的齊次解引入到關(guān)節(jié)角速度解中，就得到了式(2)的通解。

(5)

(6)

這是梯度投影法的基本公式。梯度投影法將逆運(yùn)動(dòng)學(xué)問題的解分解為最小范數(shù)解和齊次解兩部分，即如式(6)所示。

2 改進(jìn)的梯度投影法

基于速度約束的極值法求比例因子k值，并針對(duì)極值法未控制關(guān)節(jié)加速度的問題，引入關(guān)節(jié)加速度約束，介紹改進(jìn)的考慮關(guān)節(jié)加速度的新方法。再介紹選取的優(yōu)化準(zhǔn)則，最后得到改進(jìn)的梯度投影算法。

(1)比例因子。

使用傳統(tǒng)的極值法求比例因子時(shí)，引入關(guān)節(jié)速度約束。

(7)

(8)

那么，保證每個(gè)關(guān)節(jié)速度都不越限的比例因子k的極小值：

kmin=max(k1min,k2min,…，kimin，…，knmin)

(9)

但為彌補(bǔ)求解過程中加速度超限的缺陷，同時(shí)引入速度和加速度約束來選取k值。關(guān)節(jié)的速度約束同式(7)相同，引入關(guān)節(jié)加速度約束為

(10)

則由關(guān)節(jié)加速度約束，經(jīng)過Δt時(shí)間后關(guān)節(jié)的速度約束為

(11)

(12)

(13)

與式(8)類似，由關(guān)節(jié)速度約束和加速度約束確定的比例因子極小值為

(14)

(2)可優(yōu)化度和自運(yùn)動(dòng)限制因子。

式(9)定義了保證速度約束條件下的比例因子k的選擇，但還未定義k的使用條件，采用可優(yōu)化度概念，性能指標(biāo)對(duì)時(shí)間t的導(dǎo)數(shù)為

(15)

將式(6)代入得：

(16)

因?yàn)镮-J+J是一個(gè)冪等矩陣，所以：

[▽H(θ)]T(I-J+J)·[▽H(θ)]>0

(17)

因此：

(18)

定義可優(yōu)化度λ為

(19)

k值應(yīng)取為可優(yōu)化度λ的光滑連續(xù)函數(shù)：

k=(1-e-βλ)kmin

(20)

式中：β為自運(yùn)動(dòng)限制因子，將λ所表示的運(yùn)動(dòng)可優(yōu)化能力轉(zhuǎn)化為連續(xù)變換的k值。當(dāng)λ較大時(shí)，k≈kmin，對(duì)應(yīng)的自運(yùn)動(dòng)也較大，可實(shí)現(xiàn)最速優(yōu)化；當(dāng)λ較小時(shí)|k|<|kmin|，自運(yùn)動(dòng)相對(duì)減小。

假設(shè)允許最大自運(yùn)動(dòng)速度是極限值的90%，即k=0.9kmin，即：

1-e-βλm=0.9

(21)

(22)

式中：λm為λ的局部極大值。因此由式(20)可知，k值應(yīng)取為

(23)

(3)機(jī)器人關(guān)節(jié)位移極限指標(biāo)。

由于機(jī)器人關(guān)節(jié)的機(jī)械結(jié)構(gòu)和內(nèi)部走線的限制，機(jī)器人各關(guān)節(jié)不可能無限制的運(yùn)動(dòng)，因此，避免關(guān)節(jié)角越限是保證機(jī)器人安全運(yùn)行的關(guān)鍵?？蓪㈥P(guān)節(jié)許可位移的中值作為勢(shì)力場(chǎng)的引力源，構(gòu)建避關(guān)節(jié)極限位移指標(biāo)。

(24)

式中：n為關(guān)節(jié)數(shù)目；θimin為負(fù)方向關(guān)節(jié)位移極限，同理，正方向關(guān)節(jié)位移極限為θimax；ai為θimax和θimin的中值，即ai=0.5(θimax+θimin)。由上述分析可得改進(jìn)的梯度投影法公式為

(25)

3 基于改進(jìn)梯度投影法的軌跡規(guī)劃算法

根據(jù)前兩節(jié)的討論，當(dāng)輸入給定的笛卡爾空間軌跡后，經(jīng)過上述算法，可以得到關(guān)節(jié)空間的軌跡，完成冗余機(jī)器人各關(guān)節(jié)的控制，實(shí)現(xiàn)關(guān)節(jié)空間的軌跡規(guī)劃，其軌跡規(guī)劃算法如表1所示。

表1 軌跡規(guī)劃算法的偽代碼

4 仿真研究

4.1 仿真研究環(huán)境

移動(dòng)機(jī)械臂按照標(biāo)準(zhǔn)D-H方法建立坐標(biāo)系如圖1所示，按照右手定則確定Y軸。該機(jī)器人由3自由度的移動(dòng)平臺(tái)和7自由度的機(jī)械臂組成，移動(dòng)平臺(tái)由2個(gè)移動(dòng)關(guān)節(jié)和1個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)組成，機(jī)械臂由7個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)組成。其參數(shù)如表2所示。

表2 機(jī)器人D-H參數(shù)表

圖1 仿真移動(dòng)機(jī)械臂構(gòu)型和D-H坐標(biāo)系

仿真研究的參數(shù)如下：

(1)初始狀態(tài)。

初始關(guān)節(jié)角：θ0=[100，100，30，30，30，30，30，30，30，30]，移動(dòng)關(guān)節(jié)單位為mm，轉(zhuǎn)動(dòng)關(guān)節(jié)單位為(°)。初始機(jī)器人末端姿態(tài)x0=[-362.12，-27.74，1374.06，41.614，-76.492，-68.522]，末端位置的單位為mm，末端姿態(tài)的單位為(°)。限定關(guān)節(jié)的速率的最大值分別為[1000，1000，160，160，160，160，160，160，190，190]，線性速度單位為mm/s，關(guān)節(jié)角加速度單位為(°/s2)。關(guān)節(jié)的加速度絕對(duì)值的最大值分別為[1200，1200，320，320，320，320，320，320，380，380]，線性加速度單位為mm/s2，關(guān)節(jié)角加速度單位為(°/s2)。仿真步長(zhǎng)為0.01 s，總時(shí)長(zhǎng)2 s，共計(jì)200步。

(2)目標(biāo)軌跡。

笛卡爾空間軌跡：末端位置的單位為mm，末端姿態(tài)的單位為(°)，t為時(shí)間。由笛卡爾軌跡的末端位姿可得機(jī)器人末端相對(duì)于基坐標(biāo)系的任務(wù)速度：

(26)

(27)

末端位置速度的單位為mm/s，末端姿態(tài)速度的單位為(°/s)，t為時(shí)間。

仿真時(shí)間：2 s;采樣周期：0.01 s。

(3)性能指標(biāo)。

本文采用避關(guān)節(jié)位移極限準(zhǔn)則(式(24))。

在本例中有：

(28)

性能指標(biāo)的梯度函數(shù)為

(29)

在此例中：λm=0.115。

4.2 仿真結(jié)果

在相同計(jì)算平臺(tái)(CPU：Intel i7-8700 3.2 GHz，RAM：16 GB)進(jìn)行仿真研究。通過比較加速度和軌跡規(guī)劃計(jì)算時(shí)間評(píng)價(jià)相關(guān)算法。

4.2.1 考慮關(guān)節(jié)加速度約束算法的仿真結(jié)果

當(dāng)加入本文所考慮的關(guān)節(jié)加速度約束，即采用式(14)計(jì)算比例因子，仿真計(jì)算歷時(shí) 1.274461 s，機(jī)械臂軌跡如圖2所示。

圖2 考慮關(guān)節(jié)加速度約束算法的10自由度冗余機(jī)械臂運(yùn)動(dòng)軌跡圖

由圖2可知機(jī)械臂末端的軌跡符合要求。其各關(guān)節(jié)加速度變化情況如圖3所示。

圖3 考慮關(guān)節(jié)加速度約束算法的10關(guān)節(jié)加速度變化圖

由圖3可得，在此例中，10個(gè)關(guān)節(jié)的加速度絕對(duì)值最大值分別為：[35.3499，39.5947，210.2016，186.5748，156.5505，179.0516，247.9526，189.1394，152.4408，353.9720]。其中移動(dòng)關(guān)節(jié)單位為mm/s2，轉(zhuǎn)動(dòng)關(guān)節(jié)單位為(°)/s2。各關(guān)節(jié)加速度均在加速度約束范圍內(nèi)。

4.2.2 考慮關(guān)節(jié)速度約束的對(duì)比算法的仿真結(jié)果

當(dāng)采用速度約束的極值法，即采用式(8)計(jì)算比例因子，仿真計(jì)算歷時(shí) 1.248230 s，機(jī)械臂的運(yùn)動(dòng)軌跡如圖4所示。

圖4 考慮關(guān)節(jié)速度約束的對(duì)比算法的10自由度冗余機(jī)械臂運(yùn)動(dòng)軌跡圖

由圖4可得，仿真過程中，機(jī)械臂末端軌跡符合要求。機(jī)械臂各關(guān)節(jié)的運(yùn)動(dòng)加速度如圖5所示。

圖5 考慮關(guān)節(jié)速度約束的對(duì)比算法的10關(guān)節(jié)加速度變化圖

由圖5可得，此時(shí)這10個(gè)關(guān)節(jié)的加速度絕對(duì)值最大值分別為：[206.94，1610.45，2860.14，1869.84，3285.68，5072.16，5134.45，975.01，2272.34，2872.00]。其中移動(dòng)關(guān)節(jié)單位為mm/s2，轉(zhuǎn)動(dòng)關(guān)節(jié)單位為(°)/s2，遠(yuǎn)超過加速度限定范圍。

4.2.3 考慮關(guān)節(jié)加速度約束的對(duì)比算法的仿真結(jié)果

為了與考慮關(guān)節(jié)加速度約束的典型算法進(jìn)行比較，還針對(duì)文獻(xiàn)[29]提出的算法進(jìn)行了對(duì)比研究，該算法以時(shí)間沖擊最優(yōu)為目標(biāo)進(jìn)行軌跡規(guī)劃，并采用模擬退火遺傳算法求解，仿真計(jì)算歷時(shí) 6.061188 s，機(jī)械臂的運(yùn)動(dòng)軌跡如圖6所示。

圖6 考慮關(guān)節(jié)加速度最優(yōu)的對(duì)比算法的10自由度冗余機(jī)械臂運(yùn)動(dòng)軌跡圖

由圖6可知機(jī)械臂末端的軌跡符合要求。其各關(guān)節(jié)加速度變化情況如圖7所示。

圖7 考慮關(guān)節(jié)加速度最優(yōu)的對(duì)比算法的10關(guān)節(jié)加速度變化圖

由圖7可得，在此例中10個(gè)關(guān)節(jié)的加速度絕對(duì)值最大值分別為：[118.80，119.21，114.40，245.46，89.22，144.81，132.11，115.78，119.63，190.11]。其中移動(dòng)關(guān)節(jié)單位為mm/s2，轉(zhuǎn)動(dòng)關(guān)節(jié)單位為(°)/s2。各關(guān)節(jié)加速度均在加速度約束范圍內(nèi)，由于以時(shí)間沖擊作為最優(yōu)目標(biāo)，該算法的關(guān)節(jié)加速度較本文提出的算法低，但該算法的時(shí)間復(fù)雜度相對(duì)較高。

5 結(jié)束語

在分析移動(dòng)機(jī)械臂自由度高、穩(wěn)定性差、在運(yùn)動(dòng)學(xué)及軌跡規(guī)劃上存在問題的基礎(chǔ)上，采用梯度投影法進(jìn)行了逆運(yùn)動(dòng)學(xué)分析，梯度投影法將逆解分為最小范數(shù)解和零空間的齊次解兩部分，采用極值法計(jì)算決定最小范數(shù)解和齊次解所占比例的比例因子。引入可優(yōu)化度概念來定義比例因子的使用條件，引入自運(yùn)動(dòng)限制因子的概念保證比例因子的連續(xù)性。為彌補(bǔ)求解過程中關(guān)節(jié)加速度超限，在極值法求比例因子的過程中引入關(guān)節(jié)加速度約束，改進(jìn)了梯度投影算法。介紹了冗余自由度機(jī)器人運(yùn)動(dòng)學(xué)性能優(yōu)化指標(biāo)。在求解逆解的過程中可完成機(jī)器人關(guān)節(jié)空間的軌跡規(guī)劃，給出了逆解算法的偽代碼。對(duì)逆解算法進(jìn)行了仿真研究。在給出機(jī)械臂的初始狀態(tài)、目標(biāo)軌跡和性能指標(biāo)后，對(duì)傳統(tǒng)算法和改進(jìn)后的算法進(jìn)行了對(duì)比仿真，驗(yàn)證了所提算法控制關(guān)節(jié)加速度的有效性。