基于自適應閉環反饋的高空車臂架避障控制研究

摘要：針對改進梯度投影法在混合式高空作業車應用中出現的速度、軌跡不連續以及避障過程中末端速度突變等問題，提出了一種基于自適應閉環反饋的高空車臂架避障控制算法。對改進梯度投影法進行優化，實現了存在伸縮臂的高空車臂架與末端的速度、軌跡連續的運動控制。采用偽距離描述臂架與障礙物的接近程度，對傳統避障速度算法進行優化，并提出一種自適應閉環反饋方法，將反饋權重與偽距離相關聯，使反饋權重自適應變化。所提算法在保證末端軌跡精度的同時大大改善了臂架關節在避障過程中的速度突變。仿真及半物理實驗結果表明，相較于傳統方法，所提方法速度變幅大大減小且軌跡更加平滑，實現了多障礙物避障與同一障礙物多次避障。

關鍵詞：偽距離；避障；冗余臂架；梯度投影法；高空作業車；閉環反饋

中圖分類號：TH6

DOI：10.3969/j.issn.1004132X.2024.08.018

開放科學（資源服務）標識碼（OSID）：

Research on Obstacle Avoidance Control of Booms of Aerial Work Platforms

Based on Adaptive Closed-loop Feedback

HUANG Yi HU Ming TANG Qiying GUO Jinmeng ZHANG Jinlai REN Guangan

College of Automotive and Mechanical Engineering，Changsha University of Science and

Technology，Changsha，410114

Abstract： In order to solve the problems of discontinuity of velocity and trajectory in the applications of the improved gradient projection method in hybrid aerial work platforms and the problems of abrupt change in velocity of ends during obstacle avoidance， an obstacle avoidance motion control algorithm was proposed based on adaptive closed-loop feedback. The improved gradient projection method was optimized to realize the continuous motion control of the speed and trajectory of the booms and the ends of aerial work platforms with telescopic arms. The pseudo distance was used to describe the proximity between the boom and the obstacle， and the traditional obstacle avoidance speed algorithm was optimized. An adaptive closed-loop feedback method was proposed， which correlated the feedback weight with the pseudo-distance， and made the feedback weight change adaptively. The algorithm guaranteed the precision of the end trajectory， and greatly improved the sudden changes in the boom joint velocity in the course of obstacle avoidance. Simulation and semi-physical experimental results verify that the speed amplitude of the proposed method is greatly reduced and the trajectory is smoother than that of the traditional method， and multi-obstacle avoidance and multiple obstacle avoidance may be realized.

Key words： pseudo-distance； obstacle avoidance； redundant boom； gradient projection method； aerial work platform； closed loop feedback

0 引言

自動化施工是高空作業車的重要技術發展方向，高空作業車的自動化作業中最重要的兩個環節是末端避障以及臂架控制。末端避障中碰撞檢測方法是不可缺少的一環，而碰撞檢測并不受限于使用對象，故對高空作業車同樣可以參考機器人所使用的碰撞檢測方法。目前國內外對碰撞檢測的方法有歐氏距離、C空間方法、偽距離等［1-3］。KHAN等［4］采用GJK（Gilbert-Johnson-Keerthi）算法，該算法直接以機器人與障礙物之間的距離作為碰撞指標，意味著該算法適用于任意形狀的機械手以及障礙物，但無疑會增加計算量。PARK等［5］以人工勢場法作為碰撞指標，該方法適用性廣，但具有搜索量大、容易產生局部最小值以及奇異點等缺點。XU等［6］為使機器人與障礙物之間的距離描述更加簡單，將機器人和障礙物分別抽象為臨界點集，通過一般的K類函數以不等式約束的形式建立避障策略。JU等［7］根據機械臂幾何構型提出了一種基于三角碰撞平面的可預測避障模型。MU等［8］考慮到最小歐氏距離精度高、效率低以及偽距離精度低效率高的特點，采用最小歐氏距離結合偽距離的方法來實現避障的碰撞檢測。張建華等［9］采用二分試探的碰撞位置檢測算法，但需要進行多次迭代計算，這樣必然導致計算量增大。

