基于動力學降噪模型的蛇形臂機器人末端軌跡跟隨算法

摘要：針對超冗余蛇形機器臂在狹小受限空間的運動規(guī)劃問題，提出了一種基于等效多連桿動力學降噪模型的末端軌跡跟隨算法。將蛇形臂視作關節(jié)點約束在末端無碰撞軌跡曲線上運動的多連桿系統(tǒng)，基于虛功原理推導了該剛柔耦合系統(tǒng)的動力學方程。臂節(jié)的長度條件由連桿的軸向剛度保證，并通過模型降噪方法從模型層面濾除高頻分量影響，使常規(guī)顯式算法可用于等效多連桿系統(tǒng)方程的高效求解。采用所提算法可以指定蛇形臂末端或基座的速度規(guī)律，進而實現(xiàn)蛇形臂沿末端軌跡的運動規(guī)劃。由于蛇形臂關節(jié)被嚴格限制在可行性軌跡曲線上運動，因此可以實現(xiàn)在狹小深腔環(huán)境下的避障運動規(guī)劃。通過求解平面和空間典型數(shù)值算例可知，采用所提算法進行蛇形機械臂運動規(guī)劃具有足夠的計算精度和效率。

關鍵詞：蛇形臂機器人；運動規(guī)劃；末端軌跡跟隨；模型降噪；剛柔耦合

中圖分類號：TP242

Tip-trajectory Following Algorithm for Snake-arm Robots Based on Dynamics Denoising Model

ZHANG Zhigang1 JIN Yongli1 WANG Dongyin1 FU Zhijun1 QIN Guodong^2*

1.Henan Key Laboratory of Intelligent Manufacturing of Mechanical Equipment，Zhengzhou University of Light Industry，Zhengzhou，450002

2.Institute of Plasma Physics，Chinese Academy of Sciences，Hefei，230026

Abstract： Based on dynamics model smoothing formulation of equivalent multi-link system， a tip-trajectory following algorithm was proposed for motion planning of hyper-redundant SARs in narrow and confined spaces. Regarding SAR as a multi-link system of joint points constrained to move on collision-free trajectory curve of tip， and the dynamics equation of rigid-flexible coupling system was derived using virtual work principle. The length conditions of robot arm segments were guaranteed by axial stiffness of connecting rods， and the influences of high-frequency components were filtered out from the model level by employing model denoising method， so that the conventional explicit algorithm might be used to calculate the solution of equation of equivalent multi-link system efficiently. The proposed algorithm may specify the velocity law of tip or base of snake-arms， and realize the motion planning of SAR along tip-trajectory. Due to the strictly limited joint points of SAR moving on feasible trajectory curve， obstacle avoidance motion planning may be achieved in narrow and deep cavity environments. After solving typical numerical examples in planar and spatial cases， the results show that the proposed algorithm has sufficient computational accuracy and efficiency for the motion planning of SARs.

Key words： snake-arm robot（SAR）; motion planning; tip-trajectory following; model denoising; rigid-flexible coupling

0 引言

基于“仿生肌腱”設計的蛇形臂機器人（snake-arm robot，SAR）結構質量小、運動靈活性高，且臂長范圍無電子元件，特別適于高危、高輻射及狹小深腔作業(yè)環(huán)境需求，是近年來機器人前沿技術領域的研究熱點之一^［1］。

蛇形臂機器人的超長串聯(lián)結構和超冗余自由度使其具備極好的環(huán)境適應性，實現(xiàn)狹小受限空間內(nèi)的避障運動是其運動規(guī)劃的核心問題。廣泛應用于冗余機械臂逆運動學求解的雅可比矩陣偽逆法^［2］以關節(jié)整體運動量最小為優(yōu)化目標，以機械臂末端運動為約束來確定關節(jié)角，實施過程沒有考慮避障要求。在此基礎上提出的梯度投影法^［3］，可添加避障作為附加優(yōu)化目標，將梯度投影到雅可比矩陣的零空間中，利用零空間的性質，在保證跟蹤機械臂末端位姿的前提下進行避障運動規(guī)劃。然而對于超冗余度機器人，這類方法往往耗時過長，并且會出現(xiàn)奇異值問題而造成關節(jié)速度陡增和運動方向的不明確。

