冗余機械臂軌跡的增廣Lagrange?改進粒子群算法優(yōu)化

吳國強

（湖州職業(yè)技術(shù)學(xué)院，浙江湖州 313000）

1 引言

冗余機械臂可以實現(xiàn)機械臂對避障、避奇異、容錯控制等特殊需求，但是其運動學(xué)逆解和軌跡優(yōu)化研究也帶來了困難。

機械臂不同的運動軌跡具有不同的平滑性和沖擊性，進而影響機械臂的驅(qū)動能耗、工作效率、使用壽命、運行平滑性和振動性等[1]。因此研究冗余機械臂的軌跡優(yōu)化問題具有重要的實際應(yīng)用價值。

機械臂軌跡規(guī)劃是機械臂運動控制的基礎(chǔ)，根據(jù)不同的性能需求，軌跡優(yōu)化指標(biāo)包括時間最優(yōu)、能耗最優(yōu)、沖擊最小以及多指標(biāo)結(jié)合的綜合優(yōu)化等[2]。

根據(jù)工作空間的不同，機械臂軌跡規(guī)劃分為笛卡爾空間和關(guān)節(jié)空間兩類[3]，兩者之間通過運動學(xué)正解和逆解可以相互轉(zhuǎn)換，這里研究的是關(guān)節(jié)空間軌跡的綜合優(yōu)化問題。

軌跡的綜合優(yōu)化主要包括兩種思路，一是使用加權(quán)系數(shù)法將多個目標(biāo)轉(zhuǎn)化為單目標(biāo)優(yōu)化問題，通過調(diào)整不同目標(biāo)的權(quán)系數(shù)來調(diào)整優(yōu)化重心[4]；二是使用非支配排序思想得到Pareto前沿解[5]，而后依據(jù)優(yōu)化需求從Pareto解集中選擇優(yōu)化解。本質(zhì)上講，兩種優(yōu)化方法均存在優(yōu)化取舍的過程。文獻[6]考慮機械臂運動學(xué)、力矩和載荷約束，使用三次多項式作為關(guān)節(jié)空間基軌跡，使用遺傳算法規(guī)劃了時間最優(yōu)軌跡，依據(jù)實驗驗證了該方法的有效性。文獻[7]考慮機械臂重載情況下的運動約束，使用加權(quán)系數(shù)法建立了時間?加加速度的綜合優(yōu)化模型，提出了自適應(yīng)遺傳算法的模型求解方法，仿真結(jié)果驗證了算法的有效性。文獻[8]建立了調(diào)姿時長和能量消耗的加權(quán)目標(biāo)函數(shù)，提出了基于動力學(xué)RRT*算法的自由漂浮空間機器人軌跡規(guī)劃方法，仿真結(jié)果表明該方法可以快速地生成自由漂浮機器人的運動軌跡。

總的來說，一般將機械臂軌跡規(guī)劃問題通過建模轉(zhuǎn)化為帶約束的優(yōu)化問題，而優(yōu)化算法的優(yōu)化能力成為決定解質(zhì)量的關(guān)鍵，因此關(guān)于優(yōu)化算法的研究是機械臂軌跡規(guī)劃的一個焦點。

這里針對冗余機械臂的軌跡優(yōu)化問題，建立了以減小工作時間和沖擊為目標(biāo)的優(yōu)化模型。使用增廣拉格朗日乘子法將約束優(yōu)化問題轉(zhuǎn)化為無約束優(yōu)化問題，提出了多學(xué)習(xí)行為粒子群算法的求解方法，達到了減小工作時間和運動沖擊的目的。

2 問題描述與建模

2.1 冗余機械臂介紹

這里研究對象為7自由度冗余機械臂，其構(gòu)型，如圖1（a）所示。實物，如圖1（b）所示。圖中坐標(biāo)系(x0，y0，z0)為基坐標(biāo)系，坐標(biāo)系(xi，yi，zi)，i=1～7為7個關(guān)節(jié)坐標(biāo)系，其中坐標(biāo)系(x7，y7，z7)為機械臂末端執(zhí)行器坐標(biāo)系。圖1 中參數(shù)θi為關(guān)節(jié)i的轉(zhuǎn)動角度；αi為扭角，表示沿Xi?1軸從Zi?1轉(zhuǎn)動到Zi的角度；ai為連桿i的長度，di為偏置量。

圖1 7自由度機械臂構(gòu)型圖Fig.1 The Configuration of the 7?DOF Manipulator

所研究機械臂的構(gòu)型參數(shù)，如表1所示。

表1 機械臂構(gòu)型參數(shù)Tab.1 The Configuration Parameters of the Manipulator

