基于ROS的六自由度機(jī)械臂軌跡規(guī)劃

劉 磊，寧 祎

（河南工業(yè)大學(xué) 機(jī)器人研究所，鄭州 450007）

當(dāng)前，機(jī)器人的功能越來越豐富，機(jī)器人系統(tǒng)更加復(fù)雜，機(jī)器人軟件的編寫復(fù)雜且繁瑣，大量的冗余代碼，復(fù)雜的建模，可移植性差的平臺(tái)，功能層次性差，仿真的通用性差，等各種問題也顯現(xiàn)出來。機(jī)器人操作系統(tǒng)可以很好地解決這些問題。基于ROS相關(guān)軟件包，可以對(duì)各類型的機(jī)器人進(jìn)行快速建模、仿真和控制[1]。在此，利用ROS搭建了1個(gè)六自由度機(jī)械臂的仿真平臺(tái)，并在該平臺(tái)下對(duì)六自由度機(jī)械臂的直線圓弧軌跡規(guī)劃進(jìn)行了實(shí)驗(yàn)。由實(shí)驗(yàn)結(jié)果可見，機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)軌跡平滑，末端位置精度滿足要求。

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

1.1 ROS系統(tǒng)的整體框架

ROS系統(tǒng)是構(gòu)建在 Ubuntu系統(tǒng)上的一種應(yīng)用程序框架。它利用Ubuntu系統(tǒng)來獲得對(duì)硬件的支持，同時(shí)在這個(gè)應(yīng)用程序框架上有開發(fā)有多種的應(yīng)用軟件功能包[2]。ROS整合了許多工具包，如：系統(tǒng)可視化工具rqt，3D可視化工具Rviz，地圖構(gòu)建與導(dǎo)航工具SLAM，機(jī)械臂仿真包MoveIt!，等。在此，主要采用ROS的3D可視化工具Rviz和機(jī)械臂仿真包MoveIt!，構(gòu)建了機(jī)器人仿真平臺(tái)。

1.2 機(jī)械臂仿真包MoveIt!

MoveIt!是一款可用于正逆運(yùn)動(dòng)學(xué)求解、集成運(yùn)動(dòng)規(guī)劃、感知檢測等方面的機(jī)器人軟件包[3]。在此使用它進(jìn)行了運(yùn)動(dòng)學(xué)的解算和運(yùn)動(dòng)規(guī)劃。MoveIt!的節(jié)點(diǎn)框架如圖1所示。

圖1 MoveIt!的節(jié)點(diǎn)框架Fig.1 MoveIt!node framework

2 機(jī)器人模型的建立

ROS系統(tǒng)中利用通用機(jī)器人描述格式URDF來表示機(jī)器人模型[4]。該文件通過ROS中的robot_state_publisher節(jié)點(diǎn)發(fā)布機(jī)器人的坐標(biāo)變換關(guān)系，以實(shí)現(xiàn)在任意時(shí)間，將點(diǎn)、向量等數(shù)據(jù)的坐標(biāo)，在2個(gè)參考系中完成坐標(biāo)變換，可以用于機(jī)器人軌跡規(guī)劃、MoveIt!等，從而實(shí)現(xiàn)機(jī)器人各關(guān)節(jié)位姿的精確跟蹤[5]。

對(duì)于在SolidWorks中建立的機(jī)器人模型，可以用ROS官方制作的sw2urdf插件來生成URDF文件，在ROS中使用可視化URDF工具可以生成機(jī)器人模型連桿、關(guān)節(jié)之間的關(guān)系和相應(yīng)位姿的樹狀圖，如圖2所示。

圖2 機(jī)器人關(guān)節(jié)、連桿樹狀圖Fig.2 Joint and link tree structure of robot

利用URDF文件，在ROS的工作空間中編寫ROS軟件包，創(chuàng)建節(jié)點(diǎn)和launch文件，在launch文件中定義啟動(dòng)的節(jié)點(diǎn)和相應(yīng)的參數(shù)[6]。機(jī)器人模型在Rviz中顯示如圖3所示。

圖3 機(jī)器人模型顯示界面Fig.3 Robot model display interface

3 仿真平臺(tái)設(shè)計(jì)