高空作業車根據結構特點大致可以分為直臂式、折臂式、垂直升降式和混合式［10］。在臂架控制方面，針對不同結構特點的高空作業車，往往需要采取不同的控制方法。滕儒民等［11］提出了一種基于啟發式路徑搜索的避障路徑規劃方法，他們將高空作業車的臂架作為冗余自由度的機械臂并對機械臂進行分析，建立運動學模型后在MATLAB中驗證了啟發式A*算法的可行性，為大型高空作業車的軌跡規劃提供了有效的運動控制方案，但該方法僅研究末端路徑軌跡且未考慮關節規劃。王碩等［12］給出了一種直臂式高空作業車的作業平臺軌跡跟蹤控制方法，針對所研究的高空作業車建立了基于機械臂油壓系統的控制模型并設計了PID控制器，實現了作業平臺的軌跡跟蹤控制，以及高空作業車末端直線運動及刷墻運動，該方法控制簡單，但難以實現復雜運動且未考慮避障要求。混合式高空作業車的臂架結構不同于一般形式的高空作業車，它可視為具有冗余自由度的機械臂，故對于混合式高空車可以參考冗余機械臂的控制方法：梯度投影法、增廣雅可比方法、加權最小范數法以及基于以上方法的各種衍生方法等［13-15］。KORKMAZ等［16］提出一種針對欠驅動水下航行器機械手系統的逆運動學控制方法，通過仿真證明了該控制方法在參數不確定并存在干擾因素的情況下依舊有效。PENG等［17］提出一種適用于分段連桿索驅動超冗余機器人的軌跡跟蹤控制方法，以主動索張力和末端執行器位姿為優化指標，采用動態前饋控制和PD控制相結合的方法構建了軌跡跟蹤框架，通過聯合仿真系統驗證了所提方法的有效性。MARCOS等［18］通過結合閉環偽逆方法與多目標遺傳算法實現了關節位置的控制，并進行仿真研究，實現了對冗余度機械手的軌跡控制優化。WAN等［19］受到經典加權最小范數（weighted least-norm， WLN）和流行的梯度投影法（gradient projection method， GPM）優點的啟發，提出了一種求解冗余機械手多性能指標的逆運動學（inverse kinematics， IK）問題的加權梯度投影法，通過仿真和實驗與傳統的WLN和GPM方法的結果進行比較，驗證了所提方法的有效性，但該方法存在關節速度變化快的問題。ZHANG等［20］為了滿足空間在軌服務的安全要求，提出了一種基于改進粒子群算法的避障軌跡規劃方法，但該方法僅簡單地考慮了關節規劃，未考慮速度層面的約束。XIE等［21］提出了一種基于快速探索隨機樹算法和正反向運動學算法的避障與路徑規劃算法，以較低的計算成本為多關節機械臂找到無碰撞的可行路徑，但該方法僅規劃末端避障路徑，未考慮速度的連續性。LI等［22］針對超冗余度機械臂的避障路徑規劃和離散軌跡跟蹤問題，提出了一種集成優化方法，該綜合優化方法能夠快速搜索出合適的軌跡，指導機械手在實現避障和滿足關節約束的同時獲得目標。LIU等［23］針對冗余度機械臂運動過程中的碰撞問題，提出了一種基于多目標優化的末端執行器軌跡規劃方法，該方法在完成避障的同時使得關節軌跡光滑且能量消耗低。上述研究存在著僅研究末端路徑軌跡而未考慮關節規劃與速度的連續，各種研究目標的關節多由旋轉關節組成，少有存在移動關節的研究對象，對于高空車臂架控制的自動化控制方式簡單且效果欠佳，難以實現復雜運動等問題。

本文基于改進梯度投影法以及傳統速度避障法，提出一種適用于混合式高空作業車的基于自適應閉環反饋的避障控制算法。通過基于加權最小范數解的二次臂架運動分配方法，實現對存在移動關節的高空車臂架與末端的速度、軌跡連續的運動控制。采用偽距離來評估臂架與障礙物的接近程度，并對傳統避障速度方法進行改進。在閉環反饋方面，提出一種自適應閉環反饋方法，在避障后保證末端能夠回到運行軌跡的同時速度不會發生突變。以15 m混合式高空作業車為研究對象，開展了高空車臂架避障運動控制的仿真與半物理實驗研究，驗證該算法在高空車臂架避障過程中的可行性及平穩性。

1 高空車臂架運動學分析

1.1 臂架正運動學

混合式高空作業車臂架可以看作冗余機械臂，由于研究對象高空作業車臂架系統結構復雜，故建模時將其看作九自由度冗余機械臂，將高空車臂架看作剛性結構，便可進行運動學分析。由于混合式高空作業車的結構特點，各關節之間存在一定幾何關系，在實際驅動時只需要對回轉、塔臂、主臂、伸縮臂四個臂架進行驅動，采用改進D-H法對混合式高空作業車臂架系統進行正運動學分析，其平面結構簡圖見圖1（圖1中，藍色箭頭為垂直紙面方向，關節1和7均為垂直紙面向內其余均向外，關節7與8為重疊關節，為方便理解，將兩關節分開繪制）。

圖1中，關節1為回轉關節，關節2、3、4、5組成塔臂部分，關節6為主臂，關節7為伸縮臂，并令FH長度為L2，建模后通過結構公式可對末端坐標公式進行簡化，只需通過四個關節參數即可計算出末端坐標，D-H參數見表1。

由于高空車末端一般配有載物平臺且平臺通過液壓自動調平，故假設末端存在連桿JP且恒保持水平狀態，考慮到伸縮臂的關節的存在，令θ7=-180°，連桿JP長度LJP=1800 mm，故根據D-H法可求得末端坐標P：

x=c1（LJP-LAO+LBCc2+LCMc23+LMNc234+

LNFc2345+L2s23456-LHPc234568）

y=s1（LJP-LAO+LBCc2+LCMc23+LMNc234+

LNFc2345+L2s23456-LHPc234568）

z=LAB+LBCs2+LCMs23+LMNs234+LNFs2345-

L2c23456-LHPs234568（1）

式中，LJP為JP長度，其余類推；ci=cos θi；si=sin θi；cij=cos（θi+θj）；sij=sin（θi+θj）。

根據高空作業車臂架結構，有θ2=f1（θ4）， θ3=f2（θ4），θ5=f3（θ4），θ8=29.4346°，則式（1）變形為僅與θ1、θ4、θ6、L2相關的公式P（θ1，θ4，θ6，L2），求得臂架雅可比矩陣：