此外，人工勢場法^［4］、模糊邏輯算法^［5］、遺傳算法^［6］及神經(jīng)網(wǎng)絡算法^［7］等也被用于冗余機器人避障規(guī)劃，但上述研究大多基于開放空間和單一或少數(shù)規(guī)則障礙物場景，對于狹小受限空間、多連續(xù)障礙，如復雜管道、深腔環(huán)境等避障路徑連續(xù)性工況，普遍存在計算量大、實時性弱、穩(wěn)定性及靈活性差等不足。

針對超冗余蛇形臂機器人在復雜受限環(huán)境下的運動規(guī)劃問題，WILLIAMS等^［8］提出了一種末端軌跡跟隨方法（follow-the-leader approach）。該方法的核心思想是在受限空間規(guī)劃出一條無碰撞路徑軌跡的基礎上，控制蛇形臂末端沿該軌跡運動，而其余機械臂關節(jié)則跟隨末端的運動軌跡，使運動中的蛇形臂盡可能逼近路徑曲線，從而實現(xiàn)避障目標。

末端軌跡跟隨方法在實施上有兩個關鍵：①多障礙受限環(huán)境下的無碰撞軌跡規(guī)劃；②蛇形臂關節(jié)點在可行性軌跡上位置的快速確定。無碰撞軌跡可采用A*算法^［9］、蟻群算法^［10］等自動路徑規(guī)劃算法在線或離線生成，而蛇形臂沿軌跡曲線運動時關節(jié)位置的更新是該方法的核心。為此，PALMER等^［11］以蛇形臂末端位置誤差和路徑偏移量為目標函數(shù)，采用優(yōu)化方法對運動關節(jié)位置參數(shù)進行解算，但存在計算量大、末端位置跟蹤精度不高等問題。為了提高求解效率，王俊剛等^［12］提出基于迭代步長順序查找來實現(xiàn)機械臂關鍵點與離散路徑曲線快速匹配，并通過線性插值優(yōu)化方法減小因路徑離散處理引起的誤差。但這類方法仍僅從位置信息出發(fā)，本質上仍是反復迭代求解關于各個關節(jié)位置的非線性方程組。

為了提高蛇形機械臂在受限空間上避障運動規(guī)劃問題的計算效率，本文提出了一種基于動力學模型的末端軌跡跟隨運動規(guī)劃方法，并對兩個典型算例進行了仿真求解。

1 超冗余蛇形機械臂的運動學描述

本文研究的蛇形機械臂由驅動基座、直線導軌、標準臂節(jié)、鋼絲繩纜和萬向節(jié)組成。基座可沿直導軌運動，臂節(jié)間通過十字軸萬向節(jié)串聯(lián)，并通過鋼絲進行驅動，其整體結構如圖1所示。

為了對蛇形臂運動學進行描述，在各臂節(jié)質心r^（i）c處建立連體基坐標系［e^（i）1 e^（i）2 e^（i）3］，其中e^（i）1指向臂節(jié)長度方向，e^（i）2和e^（i）3分別沿臂節(jié)兩端萬向節(jié)轉軸方向。相鄰i-1與i號臂節(jié)間的運動學關系如圖2所示。

臂節(jié)i姿態(tài)矩陣Ai=［e^（i）1 e^（i）2 e^（i）3］可由臂節(jié)i-1姿態(tài)矩陣Ai－1=［e^（i－1）1 e^（i－1）2

e^（i－1）3］表示為

Ai=Ai－1Ri（1）

其中轉動矩陣Ri為

Ri=cos αicos βi－sin αicos asin βicos βisin αicos αisin asin βi－sin βi0cos βi（2）

