基于ROS的六關(guān)節(jié)機(jī)器人運(yùn)動(dòng)規(guī)劃研究

郭建根，雙 豐，闞宏林

(1.安徽工程大學(xué) 機(jī)械與汽車工程學(xué)院，安徽 蕪湖 241000；2.中國科學(xué)院合肥智能機(jī)械研究所，安徽 合肥 230031)

隨著社會(huì)發(fā)展的需求，機(jī)器人的工作環(huán)境日趨復(fù)雜，對(duì)機(jī)器人的控制系統(tǒng)提出了更高的要求。而機(jī)器人廠商都是開發(fā)自身的軟件接口，可移植性差，只能應(yīng)用于本企業(yè)生產(chǎn)的機(jī)器人，大量復(fù)雜冗余的代碼嚴(yán)重影響了開發(fā)者的工作效率。開源機(jī)器人操作系統(tǒng)(Robot Operating System，ROS)能夠很好地解決這些問題。目前，國內(nèi)外對(duì)基于ROS的機(jī)器人運(yùn)動(dòng)軌跡規(guī)劃已經(jīng)做了大量研究。如美國匹茲堡實(shí)驗(yàn)室和Inter公司聯(lián)合開發(fā)的移動(dòng)操作機(jī)器人(Home Exploring Robotic Butler，HERB)，該移動(dòng)機(jī)器人能夠?qū)崟r(shí)構(gòu)建地圖定位以及利用構(gòu)建好的地圖在室內(nèi)自由移動(dòng)，完成取物品和清理餐桌等任務(wù)[1]。錢偉[2]等基于ROS操作平臺(tái)，利用RRT算法并對(duì)其改進(jìn)，實(shí)現(xiàn)了冗余自由度機(jī)械臂在動(dòng)態(tài)環(huán)境下的自主規(guī)劃。劉磊[3]等研究了基于ROS平臺(tái)下的機(jī)器人軌跡規(guī)劃，分析其各關(guān)節(jié)角度變化和末端插補(bǔ)誤差，使機(jī)械臂各關(guān)節(jié)運(yùn)行軌跡平滑，末端位置精度滿足要求。溫寬昌[4]等利用三角函數(shù)與S形速度曲線混合插值，保證了機(jī)器人關(guān)節(jié)加速度連續(xù)變化，避免了柔性沖擊?；赗OS機(jī)器人操作系統(tǒng)，以六關(guān)節(jié)機(jī)器人為研究對(duì)象，分析機(jī)器人在笛卡爾空間的直線、圓弧插值，利用ROS集成的RRT算法實(shí)現(xiàn)機(jī)器人無碰撞路徑規(guī)劃，進(jìn)行避障運(yùn)動(dòng)測試。

1 ROS機(jī)器人操作系統(tǒng)

ROS是Willow Garage公司2010年發(fā)布的開源機(jī)器人操作系統(tǒng)，能夠提供類似傳統(tǒng)操作系統(tǒng)的諸多功能，采用分布式架構(gòu)，通過各功能獨(dú)立的節(jié)點(diǎn)(Node)傳遞消息實(shí)現(xiàn)任務(wù)的分層次運(yùn)行，憑借著點(diǎn)對(duì)點(diǎn)的設(shè)計(jì)、多語言支持、免費(fèi)且開源、代碼易拓展性等優(yōu)點(diǎn)，在機(jī)器人領(lǐng)域得到了廣泛的應(yīng)用和研究[5]。此外，ROS集成了很多仿真工具包(如Rviz，Gazebo，Moveit！等)，功能十分強(qiáng)大，為人機(jī)交互和機(jī)器人運(yùn)動(dòng)規(guī)劃提供了極大的便利。

2 仿真平臺(tái)搭建

2.1 機(jī)器人運(yùn)動(dòng)學(xué)建模

在SolidWorks中建立六關(guān)節(jié)機(jī)器人模型，并建立關(guān)節(jié)坐標(biāo)系如圖1所示。機(jī)器人機(jī)械臂D-H參數(shù)如表1所示。

圖1 機(jī)器人建模

利用旋轉(zhuǎn)運(yùn)動(dòng)和平移運(yùn)動(dòng)描述相鄰坐標(biāo)系i-1和i之間的位姿關(guān)系,從而建立齊次變換矩陣[6]，描述如式(1)所示：

i-1Ti=RX(αi-1)DX(αi-1)RZ(θi)DZ(di),

(1)

i-1Ti的通用表達(dá)式為：

(2)

