朱浩，趙清海 , ，鄭群鋒，寧長久

（1.青島大學(xué) 機電工程學(xué)院， 山東 青島 266071；2.青島大學(xué) 電動汽車智能化動力集成技術(shù)國家地方聯(lián)合工程研究中心，山東 青島 266071；3.北京理工大學(xué) 機械與車輛學(xué)院，北京 100081）

隨著無人駕駛和5G 技術(shù)的大力發(fā)展，電動汽車自動充電這一領(lǐng)域成為研究大熱，而以關(guān)節(jié)型機器人作為執(zhí)行機構(gòu)的自動充電裝置由于小巧、靈活的特性在自動充電領(lǐng)域被廣泛應(yīng)用[1]。諸多研究與應(yīng)用表明，關(guān)節(jié)型機器人的運動軌跡規(guī)劃，不單是能夠?qū)崿F(xiàn)運動任務(wù)，還要考慮運動的平穩(wěn)性、平滑性、電機能耗與工作效率，從而實現(xiàn)多維度的最優(yōu)軌跡規(guī)劃。國內(nèi)外學(xué)者針對軌跡規(guī)劃研究有時間最優(yōu)、能量最優(yōu)、沖擊最優(yōu)和綜合最優(yōu)幾個方面[2]，其中以時間最優(yōu)研究最多。

時間最優(yōu)的軌跡優(yōu)化是指滿足動力學(xué)或運動學(xué)約束條件，機械臂完成相同路徑所用時間最短的軌跡[3]。對機械臂的運動軌跡進行時間優(yōu)化的方法有多種[4]，如灰狼算法[5]、遺傳算法[6]、粒子群算法[7]、蟻群算法[8]等。田興華等[9]對慣性權(quán)重因子實施雙重選擇策略，并在局部尋優(yōu)中加入擾動算子,增強了粒子群算法的突跳能力，較之傳統(tǒng)粒子群，尋優(yōu)能力得到改善。陳波等[10]采用雙種群粒子群算法，將種群分為隨機群和跟隨群，克服時間最優(yōu)求解過程中易陷入局部尋優(yōu)的缺點。隨著智能算法的改進發(fā)展，智能算法憑借其求解優(yōu)化問題效率高而被廣泛應(yīng)用于機械臂軌跡優(yōu)化和智能駕駛路徑規(guī)劃中。巫光福等[11]在機器人路徑規(guī)劃中，對全局最優(yōu)粒子速度更新進行干擾，提高了收斂速度，在避障的同時縮短了運動時間。李東潔等[12]通過引入三次多項式規(guī)劃運動空間軌跡，最后基于遺傳算法進行時間優(yōu)化求解。鄧偉等[13]通過引入五次多項式規(guī)劃運動軌跡，針對運動軌跡時間最優(yōu)問題，提出一種基于雙種群遺傳的混沌優(yōu)化算法，提高了收斂速度與尋優(yōu)能力。Jin 等[14]將蟻群算法和粒子群算法等多種智能算法混合，對機器人運動軌跡進行時間最優(yōu)研究。臧繼元等[15]首先采用高階混合多項式插值進行軌跡擬合,獲取插值路徑點，然后采用動態(tài)學(xué)習(xí)因子的粒子群算法進行時間最優(yōu)求解，優(yōu)化了基本粒子群算法后期局部收斂的不足。

上述研究表明，遺傳算法和粒子群算法等智能算法在機器人運動軌跡和智能駕駛路徑規(guī)劃中運用廣泛。而傳統(tǒng)的遺傳算法交叉與變異具有隨機性，求解效率相比于粒子群算法較低。粒子群算法求解具有方向性，在大多數(shù)情況下，其粒子的收斂速度快于遺傳算法，粒子迭代更新后期易陷入局部收斂無法獲得最優(yōu)解。本文采用非線性動態(tài)改變學(xué)習(xí)因子的粒子群優(yōu)化算法對桁架充電機械臂的運動軌跡最優(yōu)時間問題進行研究，避免陷入局部尋優(yōu)的同時進行桁架機械臂運動軌跡的時間優(yōu)化。

1 改進D-H 法建模

1.1 改進D-H 坐標系建立