式中：αi、βi分別為萬向節(jié)依次繞轉軸e^（i）1和e^（i）2的轉角。

相鄰臂節(jié)間質心矢徑r^（i－1）c與r^（i）c的傳遞關系可以表示為

r^（i）c=r^（i－1）c+Ai－1r-^（i－1）Q－Air-^（i）P（3）

式中：r-^（i－1）Q和r-^（i）P分別為萬向節(jié)鉸點在i-1和i號臂節(jié)中的局部坐標。

2 末端無碰撞軌跡的三次樣條插值

采用末端軌跡跟隨方法進行超冗余蛇形機械臂的運動規(guī)劃，可先借助自動路徑規(guī)劃算法通過在線或離線方式生成無碰撞軌跡離散點，然后插值形成足夠光滑的末端軌跡曲線。鑒于三次樣條具有二階導數(shù)連續(xù)、能夠保證運動平順性等優(yōu)勢，本文采用三次樣條插值構造末端軌跡曲線。

將自動路徑規(guī)劃得到的m+1個離散點標記為r0，r1，r2，…，rm，選用近似弧長參數(shù)s作為三次樣條插值的一組分割參數(shù)，則位于節(jié)點ri－1和ri之間曲線可以由Hermite插值表示為

r（s）=N^（0）1ri－1+N^（0）2ri+Li（N^（1）1ri－1，s+N^（1）2ri，s）（4）

Li=si－si－1

其中，Li為曲線段弧長；si－1和si分別為兩端節(jié)點處對應弧坐標；Hermite插值基函數(shù)為

N^（0）1=1－3ξ2+2ξ3" N^（0）2=3ξ2－2ξ3

N^（1）1=ξ－2ξ2+ξ3" N^（1）2=－ξ2+ξ3（5）

ξ=（s－si－1）/Li

式中：ξ為歸一化參數(shù)。

軌跡曲線插值式（4）中出現(xiàn)的離散點導矢ri－1，s和ri，s可由邊界條件和三次樣條在節(jié)點處二階導數(shù)連續(xù)性條件提供。如對于第一類邊界條件，曲線端部切向導矢r0，s和rm，s為已知，中間節(jié)點處滿足的二階導數(shù)連續(xù)性條件可以表示為

［ri－1，s ri，s ri+1，s］Li2Li（1+Ki）LiKi=

［ri－1 ri ri+1］－33（1－K2i）3（6）

Ki=Li/Li+1

式中：Ki為相鄰梁段的弧長比。

將連續(xù)性條件（式（6））與端部已知條件r0，s和rm，s統(tǒng)一記作ri，sA=ΦmB，其中ri，s為插值節(jié)點導矢矩陣，ri，s=［r0，s r1，s … rm，s］，Φm為節(jié)點參數(shù)矩陣，Φm=［r0 r1 …rm r0，s rm，s］，系數(shù)矩陣A和B分別為

A=L1102L1（1+K1）00L1K1Li2Li（1+Ki）LiKiLm－12Lm－1（1+Km－1）00Lm－1Km－101（7）

B=－3003（1－K21）3K21－33（1－K2i）3K2i－3003（1－K2m－1）103K2m－101（8）

由此可以解得ri，s=Φm（BA^－1）=ΦmHg，因此節(jié)點i的導矢為ri，s=ΦmHgi，其中列陣Hgi=Hg（∶，i）。根據(jù)Φm的排布，節(jié)點i的矢徑可以表示為ri=ΦmHri，其中Hri為定位布爾矩陣。將其代入式（4），可得軌跡曲線上弧長s處的位矢為

r（s）=ΦmN^（i）m" s∈［si－1，si］（9）

其中，插值函數(shù)矩陣N^（i）m為

N^（i）m=N^（0）1Hri－1+N^（0）2Hri+Li（N^（1）1Hgi－1+N^（1）2Hgi）（10）

3 多連桿動力學降噪模型及末端跟隨運動規(guī)劃

3.1 等效多連桿系統(tǒng)的動力學降噪模型