J（θ）=xθ1xθ4xθ6xL2

yθ1yθ4yθ6yL2

zθ1zθ4zθ6zL2（2）

1.2 臂架逆運動學

考慮混合式高空作業車結構特點，采用改進梯度投影法對其進行逆運動學求解，以實現對末端的控制。

設混合式高空作業車臂架末端速度x·=［x·1 x·2 … x·m］T，而各關節角速度或位移速度θ·=［θ·1 θ·2 … θ·n］T，根據逆運動學，有

x·=J（θ）θ·（3）

式中，J（θ）為上文提到的雅可比矩陣，J（θ）∈Rm×n。

mlt;n時，得到上式的加權最小范數解：

θ·=W-1JT（JW-1JT）-1=J+Wx·（4）

式中，J+W為J（θ）的加權廣義逆矩陣；W為加權對稱正定對角矩陣。

W∈Rm×n，其對角元素為

Wi，j=1+|H（θi）|（5）

H（θi）=（θiM-θim）2（2θi-θiM-θiM）4（θi-θiM）2（θi-θim）2（6）

式中，θi為第i個關節轉角或位移量；θiM為關節轉角或位移量最大值；θim為關節轉角或位移量最小值。

通過最大最小值的約束可以將各關節限制在合適的范圍內，由以上公式可知，當θi→θiM或θi→θim時，H（θi）→∞， W-1ij→0，此時關節i停止運動；當θi→（θiM+θim）/2時，H（θi）→0，W-1ij→1，此時關節運動不再受限。為辨別關節是在遠離還是趨近最值，同時考慮到計算時由于精度不足可能導致θi超過最值，而此時H（θi）不為0會導致關節繼續運動，故當關節量超出最值時需使Wi，j=∞，使關節停止，但如果關節速度為遠離極限值方向，則無需變化，故將加權因子及H（θi）重新定義為

