電能表檢驗臺體串聯機械臂時間最優軌跡規劃

歐 新，周 璐，張 鷙，吳月家，趙云斌

貴州電網有限責任公司 貴陽供電局，貴陽 550002

隨著機器人技術的發展和工業自動化的推進，串聯機械臂被應用于越來越多的場景中，時間最優[1-4]軌跡規劃能夠有效縮短機械臂運動時間，提高工作效率，具有十分重要的研究意義。目前主流的時間最優軌跡規劃方法主要分為兩個階段，首先利用軌跡插值算法對機械臂運動路徑進行擬合；其次通過優化算法對機械臂在擬合路徑上的運動時間進行優化。

較為常用的機械臂軌跡插值算法有樣條插值和多項式插值兩種，其中三次樣條插值、B樣條插值等樣條插值算法通過部分已知路徑點擬合完整路徑，適用于要求復雜的連續運動任務[5-6]；三次多項式插值、五次多項式插值等多項式插值算法由機械臂首末狀態決定運動路徑，適用于點到點運動[7]。在實際應用場景中點到點運動或簡單的連續運動更為常見，并且簡單的連續運動可以離散為幾個點到點運動，所以機械臂點到點運動軌跡規劃的研究更具廣泛適應性[8]。為了保證關節首末角位移、角速度及角加速度均可控，文中選擇五次多項式插值算法對機械臂的運動路徑進行擬合。

機械臂運行時間一般通過群體智能優化算法進行優化，如遺傳算法、蟻群算法、粒子群算法等[9-12]，其中以遺傳算法在機械臂時間最優軌跡規劃問題中的研究最為深入，許多學者針對機械臂運動特性提出了很多針對性的改進。文獻[13]針對超冗余度機械臂提出了一種時間最優軌跡規劃方法，并用標準多種群遺傳算法優化了機械臂的運行時間。文獻[14]提出一種自適應精英遺傳算法，在五次多項式插值軌跡規劃算法的基礎上，采用基于遺傳算法框架的精英群和自適應調節機制提高優化性能。文獻[15]中通過自適應調整交叉和變異的概率，達到更快的收斂速度。文獻[16]提出一種基于混沌局部搜索的雙種群遺傳最優時間軌跡規劃算法，利用雙種群遺傳算法全局搜索能力強、進化速度快和混沌算法局部搜索能力強的優點，規劃時間最優的運動軌跡。文獻[17]在三次樣條插值軌跡規劃算法的基礎上采用聚類方法改進遺傳算法，保留優良個體的同時兼顧種群多樣性，提高搜索效率。

上述文獻中分別通過多種群、自適應交叉和變異、貪婪選擇策略等方面對遺傳算法進行改進，然而仍然存在算法收斂一致性差，收斂速度有待提高等問題。為此文中提出一種基于改進多種群遺傳算法的軌跡規劃方法，在五次多項式插值算法擬合的機械臂運動路徑上，通過改進的多種群遺傳算法對機械臂運動時間進行優化，改進之處包括：設計含有懲罰項的適應度函數，降低不滿足運動學約束條件的個體被選擇的概率；引入貪婪選擇策略，按照比例保留父代種群優秀個體，替換子代種群較差個體；自適應調整交叉和變異概率，加快算法收斂速度。以ER-4iA機械臂為實驗對象，通過仿真實驗驗證所提算法的可行性，并將所提算法與蟻群算法、粒子群算法、標準遺傳算法和標準多種群遺傳算法進行對比實驗，結果表明所提軌跡規劃方法具有更好的收斂一致性和更快的收斂速度。

1 串聯機械臂運動學模型

文中提出的機械臂時間最優軌跡規劃方法適用于串聯機械臂結構，串聯機械臂由一組剛性連桿通過轉動關節串聯而成，是目前最常見的機械臂結構。1955年Denavit和Hartenberg提出的D-H方法是建立串聯機械臂運動學模型的標準方法[18]，其原理是在各連桿的末端關節處建立坐標系，相鄰坐標系通過4個參數計算變換矩陣，從而建立機械臂末端和基坐標之間的位姿變換關系。然而標準D-H方法無法表示連桿關于y軸的運動，且在樹形機械臂結構的建模上存在歧義，這些問題在1986年Khalil和Kleinfinger提出的改進D-H方法中得以改善[19]。改進D-H方法在連桿首端關節處建立坐標系，且相鄰坐標系之間的參數變換順序和標準D-H方法完全相反，是目前最常用的串聯機械臂運動學建模方法。

以圖1所示的六自由度串聯機械臂為例，通過改進D-H方法建立機械臂各連桿坐標系的步驟如下：

圖1 六自由度串聯機械臂結構Fig.1 Structure of tandem manipulator with six degrees of freedom

（1）確定Z軸：Zi軸沿關節軸i的方向。

（2）確定原點：原點Oi為Zi軸和Zi+1軸的交點或其公垂線與Zi軸的交點。