蛇形臂沿末端軌跡運動時，各關節(jié)鉸點被嚴格限制在可行性曲線上，其運動學模型可等效為受軌跡曲線約束的鉸接多連桿系統(tǒng)，如圖3所示。

對于等效連桿Bi，其兩端關節(jié)鉸點在軌跡曲線上的弧坐標分別為s^（i）J和s^（i+1）J。根據(jù)式（9），關節(jié)鉸點的矢徑分別為r^（i）J=r（s^（i）J）和r^（i+1）J=r（s^（i+1）J）。因此，連桿Bi上任一點矢徑可由線性插值表示為

r^（i）B（s-）=（1－ζ）r^（i）J+ζr^（i+1）J（11）

ζ=s-/LB

式中：s-為沿連桿軸向定義的物質坐標；ζ為歸一化坐標；LB為等效連桿的長度，即臂節(jié)兩端萬向節(jié)鉸點間距離。

對式（11）兩端求導可得連桿上任一點速度

r·^（i）B（s-）=（1－ζ）r^（i）J，ss·^（i）J+ζr^（i+1）J，ss·^（i+1）J=Ν^（i）Bq·^（i）s（12）

q·^（i）s=［s·^（i）J s·^（i+1）J］T" Ν^（i）B=［（1－ζ）r^（i）J，s ζr^（i+1）J，s］

r^（i）J，s=r（s^（i）J）/s" r^（i+1）J，s=r（s^（i+1）J）/s

對式（12）再求一次導數(shù)可得加速度

r¨^（i）B（s-）=Ν^（i）Bq¨^（i）s+Ν·^（i）Bq·^（i）s（13）

等效連桿Bi的慣性力虛功率為

δp^（i）ine=mB∫10（δr·^（i）B）Tr¨^（i）Bdζ=

（δq·^（i）s）T（M^（i）rq¨^（i）s+F^（i）r）（14）

式中：mB為連桿質量，質量陣M^（i）r=mB∫10（Ν^（i）B）TΝ^（i）Bdζ，慣性力列陣F^（i）r=mB［∫10（Ν^（i）B）TΝ·^（i）Bdζ］q·^（i）s。

由柔性多連桿組成的剛柔耦合系統(tǒng)的動力學方程表現(xiàn)明顯為剛性特征，就算采用剛性微分方程數(shù)值解法的計算效率也不高，因此，有效解決這一問題也是本文方法成功實施的關鍵。

近年來提出模型降噪方法^［13］通過將彈性內(nèi)力在一小段時間域h內(nèi)平均，進而從模型層面濾除高頻分量影響，使顯式微分方程數(shù)值解法也能高效應用于傳統(tǒng)剛性問題的求解。根據(jù)模型降噪方法，可將等效連桿的內(nèi)力虛功率表示為

δp^（i）str=LB∫10δε·^（i）BEA（ahε¨^（i）B+bhε·^（i）B+ε^（i）B）dζ（15）

ah=h2/6" bh=h/2

式中：E為等效連桿的彈性模量；A為等效連桿的截面積；ε^（i）B為軸向應變；ah、bh為與降噪因子h相關的系數(shù)。

連桿Bi的軸向應變可以表示為

ε^（i）B（s-）=ΔLB/LB=（e^（i）1）T（r^（i+1）J－r^（i）J）/LB－1（16）

其中，ΔLB為等效連桿Bi的長度增量；e^（i）1為桿長方向單位基矢量：

e^（i）1=（r^（i+1）J－r^（i）J）/‖r^（i+1）J－r^（i）J‖

對式（16）求導可得軸向應變速率為

ε·^（i）B（s-）=（e^（i）1）T（r·^（i+1）J－r·^（i）J）/LB=N^（i）εq·^（i）s（17）

N^（i）ε=（1/LB）［－（e^（i）1）Tr^（i）J，s （ei1）Tr^（i+1）J，s］

對式（17）再求導可得軸向應變的二階時間導數(shù)為

