基于改進梯度投影算法的移動機械臂軌跡規(guī)劃研究

魏麗君 ，吳海波，劉海龍

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

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

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

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

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

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

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

1 梯度投影法

一般地，機器人運動學(xué)方程可以表示為[28-29]

x=f(θ)

(1)

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

(2)

(3)

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

(4)

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

(5)

(6)

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

2 改進的梯度投影法

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

(1)比例因子。

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

(7)

(8)

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

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

(9)

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

(10)

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

(11)

(12)

(13)

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

(14)

(2)可優(yōu)化度和自運動限制因子。

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

(15)

將式(6)代入得：

(16)

因為I-J+J是一個冪等矩陣，所以：

[▽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ōu)化能力轉(zhuǎn)化為連續(xù)變換的k值。當λ較大時，k≈kmin，對應(yīng)的自運動也較大，可實現(xiàn)最速優(yōu)化；當λ較小時|k|<|kmin|，自運動相對減小。

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

1-e-βλm=0.9

(21)

(22)

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

(23)

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

由于機器人關(guān)節(jié)的機械結(jié)構(gòu)和內(nèi)部走線的限制，機器人各關(guān)節(jié)不可能無限制的運動，因此，避免關(guān)節(jié)角越限是保證機器人安全運行的關(guān)鍵。可將關(guān)節(jié)許可位移的中值作為勢力場的引力源，構(gòu)建避關(guān)節(jié)極限位移指標。

(24)

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

(25)

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

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

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

4 仿真研究

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

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

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

圖1 仿真移動機械臂構(gòu)型和D-H坐標系

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

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

初始關(guān)節(jié)角：θ0=[100，100，30，30，30，30，30，30，30，30]，移動關(guān)節(jié)單位為mm，轉(zhuǎn)動關(guān)節(jié)單位為(°)。初始機器人末端姿態(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é)的加速度絕對值的最大值分別為[1200，1200，320，320，320，320，320，320，380，380]，線性加速度單位為mm/s2，關(guān)節(jié)角加速度單位為(°/s2)。仿真步長為0.01 s，總時長2 s，共計200步。

(2)目標軌跡。

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

(26)

(27)

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

仿真時間：2 s;采樣周期：0.01 s。

(3)性能指標。

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

在本例中有：

(28)

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

(29)

在此例中：λm=0.115。

4.2 仿真結(jié)果

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

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

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

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

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

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

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

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

當采用速度約束的極值法，即采用式(8)計算比例因子，仿真計算歷時 1.248230 s，機械臂的運動軌跡如圖4所示。

圖4 考慮關(guān)節(jié)速度約束的對比算法的10自由度冗余機械臂運動軌跡圖

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

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

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

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

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

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

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

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

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

5 結(jié)束語

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