破拆機器人拆除窯襯的時間最優軌跡規劃

滕儒民，張文帥，王 欣，王大洪

（1.大連理工大學 機械工程學院，遼寧 大連 116023； 2.深圳市先進智能技術研究所，廣東 深圳 518049）

破拆機器人常被應用于高粉塵環境下拆除回轉窯窯襯，可以提高效率，減少危險事故的發生，提升回轉窯的利用率［1］。操作員在操控破拆機器人拆除窯襯時，要反復調整才能準確定位到破拆點［2］，破拆點轉換費時、費力。另外，自動化和智能化是破拆機器人未來發展的趨勢［3］，因此對破拆機器人進行時間最優軌跡規劃具有重要意義。

段偉雄［4］用3次Bezier曲線對破拆機器人換裝屬具生成無碰撞路徑，對路徑上關鍵點的關節角用5 次多項式擬合，為破拆機器人自動換裝屬具提供依據。李虹等［5］采用4-3-3-3-4 分段多項式對挖掘機進行軌跡規劃，并根據粒子群算法獲得時間最優的運動軌跡。洪振宇等［6］在關節空間采用5-3-5 多項式插值規劃裝載機器人軌跡，并用遺傳算法得到時間最優軌跡，該方法運行平穩，可順利避障。

本文針對破拆機器人拆除窯襯這一工作場景，先用4-3-4 多項式進行軌跡規劃，再用改進鯨魚優化算法優化運動時間，能有效地發揮破拆機器人的運動學性能，提高工作效率。

1 破拆機器人正運動學求解

破拆機器人基本結構如圖1 所示，破碎錘通過轉臺的旋轉和4 個油缸的伸縮定位到破拆點。根據基本結構建立關節坐標系，如圖2所示。

圖1 破拆機器人基本結構Fig.1 Basic structure of demolition robot

圖2 關節坐標系Fig.2 Joint coordinate system diagram

由標準D-H 法的齊次變換矩陣和破拆機器人D-H參數，得到破碎錘錘尖的位姿表達式為

式中：px、py、pz為破碎錘錘尖在基坐標系下的X、Y和Z向坐標；a1、a2、a3、a4和a5為連桿長度；d1為連桿偏移量；ξ為破碎錘錘尖與水平面的夾角；θ1為轉臺關節角；θ2為大臂關節角；θ3為中臂關節角；θ4為小臂關節角；θ5為破碎錘關節角；c1=cosθ1；c2=cosθ2；c23=cos (θ2+θ3)；c234=cos (θ2+θ3+θ4)；c2345=cos (θ2+θ3+θ4+θ5)；s1=sinθ1；s2=sinθ2；s23=sin (θ2+θ3)；s234=sin (θ2+θ3+θ4)；s2345=sin (θ2+θ3+θ4+θ5)。

2 破拆機器人拆除窯襯軌跡規劃

2.1 拆除窯襯過程分析

根據破拆機器人拆除窯襯的特點，認為需在1個圓環上間隔一定距離和角度進行拆除，因此破碎錘需頻繁地轉換和定位破拆點。為方便研究，單獨分析其中相鄰2 個破拆點中的轉換路徑，假設破碎錘需從破拆點P1定位到相鄰破拆點P2。由于在關節空間中進行軌跡規劃，并且多關節聯動，無法直觀地看到在笛卡爾空間的運動軌跡，因此在破拆點轉換路徑中，預設防撞點D1和D2，使得破碎錘陸續經過防撞點，避免與窯襯發生碰撞，從而將轉換路徑分為3段軌跡，如圖3所示。

圖3 拆除窯襯示意圖Fig.3 Schematic diagram of demolish the kiln lining

各關節在破拆點和防撞點的角度見表1。

表1 關鍵點對應關節角度Tab.1 Key points correspond to joint angles

2.2 關節空間的4-3-4多項式插值函數

第i個關節的3段插值函數分別為

式中：θij(t)為破拆機器人第i關節在第j段軌跡內的角度變化；aijk為第i關節在第j段軌跡的第k+1個多項式系數。

破碎錘在破拆點執行拆除任務，各關節在破拆點的角速度和角加速度需為0。此外，各關節在防撞點的角度、角速度和角加速度均需連續。可得約束方程為

式中：Xi0、Xi1、Xi2和Xi3分別表示第i關節在破拆點P1、防撞點D1、防撞點D2和破拆點P2的角度；vi1和vi2分別為第i關節在防撞點D1和D2的角速度；ai1和ai2分別為第i關節在防撞點D1和D2的角加速度；ti1、ti2、ti3為第i關節在第1、2和3段軌跡內所用的時間。

式（3）改寫為矩陣的形式為

矩陣的表達式為