ε¨^（i）B（s-）=N^（i）εq¨^（i）s+N·^（i）εq·^（i）s（18）

將式（16）～式（18）代入式（15），可得連桿Bi的內(nèi)力虛功率

δp^（i）str=（δq·^（i）s）T［ahK^（i）q¨^（i）s+（ahK·^（i）c+

bhK^（i））q·^（i）s+F^（i）s］（19）

其中，廣義節(jié)點力F^（i）s=EALB∫10（N^（i）ε）Tε^（i）Bdζ，矩陣K^（i）和K·^（i）c分別為

K^（i）=EALB∫10（N^（i）ε）TN^（i）εdζ

K·^（i）c=EALB∫10（N^（i）ε）TN·^（i）εdζ（20）

等效連桿Bi的外力虛功率可以表達為

δp^（i）exp=（δq·^（i）s）TF^（i）e（21）

其中，F(xiàn)^（i）e為等效連桿Bi所受到的廣義外力列陣。

對于由n個臂節(jié)組成的蛇形機械臂，由關節(jié)鉸點在軌跡曲線上的弦長坐標組成的系統(tǒng)參數(shù)列陣為qs=［s^（1）J s^（2）J … s^（n+1）J］T，則等效連桿Bi的弦長參數(shù)可以表示為q^（i）s=Tiqs，其中Ti為定位布爾矩陣。

蛇形臂等效連桿系統(tǒng)的虛功率方程可表示為

∑ni=1（δp^（i）ine+δp^（i）str－δp^（i）exp）=0

將式（14）、式（19）和式（21）代入，并由虛變分δq·s的獨立性可得

Mq¨s+Cq·s=F（22）

式中：系統(tǒng)質量陣M=∑ni=1TTi（M^（i）r+ahK^（i））Ti；系統(tǒng)阻尼矩陣C=∑ni=1TTi（ahK·^（i）c+bhK^（i））Ti；系統(tǒng)廣義外力列陣F=∑ni=1TTi（F^（i）e－F^（i）r－F^（i）s）。

3.2 末端跟隨運動的速度控制

蛇形臂沿末端軌跡運動時，通常有兩種速度控制方式：①指定驅動基座沿直線導軌的速度；②指定蛇形臂末端沿規(guī)劃軌跡曲線的速度。

被控關節(jié)鉸點的弦長坐標可以表達為時間t的已知函數(shù)，將其記作s^ctlJ（t），除此之外其余鉸點的弧長坐標組集為列陣sa=qs－s^ctlJ，相應地將式（22）進行重新分塊表示為

MccMcaMacMaas¨^ctlJs¨a+CccCcaCacCaas·^ctlJs·a=FcFa（23）

據(jù)此可以求得

s¨a=MTaa（Fa－Macs¨^ctlJ－Cacs·^ctlJ－Caas·a）（24）

對式（24）積分便可以得到各關節(jié)沿軌跡曲線的運動規(guī)律，得到各時刻關節(jié)鉸點曲線參數(shù)s^（i）J，進而由式（9）得到各關節(jié)鉸點的位置矢量r^（i）J。

3.3 機械臂關節(jié)轉角的反解

在得到各時刻關節(jié)鉸點位置r^（i）J的基礎上，可以通過反解各萬向節(jié)對應鉸坐標αi、βi，進而確定蛇形臂姿態(tài)。由于驅動基座只沿直線導軌運動，其姿態(tài)不變，故萬向節(jié)關節(jié)角的求解可以從與驅動基座連接的1號臂節(jié)開始依次進行。

在臂節(jié)i-1姿態(tài)（e^（i－1）1，e^（i－1）2，e^（i－1）3）已遞推求得的情況下，由圖4可知臂節(jié)i相對于臂節(jié)i-1的轉角βi滿足關系cos（π/2+βi）=e^（i）1·e^（i－1）3，因此轉角βi為

βi=arccos（e^（i）1·e^（i－1）3）－π/2（25）