2.2 問題描述與模型建立

根據(jù)機械臂的活動范圍、障礙物分布及任務(wù)需求，在笛卡爾空間為機械臂規(guī)劃出若干個必經(jīng)的軌跡點，記為P0，P1，P2，…，PM，其中P0為機械臂的初始位姿，PM為最終位姿，P1～PM?1為中間軌跡點的位姿。使用文獻［9］給出的冗余機械臂逆運動學(xué)求解法，可以將笛卡爾空間點轉(zhuǎn)化為關(guān)節(jié)空間點，記為θ0，θ1，θ2，…，θM。由于研究對象為7自由度機械臂，則每個關(guān)節(jié)空間點θm由7個關(guān)節(jié)角組成，記為θm=(θm1，θm2，…，θm7)。

機械臂關(guān)節(jié)空間軌跡規(guī)劃描述為：通過優(yōu)化機械臂到達各軌跡點的時間點tm，m=1，2，…，M，實現(xiàn)機械臂工作時間短和運動沖擊小的目標(biāo)。將關(guān)節(jié)m的空間軌跡記為Qm(t)，使用加權(quán)系數(shù)法將兩個優(yōu)化目標(biāo)轉(zhuǎn)化為一個，為：

式中：minf—目標(biāo)函數(shù)；KT—時間權(quán)值；KJ—沖擊權(quán)值，且KT+KJ=1；α—調(diào)節(jié)系數(shù)，將時間參數(shù)與沖擊參數(shù)調(diào)節(jié)在同一數(shù)量級；tM—機械臂工作時間；Q…m(t)—軌跡dQm(t)的3 階導(dǎo)數(shù)。

優(yōu)化的約束條件包括轉(zhuǎn)動角速度約束、轉(zhuǎn)動角加速度約束及轉(zhuǎn)角沖擊約束等，即：

式中：Vmmax—關(guān)節(jié)m的最大轉(zhuǎn)動角速度；Ammax—關(guān)節(jié)m的最大轉(zhuǎn)動角加速度；Jmmax—關(guān)節(jié)m可承受的最大轉(zhuǎn)角沖擊。

綜合式（1）與式（2），即為冗余機械臂軌跡優(yōu)化的帶約束優(yōu)化模型。

2.3 軌跡插值函數(shù)

三次樣條插值函數(shù)具有簡單易實現(xiàn)、可以克服軌跡點間的過度振蕩和沖擊等優(yōu)勢，因此這里使用三次樣條插值函數(shù)作為軌跡插值函數(shù)。在前文中，M?1 個時間點將[t0，tM]劃分為{(t0，t1)，(t1，t2)，…(tM?1，tM)}，共M個時間段。在每個時間段上關(guān)節(jié)空間軌跡均為三次函數(shù)，且滿足以下條件：

（1）在每個區(qū)間(ti，ti+1)上，S(t)=St(t)均為三次函數(shù)；

（2）滿足插值條件，即S(ti)=θ(ti)；

則稱曲線S(t)為三次樣條插值函數(shù)。

將S(t)記為一般形式，為：

對于M段插值函數(shù)Si(t)，i=1，2，…M，共4M個參數(shù)待定，需要4M個方程才能求解。

根據(jù)以上4M個等式可以確定4M個未知參數(shù)，從而得到三次插值函數(shù)。

3 增廣拉格朗日?粒子群算法求解

3.1 帶約束優(yōu)化轉(zhuǎn)化為無約束優(yōu)化問題

將約束優(yōu)化問題轉(zhuǎn)化為無約束優(yōu)化問題一般使用罰函數(shù)法，但是此方法要求罰因子趨于無窮大或者無窮小才能取得目標(biāo)函數(shù)最值，使得迭代過程收斂較慢，而增廣拉格朗日乘子法可以較好解決這一問題，其核心思想是通過構(gòu)造外點懲罰函數(shù)從而將約束優(yōu)化轉(zhuǎn)化為非約束優(yōu)化。

這里的約束為不等式約束，因此使用適用于不等式約束的增廣拉格朗日乘子法。

首先給出帶不等式約束的一般優(yōu)化模型，為：

式中：J—不等式約束的數(shù)量。使用增廣拉格朗日乘子法可以將其轉(zhuǎn)化為無約束優(yōu)化問題，為：

式中：x—優(yōu)化變量；λj—約束條件j的拉格朗日乘子；rj—約束條件j的懲罰因子，φj表達式為：

結(jié)合式（5）和式（6）可以將約束優(yōu)化問題轉(zhuǎn)化為無約束優(yōu)化問題。其中拉格朗日乘子λj的迭代公式為：

