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

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

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

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

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

1 改進(jìn)D-H 法建模

1.1 改進(jìn)D-H 坐標(biāo)系建立

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

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

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

圖2 桁架充電機(jī)器人改進(jìn)D-H 坐標(biāo)系Fig.2 Improved D-H coordinate system for trusscharging robot

1.2 D-H 參數(shù)表

對(duì)桁架自動(dòng)充電機(jī)器人進(jìn)行標(biāo)準(zhǔn)D-H 建模，獲取運(yùn)動(dòng)學(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 運(yùn)動(dòng)學(xué)求解

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

式中：cqi=cosqi；sqi=sinqi。

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

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

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

1.4 解析法逆運(yùn)動(dòng)學(xué)求解

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

將式（5）化簡可得

由式（6）可知：

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

1.5 工作區(qū)域分析

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

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

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

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

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

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

2.1 3-5-3 多項(xiàng)式運(yùn)動(dòng)軌跡插值模型

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

當(dāng)一個(gè)充電運(yùn)動(dòng)周期的軌跡由3-5-3 多項(xiàng)式插值時(shí)，其軌跡運(yùn)動(dòng)表達(dá)式為：

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

對(duì)3-5-3 多項(xiàng)式插值進(jìn)行多項(xiàng)式系數(shù)求解，可得：

2.2 時(shí)間優(yōu)化目標(biāo)

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

2.3 改進(jìn)粒子群算法

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

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

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

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

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

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

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

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

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

圖5 桁架充電機(jī)械臂時(shí)間優(yōu)化實(shí)驗(yàn)平臺(tái)Fig.5 Experimental platform for time optimization of truss charging robot arm

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

圖6 桁架充電機(jī)械臂運(yùn)動(dòng)學(xué)三維模型Fig.6 Three dimensional kinematic model of truss charging robotic arm

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

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

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

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

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

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

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

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

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

4 結(jié)論

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