基于ROS平臺的六自由度機械臂軌跡規劃及仿真研究

劉宇程，姜振華，胡 俊

（東華大學 機械工程學院，上海 201620）

0 引言

近年來，機械臂在工業生產中所起的作用越來越大，逐漸改變生產和生活方式，雖然當前機器人技術不斷提高，也很成熟，但隨著機器人功能的日益豐富，機器人編程技術會更加復雜且繁瑣[1]，在這種情況下，將機器人軌跡規劃與機器人操作系統及仿真平臺結合起來，這能給機器人技術研究提供新的方法。目前大多數商業機器人仿真平臺不開源，且成本較高，不利于研究，而開源機器人操作系統ROS具有編程語言豐富特點，用戶可根據自己的要求在該平臺上開發出自已功能算法，完全符合軌跡規劃的要求。

基于ROS平臺，利用其所有的功能包方便快速地對埃夫特機械臂進行建模，控制，仿真。根據ROS的功能優勢，搭建了六自由度機械臂的仿真平臺，并在該平臺下對機械臂的三種軌跡進行了實驗，通過其中的運動規劃庫完成了機械臂在笛卡爾空間下的運動規劃仿真。

1 ROS及其模型配置

1.1 三維可視化平臺Rviz

Rviz是一款三維可視化工具，很好的兼容了各種基于ROS軟件框架的機器人平臺。通過該工具，能夠快速方便地進行機器人的相關開發，而且ROS是一款通用的分布式機器人軟件開發框架，各功能模塊通過松耦合的方式組合，互不干涉，許多應用功能代碼都能夠重復利用，可以有效提高工作效率[2]。在Rviz中，可以使用XML對機器人、周圍物體等任何實物進行尺寸、質量、位置、材質、關節等屬性的描述，并且在界面中呈現出來。同時Rviz顯示機器人有關信息如機器人的運動狀態、位置變化，以及周圍環境的變化等[3]。Rviz集成了許多接口，利用其中的點云庫將要規劃的軌跡信息顯示出來，通過MotionPlanning插件將機械臂運動過程的軌跡與軌跡信息比較，可以觀察機械臂運動是否準確。由于Rviz是基于Qt開發的，且很多功能插件都是集成在該工具上的的，開發者可在Rviz上開發自己的功能插件以及人機交互界面。

1.2 機器人模型建立

URDF(統一機器人描述格式)是一種基于XML規范、用于描述機器人結構的格式。以ROS中Rviz可視化仿真界面為基礎，利用該三維仿真平臺的特性，引入話題通信機制，實現機器人模型和軌跡信息的導入。根據六自由度機器人的結構特點，通過SolidWorks三維軟件建立機器人仿真模型，選擇該軟件的SW2URDF插件進行加載，同時設定每個連桿和關節的參數如慣性矩，轉動慣量，直角坐標以及坐標原點，這對于后面仿真和軌跡規劃分析至關重要。通過導入獲得URDF文件，建立了機器人和周圍的可視化虛擬環境。如圖1所示。

圖1 可視化虛擬環境

1.3 機械臂控制功能包MoveIt!及其配置

move_group是MoveIt!中的核心[4]，move_group架構圖如圖2所示。開發者可以通過move_group與其他接口連接，如C++、python接口，MotionPlanning插件。通過MotionPlanning插件，開發者可設計有關運動規劃算法驅動機械臂。在驗證URDF文件正確之后運用MoveIt!工具包對該文件進行相應的配置，通過MoveIt!配置助手獲得一個完整的運動配置功能包。經過配置后生成的功能包包含launch文件和運動規劃所需的配置文件。利用運動規劃器，運動學求解器找到一條到達目標姿態的較優路徑，在沒有機器人實體的情況下同時添加ArbotiX關節控制器，從而完成模型的運動仿真。

圖2 架構圖

2 機械臂軌跡規劃

2.1 直線軌跡

機械臂末端從初始點P0運動到終點P1，在笛卡爾坐標系下定義該直線路徑為：

s∈[0,1]保證機械臂末端從P0經過中間一系列點到P1，且是以直線方式運動。這里的s可以看作是關于時間t的函數，隨時間的增加完成規劃。同時也可看作步距，隨步距增加，得到下一個空間點直至達到終點[5]。在ROS中利用Rviz進行直線規劃的仿真，首先給定起始點P0和目標點P1，利用Rviz中的功能模塊進行仿真，仿真如圖3所示。

圖3 直線插補

直線規劃是在笛卡爾空間下進行的，將其轉換為關節空間下的各關節變化，關節狀態信息可通過訂閱/joint_states話題，再通過rqt_plot 插件將關節隨時間變化的曲線繪制出來，如圖4所示。將生成的末端位置和理論位置相比較，得到誤差曲線，如圖5所示。

圖4 關節位置變化

圖5 誤差曲線

2.2 圓弧軌跡

圓弧插補的實現流程為，將空間點轉換到三個點形成的平面，將三維問題轉換為二維。然后計算圓弧角，并在該平面上進行插補，最后通過變換矩陣，將插補點從二維坐標轉換為三維坐標[6]。給定空間中3點P1，P2，P3構造新的坐標系P1-UVW，以P1為原點，為U軸，以該三點平面法向量為W軸，則根據叉乘得到V軸表達式，三軸表達式如下得到：