式中：t—迭代次數(shù)。懲罰因子rj的迭代方法為：

式中：εg—描述約束的誤差精度。以上迭代的終止條件為：

式中：ε—收斂精度。

3.2 多學(xué)習(xí)行為粒子群算法

將粒子規(guī)模記為N，粒子維度記為D，粒子i位置記為Xi=(xi1，xi2，…，xiD)，速度記為Vi=(vi1，vi2，…，viD)，粒子i的歷史最優(yōu)位置記為Pi=(pi1，pi2，…，piD)，粒子群體的最優(yōu)位置記為Pg=(pg1，pg2，…，pgD)，則粒子i搜索過程描述為［10］：

式中：ω—運動慣性；t—迭代次數(shù)；

c1、c2—自身學(xué)習(xí)因子和種群學(xué)習(xí)因子；

r1、r2—（0，1）間隨機數(shù)。

參考生物進化論的思想可知，單一的物種進化方式會極大地降低物種多樣性，從而使算法失去進化能力。為了解決單一進化帶來的問題，這里提出了多樣的進化方式。

（1）依據(jù)群體認知獲得進化方向，此時將種群質(zhì)心位置作為一個進化方向，為：

分析式（11）可知，粒子在自身慣性和質(zhì)心引導(dǎo)之下進行搜索。

（2）從種群最優(yōu)和其他粒子中獲得進化方向，此時適用于較差粒子，因較差粒子位置不具備參考價值，因此取消慣性作用，為：

式中：i1、i2—種群中任意不相同的兩個個體。

（3）以差分方式從其余粒子中獲得進化方向，即：

式中：i1、i2、i3—種群中任意不相同的三個個體。

3.3 學(xué)習(xí)行為的價值和選擇

在前文中，新定義了3種粒子學(xué)習(xí)行為，加上傳統(tǒng)的學(xué)習(xí)行為，共4種粒子學(xué)習(xí)行為。每種學(xué)習(xí)行為的價值依據(jù)目標(biāo)函數(shù)值的減少率定義，為：

式中：Vij(k)—粒子i選擇學(xué)習(xí)行為j的價值；

fj—學(xué)習(xí)行為j的目標(biāo)函數(shù)。

依據(jù)各學(xué)習(xí)行為的價值構(gòu)造學(xué)習(xí)行為的選擇概率，為：

式中：Pij(k)—粒子i選擇學(xué)習(xí)行為j的概率；

Pmin—最小選擇概率，此參數(shù)設(shè)置目的是防止某一學(xué)習(xí)行為的被選概率為0。

按照式（15）計算各學(xué)習(xí)行為被選概率，按照輪盤賭的規(guī)則確定最終選擇的學(xué)習(xí)概率，這樣既能夠保證較好學(xué)習(xí)行為具有最大被選概率，同時能夠保證學(xué)習(xí)行為的多樣性，從而促進粒子的多樣性。

3.4 增廣拉格朗日-粒子群算法求解流程

按照增廣拉格朗日乘子法和多學(xué)習(xí)行為粒子群算法的原理，制定基于拉格朗日—粒子群算法的求解流程，如圖2所示。

圖2 求解流程圖Fig.2 Solving Flow

4 實驗驗證

4.1 實驗條件設(shè)置

以2.1節(jié)介紹的機械臂為實驗對象，根據(jù)任務(wù)需要、機械臂活動范圍及障礙物分布，設(shè)置7個機械臂的空間軌跡點，如表2所示。由表2中數(shù)據(jù)可知，關(guān)節(jié)2和關(guān)節(jié)6的關(guān)節(jié)角未發(fā)生變化，為鎖死狀態(tài)。

表2 關(guān)節(jié)空間軌跡點Tab.2 Trajectory Points of Joint Space

關(guān)節(jié)運動能力參數(shù)為：Vmax=3°/s、Amax=3°/s2、Jmax=5°/s3。

拉格朗日乘子初值為：λj=0，j∈[1，7]，初始罰因子為：rj=1000，j∈[1，7]，約束誤差精度為：εg=10?4，收斂精度為：ε=10?4。粒子群算法最大迭代次數(shù)為：Kmax=100，粒子數(shù)量為40。

4.2 實驗結(jié)果與分析

機械臂的最大運行時間為150s，為了進行比較，本文同時使用增廣拉格朗日乘子法?傳統(tǒng)粒子群算法、增廣拉格朗日乘子法?多學(xué)習(xí)行為粒子群算法對優(yōu)化模型進行求解。

KT取1.0，0.5，0等3種情況，KJ取相應(yīng)值，α=100。以KT=1為例，此時為純時間優(yōu)化，粒子群算法與多學(xué)習(xí)行為粒子群算法的求解過程，如圖3所示。

