冗余繩索驅動蛇形機器人的軌跡規劃研究

摘 要：傳統機械臂無法進入狹窄封閉的空間作業，冗余繩索驅動機器人有很好的靈活性，因此在狹小封閉空間應用潛力巨大。針對冗余繩索驅動機械臂在狹窄空間作業時的避障問題，提出一套軌跡規劃算法。該算法利用搜索算法得到離散模式下的初始規劃路徑，等間隔法選取初始規劃路徑的關鍵點，基于三次B樣條曲線，將關鍵點生成可行路徑。通過仿真實驗，驗證了所提出算法的可行性和有效性。

關鍵詞：繩索驅動；蛇形機器人；軌跡規劃

中圖分類號：TP242.2" 文獻標志碼：B" 文章編號：1671-5276（2024）05-0251-04

Research on Trajectory Planning of Snake Robot Driven by Redundant Rope

Abstract：Redundant rope-driven robot， flexible and different to traditional manipulator which is incabable of entering the narrow and closed space for operation， has potentiality for application in a narrow and closed space. To cope with the obstacle avoidance trouble of redundant cable-driven manipulator operating in a narrow space， a set of trajectory planning algorithm is proposed， which implements the search algorithm to obtain the initial planning path in discrete mode， selects the key points of the initial planning path by the equal interval method， and generates a feasible path from the key points based on the cubic B-spline curve. The simulation experiments verify the feasibility and effectiveness of the proposed algorithm.

Keywords：rope drive；snake robot；trajectory planning

0 引言

冗余繩索驅動機器人具有輕量化、機電分離等特點，在狹小空間中有著很大優勢。路徑規劃是實現機器人控制的關鍵技術之一［1］。由于冗余自由度的存在，冗余繩索驅動機器人的路徑規劃變得困難。目前對冗余機械臂的路徑規劃問題已經取得了一些成果，求解路徑規劃的方法主要包括人工勢場法［2-3］和人工智能啟發式搜索算法，如A*算法［4］、Dijkstra算法、遺傳算法［5-6］和蟻群算法。KHATIB等［7］提出了一種利用人工勢場法滿足機械臂內部關節約束。宮金良等［8］利用Dijkstra算法對校園送餐路線進行路徑規劃，創建了節點鄰接集合方法，縮短了最小路徑的搜索域，提高了路徑規劃效率。羅德林等［9］提出一種基于人工勢場和蟻群算法相結合的路徑規劃算法，提高了最優路徑的搜索效率。DONG等［10］利用機械臂運動的傅里葉變換概念，得到冗余機械臂單個連桿的工作空間密度，求解路徑規劃問題，可生成平滑且距離短的路徑，多障礙物避障十分有效。王建濱等［11］提出了一種對超冗余度機械臂路徑安全優化算法，結合安全性優化的數學模型，利用Dijkstra算法進行求解，極大地提高了路徑的安全性。張偉民等［12］為了保證路徑的可行性，利用三次B樣條曲線對改進的RRT*算法搜索到的路徑進行平滑處理，使曲線無過大折角且光滑連續。

以上算法主要是在給定起點、終點和障礙物的坐標之后，得到一種離散模式下的路徑，這種路徑并不完全滿足蛇形機器人的任務需求。因此，本文提出一種基于Dijkstra搜素算法和三次B樣條曲線相結合的冗余繩索驅動機器人路徑規劃算法，并對算法開展了仿真實驗。

1 二維軌跡規劃算法研究

1.1 基于Dijkstra的初始路徑生成

Dijkstra算法可以尋找最短路徑。根據蛇形機器人的實際應用場景，利用Dijkstra算法生成起點位置到終點位置最短的初始路徑，如圖1所示。生成的路徑為折線圖，該路徑可用于離散體機器人，而蛇形機器人由多個剛性連桿串聯，運動時需要協調關節限制和整體運動。該路徑轉折點曲率過大可能會超過關節運動限制，蛇形機器人無法沿該路徑運動。

1.2 基于三次B樣條的路徑規劃