（3）確定X軸：Xi軸沿Zi軸和Zi+1軸的公垂線由關節軸i指向關節軸i+1，若Zi軸與Zi+1軸相交則Xi軸垂直于它們所在的平面。

（4）確定Y軸：Yi軸由右手定則確定。

以地面坐標系為基坐標系，相鄰兩個關節坐標系之間的變換矩陣可以通過鏈式法則計算：

其中，αi-1是Zi-1到Zi繞Xi-1正方向轉動的角度，ai-1是轉動后的Zi-1到Zi沿Xi-1正方向的距離，θi是移動后的Xi-1到Xi繞Zi正方向轉動的角度，也是實際控制機械臂運動的關節角度，di是轉動后的Xi-1到Xi沿Zi方向上的距離，坐標系(Xi-1,Yi-1,Zi-1)通過上述四個變換之后能夠與坐標系(Xi,Yi,Zi)完全重合。機械臂末端坐標系與基坐標系的變換矩陣為：

根據上述機械臂運動學模型，已知各關節角度即可計算機械臂末端位姿，通過控制機械臂各關節角θ關于時間的角位移曲線Qi(t)就能夠控制機械臂末端的運動軌跡，從而將機械臂的空間軌跡規劃問題轉換為各關節角運動曲線規劃問題。

2 優化問題描述

串聯機械臂在工業生產和物流運輸等領域擁有廣泛的應用，時間最優軌跡規劃能夠有效縮短機械臂的運行周期，提高工作效率。文中以機械臂點到點軌跡的運行時間為主要優化目標，假設機械臂從初始關節狀態Q0運動到終止關節狀態Qf的時間為tf，則目標函數可以表示為：

此外機械臂在運動過程中應嚴格滿足其固有的運動學約束條件，因此優化問題可以描述為：

式中，V、A、J分別表示機械臂各關節運動過程中的角速度、角加速度和角加加速度，Vs、As、Js分別為它們的額定最大值。

對于五次多項式插值算法擬合的運動軌跡，關節角θ關于時間的角位移曲線Q(t)、角速度曲線V(t)、角加速度曲線A(t)及角加加速度曲線J(t)可以表示為：

通過求解五次多項式中的6個系數a0～a5即可得到各關節運動曲線表達式。

3 機械臂時間最優軌跡規劃方法

3.1 多種群遺傳算法

多種群遺傳算法是遺傳算法的一種改進，多個種群同時沿不同方向進化，種群之間通過移民算子進行交互，最優解從各種群最優個體構成的精英種群中產生，算法流程圖如圖2所示。

圖2 多種群遺傳算法流程圖Fig.2 Flow chart of multi-population genetic algorithm

其中將自身種群的最優個體作為移民算子，在每次選擇父代個體之前遷移到相鄰種群中，替代目標種群中的最劣個體，這有三點好處：

（1）為目標種群帶去新的優秀的基因，豐富了目標種群的多樣性。

（2）目標種群陷入局部最優時，移民算子的加入有利于打破僵局。

（3）種群之間協同進化，加速種群收斂。

3.2 基于改進多種群遺傳算法的時間最優軌跡規劃

針對機械臂點到點軌跡規劃任務，以五次多項式軌跡插值的時間tf為優化目標，在多種群遺傳算法的基礎上進行相應的調整和改進，提出一種基于改進多種群遺傳算法的時間最優軌跡優化方法，具體改進點如下：

帶有懲罰項的適應度函數。根據式（4）描述的優化問題，設計如下個體適應度函數：

式中，x是個體基因的表現形式，即運行時間tf,kt是時間調和系數，Ei是各關節根據機械臂運動約束條件設計的懲罰項，其計算公式為：

貪婪選擇策略：在父代種群選擇階段，按照一定比例保留當前種群的部分優秀個體，經過交叉和變異操作之后通過重插入的方法，用保留個體逐個替換子代種群中的較差個體，既保證了優秀個體不被破壞，也能加快算法收斂。

自適應交叉和變異：在多種群遺傳算法中，為了實現不同種群進化方向的差異性，各種群的交叉和變異概率Pm和Pc在特定范圍[PmL,PmU]和[PcL,PcU]內隨機產生，為了達到更快的收斂速度，將交叉和變異概率設置為隨迭代次數自適應降低，迭代早期通過較大的交叉和變異概率保證種群多樣性，迭代后期通過較小的交叉和變異概率保留最優解并加快收斂速度，自適應交叉和變異概率表達式為：

其中，gen為當前算法迭代次數。

3.3 算法整體流程

綜上所述，文中設計的基于改進多種群遺傳算法的機械臂時間最優軌跡規劃方法主要步驟如下：

（1）設置主要參數：基因編碼長度chromlen，種群大小popsize，種群數量popnum，式（7）～（9）中的參數。

（2）初始化種群。

（3）進行貪婪選擇、自適應交叉和變異操作。

（4）計算適應度值：對個體解碼獲取實數x，根據式（5）和式（6）計算五次多項式系數，得到各關節角速度、角加速度、角加加速度函數，根據式（7）和式（8）計算個體適應度函數值。