H（θi）=∞""" θigt;θiM，θilt;θim

H0（θi）θim≤θi≤θiM（7）

H0（θi）=（θiM-θim）2（2θi-θiM-θiM）4（θi-θiM）2（θi-θim）2

而

Wi，j=1+|H（θi）|" Δ|H（θi）|≥0

1Δ|H（θi）|lt;0（8）

其中，Δ|H（θi）|為|H（θi）|的變化率。以上公式說明，當關節量θi處于最值附近且關節速度為遠離極限值方向時，運動狀態會發生改變，故可以保持運動。但當關節量超過最值且Δ|H（θi）|≥0時，關節保持靜止。當Δ|H（θi）|lt;0時，關節可以運動。

在此基礎上加入末端位置閉環反饋，得到加入末端位置反饋的加權閉環逆運動學算法公式：

θ·=J+W（x·+K（pd-pe））（9）

式中，K為單位正定矩陣；pd為臂架末端預期坐標；pe為臂架末端實際坐標。

1.3 二次臂架運動分配

混合式高空作業車通過協調回轉、塔臂、主臂及伸縮臂進行臂架運動控制。其中，伸縮臂為移動關節，回轉、塔臂與主臂關節均為轉動關節，分別對應位移變量和轉角變量。采用式（9）計算加權對稱正定對角矩陣Wi，j時會優先驅動塔臂及主臂協調運動，而伸縮臂被“視為”固定長度的連桿。末端垂直運動時，末端達到一定位置（例如主臂水平）時，采用該公式會優先通過塔臂、主臂實現末端垂直運動，此時就會出現速度、軌跡突變現象。

本文提出了一種基于加權最小范數解的二次臂架運動分配方法。考慮到塔臂在高空車臂架作業特點以及在運行過程中僅影響末端點Z軸坐標，將末端速度拆解為兩部分，各部分所占權重與塔臂角度θ4相關，將式（9）改寫為

θ·=J+W1（x·+K（pd-pe））（1-θ4M-θ4θ4M-θ4m）+

J+W2（x·+K（pd-pe））θ4M-θ4θ4M-θ4m（10）

J+W1=W-11JT（JW-11JT）-1

J+W2=W-12JT（JW-12JT）-1

W1=W11

∞

W33

W44

W2=W′11

W′22

∞

W′44

式中，J+W1、J+W2分別為令θ·4、θ·6為0的加權廣義逆矩陣。

式（10）表明，將末端位移的實現分解為兩部分，分別通過θ1、θ4、L2與θ1、θ6、L2實現末端運動即第一次臂架運動分配。而兩者實現末端運動的占比與塔臂角度θ4相關，當θ4越接近θ4M，式（10）中等號右邊前一項占比越小，后一項占比越大，即θ4越接近θ4M，θ1、θ4、L2實現更少的末端速度，θ1、θ6、L2實現更多的末端速度，即第二次臂架運動分配。通過此方法保持總體末端速度不變。此方法在合理分配各關節的同時保證末端運動、各關節速度變化連續平滑，能實現混合式高空作業車臂架逆運動學控制。

2 碰撞檢測與避障控制

2.1 碰撞檢測

實現避障最主要是判斷臂架與障礙物是否產生碰撞，現有判斷方法有很多種并且可以互相結合，考慮到高空作業車體積大且精度要求比小型機械低，故以效率較高的偽距離作為臂架與障礙物之間的碰撞檢測指標。

對于障礙物的建模，通常用一個或多個規則空間包絡來進行擬合，采用超二次曲面描述三維空間中的物體，統一的超二次曲面解析式如下：

（x-x0h1）2m+（y-y0h2）2n+（z-z0h3）2p=1（11）

式中，（x0，y0，z0）為超二次曲面中心點坐標；（x，y，z）為擬合障礙物形狀的超二次曲面上任意一點坐標；m、n、p和h1、h2、h3分別為描述超二次曲面形狀的形狀參數與體積參數。

則根據上式可以定義空間任意一點到超二次曲面的偽距離表達式：

dp（x，y，z）=（x-x0h1）2m+（y-y0h2）2n+

（z-z0h3）2p-1（12）

本文研究的混合式高空作業車通常用于噴涂作業、玻璃安裝等，工作中遇到的障礙物多為墻體結構的突出部分或電線，且考慮到避障軌跡的順滑及計算效率，本文障礙物以三維空間圓柱體為例，其偽距離表達式為

dp（x，y，z）=（x-x0R）2+（y-y0R）2+（z-z0H）8-1（13）

式中，R為圓柱體擬合半徑；H為圓柱體擬合高度。

三維空間任一點與擬合的超二次圓柱體曲面存在三種位置關系：偽距離dp（x，y，z）lt;0時，表示空間點（x，y，z）位于圓柱體內部，此時高空作業車臂架已和障礙物發生碰撞；dp（x，y，z）=0時，表示空間點（x，y，z）位于圓柱體曲面上，此時高空作業車臂架剛與障礙物發生碰撞；dp（x，y，z）gt;0時，表示空間點（x，y，z）位于圓柱體外部，此時臂架與障礙物未產生碰撞，如圖2所示。同時，偽距離越大，空間點離障礙物越遠，故可以用偽距離來進行碰撞檢測與避障。

根據式（13）定義某一臂架i到障礙物的偽距離為臂架任一點到障礙物表面偽距離的最小值，即

dp，i=min（dp（P0，i），…，dp（Pj，i），…，dp（Pn，i））

式中，P0，i、Pn，i分別為臂架i的首末端點；Pj，i為臂架i的中間任一點，點的數量越多，精度越高但計算量越大。

對于高空作業車臂架系統，整個高空作業車臂架到障礙物的偽距離定義為

dp=min（dp，1，…，dp，i，…，dp，8）

式中，dp，1、dp，8分別為臂架1和8的偽距離；dp，i為臂架i的偽距離。

考慮到高空作業車工作中多面向墻體，且末端常搭載機械手配合工作，一般情況下其他臂架均無需避障，所以取最后一節臂與障礙物的偽距離作為碰撞檢測指標不僅滿足避障需求，還能提高計算效率。當環境中存在N個障礙物時，同樣取最后一節臂架與N個障礙物之間的最小偽距離值即可，即

De=min（dp1，…，dpi，…，dpN）（14）

2.2 基于偽距離的避障算法及優化

為實現避障與控制避障距離，通常會設定一個閾值dm，隨后根據式（14）計算得到偽距離De，當Delt;dm時賦予臂架末端避障速度vp，方向沿障礙物超二次曲面中心指向臂架上最小偽距離值對應點，傳統避障速度計算公式如下：

vp=0" Degt;dm

vDe≤dm（15）

v=vm2（cos（πDedm）+1）np‖np‖（16）

式中，vm為最大避障速度；np為由障礙物擬合超二次曲面中心指向臂架末端點的矢量。

此時式（10）改寫為

θ·=J+W1（x·+vp+K（pd-pe））（1-θ4M-θ4θ4M-θ4m）+

J+W2（x·+vp+K（pd-pe））θ4M-θ4θ4M-θ4m（17）

以上方法雖能夠實現避障但存在一定問題，傳統的偽距離避障方法中，np為障礙物擬合超二次曲面中心指向臂架標志點的矢量，當障礙物中心不在末端運行軌跡上時，該計算方法得到的np會指向非理想方向，對于高空作業車必然導致避障軌跡不合理甚至避障失敗。同時，該方法用于作為工程機械的高空作業車，在避障以及回到預期點時會產生較大的末端速度且速度變化快，即使能夠實現也會給各關節帶來巨大的沖擊。導致這一現象有兩個原因，首先為實現避障與閉環反饋相平衡，需要通過不斷試錯來選擇合適的避障速度，為與閉環反饋平衡，往往合適的避障速度都會較大。其次在避障時臂架末端實際點相較于預期點會產生偏移，隨著末端實際點離預期點越遠，閉環反饋作用越大，為使末端實際點回到預期點，必然會產生較大的速度。但如果將避障速度減小將導致避障失敗，若將閉環反饋參數K減小則會導致末端無法回到軌跡。

由式（15）、式（16）發現，避障速度在偽距離由閾值dm減小到0的過程中不斷增大，在末端剛進入避障區域時被賦予的避障速度較小，臂架末端仍沿原軌跡運行且繼續向障礙物靠近，所以用式（15）、式（16）計算避障速度時有可能出現臂架離障礙物過近的現象，部分情況下還會導致避障失敗，通過調整閾值dm也難以實現最小偽距離的控制，故為方便控制最小偽距離，提出絕對閾值與設定閾值，將式（15）、式（16）改寫為

vp=0""""" Degt;dm