得到新坐標后則P2，P3的坐標位置分別為(P2x，P2y)=((P2-P1)?u,0)，(P3x,P3y)=((P3-P1)?u,(P3-P1)?v)，由三點位置可知圓心坐標為：

從而得到圓弧半徑，以圓心P0為原點，得到平面M新坐標系P0-UVW，根據平面方程可得法向量：

根據變換矩陣，得到P1，P2，P3在世界坐標系中的表達式：

按照逆時針插補順序，在新坐標系下由P1，P3計算出該兩點的夾角θ13得到插補過程中插補點的變化坐標：

此處N為插補點個數，由上述公式得到各點的坐標還要通過變換矩陣得到在世界坐標系中的表達。結合末端執行器的姿態歐拉角，可得到末端執行器的姿態變化，在ROS中實現的圓弧插補仿真如圖6所示。

圖6 空間圓弧插補

通過rqt_plot插件繪制出的關節角度變化曲線如圖7所示，圓弧軌跡規劃中，6個關節角度均出現變化，從圖中可以看出，關節變化平滑。將ROS中生成的末端位置和理論插值位置相比較，得到誤差曲線，如圖8所示。

圖7 圓弧插補關節位置變化

圖8 誤差曲線

2.3 非均勻有理B樣條軌跡

非均勻有理B樣條（NURBS）公式如下：

式中Pi為控制點，i為序號，為p次規范B樣條基函數，且滿足以下條件：

非均勻有理B樣條由(m=n+p+1)個節點U={u0,u1,…,um}，(n+1)個控制節點P={P0,P1,…,Pm}組成，通過給定相應節點矢量，根據公式得到C(u)，從而獲得曲線方程。對于求C(u)的k階導數，令，由公式：

由萊布尼茲求導公式知A(u)的k階導數，得到C(u)k階導：

根據指定控制點Pi得到NURBS曲線軌跡后，對其進行軌跡規劃，這就要求根據指定速度控制規劃周期性選取插補點，這里涉及到時間t的函數，但是NURBS曲線是關于u的函數，所以要計算出插補點對應的u值，從而得到插補點對應的坐標值。

結合前人的工作，選取泰勒級數二階展開式進行插補，MiChing Tsai等人對這種方法進一步做了改進，提出了一種基于“預測－校正”的算法，并在算法的收斂性上給出了證明[7]。該法通過給定u的一個初值預測下個周期的u值，再通過當前速度與期望速度間的誤差來矯正u值，從而滿足速度規劃要求。

2.3.1 NURBS曲線長度求取

得到NURBS曲線長度是進行速度規劃的前提，根據Simpson自適應積分方法[8]求得該曲線長度，從C(u1)到C(u2)的曲線長度為：

得到每個區間的曲線長度，然后求得總和便是整個曲線的長度。

3.3.2 速度規劃

在進行速度規劃算法時，要求具有良好的平滑性，所以通常用多項式函數法，T曲線，指數曲線和S曲線加減速算法等，以T曲線為例，設Ts為插補周期，Ta為加速時間，n=S/(vmax*Ts)則其速度方程為：

給定曲線長度，最大速度，最大加速度即能實現速度規劃。

2.3.3 預測-矯正

根據前述的泰勒展開式對下一個插補周期的u值預測：

泰勒展開式對uk+1進行預測，但是計算量是比較大的[9]，為了減少計算量，采用向后差分方程來計算：

代入泰勒展開式可得uk+1=3uk-3uk-1+uk-2，給定u0值，通過泰勒展開式得到u1，u2，在利用上式進行迭代。但通過上式僅僅是預測出下一個插補周期的uk+1還需要通過速度去矯正該預測點。通過求得當前速度Vk，與期望速度相比較，得到較為準確的uk+1[10]。

其中β為矯正系數，取[0,1]之間，Vke為該點的期望速度，也即速度規劃的速度，Vk(n-1)為迭代(n-1)次得到的當前速度，迭代通過下式終止[11]：

通過ROS 中C++API 對上述算法進行仿真，給定控制點P1(0,150)，P2(-100，100)，P3(100,100)，P4(0,150)，P5(-100，200)，P6(100,200)，P7(0,150)給定權重[1,30,10,1,10,30,1],節點[0,0,0,0.25,0.5,0.5,0.75,1,1,1]，插補周期0.01s，以及最大速度100mm/s，最大加速度40mm/s2，根據自適應Simpson積分法，可估算出曲線的長度為696.619mm。根據程序得到該曲線在 ROS中實現的NURBS曲線仿真如圖9所示，通過話題機制以及rqt_plot插件，繪制出的各關節角度隨時間變化曲線如圖10所示。

圖9 仿真

圖10 關節位置變化

根據速度公式迭代得到u值，從而得到速度規劃。由速度規劃圖得知該算法有效(速度誤差小于0.01)，如圖11所示。

圖11 速度規劃

3 結語

本文通過ROS機器人平臺對六軸工業機器人進行軌跡規劃算法研究和仿真實驗，完成了在笛卡爾空間的直線和圓弧軌跡，NURBS曲線的規劃，并用Rviz進行了仿真；通過預測-矯正環節，對NURBS曲線進行速度規劃，使得速度具有平滑性，避免機器震動引起故障。結合在ROS平臺上的仿真，以及關節狀態信息變化驗證了該方法的正確性，對ROS機械臂開發具有一定的指導意義。同時利用ROS該平臺的功能優勢，有效的減小了開發難度，提高了開發效率。