桁架充電機械臂的總體結(jié)構(gòu)如圖1 所示，是由移動桁架，伸縮機械臂，旋轉(zhuǎn)關(guān)節(jié)等構(gòu)成的一種5 自由度充電機械臂。理論上，其可覆蓋桁架立方區(qū)域，并為充電口布置在車身不同位置的電動汽車進行充電。為了簡化正逆運動學(xué)求解難度，采用改進D-H坐標系進行建模，基座標系與坐標系1 的原點重合。

圖1 桁架充電機械臂三維模型Fig.1 Three-dimensional model of truss charging arm

依據(jù)D-H 坐標系的建立原則，對桁架充電機械臂進行建模，其改進D-H 坐標系如圖2 所示。

圖2 桁架充電機器人改進D-H 坐標系Fig.2 Improved D-H coordinate system for trusscharging robot

1.2 D-H 參數(shù)表

對桁架自動充電機器人進行標準D-H 建模，獲取運動學(xué)模型的D-H 參數(shù)， θ、d、a和 α分別代表關(guān)節(jié)轉(zhuǎn)角、連桿偏移、連桿長度和連桿扭轉(zhuǎn)角。以此為基礎(chǔ)，生成的D-H 參數(shù)如表1 所示。

表1 D-H 參數(shù)表Tab.1 D-H parameters

1.3 運動學(xué)求解

關(guān)節(jié)角已知，求取桁架充電機器人末端的空間姿態(tài)和位置，這個過程就是正運動學(xué)求解。相鄰兩連桿的齊次變換矩陣為

式中：cqi=cosqi；sqi=sinqi。

相鄰兩連桿的變換矩陣由式（1）可以推導(dǎo)可得：

由相鄰兩連桿的其次變換矩陣可以推出桁架充電機器人執(zhí)行端相對于基于的總齊次變換矩陣為

其中，桁架充電機器人末端姿態(tài)方程為：

1.4 解析法逆運動學(xué)求解

根據(jù)目標位置點的空間位置，求解關(guān)節(jié)1，關(guān)節(jié)2和關(guān)節(jié)3 的移動距離。

將式（5）化簡可得

由式（6）可知：

解關(guān)節(jié)4 和關(guān)節(jié)5 的轉(zhuǎn)動角度分別為：

1.5 工作區(qū)域分析

桁架充電機械臂的工作空間是指該裝置正常運行時所能到達的空間范圍，即末端關(guān)節(jié)坐標系的原點在空間活動的最大范圍[15]，如圖3 所示。

圖3 桁架充電機機械臂工作區(qū)域分析Fig.3 Analysis of the working area of the truss charging arm

將桁架充電機械臂的工作空間記作W（P）。則關(guān)節(jié)的運動變化量和工作空間的關(guān)系可表示為

式中：W（P）為桁架機械臂的工作區(qū)域；Q為空間約束。

從圖3 可知，桁架充電機械臂可以覆蓋整個立方區(qū)域，使得各關(guān)節(jié)配合能到達工作區(qū)域的任意一點，x軸方向，機械臂的工作范圍可達（0, 5.5） m；y軸方向，機械臂的工作范圍可達（0, 5.5） m；z軸方向，機械臂的工作范圍可達（-1.8, 0） m。根據(jù)運動工作域，可以更好的配置關(guān)節(jié)大小，并軌跡規(guī)劃做準備。

2 充電機械臂軌跡優(yōu)化

2.1 3-5-3 多項式運動軌跡插值模型

機械臂軌跡規(guī)劃要考慮運動的平順性，也就是說既要保證軌跡的平順性還要考慮位移和加速度不發(fā)生突變，因此對運動過程中速度和加速度進行約束，低階次多項式容易使加速度產(chǎn)生突變，高階次多項式雖然可以保證加速度的平順性，但計算量高且導(dǎo)致運動曲線凸包性變差，綜上采用3-5-3 多項式插值進行軌跡規(guī)劃。

當(dāng)一個充電運動周期的軌跡由3-5-3 多項式插值時，其軌跡運動表達式為：

式中：L1（t）,L2（t）,L3（t）均為充電機械臂各關(guān)節(jié)3-5-3多項式插值的運動軌跡。

對3-5-3 多項式插值進行多項式系數(shù)求解，可得：

2.2 時間優(yōu)化目標

