基于ROS平臺(tái)的六自由度機(jī)械臂軌跡規(guī)劃及仿真研究

劉宇程，姜振華，胡 俊

（東華大學(xué) 機(jī)械工程學(xué)院，上海 201620）

0 引言

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

基于ROS平臺(tái)，利用其所有的功能包方便快速地對(duì)埃夫特機(jī)械臂進(jìn)行建模，控制，仿真。根據(jù)ROS的功能優(yōu)勢(shì)，搭建了六自由度機(jī)械臂的仿真平臺(tái)，并在該平臺(tái)下對(duì)機(jī)械臂的三種軌跡進(jìn)行了實(shí)驗(yàn)，通過其中的運(yùn)動(dòng)規(guī)劃庫(kù)完成了機(jī)械臂在笛卡爾空間下的運(yùn)動(dòng)規(guī)劃仿真。

1 ROS及其模型配置

1.1 三維可視化平臺(tái)Rviz

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

1.2 機(jī)器人模型建立

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

圖1 可視化虛擬環(huán)境

1.3 機(jī)械臂控制功能包MoveIt!及其配置

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

圖2 架構(gòu)圖

2 機(jī)械臂軌跡規(guī)劃

2.1 直線軌跡

機(jī)械臂末端從初始點(diǎn)P0運(yùn)動(dòng)到終點(diǎn)P1，在笛卡爾坐標(biāo)系下定義該直線路徑為：

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

圖3 直線插補(bǔ)

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

圖4 關(guān)節(jié)位置變化

圖5 誤差曲線

2.2 圓弧軌跡

圓弧插補(bǔ)的實(shí)現(xiàn)流程為，將空間點(diǎn)轉(zhuǎn)換到三個(gè)點(diǎn)形成的平面，將三維問題轉(zhuǎn)換為二維。然后計(jì)算圓弧角，并在該平面上進(jìn)行插補(bǔ)，最后通過變換矩陣，將插補(bǔ)點(diǎn)從二維坐標(biāo)轉(zhuǎn)換為三維坐標(biāo)[6]。給定空間中3點(diǎn)P1，P2，P3構(gòu)造新的坐標(biāo)系P1-UVW，以P1為原點(diǎn)，為U軸，以該三點(diǎn)平面法向量為W軸，則根據(jù)叉乘得到V軸表達(dá)式，三軸表達(dá)式如下得到：

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

從而得到圓弧半徑，以圓心P0為原點(diǎn)，得到平面M新坐標(biāo)系P0-UVW，根據(jù)平面方程可得法向量：

根據(jù)變換矩陣，得到P1，P2，P3在世界坐標(biāo)系中的表達(dá)式：

按照逆時(shí)針插補(bǔ)順序，在新坐標(biāo)系下由P1，P3計(jì)算出該兩點(diǎn)的夾角θ13得到插補(bǔ)過程中插補(bǔ)點(diǎn)的變化坐標(biāo)：

此處N為插補(bǔ)點(diǎn)個(gè)數(shù)，由上述公式得到各點(diǎn)的坐標(biāo)還要通過變換矩陣得到在世界坐標(biāo)系中的表達(dá)。結(jié)合末端執(zhí)行器的姿態(tài)歐拉角，可得到末端執(zhí)行器的姿態(tài)變化，在ROS中實(shí)現(xiàn)的圓弧插補(bǔ)仿真如圖6所示。

圖6 空間圓弧插補(bǔ)

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

圖7 圓弧插補(bǔ)關(guān)節(jié)位置變化

圖8 誤差曲線

2.3 非均勻有理B樣條軌跡

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

式中Pi為控制點(diǎn)，i為序號(hào)，為p次規(guī)范B樣條基函數(shù)，且滿足以下條件：

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

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

根據(jù)指定控制點(diǎn)Pi得到NURBS曲線軌跡后，對(duì)其進(jìn)行軌跡規(guī)劃，這就要求根據(jù)指定速度控制規(guī)劃周期性選取插補(bǔ)點(diǎn)，這里涉及到時(shí)間t的函數(shù)，但是NURBS曲線是關(guān)于u的函數(shù)，所以要計(jì)算出插補(bǔ)點(diǎn)對(duì)應(yīng)的u值，從而得到插補(bǔ)點(diǎn)對(duì)應(yīng)的坐標(biāo)值。

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

2.3.1 NURBS曲線長(zhǎng)度求取

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

得到每個(gè)區(qū)間的曲線長(zhǎng)度，然后求得總和便是整個(gè)曲線的長(zhǎng)度。

3.3.2 速度規(guī)劃

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

給定曲線長(zhǎng)度，最大速度，最大加速度即能實(shí)現(xiàn)速度規(guī)劃。

2.3.3 預(yù)測(cè)-矯正

根據(jù)前述的泰勒展開式對(duì)下一個(gè)插補(bǔ)周期的u值預(yù)測(cè)：

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

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

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

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

圖9 仿真

圖10 關(guān)節(jié)位置變化

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

圖11 速度規(guī)劃

3 結(jié)語(yǔ)

本文通過ROS機(jī)器人平臺(tái)對(duì)六軸工業(yè)機(jī)器人進(jìn)行軌跡規(guī)劃算法研究和仿真實(shí)驗(yàn)，完成了在笛卡爾空間的直線和圓弧軌跡，NURBS曲線的規(guī)劃，并用Rviz進(jìn)行了仿真；通過預(yù)測(cè)-矯正環(huán)節(jié)，對(duì)NURBS曲線進(jìn)行速度規(guī)劃，使得速度具有平滑性，避免機(jī)器震動(dòng)引起故障。結(jié)合在ROS平臺(tái)上的仿真，以及關(guān)節(jié)狀態(tài)信息變化驗(yàn)證了該方法的正確性，對(duì)ROS機(jī)械臂開發(fā)具有一定的指導(dǎo)意義。同時(shí)利用ROS該平臺(tái)的功能優(yōu)勢(shì)，有效的減小了開發(fā)難度，提高了開發(fā)效率。