根據式（4）~式（7），可求得4-3-4 多項式的系數表達式。

2.3 優化目標和約束條件

優化目標是破拆點轉換路徑中3 段軌跡的使用時間最少，第i關節的優化目標函數為

運動學約束條件為

式中：Vij、Aij為第i個關節在第j段軌跡的角速度變化和角加速度變化；vimax、aimax為第i個關節的最大角速度和最大角加速度。

各關節角速度和角加速度約束見表2。

表2 各關節角速度和角加速度約束Tab.2 Angular velocity and angular acceleration constraints for each joint

3 改進鯨魚優化算法描述

3.1 標準鯨魚優化算法

鯨魚優化算法是2016 年由Mirjalili 等［7-8］提出的一種群體智能優化算法，分為圍著獵物、氣泡網攻擊和探索獵物3種階段。

（1） 圍著獵物。

座頭鯨群體會先假定1 個最優位置，其他座頭鯨以最優位置更新自己的位置，表達式如下：

式中：D為鯨魚個體與最優位置的距離；X*(t)為最優位置；X(t)為當前位置；X(t+1)為更新后的位置；r為［0，1］的隨機數。

（2） 氣泡網攻擊。

氣泡網攻擊有收縮包圍和沿著螺旋形路徑2種更新方式。鯨魚個體采用螺旋形路徑來更新位置的公式如下：

式中：D'=|X*(t)-X(t)|代表鯨魚個體與當前最優位置的距離；b為控制螺旋形狀的常數，取1；l為[-1，1 ] 內的隨機數。

由于座頭鯨同時以收縮包圍和沿著螺旋形路徑圍繞獵物游動，采用概率p確定下一代更新位置的方式，公式如下：

式中：p為[0，1 ] 內的隨機數。

（3） 探索獵物。

當|A|≥1 時，鯨魚個體通過隨機選擇其他個體位置來更新自己的位置，公式如下：

式中：Xrand(t)為當前隨機選擇的其他個體位置。

3.2 改進方法

（1） 混沌映射。

標準鯨魚優化算法通常是隨機初始化鯨魚種群，種群多樣性低。混沌具有隨機性和遍歷性，采用Logistic混沌映射初始化種群［9］，公式如下：

式中：Xk為第k個鯨魚位置的邏輯混沌值；X0∈(0，1)，X0?{0.0，0.25，0.5，0.75，1 .0} ，η=4。

（2） 非線性收斂因子。

鯨魚優化算法通過收斂因子a控制參數A的變化，進而調整搜索區域［10］。對a采用非線性策略，使得a在搜索前期速度下降明顯，加強全局的搜索能力；a在搜索后期的下降速度逐漸變緩，增強局部的搜索能力。公式如下：

式中：t為當前迭代次數；tmax為最大迭代次數。

改進鯨魚優化算法優化運動時間流程如圖4所示。

圖4 優化流程Fig.4 Optimization flowchart

4 時間最優軌跡規劃仿真

設置鯨魚種群數量為50，種群維度為3，最大迭代次數為100。用改進鯨魚優化算法迭代得到轉臺關節在每段軌跡上的插值時間，如圖5所示。改進鯨魚優化算法對每個關節優化后的插值時間見表3。

表3 每個關節優化后的插值時間Tab.3 Optimized interpolation time for each joint

圖5 轉臺關節插值時間迭代Fig.5 Rotary joint interpolation time iteration diagram

為保證每個關節能夠同步運行，不發生碰撞，將每段軌跡的最大時間值作為最終插值時間，可得改進鯨魚優化算法優化后的總時間為5.882 5 s，未優化前所用總插值時間為7.500 0 s，改進鯨魚優化算法優化后的總時間比未優化前節省1.617 5 s。

將最終插值時間代入4-3-4 多項式的系數表達式中，得到各關節的位置、角速度和角加速度曲線，如圖6~圖8 所示。由圖可知，優化后的關節位置、角速度和角加速度均連續，且未出現突變，各關節的初末角速度和角加速度均為0。此外，各關節的角速度和角加速度均在運動學約束條件以內，可較好地發揮破拆機器人的運動學性能。

圖6 各關節位置曲線Fig.6 The position curve of each joint

圖7 各關節角速度曲線Fig.7 Angular velocity curve of each joint

圖8 各關節角加速度曲線Fig.8 Angular acceleration curve of each joint

5 結語

本文以破拆機器人拆除窯襯為研究對象，采用4-3-4 多項式插值法在關節空間進行軌跡規劃，在滿足運動學約束條件下，以運動時間最短為優化目標，采用改進鯨魚優化算法優化插值時間，運動時間從7.500 0 s 減少到5.882 5 s，提高了破拆機器人的工作效率。