在圖3中，傳統(tǒng)粒子群算法的收斂次數(shù)為62次，最優(yōu)結(jié)果為74.92s；多學(xué)習(xí)行為粒子群算法的收斂次數(shù)為33次，最優(yōu)結(jié)果為73.21s，以上數(shù)據(jù)說明多學(xué)習(xí)行為粒子群算法的收斂速度和求解結(jié)果均優(yōu)于傳統(tǒng)粒子群算法。

圖3 兩種算法的求解程Fig.3 Solving Process of the Two Algorithms

統(tǒng)計KT取1.0，0.5，0等3種情況下，粒子群算法和多學(xué)習(xí)行為粒子群算法的優(yōu)化結(jié)果，如表3所示。

表3 優(yōu)化結(jié)果統(tǒng)計Tab.3 Statistics of the Optimization Result

表3中，當(dāng)KT=0時為單純的沖擊目標(biāo)優(yōu)化，此時粒子群算法和改進粒子群算法都將時間優(yōu)化為了最大值，從而得到?jīng)_擊最小值，符合常識。

由表3可知，在KT取1.0，0.5兩種情況下，多學(xué)習(xí)行為粒子群算法優(yōu)化路徑的時間和沖擊均小于傳統(tǒng)粒子群算法。

結(jié)合圖3可知，多學(xué)習(xí)行為粒子群算法的收斂速度也快于傳統(tǒng)粒子群算法。

這是因為改進粒子群算法定義了多種粒子的學(xué)習(xí)行為，并依據(jù)各學(xué)習(xí)行為的價值確定學(xué)習(xí)行為的選擇概率，在保證粒子多樣性的同時，能夠選擇粒子進化最快的學(xué)習(xí)行為，因此改進粒子群算法的收斂速度和優(yōu)化結(jié)果均好于粒子群算法。

由于篇幅有限，以KT=0.5為例，得到7個關(guān)節(jié)的角位置、角速度、角加速度、角加加速度曲線，傳統(tǒng)粒子群算法規(guī)劃軌跡，如圖4所示。

圖4 傳統(tǒng)粒子群算法的機械臂軌跡Fig.4 Manipulator Trajectory Planned by Traditional PSO

多學(xué)習(xí)行為粒子群算法規(guī)劃軌跡，如圖5所示。

圖5 改進粒子群算法的機械臂軌跡Fig.5 Manipulator Trajectory Planned by Improved PSO

由于關(guān)節(jié)2和關(guān)節(jié)6為鎖死狀態(tài)，因此關(guān)節(jié)2和關(guān)節(jié)6的曲線不再給出。

由圖4、圖5可以看出，當(dāng)KT=0.5時，使用傳統(tǒng)粒子群算法和多學(xué)習(xí)行為粒子群算法規(guī)劃的機械臂關(guān)節(jié)空間軌跡在角位置、角速度上是平滑的，在角加速度上是連續(xù)的。

在角加加速度方面未超出約束范圍，滿足機械臂關(guān)節(jié)空間軌跡的可用條件。但是對比圖4（d）、圖5（d）可知，傳統(tǒng)粒子群算法規(guī)劃的軌跡沖擊明顯大于改進粒子群算法規(guī)劃的軌跡，這是因為傳統(tǒng)粒子群算法進行軌跡規(guī)劃時陷入了局部最優(yōu)，優(yōu)化不夠徹底，而改進粒子群算法兼顧了算法多樣性和收斂性，優(yōu)化能力強于傳統(tǒng)粒子群算法。以上優(yōu)化軌跡和統(tǒng)計參數(shù)表明，基于增廣拉格朗日?多學(xué)習(xí)行為粒子群算法的機械臂軌跡優(yōu)化方法是有效的，且規(guī)劃效果較好。

5 結(jié)論

這里研究了冗余機械臂的關(guān)節(jié)空間軌跡優(yōu)化問題，使用加權(quán)系數(shù)法將多個優(yōu)化目標(biāo)統(tǒng)一為單個優(yōu)化目標(biāo)，使用增廣拉格朗日乘子法將約束優(yōu)化問題轉(zhuǎn)化為無約束優(yōu)化問題，提出了多學(xué)習(xí)行為粒子群算法的求解方法。經(jīng)過驗證可以看出，多學(xué)習(xí)行為粒子群算法的規(guī)劃速度和優(yōu)化結(jié)果好于傳統(tǒng)粒子群算法，且多學(xué)習(xí)行為粒子群算法規(guī)劃的軌跡平滑可用。