針對桁架充電機械臂的運動軌跡時間優(yōu)化，已知運動起點與終點的速度及其運動過程中的速度約束，適應(yīng)度函數(shù)及其約束條件可表示為：

2.3 改進粒子群算法

基本粒子群算法是模仿鳥類覓食行為，采用速度-位置搜索模型迭代獲取最優(yōu)解的一種智能算法，其表達式為：

式中：w為慣性權(quán)重因子；m為粒子迭代次數(shù)；i為第i個粒子；c1，c2為學(xué)習(xí)因子；r1，r2為[0, 1]之間的任意值；xid為粒子的位置，滿足空間位置約束；vid為粒子的速度，也滿足速度約束條件。

慣性權(quán)重系數(shù)采用線性遞減，Wmax= 0.9，Wmin=0.4，既可以保證有良好的全局搜索能力，又能兼顧其局部搜索能力。

在基本粒子群算法中學(xué)習(xí)因子一般取0 ～ 4 之間的定值，此處改進粒子群算法中的學(xué)習(xí)因子采用非線性動態(tài)學(xué)習(xí)因子，可以提高局部收斂的能力。

式中：n為當(dāng)前粒子迭代次數(shù)；N為粒子最大迭代次數(shù)。

基于改進粒子群算法求解桁架充電機械臂最優(yōu)時間優(yōu)化流程如圖4 所示。

圖4 改進粒子群軌跡時間優(yōu)化流程圖Fig.4 Improved particle swarm trajectory time optimization flow chart

3 時間優(yōu)化實驗仿真分析

搭建的桁架充電機械臂時間優(yōu)化實驗平臺如圖5 所示，該平臺由桁架充電機械臂、控制箱體、電動汽車、PC 端等構(gòu)成。以該實驗平臺為基礎(chǔ)進行運動軌跡時間優(yōu)化實驗。

圖5 桁架充電機械臂時間優(yōu)化實驗平臺Fig.5 Experimental platform for time optimization of truss charging robot arm

參照圖5 搭建的實驗平臺構(gòu)建桁架充電機器人運動仿真虛擬工作平臺，將桁架充電機械臂簡化為3 個移動自由度，2 個旋轉(zhuǎn)自由度的串聯(lián)機器人運動模型，如圖6 所示。通過對比在真實與虛擬實驗情況下改進粒子群與基本粒子群運動時間優(yōu)化數(shù)據(jù)，來驗證所提算法的有效性。

圖6 桁架充電機械臂運動學(xué)三維模型Fig.6 Three dimensional kinematic model of truss charging robotic arm

以桁架充電機械臂為研究對象，根據(jù)正逆運動學(xué)求解獲知運動的起始點和終止點的空間位置，并獲取該位置點下關(guān)節(jié)的移動和旋轉(zhuǎn)變化量。再調(diào)用機器人工具箱里的庫函數(shù)得到該運動軌跡中的兩個插值點，最后對4 個路徑點所構(gòu)成的3 段區(qū)間進行3-5-3 多項式插值規(guī)劃，并基于改進和基本粒子群進行時間優(yōu)化分析，如表2 所示。

表2 各關(guān)節(jié)空間運動變量插值點Tab.2 Interpolation points of motion variables in each joint space

圖7 為關(guān)節(jié)1 ～ 關(guān)節(jié)5 位置迭代過程中3 段插值曲線進入收斂時的時間分布情況，隨著粒子的更新迭代，3 段插值軌跡收斂所用迭代次數(shù)減少。在第一段插值曲線中可知關(guān)節(jié)1、關(guān)節(jié)3、關(guān)節(jié)4 和關(guān)節(jié)5 很快進入收斂，所用時間分別縮減至3.160 s、2.095 s、2.350 s 和2.000 s，關(guān)節(jié)2 由于速度處于臨界速度，隨迭代次數(shù)增加，時間不會進行優(yōu)化，仍處于4 s。若對關(guān)節(jié)2 運動時間進行優(yōu)化，需調(diào)高關(guān)節(jié)電機速度，更改速度約束。在第二段和第三段插值曲線中關(guān)節(jié)2 的情況類似。上述結(jié)果表明在3 段插值曲線中各關(guān)節(jié)所用的收斂時間都得到了優(yōu)化與改善。

