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

關(guān)鍵詞：繩索驅(qū)動(dòng)；蛇形機(jī)器人；軌跡規(guī)劃

中圖分類(lèi)號(hào)：TP242.2" 文獻(xiàn)標(biāo)志碼：B" 文章編號(hào)：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 引言

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

以上算法主要是在給定起點(diǎn)、終點(diǎn)和障礙物的坐標(biāo)之后，得到一種離散模式下的路徑，這種路徑并不完全滿足蛇形機(jī)器人的任務(wù)需求。因此，本文提出一種基于Dijkstra搜素算法和三次B樣條曲線相結(jié)合的冗余繩索驅(qū)動(dòng)機(jī)器人路徑規(guī)劃算法，并對(duì)算法開(kāi)展了仿真實(shí)驗(yàn)。

1 二維軌跡規(guī)劃算法研究

1.1 基于Dijkstra的初始路徑生成

Dijkstra算法可以尋找最短路徑。根據(jù)蛇形機(jī)器人的實(shí)際應(yīng)用場(chǎng)景，利用Dijkstra算法生成起點(diǎn)位置到終點(diǎn)位置最短的初始路徑，如圖1所示。生成的路徑為折線圖，該路徑可用于離散體機(jī)器人，而蛇形機(jī)器人由多個(gè)剛性連桿串聯(lián)，運(yùn)動(dòng)時(shí)需要協(xié)調(diào)關(guān)節(jié)限制和整體運(yùn)動(dòng)。該路徑轉(zhuǎn)折點(diǎn)曲率過(guò)大可能會(huì)超過(guò)關(guān)節(jié)運(yùn)動(dòng)限制，蛇形機(jī)器人無(wú)法沿該路徑運(yùn)動(dòng)。

1.2 基于三次B樣條的路徑規(guī)劃

基于Dijkstra算法生成的初始路徑，利用等步長(zhǎng)的方式對(duì)關(guān)鍵點(diǎn)進(jìn)行選取。

1）關(guān)鍵點(diǎn)選取

基于Dijkstra算法生成的初始路徑，從該路徑上初始點(diǎn)開(kāi)始，利用等步長(zhǎng)的方式選取點(diǎn)，若終點(diǎn)沒(méi)有包含在內(nèi)，需將終點(diǎn)添加到選取點(diǎn)集合，如圖2所示。該點(diǎn)集合為生成光滑路徑的關(guān)鍵點(diǎn)。

2）三次B樣條曲線控制點(diǎn)求解

在圖形學(xué)計(jì)算中，Bezier曲線和B樣條曲線都是利用所有數(shù)據(jù)點(diǎn)生成插值曲線的方法。結(jié)合二者各自的優(yōu)缺點(diǎn)，經(jīng)過(guò)對(duì)比，本文選擇B樣條曲線插值方法，如下式所示。

式中：Pi為三次B樣條曲線的控制點(diǎn)；Bi，k（u）為B樣條函數(shù)的基函數(shù)。

三次B樣條曲線的逆向計(jì)算過(guò)程：給定的關(guān)鍵點(diǎn)ki（i=1，2，…，n）的坐標(biāo)數(shù)據(jù)，反求出相應(yīng)的控制點(diǎn)pi（i=0，1，…，n+1）。三次B樣條曲線的關(guān)鍵點(diǎn)與控制點(diǎn)的關(guān)系如下式所示。

3）三次B樣條曲線正向運(yùn)算

三次B樣條曲線的正向計(jì)算：由三次B樣條曲線的逆向計(jì)算求解出控制點(diǎn)的p0，p1，p2，…，pn，pn+1，第i段三次B樣條函數(shù)曲線取4個(gè)控制點(diǎn)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樣條曲線拼接起來(lái)，即得到光滑且符合要求的路徑。

1.3 軌跡規(guī)劃

本文采用笛卡兒空間軌跡規(guī)劃，根據(jù)應(yīng)用場(chǎng)景，聯(lián)合表示路徑的曲線方程和圓方程進(jìn)行求解。取其中橫坐標(biāo)大于圓心的交點(diǎn)作為下一時(shí)刻圓的圓心，各圓心的坐標(biāo)是關(guān)節(jié)中心的坐標(biāo)，如下式所示。

式中：y=f（x）為路徑的曲線函數(shù)；（xi，yi） 為當(dāng)前圓心的坐標(biāo)。