基于Dijkstra算法生成的初始路徑，利用等步長的方式對關鍵點進行選取。

1）關鍵點選取

基于Dijkstra算法生成的初始路徑，從該路徑上初始點開始，利用等步長的方式選取點，若終點沒有包含在內，需將終點添加到選取點集合，如圖2所示。該點集合為生成光滑路徑的關鍵點。

2）三次B樣條曲線控制點求解

在圖形學計算中，Bezier曲線和B樣條曲線都是利用所有數據點生成插值曲線的方法。結合二者各自的優缺點，經過對比，本文選擇B樣條曲線插值方法，如下式所示。

式中：Pi為三次B樣條曲線的控制點；Bi，k（u）為B樣條函數的基函數。

三次B樣條曲線的逆向計算過程：給定的關鍵點ki（i=1，2，…，n）的坐標數據，反求出相應的控制點pi（i=0，1，…，n+1）。三次B樣條曲線的關鍵點與控制點的關系如下式所示。

3）三次B樣條曲線正向運算

三次B樣條曲線的正向計算：由三次B樣條曲線的逆向計算求解出控制點的p0，p1，p2，…，pn，pn+1，第i段三次B樣條函數曲線取4個控制點pi－1，pi，pi+1，pi+2帶入下式。

Pi（t）=F0，3（t）pi－1+F1，3（t）pi+F2，3（t）pi+1+F3，3（t）pi+2 （i=1，2，3，…，n）（3）

將以上n段三次B樣條曲線拼接起來，即得到光滑且符合要求的路徑。

1.3 軌跡規劃

本文采用笛卡兒空間軌跡規劃，根據應用場景，聯合表示路徑的曲線方程和圓方程進行求解。取其中橫坐標大于圓心的交點作為下一時刻圓的圓心，各圓心的坐標是關節中心的坐標，如下式所示。

式中：y=f（x）為路徑的曲線函數；（xi，yi） 為當前圓心的坐標。

將n連桿單自由度蛇形機器人軌跡規劃算法分為3個步驟：

1） 計算運動周期為T；

2） 設置2n+1個輔助圓心的初始位置；

3） 更新下一時刻的2n+1個圓心的坐標。

以6連桿平面蛇形機器人為研究對象，將A定義為關節，B定義為連桿，C定義為末端。連桿數n=6，需計算13個輔助圓心O－6，O－5，…，O5，O6的坐標，其中6個輔助圓心O－6，O－5，O－4，…，O－1的初始坐標如表1所示，剩余的圓心按照軌跡規劃算法計算。蛇形機器人構型如圖3所示。

初始時刻t=0s、第一周期時刻t=ti（0≤ti≤T）和特定時刻t=6T時蛇形機器人各關節中心與圓心的對應關系及其相對位置關系如圖4所示。

2 仿真試驗與分析

為了驗證提出的Dijkstra算法和三次B樣條曲線的冗余繩索驅動蛇形機器人路徑規劃算法，以平面6自由度蛇形機器人為例，利用Matlab搭建仿真模型，進行仿真試驗。

假設有一條已經規劃好的路徑，規劃路徑可以用以下函數表示：

以6連桿蛇形機器人為例，設置圓的半徑為50mm，連桿數為6，需計算13個輔助圓的圓心。已知基座的進給速度為10mm/s，軌跡規劃的時間小于30s。t=0s時，圓心O－6，O－5，O－4，…，O－1的橫坐標如表2所示，縱坐標初始位置均為0。

剩余的輔助圓心坐標可以根據以下公式進行計算：

輔助圓心O1，O2，O3，…，O6計算后的初始坐標如表3所示。

為了滿足軌跡規劃的精度要求，對實際位置和期望位置的誤差進行了分析，結果如表4所示。機器人各關節中心位置的誤差都小于10－10，可滿足機器人軌跡規劃的精度要求。