式中,cθi=cosθi，sθi=sinθi，cαi=cosαi，sαi=sinαi,機(jī)器人的運(yùn)動(dòng)學(xué)方程為：

0T6=0T11T22T33T44T55T6,

(3)

聯(lián)立式(2)、式(3)并代入表1的D-H參數(shù)，即可得機(jī)器人末端相對(duì)于基座的位姿。

表1 機(jī)械臂D-H參數(shù)

2.2 獲取URDF文件

在SolidWorks中建立機(jī)器人模型后，可以用ROS官方的Sw2urdf插件來生成URDF文件，需要在其中定義各個(gè)連桿的相對(duì)關(guān)系以及關(guān)節(jié)軸的位置、速度等一些物理參數(shù)的取值范圍，然后直接生成URDF模型，整個(gè)過程如圖2所示。

在ROS中使用可視Check_urdf工具可以生成機(jī)器人連桿的父子級(jí)關(guān)系，機(jī)器人連桿關(guān)系如圖3所示。圖3能清晰地顯示機(jī)器人的結(jié)構(gòu)。

2.3 Moveit!運(yùn)動(dòng)配置

ROS MoveIt！是專門為可移動(dòng)操作平臺(tái)開發(fā)的軟件包，它集成了運(yùn)動(dòng)規(guī)劃、三維感知、運(yùn)動(dòng)控制領(lǐng)域的最新成果，提供了一個(gè)開發(fā)高級(jí)機(jī)器人應(yīng)用的軟件平臺(tái)。ROS Movelt！整體框架如圖4所示。由圖4可知，在MoveIt！架構(gòu)中，Move_Group節(jié)點(diǎn)處于核心地位，此節(jié)點(diǎn)作為一個(gè)“管理器”，將各個(gè)接口組合到一起，Move_Group使用ROS topic和action的方式與機(jī)器人通信，獲取機(jī)器人的實(shí)時(shí)狀態(tài)和傳感器數(shù)據(jù)等信息，通過FollowJointTrajectoryAction接口控制機(jī)器人運(yùn)動(dòng)。

MoveIt!設(shè)置助手MoveIt Setup Assistant會(huì)根據(jù)導(dǎo)入的URDF機(jī)器人模型生成Semantic Robot Description Format(SRDF)文件，運(yùn)動(dòng)學(xué)配置文件和運(yùn)動(dòng)規(guī)劃庫配置文件以及相應(yīng)的啟動(dòng)文件，完成機(jī)器人的配置、可視化和仿真等工作，其配置流程如圖5所示。

圖2 URDF文件生成

圖4 ROS MoveIt！整體框架

圖5 MoveIt!配置流程

生成的配置文件中主要有config和launch文件，其中config文件下的.srdf記錄了所有的配置信息，虛擬關(guān)節(jié)用來設(shè)置機(jī)器人與世界坐標(biāo)的相對(duì)位置，這里將base_link設(shè)為虛擬關(guān)節(jié)，表示固定在世界坐標(biāo)系中；規(guī)劃組可以將機(jī)器人的多個(gè)連桿和關(guān)節(jié)集成到一個(gè)組中，運(yùn)動(dòng)規(guī)劃器會(huì)針對(duì)一個(gè)規(guī)劃組完成特定任務(wù)，配置時(shí)需要選擇相應(yīng)的運(yùn)動(dòng)學(xué)求解器(這里選的是KDL求解器，相關(guān)信息生成在kinematic.yaml中)，這里創(chuàng)建了2個(gè)規(guī)劃組，分別是arm_group和gripper_group。joint_limits.yaml記錄了機(jī)器人各關(guān)節(jié)的運(yùn)動(dòng)參數(shù)(位置、速度、加速度)，與URDF模型描述文件中的一致，fake_controllers.yaml為仿真搭建了虛擬控制器。

3 笛卡爾空間位置規(guī)劃仿真

空間很多曲線都可以分割為多段直線或圓弧，這是機(jī)器人笛卡爾空間下軌跡規(guī)劃最基本的組成部分。在MoveIt!中，選擇Kinematics and Dynamics Library(KDL)求解器，通過數(shù)值迭代求解機(jī)器人逆運(yùn)動(dòng)學(xué)。

3.1 空間直線軌跡規(guī)劃

圖6 笛卡爾空間直線軌跡

空間直線的軌跡規(guī)劃是已知直線起始點(diǎn)的位置和姿態(tài)，按照插補(bǔ)算法求直線軌跡上中間點(diǎn)(插補(bǔ)點(diǎn))的位置和姿態(tài)。