vdm≥Degt;dc

vmnp‖np‖De≤dc（18）

v=vm2（cos（πDe-dcdm-dc）+1）np‖np‖（19）

其中，dc為絕對閾值，原公式偽距離達到0時避障速度才達到最大，改寫后偽距離達到絕對閾值時即可達到避障速度最大值。

2.3 避障方向矢量計算以及標志點的選取

由于np定義為由障礙物擬合超二次曲面中心指向臂架末端點的矢量，當障礙物中心不在末端運行軌跡上時，np會指向非理想方向，可能導致避障軌跡不合理，一些工況下更意味著避障失敗。為解決這一問題，將np重新定義如下：

np=pei-po

式中，pei為末端標志點坐標；po為障礙物標志點坐標。

對于圓柱體障礙物，障礙物標志點為末端點到障礙物擬合超二次曲面的中心軸線的垂點。綜合考慮高空作業車結構特征以及高空作業車多面向墻體作業等因素，末端通常應向臂架所在側避障。同時為使避障軌跡平滑，制定末端標志點選擇策略如下：首先將末端臂架分為N份，此時末端臂架存在N+1個待選擇點，隨后從末端點開始，計算障礙物標志點到待選擇點的矢量與末端臂架的角度，選擇第一個使該角度為鈍角或更大角度的待選擇點為末端標志點，此時即便障礙物中心相對于運行軌跡存在偏移，仍能成功實現避障并且速度變化更加平滑，但存在速度較大的問題。考慮到末端臂架各點速度方向平行以及末端速度的穩定，避障速度仍然賦予末端點而非末端標志點。

以將末端桿分為5份為例，示意圖見圖3。圖3中，O為障礙物標志點，末端桿件存在5個待選擇點，其中障礙物標志點到點1、2、3的矢量與末端臂架之間的角度均為鈍角，但障礙物標志點O到點3的矢量為從末端點5開始計算的第一個與末端臂架為鈍角的矢量，故選擇點3作為末端標志點。

2.4 設計自適應閉環反饋

傳統閉環反饋方法是利用末端預期點與末端實際點的位置差進行閉環反饋，即賦予末端由末端實際位置指向末端預期位置的速度。避障時實際點必然會與預期點產生位置差，由式（9）可知K不變的情況下，位置差越大，閉環反饋產生的速度越快，當避障結束時避障速度驟降加上實際點與預期點位移差大，導致末端速度突變。閉環反饋產生的速度可分解為平行于運行軌跡的速度以及垂直于運行軌跡的速度，平行于運行軌跡的速度對末端回到運行軌跡并無幫助，故舍棄該速度，將閉環反饋方向由原來的實際點到預期點改為實際點到運行軌跡。該方法保證末端回到運行軌跡的同時，減小由閉環反饋引起的速度過快與突變，從而改寫式（17）如下：

θ·=J+W1（x·+vp+K（pn-pe））（1-θ4M-θ4θ4M-θ4m）+

J+W2（x·+vp+K（pn-pe））θ4M-θ4θ4M-θ4m（20）

其中，pn為末端實際點到運行軌跡的垂足坐標，以末端運行到O點為例，示意圖見圖4，圖中O為末端實際點，v為運行速度單位向量，N為末端實際點到運行軌跡的垂足點，B為預期點，黑色實線為避障軌跡，虛線為運行軌跡，藍色箭頭ON為末端實際點指向運行軌跡方向的向量即自適應閉環反饋方向，橙色箭頭OB為末端實際點指向末端預期點的向量即傳統閉環反饋方向。

圖4中，N點坐標為pn=（xn，yn，zn）。閉環反饋幅值方面，傳統方法中K是不變的，若K設定過大，會導致無法避障，設定過小會導致末端回到運行軌跡速度較慢，即使K取得合適的值，當避障實際點與預期點之間存在較大位置差時，仍會導致速度過快。針對此問題，提出一種根據偽距離自動調節的自適應閉環反饋Ke值，首先設定最大Kem值，在未避障時Ke≡Kem，一旦進入閾值范圍，標志點到障礙物的偽距離越來越小，Ke值也相應減小，即末端離障礙物越近，閉環反饋對末端運動的影響越小，使得末端成功避開障礙物。當末端遠離障礙物，偽距離逐漸變大，使得Ke值逐漸變大，此時閉環反饋的影響加大，讓末端能夠從避障軌跡回到運行軌跡，這樣既能保證末端的避障軌跡，又能保證末端回到運行軌跡。Ke值計算公式如下：

Ke=Kem" Degt;dm

Keidm≥Degt;dc

0De≤dc（21）

Kei=Kem（1-cos（πDedm））

式中，Kem為最大閉環反饋值。

從而式（20）改寫為

θ·=J+W1（x·+vp+Ke（pn-pe））（1-θ4M-θ4θ4M-θ4m）+

J+W2（x·+vp+Ke（pn-pe））θ4M-θ4θ4M-θ4m（22）