記基矢量e^（i）1與e^（i－1）1的夾角為γi，根據(jù)三余弦定理有cos γi=cos αicos βi，其中cos γi=e^（i－1）1·e^（i）1。因此繞e^（i－1）3軸的轉角αi為

αi=arccos（cos γi/cos βi）（26）

按照這個流程依次遞推求解便可得到全部十字軸萬向節(jié)的轉角αi、βi，進而確定蛇形臂姿態(tài)。

4 仿真算例

為了驗證本文提出算法的有效性，在MATLAB中編寫了相應程序，并采用其顯式常微分方程求解器ode45仿真求解。本節(jié)算例中蛇形臂共有12個標準臂節(jié)，單臂節(jié)長104 mm，直徑40 mm，驅動基座最大進給距離1248 mm。為了保證臂節(jié)長度條件，設等效連桿系統(tǒng)的連桿抗拉剛度EA=1×109 N。本節(jié)算例的仿真計算均在CPU為Intel Core i7-12650H、內(nèi)存為16GB的筆記本電腦上完成。

4.1 平面軌跡跟隨

在平面問題算例中，蛇形臂機器人需要穿過特定間隙并繞過直徑為160 mm的圓形障礙。據(jù)此任務，規(guī)劃出了直徑為200 mm的圓弧形可行性路徑曲線，如圖5中黑色曲線所示。

不失一般性，這里設置驅動基座沿直線導軌的運動規(guī)律為

s·^ctlJ（t）=0.5v［1+sin（（t+3/2））π］" 0≤t≤1

v1lt;t≤9

0.5v［1+sin（（t－17/2））π］9lt;t≤10（27）

其中，勻速運動階段v=0.12 m/s。采用本文算法仿真得到的蛇形臂進行平面軌跡跟隨運動的結果如圖5所示。

在用ode45進行計算求解時，設定仿真時間為10 s，絕對誤差限為εa=1.0×10^-6，相對誤差限為εr=1.0×10^-3。根據(jù)文獻［14］推薦，對于蛇形臂這類剛度極大問題，取模型降噪因子h=0.1，采用MATLAB非剛性求解器ode45僅需3.19 s就可以完成仿真計算。相比之下，當取h=0（即不進行模型降噪），由于該系統(tǒng)方程剛性問題突出，導致非剛性常微分方程求解器ode45無法完成仿真計算。即便采用基于隱式算法的MATLAB剛性方程求解器ode15s，完成10 s仿真所耗費機時也達到了823.88 s。這也進一步證明了本文提出算法具有較高的計算效率。圖6給出了蛇形臂第3、6、9及12號萬向節(jié)關節(jié)轉角隨時間的變化曲線。

將求得的轉角代入正向運動學模型可解得蛇形臂末端位置，圖7給出了末端位置與期望路徑曲線的偏差距離。分析可知，蛇形臂末端的最大偏差小于0.005 mm，具有非常高的跟蹤精度。

圖8對比了驅動基座與蛇形臂末端速度，從中可以看出，兩者存在明顯差異。這是由于當蛇形臂進入曲線段運動時，臂節(jié)會產(chǎn)生角速度，臂節(jié)上兩端鉸點必然產(chǎn)生由角速度不為零帶來的速度差。因此，要實現(xiàn)對蛇形臂末端運動速度的進一步控制，就需要對所采用方法有更嚴苛要求。本文所提方法可以輕易實現(xiàn)對基座或末端的速度控制，這一點將在接下來算例中進一步說明。

4.2 空間軌跡跟隨

在空間問題算例中，模擬蛇形臂穿過飛機機翼油箱內(nèi)部狹小空腔進行檢修任務。這里油箱的整體尺寸為1200 mm×800 mm×200 mm，并由隔板將其分隔為四個儲油倉，如圖9所示。