已知機(jī)器人末端執(zhí)行器起始點(diǎn)P1(x1,y1,z1)和終止點(diǎn)P2(x2,y2,z2)，并且以直線從P1點(diǎn)運(yùn)動(dòng)到P2點(diǎn)，笛卡爾空間直線軌跡如圖6所示。

設(shè)定笛卡爾空間中直線插值運(yùn)動(dòng)笛卡爾空間直線軌跡最大速度和加速度分別為vmax和a，則可計(jì)算出勻速運(yùn)動(dòng)時(shí)間和加速時(shí)間tvm和ta，在任意時(shí)刻t∈[0,T]，當(dāng)前位置距離初始位置P1的位移為VSt，在笛卡爾空間中的基坐標(biāo)系(參考坐標(biāo)系)下，使用矢量對(duì)起始點(diǎn)P1和終止點(diǎn)P2進(jìn)行描述[6]：

P1=x1i+y1j+z1k,

(4)

P2=x2i+y2j+z2k,

(5)

則矢量P21可表示為：

P21=P2-P1=(x2-x1)i+(y2-y1)j+(z2-z1)k,

(6)

空間直線的長度為：

(7)

設(shè)機(jī)器人末端執(zhí)行器從起始點(diǎn)P1以速度v運(yùn)動(dòng)到終止點(diǎn)P2的總時(shí)間為T，則任意時(shí)刻t∈[0,T]機(jī)器人末端執(zhí)行器的位置可表示為:

(8)

在ROS/RViz中的仿真結(jié)果如圖7所示。

圖7 直線插值運(yùn)動(dòng)仿真

3.2 空間圓弧軌跡規(guī)劃

空間中不共線的3點(diǎn)可以確定唯一一個(gè)圓，設(shè)笛卡爾空間不共線的3點(diǎn)為P1(x1,y1,z1)、P2(x2,y2,z2)、P3(x3,y3,z3)，機(jī)器人以圓弧從P1經(jīng)過P2運(yùn)動(dòng)到P3。

(1)求圓心坐標(biāo)N(x0,y0,z0)和圓弧半徑R[7-9]。

過P1、P2、P3的平面方程為：

(9)

記為：

Ax+By+Cz+D=0,

(10)

其中

(11)

在笛卡爾空間圓弧的軌跡規(guī)劃中，為了計(jì)算簡便，需要先在圓弧所在平面新建一個(gè)坐標(biāo)系N，原點(diǎn)為圓弧中心，在這新的直角坐標(biāo)系中計(jì)算出圓弧的各插補(bǔ)點(diǎn)在新坐標(biāo)系中的值，然后通過運(yùn)動(dòng)學(xué)關(guān)系將這些值映射到參考坐標(biāo)系，記參考坐標(biāo)系為B，從而得出各插補(bǔ)點(diǎn)在參考坐標(biāo)系下的值。圓弧中心坐標(biāo)可由3個(gè)平面(圓弧所在平面、P1P2中垂面、P2P3中垂面)的交點(diǎn)得出。

過P1P2中點(diǎn)且與P1P2垂直的平面方程為：

(12)

過P2P3中點(diǎn)且與P2P3垂直的平面方程為：

(13)

聯(lián)立3個(gè)平面方程可得圓心坐標(biāo)N(x0,y0,z0)，從而圓弧半徑：

(14)

(2)坐標(biāo)變換，建立新坐標(biāo)系N(N-UVW)，如圖8所示，以基坐標(biāo)系表示坐標(biāo)系N。

圖8 笛卡爾空間圓弧軌跡

(15)

(16)

由右手定則可知V軸在U軸和W軸的叉乘方向，其單位向量為：

v=u×w,

(17)

由基坐標(biāo)系到N-UVW的姿態(tài)變換矩陣為：

(18)

位置向量為：

BPN=(x0,y0,z0),

(19)

坐標(biāo)系N相對(duì)于基坐標(biāo)系B的位姿矩陣為：

(20)

笛卡爾空間圓弧的軌跡通過矩陣變換，可轉(zhuǎn)化為新坐標(biāo)系N中的一個(gè)二維平面圓，設(shè)圓弧上任意一點(diǎn)P(x,y,z),∠P1NP=θ,點(diǎn)P在坐標(biāo)系N下的坐標(biāo)向量為：

(21)

(22)

從而轉(zhuǎn)化為在基坐標(biāo)系下對(duì)圓弧進(jìn)行描述，P的軌跡點(diǎn)組成了笛卡爾空間圓弧的軌跡，在ROS中的圓弧軌跡規(guī)劃仿真結(jié)果如圖9所示。