給定初始坐標，按上述路徑規劃方法設置相關參數，t=0s起始，t=30s結束，進行仿真實驗。如圖5所示為蛇形臂機器人分別在2s、15s、18s和30s時的仿真狀態，其中黑色線段為機器人的連桿，綠色點為機器人的關節旋轉中心，黑色點為機器人的末端執行器，紅色曲線為規劃的路徑（本刊為黑白印刷，疑問之處請咨詢作者）。

根據機器人在仿真過程中關節變量的值（表5），機器人的所有關節變量都在其旋轉角度限制范圍內，表明了所提出的軌跡算法的有效性。

為驗證本文提出方法的有效性，首先，對本文提出的軌跡規劃方法的精度進行分析，實際位置和期望位置的誤差都小于10－10，可滿足機器人軌跡規劃的精度要求。然后，按上述路徑規劃方法設置相關參數，進行了仿真實驗，實驗結果表明機器人可按照規劃的路徑進行運動。最后，對仿真過程中的關節變量值進行分析，以判斷機器人是否運動超限。結果表明：在機器人運動過程中所有關節變量都在其旋轉角度限制范圍內，均在機器人的可達工作空間。綜上表明所提出的軌跡規劃方法可實現冗余繩索驅動機器人軌跡的有效規劃，且精度較高。

3 結語

本文提出了一種基于Dijkstra算法和三次B樣條曲線的冗余繩索驅動蛇形機器人路徑規劃算法。該方法首先基于Dijkstra算法生成初始路徑，然后以等間隔的方式在初始路徑上選取一系列關鍵點，最后基于三次B樣條曲線算法利用獲取的關鍵點生成光滑無碰撞路徑。仿真試驗結果驗證了所提出的冗余繩索驅動蛇形機器人路徑規劃算法的有效性。

參考文獻：

［1］ GONALVES V M，FRAISSE P，CROSNIER A，et al. Parsimonious kinematic control of highly redundant robots［J］. IEEE Robotics and Automation Letters，2016，1（1）：65-72.

［2］ 曹博，畢樹生，鄭晶翔，等. 改進人工勢場法的冗余機械臂避障算法［J］. 哈爾濱工業大學學報，2019，51（7）：184-191.

［3］ 霍鳳財，任偉建，劉東輝. 基于改進的人工勢場法的路徑規劃方法研究［J］. 自動化技術與應用，2016，35（3）：63-67.

［4］ 汪首坤，邸智，王軍政，等. 基于A*改進算法的機械臂避障路徑規劃［J］. 北京理工大學學報，2011，31（11）：1302-1306.

［5］ ABU-DAKKA F J，VALERO F J，SUER J L，et al. A direct approach to solving trajectory planning problems using genetic algorithms with dynamics considerations in complex environments［J］. Robotica，2015，33（3）：669-683.

［6］ TIAN L， COLLINS C. An effective robot trajectory planning method using a genetic algorithm［J］. Mechatronics，2004，14（5）：455-470：

［7］ KHATIB O. Real-time obstacle avoidance for manipulators and mobile robots［C］//Proceedings of 1985 IEEE International Conference on Robotics and Automation. St. Louis，MO，USA： IEEE，2003：500-505.

［8］ 宮金良，牛作碩，張彥斐. 基于局域降維Dijkstra算法的校園送餐機器人多目標路徑規劃［J］. 山東理工大學學報（自然科學版），2021，35（4）：75-80.

［9］ 羅德林，吳順祥. 基于勢場蟻群算法的機器人路徑規劃［J］. 系統工程與電子技術，2010，32（6）：1277-1280.

［10］ DONG H，DU Z J. Obstacle avoidance path planning of planar redundant manipulators using workspace density［J］. International Journal of Advanced Robotic Systems，2015，12（2）：1-10.

［11］ 王建濱，馬培蓀，徐軍，等. 超冗余度機械臂的路徑安全性優化研究［J］. 上海交通大學學報，2003，37（5）：710-714.

［12］ 張偉民，付仕雄. 基于改進RRT*算法的移動機器人路徑規劃［J］. 華中科技大學學報（自然科學版），2021，49（1）：31-36.