將n連桿單自由度蛇形機(jī)器人軌跡規(guī)劃算法分為3個(gè)步驟：

1） 計(jì)算運(yùn)動(dòng)周期為T(mén)；

2） 設(shè)置2n+1個(gè)輔助圓心的初始位置；

3） 更新下一時(shí)刻的2n+1個(gè)圓心的坐標(biāo)。

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

初始時(shí)刻t=0s、第一周期時(shí)刻t=ti（0≤ti≤T）和特定時(shí)刻t=6T時(shí)蛇形機(jī)器人各關(guān)節(jié)中心與圓心的對(duì)應(yīng)關(guān)系及其相對(duì)位置關(guān)系如圖4所示。

2 仿真試驗(yàn)與分析

為了驗(yàn)證提出的Dijkstra算法和三次B樣條曲線的冗余繩索驅(qū)動(dòng)蛇形機(jī)器人路徑規(guī)劃算法，以平面6自由度蛇形機(jī)器人為例，利用Matlab搭建仿真模型，進(jìn)行仿真試驗(yàn)。

假設(shè)有一條已經(jīng)規(guī)劃好的路徑，規(guī)劃路徑可以用以下函數(shù)表示：

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

剩余的輔助圓心坐標(biāo)可以根據(jù)以下公式進(jìn)行計(jì)算：

輔助圓心O1，O2，O3，…，O6計(jì)算后的初始坐標(biāo)如表3所示。

為了滿足軌跡規(guī)劃的精度要求，對(duì)實(shí)際位置和期望位置的誤差進(jìn)行了分析，結(jié)果如表4所示。機(jī)器人各關(guān)節(jié)中心位置的誤差都小于10－10，可滿足機(jī)器人軌跡規(guī)劃的精度要求。

給定初始坐標(biāo)，按上述路徑規(guī)劃方法設(shè)置相關(guān)參數(shù)，t=0s起始，t=30s結(jié)束，進(jìn)行仿真實(shí)驗(yàn)。如圖5所示為蛇形臂機(jī)器人分別在2s、15s、18s和30s時(shí)的仿真狀態(tài)，其中黑色線段為機(jī)器人的連桿，綠色點(diǎn)為機(jī)器人的關(guān)節(jié)旋轉(zhuǎn)中心，黑色點(diǎn)為機(jī)器人的末端執(zhí)行器，紅色曲線為規(guī)劃的路徑（本刊為黑白印刷，疑問(wèn)之處請(qǐng)咨詢(xún)作者）。

根據(jù)機(jī)器人在仿真過(guò)程中關(guān)節(jié)變量的值（表5），機(jī)器人的所有關(guān)節(jié)變量都在其旋轉(zhuǎn)角度限制范圍內(nèi)，表明了所提出的軌跡算法的有效性。

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

3 結(jié)語(yǔ)

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

參考文獻(xiàn)：

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

［2］ 曹博，畢樹(shù)生，鄭晶翔，等. 改進(jìn)人工勢(shì)場(chǎng)法的冗余機(jī)械臂避障算法［J］. 哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2019，51（7）：184-191.

［3］ 霍鳳財(cái)，任偉建，劉東輝. 基于改進(jìn)的人工勢(shì)場(chǎng)法的路徑規(guī)劃方法研究［J］. 自動(dòng)化技術(shù)與應(yīng)用，2016，35（3）：63-67.

［4］ 汪首坤，邸智，王軍政，等. 基于A*改進(jìn)算法的機(jī)械臂避障路徑規(guī)劃［J］. 北京理工大學(xué)學(xué)報(bào)，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ī)器人多目標(biāo)路徑規(guī)劃［J］. 山東理工大學(xué)學(xué)報(bào)（自然科學(xué)版），2021，35（4）：75-80.

［9］ 羅德林，吳順祥. 基于勢(shì)場(chǎng)蟻群算法的機(jī)器人路徑規(guī)劃［J］. 系統(tǒng)工程與電子技術(shù)，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ī)械臂的路徑安全性?xún)?yōu)化研究［J］. 上海交通大學(xué)學(xué)報(bào)，2003，37（5）：710-714.

［12］ 張偉民，付仕雄. 基于改進(jìn)RRT*算法的移動(dòng)機(jī)器人路徑規(guī)劃［J］. 華中科技大學(xué)學(xué)報(bào)（自然科學(xué)版），2021，49（1）：31-36.