由式（21）可以看出，Ke可以根據當前偽距離自動調整大小，故無需再設置較大的避障速度vm與之“對抗”，且自適應閉環反饋能夠在避障結束末端開始遠離障礙物時使末端準確回到運行軌跡，從而消除末端誤差。

2.5 速度整合

由式（22）可以看出臂架末端的實際移動速度由運行速度x·、避障速度vp、閉環反饋Ke（pn-pe）三個部分組成。其中，vp、Ke（pn-pe）會根據末端與障礙物的偽距離變化，導致末端實際移動速度的變化，并且避障速度方向與運行速度方向往往呈反向，使得避障前期的末端實際移動速度較慢，避障后速度較快，故采用速度整合方法。通過將三部分的速度合成，使得避障速度與閉環反饋不影響末端速度大小，僅改變速度方向，保證末端速度恒定的同時實現避障，具體公式如下：

ve=vv（vp+x·）‖vp+x·‖+Ke（pn-pe）‖v（vp+x·）‖vp+x·‖+Ke（pn-pe）‖（23）

式中，v為設定速度。

將式（22）改寫后，得到最終的逆運動學算法公式：

θ·=J+W1（ve）（1-θ4M-θ4θ4M-θ4m）+J+W2（ve）θ4M-θ4θ4M-θ4m（24）

3 仿真與實驗分析

3.1 仿真分析

為驗證本文算法的有效性和優越性，以15 m混合式高空作業車為研究對象，分別采用傳統偽距離避障方法和本文避障方法，在同等初始條件下進行仿真對比。其中，初始關節量θ1、θ4、θ6、L2分別為-25°、65.1287°、12.5878°、6387 mm，末端速度方向均為沿y軸正方向。為加快仿真進度，運行速度與設定速度均為100 mm/s，避障速度幅值分別為7500 mm/s與100 mm/s，兩種方法的閉環反饋參數均為3，傳統避障方法的偽距離閾值為5，本文采用的避障方法設定閾值為10、絕對閾值為5，擬合后障礙物中心點的坐標為（7338.8 mm，0 mm，3329.42 mm）。仿真結果見圖5。圖5a、圖5d從軸測圖與俯視圖兩個角度展示軌跡姿態變化。

對比圖5a、5d可以發現本文末端軌跡更加平滑，對比圖5b、圖5e及圖5i、圖5l可以發現本文方法中各關節速度變化更加平穩、速度更小且在避障以及末端回到軌跡時末端速度依舊保持不變。傳統偽距離避障方法中伸縮臂及回轉最大速度分別達到1538 mm/s與24.5°/s，而本文方法中這兩個關節最大速度僅為99 mm/s與0.94°/s，在同樣的運行速度下本文方法的最大速度平均減小約94.83%，證明本文方法更適合工程機械。圖5g、圖5j中紅色虛線分別對應傳統偽距離避障算法中的閾值與本文避障算法的絕對閾值，通過對比可以發現本文方法對最小偽距離的控制更加明顯，能夠通過絕對閾值來直接控制最小偽距離從而實現控制末端到障礙物的距離。對比圖5h與圖5k發現末端同樣具有回到運行軌跡的能力。綜上所述，本文方法在成功避障的基礎上，較好解決了各關節速度變化快、速度大以及障礙物中心相較于運行軌跡產生偏移時避障軌跡錯誤等問題，并且相較于傳統的偽距離避障方法，本文方法能夠更方便地控制最小偽距離值。

同時，本文避障算法可以實現橫向運行以及斜向運行時的多障礙物避障。以空間同時存在兩障礙物為例進行仿真，障礙物中心坐標分別為（7012.65，0，3329.42）mm和（6812.65，－1000，3329.42）mm。圖6a中運行速度為100 mm/s、方向為沿y軸正方向，圖6b中運行速度為100 mm/s、方向為yoz平面內與y軸正方向呈45°斜向運行，由圖6可以看出兩種情況下均實現了避障，證明本文方法在多障礙物情況下同樣能夠實現避障。

3.2 半物理實驗分析

為驗證本文避障算法在實際高空作業車結構下是否有效，對高空作業車上車臂架進行半物理實驗，實驗裝置如圖7所示。物理部分由高空作業車遙控器、遙控信號接收器、USBCAN分析儀、控制器等組成，虛擬部分由從Solidworks導入MATLAB/simscape模塊的高空作業車三維物理模型以及MATLAB R2022b/Simulink中的虛擬傳感器組成。半物理仿真的目的在于使用實際高空作業車的遙控器對高空車三維模型末端進行控制，驗證本文避障算法能否有效避開空間障礙物，對比D-H建模計算的末端坐標以及模型直接輸出的末端坐標是否存在較大誤差，從而驗證建模的準確性。