在這個算例中，指定蛇形臂末端速度滿足式（27）的運動規(guī)律。仿真時間仍為10 s，同樣采用非剛性求解器ode45，誤差限設置與上一個算例相同。同樣設置磨光因子h=0.1，在相同配置臺式機上完成計算僅需耗時2.85 s，表明本文方法具有極高的計算效率。圖9給出了采用本文算法仿真得到的蛇形臂進入機翼油箱進行三維空間軌跡跟隨運動的情形。

圖10給出了蛇形臂驅動基座與末端的速度對比。利用基座驅動規(guī)律便可以實現(xiàn)蛇形臂末端速度大小的精準控制。這也是本文方法相較于現(xiàn)有末端軌跡跟隨算法所具有的獨特優(yōu)勢。

圖11給出了蛇形臂沿規(guī)劃路徑進入機翼油箱狹小空間過程中，第3、6、9和12號萬向節(jié)關節(jié)轉角隨時間的變化曲線。

圖12給出了蛇形臂末端位置與期望路徑曲線的偏差距離，從中可知在進行空間軌跡跟蹤過程中最大偏差小于0.018 mm，這再次證明了本文方法具有很高的跟蹤精度。

5 結論

基于等效多連桿動力學降噪模型提出了一種適用于超冗余蛇形臂機器人狹小受限環(huán)境運動規(guī)劃的末端跟隨運動規(guī)劃算法。

首先，利用路徑規(guī)劃算法得到無碰撞軌跡離散點，并采用三次樣條插值得到滿足連續(xù)性和光滑性要求的蛇形臂末端軌跡曲線。其次，將蛇形臂機器人視作關節(jié)鉸點沿末端軌跡曲線運動的等效多連桿，利用虛功原理推導了該剛柔耦合系統(tǒng)的動力學方程。為了降低剛柔耦合系統(tǒng)方程求解的難度，采用模型降噪方法從模型層面濾除了高頻分量，使得常規(guī)顯式算法可以高效求解系統(tǒng)動力學方程。最后，求解了蛇形臂機器人在平面和空間狹小空間上的運動規(guī)劃問題典型算例，驗證了所提方法的精度和效率。

參考文獻：

［1］ HUANG Long， LIU Bei， ZHANG Leiyu， et al. Equilibrium Conformation of a Novel Cable-driven Snake-arm Robot under External Loads［J］. Micromachines， 2022， 13（7）：1149.

［2］ JAIN T， JAIN J K， ROY D. Joint Space Redundancy Resolution of Serial Link Manipulator：an Inverse Kinematics and Continuum Structure Numerical Approach［J］. Materials Today：Proceedings， 2021， 38：423-431.

［3］ 李亞昕， 王國磊， 張劍輝， 等. 基于碰撞反饋的冗余機器人避障規(guī)劃算法［J］. 清華大學學報（自然科學版）， 2022， 62（3）：408-415.

LI Yaxin， WANG Guolei， ZHANG Jianhui， et al. Obstacle Avoidance Algorithm for Redundant Robots Based on Collision Feedback［J］. Journal of Tsinghua University（Science and Technology）， 2022， 62（3）：408-415.

［4］ 萬俊， 孫薇， 葛敏， 等. 基于含避障角人工勢場法的機器人路徑規(guī)劃［J］. 農(nóng)業(yè)機械學報， 2024， 55（1）：409-418.

WAN Jun， SUN Wei， GE Min， et al. Robot Path Planning Based on Artificial Potential Field Method with Obstacle Avoidance Angles［J］. Transactions of the Chinese Society for Agricultural Machinery， 2024， 55（1）：409-418.

［5］ YAN Fei， WANG Yaoyao， JU Feng， et al. Adaptive Time Delay Control for Cable-driven Manipulators Using Fuzzy Logic Algorithm［J］. Proceedings of the Institution of Mechanical Engineers， Part I：Journal of Systems and Control Engineering， 2021， 235（6）：898-913.

［6］ 馬宇豪， 梁雁冰. 一種基于六次多項式軌跡規(guī)劃的機械臂避障算法［J］. 西北工業(yè)大學學報， 2020， 38（2）：392-400.