圖9 圓弧插值運(yùn)動(dòng)仿真

4 避障規(guī)劃

4.1 RRT算法原理

對(duì)于高維空間和復(fù)雜約束條件下的運(yùn)動(dòng)規(guī)劃問題，一般采用快速搜索隨機(jī)樹(Rapidly-exploring Random Trees，RRT)算法，該算法基于隨機(jī)采樣進(jìn)行規(guī)劃，通過對(duì)采樣點(diǎn)進(jìn)行碰撞檢測，把搜索方向?qū)驘o碰撞區(qū)域，從而規(guī)劃出一條從初始點(diǎn)到目標(biāo)點(diǎn)的無碰撞路徑，尤其適合多自由度機(jī)器人在復(fù)雜環(huán)境和動(dòng)態(tài)環(huán)境中的運(yùn)動(dòng)規(guī)劃問題[10]，其原理如圖10所示。RRT算法的執(zhí)行流程如圖11所示。

圖10 RRT算法原理

圖11 RRT算法執(zhí)行流程

(1)將初始點(diǎn)作為根節(jié)點(diǎn)，初始化根節(jié)點(diǎn)qinit；

(2)在狀態(tài)空間隨機(jī)采樣一個(gè)點(diǎn)qrand；

(3)判斷是否搜索超時(shí)；

(4)在已生成的隨機(jī)樹中選擇一個(gè)距離qrand最近的節(jié)點(diǎn)qnearest；

(5)qnearest以步長ε向qrand擴(kuò)展一段距離，得到新節(jié)點(diǎn)qnew；

(6)檢測qnew是否與障礙物發(fā)生碰撞，若不發(fā)生碰撞，則進(jìn)行隨機(jī)樹的生長，否則，放棄本次生長；

(7)重復(fù)步驟(2)，直到qnearest與qgoal的距離小于設(shè)定的閾值；

(8)返回規(guī)劃結(jié)果。

偽代碼如下：

Function Build_RRT:BOOL(env:environment，T:RRT_Tree，qgoal:node)

varqrand，qnearest，qnew:node

while (search_time) do

qrand=Choose_Rand()

qnearest=Nearest(qrand，T)

if (Distance(qnearest，qgoal)

return T

qnew=Extend(qnearest，qrand，ε)

if (qnew≠NULL) then

T.Add_Node(qnew)

return false

4.2 算法測試

在ROS Rviz中，添加MotionPlanning插件，選擇OMPL運(yùn)動(dòng)規(guī)劃庫中的RRT算法，利用Python定義障礙物的尺寸并設(shè)置障礙物相對(duì)于機(jī)器人參考坐標(biāo)系的位置，桌面高0.9 m，厚度0.05 m，兩長方體大小一致，長寬高分別是0.1 m、0.05 m、0.4 m，如下所示：

#設(shè)置桌面的高度

table_ground=0.9

#設(shè)置table、box1和box2的三維尺寸

table_size=[1,0.5,0.05]

box1_size=[0.1,0.05,0.4]

box2_size=[0.1,0.05,0.4]

設(shè)定機(jī)器人運(yùn)動(dòng)的起始點(diǎn)A(0，1.05，1.28)和目標(biāo)點(diǎn)的位置B(0.5，1，1.25)，機(jī)器人從兩障礙物正中間開始運(yùn)動(dòng)，繞過左邊障礙物運(yùn)動(dòng)至目標(biāo)點(diǎn)，運(yùn)行Python腳本執(zhí)行RRT算法無碰撞路徑規(guī)劃，仿真結(jié)果如圖12所示。

圖12 RRT無碰撞路徑規(guī)劃

5 結(jié)論

基于ROS平臺(tái)，在SolidWorks中建立機(jī)器人模型，通過Sw2urdf插件將機(jī)器人3D模型轉(zhuǎn)化為ROS下的urdf模型，通過MoveIt!和Rviz仿真工具實(shí)現(xiàn)了笛卡爾空間下的直線和圓弧軌跡規(guī)劃。同時(shí)，基于RRT算法進(jìn)行無碰撞路徑規(guī)劃，仿真結(jié)果表明，機(jī)器人能成功繞開障礙物運(yùn)動(dòng)到目標(biāo)點(diǎn)，驗(yàn)證了算法的有效性。未來的研究工作將集中在機(jī)器人復(fù)雜環(huán)境下的無碰撞路徑優(yōu)化問題，減小關(guān)節(jié)電機(jī)運(yùn)行時(shí)間并降低能量消耗。