圖7 關(guān)節(jié)1 ～ 5 最優(yōu)粒子位置迭代各曲線時間分布Fig.7 Time distribution of the optimal particle position iteration curves for joints 1-5

圖8 為關(guān)節(jié)1 ～ 關(guān)節(jié)5 在改進粒子群算法和基本粒子群算兩種情況下的收斂過程對比，從圖中可知，改進后的粒子群算法關(guān)節(jié)1 ～ 關(guān)節(jié)3 的總體收斂速度比基本粒子群算法的收斂速度要快，由于關(guān)節(jié)4 與關(guān)節(jié)5 為轉(zhuǎn)動關(guān)節(jié)，是充電過程中的后置動作，比較容易陷入局部收斂，因此要達到收斂，所需要粒子的迭代更新次數(shù)更多，因此基本粒子群算法在關(guān)節(jié)4與關(guān)節(jié)5 收斂所用的迭代次數(shù)大于關(guān)節(jié)1 ～ 關(guān)節(jié)3，而采用非線性動態(tài)學(xué)習(xí)因子的粒子群算法在關(guān)節(jié)4與關(guān)節(jié)5 處的收斂速度也明顯快于基本粒子群，提高了后期收斂的速度，進一步優(yōu)化了運動時間。

圖8 關(guān)節(jié)1 ～ 5 的兩種算法收斂過程對比圖Fig.8 Comparison of convergence processes between two algorithms for joints 1-5

圖9 為改進粒子群與基本粒子群算法的位置、速度和加速度對比圖，結(jié)果表明：充電機械臂由起始點經(jīng)過插值路徑點到達終止點，充電機械臂規(guī)劃的軌跡運動平滑，速度過渡平順，運動時間有效減少。優(yōu)化前，位置曲線呈現(xiàn)先上升后下降的趨勢，說明關(guān)節(jié)1 ～ 5 運動過程中出現(xiàn)超出插值點又返回的現(xiàn)象，所以優(yōu)化前的速度曲線的變化范圍為-0.2 ～ 0.6 m/s，這也造成充電機械臂運動時間過長。優(yōu)化后，關(guān)節(jié)1 ～ 5位置曲線呈現(xiàn)逐步上升的趨勢，速度曲線由靜止狀態(tài)先增加0.7 m/s，然后在此速度附近波動運行一段時間后，最后下降至0，整體運動速度曲線呈現(xiàn)梯形樣式。運動時間由運動距離最長的關(guān)節(jié)2 決定，運動時間由優(yōu)化前的15 s 縮減到10 s。加速度曲線優(yōu)化前后對比，優(yōu)化后加速度曲線變化更為劇烈，但加速度小于1 m/s2，仍滿足約束條件，運動時間優(yōu)化得到預(yù)期仿真效果。為了驗證仿真結(jié)果的真實性，采用圖5 的桁架充電機械臂時間優(yōu)化實驗平臺進行實驗驗證，分別測得10 次基于改進粒子群桁架充電機械臂運動時間，對比仿真模擬優(yōu)化所得運動時間可知，改進粒子群算法優(yōu)化時間基本與仿真結(jié)果相符合，波動范圍未超過10%，如圖10 所示，進而表明改進粒子群算法對桁架充電機械臂運動時間優(yōu)化的適用性。

圖9 優(yōu)化前后位置、速度與加速度對比Fig.9 Comparison of position, velocity, and acceleration before and after optimization

圖10 實驗與仿真時間優(yōu)化結(jié)果對比圖Fig.10 Comparison of experimental and simulation time optimization results

4 結(jié)論

仿真結(jié)果表明，采用非線性動態(tài)學(xué)習(xí)因子的改進粒子群算法在滿足運動學(xué)約束條件下，可以尋找到時間最優(yōu)路徑。非線性動態(tài)學(xué)習(xí)因子，克服了基本粒子群算法在時間優(yōu)化求解過程中易陷入局部收斂的問題，提高了全局搜索尋優(yōu)的能力，加快了粒子的收斂速度，進而縮短了桁架充電機械臂軌跡運動時間。運動時間由15 s 變?yōu)?0 s，縮短了33%，最后通過實驗對比驗證，證明了所提算法的有效性。