MA Yuhao， LIANG Yanbing. An Obstacle Avoidance Algorithm for Manipulators Based on Six-order Polynomial Trajectory Planning［J］. Journal of Northwestern Polytechnical University， 2020， 38（2）：392-400.

［7］ ZHANG Zhijun， ZHENG Lunan， CHEN Zhuo-ming， et al. Mutual-collision-avoidance Scheme Synthesized by Neural Networks for Dual Redundant Robot Manipulators Executing Cooperative Tasks［J］. IEEE Transactions on Neural Networks and Learning Systems， 2021， 32（3）：1052-1066.

［8］ WILLIAMS R L， TAMASI G. Follow-the-leader Control for the Payload Inspection and Processing System［C］∥Proceedings of ASME 1996 Design Engineering Technical Conferences and Computers in Engineering Conference. Irvine， 1996：V02BT02A057.

［9］ DAI Xiaolin， LONG Shuai， ZHANG Zhiwen， et al. Mobile Robot Path Planning Based on Ant Colony Algorithm with A* Heuristic Method［J］. Frontiers in Neurorobotics， 2019， 13：15.

［10］ LIU Yiyang， HOU Zheng， TAN Yuanyuan， et al. Research on Multi-AGVs Path Planning and Coordination Mechanism［J］. IEEE Access， 2020， 8：213345-213356.

［11］ PALMER D， COBOS-GUZMAN S， AXINTE D. Real-time Method for Tip Following Navigation of Continuum Snake Arm Robots［J］. Robotics and Autonomous Systems， 2014， 62（10）：1478-1485.

［12］ 王俊剛， 湯磊， 谷國迎， 等. 超冗余度機械臂跟隨末端軌跡運動算法及其性能分析［J］. 機械工程學報， 2018， 54（3）：18-25.

WANG Jungang， TANG Lei， GU Guoying， et al. Tip-following Path Planning and Its Performance Analysis for Hyper-redundant Manipulators［J］. Journal of Mechanical Engineering， 2018， 54（3）：18-25.

［13］ 張志剛， 周翔， 毛紅生， 等. 絕對節(jié)點坐標方法的模型降噪研究［J］. 振動與沖擊， 2021， 40（11）：139-146.

ZHANG Zhigang， ZHOU Xiang， MAO Hongsheng， et al. Model Denoising Based on Absolute Node Coordinate Method［J］. Journal of Vibration and Shock， 2021， 40（11）：139-146.

［14］ 齊朝暉， 曹艷， 王剛. 多柔體系統(tǒng)數(shù)值分析的模型降噪方法［J］. 力學學報， 2018， 50（4）：863-870.

QI Zhaohui， CAO Yan， WANG Gang. Model Smoothing Methods in Numerical Analysis of Flexible Multibody Systems［J］. Chinese Journal of Theoretical and Applied Mechanics， 2018， 50（4）：863-870.

（編輯 王旻玥）

基金項目：國家自然科學基金（12305251）；河南省科技攻關項目（232102221036，242102241049）；河南省外籍科學家工作室項目（GZS2023011）

作者簡介：

張志剛，男，1984年生，博士、副教授。研究方向為多體系統(tǒng)動力學與控制。E-mail：zhigangzhang@foxmail.com。

秦國棟*（通信作者），男，1991年生，博士、助理研究員。研究方向為聚變堆遙操作維護機器人系統(tǒng)設計。E-mail：gdqin@ipp.ac.cn。

本文引用格式：

張志剛，靳勇利，王棟銀，等.基于動力學降噪模型的蛇形臂機器人末端軌跡跟隨算法［J］. 中國機械工程，2025，36（3）：407-413.

ZHANG Zhigang， JIN Yongli， WANG Dongyin， et al. Tip-trajectory Following Algorithm for Snake-arm Robots Based on Dynamics Denoising Model［J］. China Mechanical Engineering， 2025， 36（3）：407-413.