考慮到高空作業車臂架在實際應用中通過油缸驅動并通過在油缸安裝的位移傳感器進行閉環反饋，故在半物理仿真實驗中，同樣通過控制油缸位移以及油缸虛擬傳感器來實現模型的驅動及閉環反饋。在控制器中通過結構幾何將各關節角度換算成油缸長度，分別為塔臂油缸l1、主臂油缸l2、伸縮臂油缸l3。仿真中，設置仿真時間為160 s，回轉、塔臂、主臂、伸縮臂各關節初始量為-30°、65.1287°、11.4122°、6387 mm，對應塔臂、主臂、伸縮臂的油缸伸縮量分別為339.43 mm、179.39 mm、2489 mm，障礙物中心坐標為（6941.26，0，5220.88）mm，設定速度v為100 mm/s，速度實際大小與方向通過遙控器的搖桿來控制，自適應閉環反饋Ke為20，設定閾值dm為10，絕對閾值dc為2。仿真方案如下：首先通過遙控器控制末端沿y軸正方向移動直到完成避障，用于驗證程序的可行性，隨后控制末端沿z軸負方向移動一段距離，最后控制末端沿y軸負方向移動進行第二次避障，驗證程序對同一障礙物是否能完成多次避障，考慮到加速度與慣性的影響，通過遙控器實現末端速度的緩慢變化，仿真結果如圖8所示。

由圖8可以看出，為模擬實際操作，防止過大的加速度產生較大沖擊，T=0時通過遙控器控制末端速度逐漸增大，到T=3 s時達到最大值100 mm/s，T=32 s時末端開始第一次避障，T=74 s時末端完成避障并且末端回到運行軌跡，在0～74 s過程中偽距離最小值為2.85，遠大于絕對閾值2。T=68 s時為防止慣性加速度對臂架各關節帶來較大沖擊，通過遙控器控制速度逐漸減小。74～92 s時末端沿z軸負方向運動，同樣通過手柄控制末端速度逐漸變化。T=94 s時末端沿y軸正方向移動進入第二次避障并在T=143 s時成功完成避障，由圖8f可以看到在兩次避障中，臂架第二次避障的偽距離同樣大于絕對閾值;且由圖8e、圖8b可以看出，運行過程中各油缸的伸縮速度以及各關節量變化連續且平緩，針對不同的速度要求，可以通過修改設定速度v直接實現降低末端速度。同時，通過對比模型末端坐標與計算所得末端坐標，發現整個運行過程中末端各方向上的計算誤差恒定在1 mm以內。

以上半物理仿真實驗實現了對同一障礙物的多次避障并回到相應的運行軌跡，成功驗證了該避障算法對于速度、最小偽距離的控制及閉環反饋的有效性，為實際工程應用提供了重要依據。

4 結論

本文針對改進梯度投影法在混合式高空作業車應用中出現的速度、軌跡不連續問題以及避障過程中的速度突變問題，提出一種基于自適應閉環反饋的高空車臂架避障運動控制算法，該算法在完成避障的基礎上大大改善了高空車臂架避障過程中的速度突變，保證高空作業車運行平穩。得出結論如下：

（1）本文在改進梯度投影法的基礎上進行優化，提出了一種適用于混合式高空作業車的基于加權最小范數解的二次臂架運動分配方法，實現了存在伸縮臂的高空車臂架末端的速度、軌跡連續的運動控制。

（2）采用偽距離評估臂架與障礙物的接近程度，并對避障算法進行了優化，通過設定閾值與絕對閾值的方法成功實現了對最小偽距離的控制，同時對障礙物標志點以及臂架標志點的選取方案進行優化，實現了不同情況下臂架末端正確的避障。

（3）提出一種自適應閉環反饋方法來替代傳統閉環反饋方法，減少了閉環反饋在避障時對末端速度的影響，解決了避障后末端的速度突變問題。

（4）對組成末端速度的三部分（運行速度、避障速度、閉環反饋）進行速度整合，實現了對末端速度最大值以及速度突變的控制，使所提方法更適用于工程機械。

（5）進行了半物理仿真實驗，成功驗證了該避障算法對速度、最小偽距離的控制以及閉環反饋的有效性，為實際工程應用提供了重要依據。

參考文獻：

［1］ ELIZONDO-LEAL J C， PARRA-GONZLEZ E F， RAMREZ-TORRES J G. The Exact Euclidean Distance Transform：a New Algorithm for Universal Path Planning［J］. International Journal of Advanced Robotic Systems， 2013， 10（6）：266.

［2］ SUN D， HWANG S， KIM B， et al. Creation of one Excavator as an Obstacle in C-space for Collision Avoidance during Remote Control of the Two Excavators Using Pose Sensors［J］. Remote Sensing， 2020， 12（7）：1122.

［3］ PERDEREAU V， PASSI C， DROUIN M. Real-time Control of Redundant Robotic Manipulators for Mobile Obstacle Avoidance［J］. Robotics and Autonomous Systems， 2002， 41（1）：41-59.

［4］ KHAN A H， LI Shuai， LUO Xin. Obstacle Avoidance and Tracking Control of Redundant Robotic Manipulator：an RNN-based Metaheuristic Approach［J］. IEEE Transactions on Industrial Informatics， 2020， 16（7）：4670-4680.

［5］ PARK M G， JEON J H， LEE M C. Obstacle Avoidance for Mobile Robots Using Artificial Potential Field Approach with Simulated Annealing［C］∥2001 IEEE International Symposium on Industrial Electronics Proceedings（Cat. No.01TH8570）. Pusan， 2001：1530-1535.

［6］ XU Zhihao， ZHOU Xuefeng， LI Shuai. Deep Recurrent Neural Networks Based Obstacle Avoidance Control for Redundant Manipulators［J］. Frontiers in Neurorobotics， 2019， 13：47.

