郭建根，雙 豐，闞宏林

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

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

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

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

2 仿真平臺搭建

2.1 機器人運動學(xué)建模

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

圖1 機器人建模

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

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

(1)

i-1Ti的通用表達式為：

(2)

式中,cθi=cosθi，sθi=sinθi，cαi=cosαi，sαi=sinαi,機器人的運動學(xué)方程為：

0T6=0T11T22T33T44T55T6,

(3)

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

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

2.2 獲取URDF文件

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

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

2.3 Moveit!運動配置

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

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

圖2 URDF文件生成

圖4 ROS MoveIt！整體框架

圖5 MoveIt!配置流程

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

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

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

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

圖6 笛卡爾空間直線軌跡

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

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

設(shè)定笛卡爾空間中直線插值運動笛卡爾空間直線軌跡最大速度和加速度分別為vmax和a，則可計算出勻速運動時間和加速時間tvm和ta，在任意時刻t∈[0,T]，當前位置距離初始位置P1的位移為VSt，在笛卡爾空間中的基坐標系(參考坐標系)下，使用矢量對起始點P1和終止點P2進行描述[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è)機器人末端執(zhí)行器從起始點P1以速度v運動到終止點P2的總時間為T，則任意時刻t∈[0,T]機器人末端執(zhí)行器的位置可表示為:

(8)

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

圖7 直線插值運動仿真

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

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

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

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

(9)

記為：

Ax+By+Cz+D=0,

(10)

其中

(11)

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

過P1P2中點且與P1P2垂直的平面方程為：

(12)

過P2P3中點且與P2P3垂直的平面方程為：

(13)

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

(14)

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

圖8 笛卡爾空間圓弧軌跡

(15)

(16)

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

v=u×w,

(17)

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

(18)

位置向量為：

BPN=(x0,y0,z0),

(19)

坐標系N相對于基坐標系B的位姿矩陣為：

(20)

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

(21)

(22)

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

圖9 圓弧插值運動仿真

4 避障規(guī)劃

4.1 RRT算法原理

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

圖10 RRT算法原理

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

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

(2)在狀態(tài)空間隨機采樣一個點qrand；

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

(4)在已生成的隨機樹中選擇一個距離qrand最近的節(jié)點qnearest；

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

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

(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運動規(guī)劃庫中的RRT算法，利用Python定義障礙物的尺寸并設(shè)置障礙物相對于機器人參考坐標系的位置，桌面高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è)定機器人運動的起始點A(0，1.05，1.28)和目標點的位置B(0.5，1，1.25)，機器人從兩障礙物正中間開始運動，繞過左邊障礙物運動至目標點，運行Python腳本執(zhí)行RRT算法無碰撞路徑規(guī)劃，仿真結(jié)果如圖12所示。

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

5 結(jié)論

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