基于機(jī)器人操作系統(tǒng)所搭建的軟件框架如圖4所示。其中，MoveIt!為機(jī)械臂運(yùn)動(dòng)規(guī)劃的核心工具包，主要負(fù)責(zé)運(yùn)動(dòng)學(xué)解算[7]；OMPL為運(yùn)動(dòng)規(guī)劃算法庫，向MoveIt!提供復(fù)雜算法支持；3D可視化軟件包Rviz，可直接查看機(jī)器人的結(jié)構(gòu)和坐標(biāo)系，進(jìn)行實(shí)時(shí)仿真。

ROS平臺(tái)下，MoveIt!是機(jī)械臂仿真運(yùn)算的核心，Move_group節(jié)點(diǎn)是MoveIt!的核心節(jié)點(diǎn)，所有的信息都在這里匯集。通過解析URDF描述文件初始化機(jī)器人模型的運(yùn)動(dòng)學(xué)信息，由OMPL運(yùn)動(dòng)規(guī)劃庫生成軌跡消息包，并傳遞給控制器節(jié)點(diǎn)。控制器將數(shù)據(jù)送到ROS底層的角度軌跡控制器作為加減速控制和插補(bǔ)的輸入；控制器在進(jìn)行實(shí)時(shí)運(yùn)算時(shí)，不斷修改URDF模型，在Rviz中實(shí)時(shí)仿真[8]。實(shí)時(shí)運(yùn)算輸出的插補(bǔ)結(jié)果，也可以通過通訊節(jié)點(diǎn)輸出到真實(shí)環(huán)境中的機(jī)器人上，實(shí)現(xiàn)實(shí)時(shí)運(yùn)動(dòng)。

圖4 軟件框架Fig.4 Sofeware framework

4 仿真實(shí)驗(yàn)

在所搭建的仿真實(shí)驗(yàn)平臺(tái)上，對(duì)六自由度機(jī)械臂進(jìn)行笛卡爾空間的直線和圓弧軌跡規(guī)劃，以保證機(jī)器人快速平穩(wěn)的運(yùn)動(dòng)，實(shí)現(xiàn)滿足精度要求的軌跡。MoveIt!默認(rèn)使用KDL（kinematics and dynamics library），通過數(shù)值迭代算法求解逆運(yùn)動(dòng)學(xué)問題。該方法對(duì)于不同機(jī)器人模型的通用性較強(qiáng)。

4.1 直線軌跡規(guī)劃

已知機(jī)械臂末端起點(diǎn) P1（xa，ya，za）和終點(diǎn) P2（xb，yb，zb），并沿直線從 P1點(diǎn)運(yùn)動(dòng)到 P2點(diǎn)，插補(bǔ)的次數(shù)為N，則中間插補(bǔ)點(diǎn)的坐標(biāo)（xi+1，yi+1，zi+1）為

式中：i=1，2，3，…，N。

在建立的ROS仿真平臺(tái)中，實(shí)現(xiàn)直線插補(bǔ)運(yùn)算過程如下：先給定起點(diǎn)和終點(diǎn)，由P1運(yùn)動(dòng)到P2，規(guī)劃的直線軌跡運(yùn)動(dòng)圖5所示。采用笛卡爾空間的軌跡約束，通過逆解運(yùn)算求得各關(guān)節(jié)的變化，并繪制關(guān)節(jié)角度變化，如圖6所示。將ROS中生成的機(jī)械臂末端點(diǎn)位置和理論的差值位置進(jìn)行比較，繪制誤差曲線，如圖7所示。

圖5 直線軌跡運(yùn)動(dòng)Fig.5 Linear trajectory planning

圖6 直線軌跡關(guān)節(jié)角度變化曲線Fig.6 Curve joint angle change curve of straight line

圖7 直線軌跡規(guī)劃誤差曲線Fig.7 Error curve of linear trajectory planning

4.2 圓弧軌跡規(guī)劃

在三維空間中，給定 3 個(gè)點(diǎn) P1（x1，y1，z1），P2（x2，y2，z2），P3（x3，y3，z3），如圖8所示。 將 P1，P2，P3這 3 點(diǎn)確定的唯一平面設(shè)為α，該平面方程為

在平面α上取直線P1P2和P2P3的垂直平分線，兩線相交于點(diǎn) O1（x0，y0，z0），O1點(diǎn)即為圓弧的圓心坐標(biāo)，從而可得半徑R，也可求得弧P1P2和弧P2P3對(duì)應(yīng)的圓心角 θ1和 θ2。