［7］ JU Fengjia， JIN Hongzhe， WANG Binluan， et al. A Predictable Obstacle Avoidance Model Based on Geometric Configuration of Redundant Manipulators for Motion Planning［J］. Sensors， 2023， 23（10）：4642.

［8］ MU Zonggao， LIU Tianliang， XU Wenfu， et al. A Hybrid Obstacle-avoidance Method of Spatial Hyper-redundant Manipulators for Servicing in Confined Space［J］. Robotica， 2019， 37（6）：998-1019.

［9］ 張建華， 任寶珍， 趙巖， 等. 基于二分試探的平面冗余機械臂碰撞檢測與避障［J］. 機械工程學報， 2023， 59（1）：113-122.

ZHANG Jianhua， REN Baozhen， ZHAO Yan， et al. Collision Detection and Obstacle Avoidance of Planar Redundant Manipulator Based on Dichotomous Heuristic［J］. Journal of Mechanical Engineering， 2023， 59（1）：113-122.

［10］ 國家市場監督管理總局， 國家標準化管理委員會. 高空作業車：GB/T 9465—2018［S］. 北京：中國標準出版社， 2018.

State Administration for Market Regulation， Standardization Administration of the People’s Republic of China. Vehicle-mounted Mobile Elevating Work Platform：GB/T 9465—2018［S］. Beijing：Standards Press of China， 2018.

［11］ 滕儒民， 賀浩， 項慧， 等. 基于啟發式路徑搜索的高空作業車避障軌跡規劃［J］. 機械工程學報， 2013， 49（10）：194-198.

TENG Rumin， HE Hao， XIANG Hui， et al. Trajectory Planning with Obstacles Considered for Aerial Work Platform Based on Heuristic Path-searching［J］. Journal of Mechanical Engineering， 2013， 49（10）：194-198.

［12］ 王碩， 李恩， 趙曉光， 等. 伸縮臂高空作業車軌跡跟蹤控制方法研究［J］. 華中科技大學學報（自然科學版）， 2011， 39（增刊2）：216-219.

WANG Shuo， LI En， ZHAO Xiaoguang， et al. Research on Trajectory Tracking Control Method of Aerial Work Platform with Telescopic Booms［J］. Journal of Huazhong University of Science and Technology（Natural Science Edition）， 2011， 39（S2）：216-219.

［13］ CHANG Shuai， FU Yongling， SUN Jian， et al. Inverse Kinematics Solution and Trajectory Planning of Redundant Manipulator Based on Improved Gradient Projection Method［J］. Journal of Physics：Conference Series， 2022， 2396（1）：012045.

［14］ SHI Zhong， HUANG Xuexiang， HU Tianjian， et al. Weighted Augmented Jacobian Matrix with a Variable Coefficient Method for Kinematics Mapping of Space Teleoperation Based on Human-robot Motion Similarity［J］. Advances in Space Research， 2016， 58（7）：1401-1416.

［15］ HUANG Shuihua， PENG Yonggang， WEI Wei， et al. Clamping Weighted Least-norm Method for the Manipulator Kinematic Control with Constraints［J］. International Journal of Control， 2016， 89（11）：2240-2249.

［16］ KORKMAZ O， IDER S K， OZGOREN M K. Trajectory Tracking Control of an Underactuated Underwater Vehicle Redundant Manipulator System［J］. Asian Journal of Control， 2016， 18（5）：1593-1607.

［17］ PENG Jianqing， XU Wenfu， YANG Taiwei， et al. Dynamic Modeling and Trajectory Tracking Control Method of Segmented Linkage Cable-driven Hyper-redundant Robot［J］. Nonlinear Dynamics， 2020， 101（1）：233-253.

［18］ MARCOS M D G， MACHADO J T， AZEVEDO-PERDICOLIS T P. A Multi-objective Approach for the Motion Planning of Redundant Manipulators［J］. Applied Soft Computing， 2012， 12（2）：589-599.

［19］ WAN Jun，YAO Jiafeng， ZHANG Liang’an， et al. A Weighted Gradient Projection Method for Inverse Kinematics of Redundant Manipulators Considering Multiple Performance Criteria［J］. Journal of Mechanical Engineering， 2018：，64（7/8）：475-487.

［20］ ZHANG Jianxia， ZHANG Jianxin， ZHANG Qiang， et al. Obstacle Avoidance Path Planning of Space Robot Based on Improved Particle Swarm Optimization［J］. Symmetry， 2022， 14（5）：938.

［21］ XIE Yaen， ZHANG Zhidan， WU Xiande， et al. Obstacle Avoidance and Path Planning for Multi-joint Manipulator in a Space Robot［J］. IEEE Access， 2019， 8：3511-3526.

［22］ LI Chen， YING Ma， YU Zhang， et al. Obstacle Avoidance and Multitarget Tracking of a Super Redundant Modular Manipulator Based on Bezier Curve and Particle Swarm Optimization［J］. Chinese Journal of Mechanical Engineering， 2020， 33（5）：101-119.

［23］ LIU Yong， LI Xiang， JIANG Peiyang， et al. Evolutionary Multi-objective Trajectory Optimization for a Redundant Robot in Cartesian Space Considering Obstacle Avoidance［J］. Mechanical Sciences， 2022， 13（1）：41-53.

（編輯 陳 勇）