（5）獲取移民算子和精英種群。

（6）判斷是否滿足收斂條件：若滿足則輸出最優個體，不滿足則進行移民操作并重復步驟（3），收斂條件為精英種群最優個體保持一定的代數。

4 仿真實驗與結果分析

4.1 仿真實驗平臺

文中使用發那科的ER-4iA機械臂作為實驗驗證平臺，通過MATLAB中的Robotics Toolbox工具箱進行機械臂仿真建模。根據改進D-H方法計算ER-4iA機械臂的D-H參數如表1所示，用SerialLink函數建立如圖3所示的ER-4iA機械臂仿真模型。

圖3 ER-4iA機械臂仿真模型Fig.3 Simulation model of manipulator ER-4iA

表1 ER-4iA機械臂D-H參數Table 1 D-H parameters of manipulator ER-4iA

為了在實驗平臺上驗證文中時間最優軌跡規劃方法的可靠性，基于控制變量的原則，設置機械臂各關節始末角度、速度和加速度為定值：

對此只要提供機械臂運行時間tf就可以求解五次多項式的6個系數a0～a5，從而根據式（5）得到各關節角速度函數、角加速度函數和角加加速度函數。

4.2 算法仿真實驗

設置基因編碼長度chromlen=10,種群大小popsize=40,種群數量popnum=10,適應度函數中的時間調和系數kt=0.1，懲罰系數kV=kA=kJ=1，隨機交叉概率范圍[PmL,PmU]=[0.7,0.9]，隨機變異概率[PcL,PcU]=[0.001,0.05]，機械臂關節運動學約束條件如表2所示。

表2 機械臂關節約束條件Table 2 Constraints of mechanical arm joint

按照上述參數設置進行多組實驗，最優運行時間均為3.914 9 s，此時各關節角位移、角速度、角加速度及角加加速度隨時間的變化曲線如圖4所示。

圖4 各關節角運動曲線Fig.4 Angular motion curves of each joint

仿真實驗結果表明：各關節角運動軌跡平滑，始末點狀態均符合參數設定，運動過程中角速度、角加速度及角加加速度均無突變，并始終滿足機械臂關節約束條件，驗證了文中設計的機械臂時間最優軌跡規劃方法的有效性。

4.3 優化算法對比實驗

為了驗證文中改進的多種群遺傳算法具有更好的收斂一致性和更快的收斂速度，在優化目標函數和收斂條件不變的情況下，分別用蟻群算法（ACO）、粒子群算法（PSO）、標準遺傳算法（SGA）、標準多種群遺傳算法（MPGA）和文中改進的多種群遺傳算法（IMPGA）對4.1節中的運動軌跡進行10組重復實驗，對比5種優化算法在收斂一致性和收斂速度上的表現，實驗結果如表3所示。

表3 5種優化算法的實驗結果Table 3 Experimental results of five optimization algorithms

5種優化算法的進化過程如圖5所示，對比實驗結果表明：蟻群算法和粒子群算法收斂速度較慢，多次重復實驗收斂結果基本一致；標準遺傳算法收斂速度較快但多次重復實驗結果很難保持一致，算法穩定性較差；標準多種群算法收斂速度較快且在多次重復實驗中均能搜索到全局最優解；文中改進的多種群遺傳算法在標準多種群算法穩定搜索最優解的基礎上，進一步加快了收斂速度。由此驗證了文中改進的多種群遺傳算法相較于其他同類算法具有更好的收斂一致性和更快的收斂速度。

圖5 5種優化算法的進化過程Fig.5 Evolution of five optimization algorithms

5 結束語

針對串聯機械臂時間最優軌跡規劃問題，在5次多項式軌跡插值的基礎上，利用改進的多種群遺傳算法對機械臂運動軌跡進行優化。以發那科ER-4iA機械臂為實驗驗證平臺，以機械臂點到點軌跡運行時間為優化目標，分別用蟻群算法、粒子群算法、標準遺傳算法、標準多種群遺傳算法和文中提出的改進多種群遺傳算法進行優化。實驗結果表明文中算法在多組重復實驗中均能收斂至全局最優解，且相較于同類算法具有更快的收斂速度；以最優運行時間擬合機械臂運動軌跡，各關節角速度、角加速度及角加加速度曲線均平滑且滿足機械臂運動學約束條件，驗證了文中改進多種群遺傳算法的機械臂時間最優軌跡規劃方法的可行性。

文中以機械臂點到點運動的時間作為優化目標進行仿真和對比實驗，然而機械臂實際工作任務可能由多段點到點運動構成，文中改進的多種群遺傳算法在以多段運動時間為目標的多目標優化任務中是否仍然具備優勢仍有待驗證。此外，在某些特殊任務中，除了對機械臂關節運動有所約束，往往還對末梢速度、關節力矩等指標有所要求，如何調整算法適應更為復雜的優化目標也是未來潛在的研究方向之一。