圖8 笛卡爾空間的圓弧軌跡Fig.8 Arc planning in Cartesian space

由圓心坐標(biāo)建立新的坐標(biāo)系如圖8所示，便可得到UVW坐標(biāo)系相對(duì)于基坐標(biāo)系的變換矩陣為

為X軸在基坐標(biāo)系下的方向余弦；

為Y軸在基坐標(biāo)系下的方向余弦；o=a×n為Z軸在基坐標(biāo)下的方向余弦。圓弧軌跡的圓心角之和θ=θ1+θ2，插補(bǔ)的角度位移量為Δθ，總的插補(bǔ)次數(shù)為 N＝，則圓弧在UV平面的插補(bǔ)為

式中：i=1，2，3，…，N。對(duì)于平面 UV 的任意點(diǎn) P 的齊次坐標(biāo)［u v 0］T，可通過式（5）將其轉(zhuǎn)化到基坐標(biāo)系 X0Y0Z0下，即

借助于該變換矩陣公式，對(duì)新坐標(biāo)系下的圓弧軌跡點(diǎn)進(jìn)行矩陣計(jì)算，從而可以得到在笛卡爾空間上的圓弧軌跡。在ROS平臺(tái)上，實(shí)現(xiàn)的圓弧插補(bǔ)如圖9所示，關(guān)節(jié)角度變化如圖10所示。將ROS中生成的機(jī)械臂末端點(diǎn)位置和理論的差值位置進(jìn)行比較，繪制的誤差曲線如圖11所示。

5 結(jié)語

搭建了基于ROS平臺(tái)的機(jī)器人仿真平臺(tái)，利用URDF文件完成了六自由度機(jī)械臂的建模，利用

圖9 圓弧軌跡規(guī)劃Fig.9 Circular trajectory planning

圖10 圓弧軌跡關(guān)節(jié)角度變化Fig.10 Arc trajectory joint Angle change

圖11 圓弧軌跡規(guī)劃誤差曲線Fig.11 Error curve of arc trajectory planning

MoveIt!進(jìn)行了六自由度機(jī)械臂的運(yùn)動(dòng)學(xué)解算和軌跡規(guī)劃，并在Rviz上進(jìn)行了三維實(shí)時(shí)仿真模擬；完成了笛卡爾空間的直線圓弧軌跡規(guī)劃，實(shí)現(xiàn)了插補(bǔ)運(yùn)算；在仿真平臺(tái)進(jìn)行了實(shí)驗(yàn)，通過ROS顯示的各個(gè)關(guān)節(jié)的角度變化信息，可以看出六自由度機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)軌跡平滑，軌跡誤差滿足要求，驗(yàn)證了該方法的有效性。

參考文獻(xiàn)：

[1]陳盛龍，平雪良，曹正萬，等.基于ROS串聯(lián)機(jī)器人虛擬運(yùn)動(dòng)控制及仿真研究[J].組合機(jī)床與自動(dòng)化加工技術(shù)，2015，52（10）：108-111.

[2]曹正萬，平雪良，陳盛龍，等.基于ROS的機(jī)器人模型構(gòu)建方法研究[J].組合機(jī)床與自動(dòng)化加工技術(shù)，2015，52（8）：51-54.

[3]徐扣.六自由度機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解與軌跡規(guī)劃研究 [D].廣州：廣東工業(yè)大學(xué)，2016.

[4]Hoske M T.ROS Industrial aims to open unify advanced robotic programming[J].Control Engineering，2013，60（2）：20-21.

[5]鄭梓均.基于ROS系統(tǒng)的簡易服務(wù)機(jī)器人關(guān)鍵技術(shù)的研究[D].無錫：江南大學(xué)，2016.

[6]JOSEPH L.Mastering ROS for robotics rrogramming[M].Birmingham：Packet Publishing Ltd，2015.

[7]Chitta S，Sucan I，Cousins S.“Moveit![ROS topics]”[J].Robotics&Automation Magazine IEEE，2012（19）：18-19.

[8]左軒塵，韓亮亮，莊杰，等.基于ROS的空間機(jī)器人人機(jī)交互系統(tǒng)設(shè)計(jì)[J].計(jì)算機(jī)工程與設(shè)計(jì)，2015，36（12）：3370-3374.