霍寒旭，袁昌耀，曹秀芳，楊秀萍，喬志峰，王收軍

（1.天津理工大學(xué)機(jī)械工程學(xué)院，天津市先進(jìn)機(jī)電系統(tǒng)設(shè)計(jì)與智能控制重點(diǎn)實(shí)驗(yàn)室，機(jī)電工程國(guó)家級(jí)實(shí)驗(yàn)教學(xué)示范中心，天津300384；2.南京晨光集團(tuán)有限公司，南京210006）

開源機(jī)器人操作系統(tǒng)ROS 可以有效地處理這些問題，成為當(dāng)下熱門的機(jī)器人研究平臺(tái).

本文基于ROS 平臺(tái)，以某七自由度液壓重載機(jī)械臂為研究對(duì)象進(jìn)行避障仿真.

1 機(jī)械臂運(yùn)動(dòng)學(xué)分析

圖1 為機(jī)械臂的運(yùn)動(dòng)示意圖.

機(jī)械臂的模型可看作由多個(gè)彼此關(guān)聯(lián)的運(yùn)動(dòng)關(guān)節(jié)組成.正運(yùn)動(dòng)學(xué)問題的計(jì)算即為通過各關(guān)節(jié)的位置變換得到末端執(zhí)行器的位置與姿態(tài)的過程[1].建立該機(jī)械臂的D-H 坐標(biāo)系，如圖2 所示，用D-H 參數(shù)來描述各關(guān)節(jié)位置變換的關(guān)系，由此計(jì)算機(jī)械臂的正運(yùn)動(dòng)學(xué)問題[2-3].

圖2 中X0Y0Z0為基坐標(biāo)系，X7Y7Z7為末端執(zhí)行器的坐標(biāo)系.機(jī)械臂的D-H 參數(shù)表如表1 所示.

表1 中，di為相鄰關(guān)節(jié)間的距離，ai為連桿的長(zhǎng)度，αi為連桿相對(duì)上個(gè)連桿的扭角，θi為關(guān)節(jié)轉(zhuǎn)角范圍.

表1 中的參數(shù)由機(jī)械臂相鄰關(guān)節(jié)的相對(duì)位置得到，用于計(jì)算機(jī)械臂的正逆解.實(shí)現(xiàn)正逆解求解可以利用ROS 平臺(tái)中MoveIt! 工具中的RRT 算法得到.

圖1 機(jī)械臂的運(yùn)動(dòng)示意圖Fig.1 The motion diagram of manipulator

圖2 機(jī)械臂坐標(biāo)系Fig.2 The coordinate of manipulator

表1 機(jī)械臂的D-H 參數(shù)表Tab.1 The D-H parameters of manipulator

式中，cθi表示cosθi，sθi表示sinθi.

式（2）中，n，o，a表示末端執(zhí)行器相對(duì)于基坐標(biāo)的姿態(tài)，即n=[nx，ny，nz]T，o=[ox，oy，oz]T，a=[ax，ay，az]T分別為末端執(zhí)行器在基坐標(biāo)中的X軸方向，Y軸方向和Z軸方向，p=[px，py，pz]T示末端執(zhí)行器相對(duì)于基坐標(biāo)的位置關(guān)系.矩陣由各關(guān)節(jié)積累獲得，表示從基坐標(biāo)變換為末端經(jīng)過的位置變換與姿態(tài)改變.將表1 中的各關(guān)節(jié)變量代入式（2），即可得機(jī)械臂的從基坐標(biāo)到末端執(zhí)行器的位姿改變.

2 仿真功能包配置

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

ROS 以Linux 系統(tǒng)的發(fā)行版Ubuntu 為基礎(chǔ)，充分利用了該系統(tǒng)運(yùn)行和管理的特點(diǎn).為了提升機(jī)器人領(lǐng)域的研發(fā)效率，ROS 對(duì)機(jī)器人許多現(xiàn)已存在項(xiàng)目的代碼進(jìn)行了存儲(chǔ).在ROS 的庫(kù)中能找到大部分復(fù)雜的代碼，只有一小部分的代碼用于展示ROS 庫(kù)的各種功能并且創(chuàng)建了一些小的應(yīng)用.ROS 提供許多公開的包，如創(chuàng)建地圖包、定位算法包等，適用于各種軟件及硬件[4].

ROS 集成了眾多簡(jiǎn)單實(shí)用的工具以及先進(jìn)的機(jī)器人算法，相較于其它系統(tǒng)有著許多鮮明的特點(diǎn).它的中立性框架使得C++、Python、Octave 和Lisp 等編程語言都可以在ROS 系統(tǒng)中使用.再借由接口定義語言，使得ROS 能夠利用交叉語言實(shí)現(xiàn)模塊之間的消息傳送.ROS 操作系統(tǒng)提供許多開源工具來編譯和運(yùn)行不同的ROS 組件，管理復(fù)雜的軟件架構(gòu). 如機(jī)器人三維物理仿真工具Gazebo、機(jī)械臂運(yùn)動(dòng)規(guī)劃工具M(jìn)oveIt、可視化工具Rviz 等. ROS 系統(tǒng)是免費(fèi)且開源的，這有利于ROS 軟件層面的校正并可被用作多種用途.

2.2 URDF 文件

URDF（Unified Robot Description Format），是一種在ROS 中大量使用的特殊的xml 文件[5].它可以描述機(jī)器人各個(gè)關(guān)節(jié)的基本信息、連接關(guān)系等，確定機(jī)械臂模型的基坐標(biāo)及各關(guān)節(jié)的坐標(biāo)系，同時(shí)支持模型的干涉碰撞檢查.本文使用SolidWorks 建立液壓重載機(jī)械臂的模型，在不改變模型結(jié)構(gòu)情況下對(duì)模型中螺栓等緊固零件進(jìn)行簡(jiǎn)化以減少計(jì)算量，這對(duì)仿真精度的影響可以忽略.利用SW2urdf 插件，設(shè)置各個(gè)關(guān)節(jié)的運(yùn)動(dòng)類型和參數(shù)，然后將模型導(dǎo)入ROS 環(huán)境中生成可用的URDF 文件.

2.3 配置功能包

MoveIt! 是ROS 中進(jìn)行機(jī)械臂避障規(guī)劃的重要工具，由一系列移動(dòng)操作的功能包組成了的運(yùn)動(dòng)規(guī)劃庫(kù)，可提供運(yùn)動(dòng)規(guī)劃、操作控制、環(huán)境感知、正逆運(yùn)動(dòng)學(xué)求解等功能[6].使用MoveIt! 配置助手配置仿真所需的功能包流程如圖3 所示.

圖3 MoveIt! 配置流程圖Fig.3 The flow chart of configuration process for MoveIt!

在配置添加運(yùn)動(dòng)鏈時(shí)，選用KDL（The Kinematics and Dynamics Library）即運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)的庫(kù)來求解正逆運(yùn)動(dòng)學(xué)解.其優(yōu)勢(shì)在于解決串聯(lián)機(jī)械臂運(yùn)動(dòng)學(xué)數(shù)值解的問題，可應(yīng)用于機(jī)械臂正逆運(yùn)動(dòng)學(xué)解的情況[7].按照配置流程就可以生成機(jī)械臂避障仿真所需要的功能包.

3 機(jī)械臂避障仿真

3.1 避障規(guī)劃算法

對(duì)于求解過程，有兩種不同解決方案，一是尋找最優(yōu)解，使用最優(yōu)規(guī)劃；二是快速尋找有效解，使用采樣規(guī)劃.機(jī)械臂的避障規(guī)劃旨在解決存有障礙物情況下機(jī)械臂的路徑選擇問題.在滿足要求標(biāo)準(zhǔn)的情況下，尋找從起始點(diǎn)至目標(biāo)點(diǎn)的最優(yōu)解. ROS 的OMPL 是一個(gè)用于運(yùn)動(dòng)規(guī)劃的C++采樣規(guī)劃庫(kù)，包含眾多運(yùn)動(dòng)規(guī)劃可用的C++算法，并能提供模塊化的設(shè)計(jì)及穩(wěn)定更新.本文使用采樣規(guī)劃庫(kù)中中常見的RRT（Rapidly-exploring Random Trees）算法.

RRT 算法通過隨機(jī)采樣的方式，尋找從初始點(diǎn)到目標(biāo)點(diǎn)的路徑.該算法類似樹狀規(guī)劃方式，以初始點(diǎn)為根節(jié)點(diǎn)，生成隨機(jī)拓展樹向外隨機(jī)延伸[8]，當(dāng)目標(biāo)節(jié)點(diǎn)出現(xiàn)在隨機(jī)樹節(jié)點(diǎn)搜索區(qū)域時(shí)，可以沿隨機(jī)樹生成從根節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的軌跡[9-12].完成整個(gè)規(guī)劃過程，從該節(jié)點(diǎn)位置返回到初始節(jié)點(diǎn)連接成路徑.

RRT 算法的搜索方式如圖4 所示.

圖4 RRT 算法示意圖Fig.4 The schematic diagram of RRT algorithm

RRT 算法生成路徑的過程為：選定初始點(diǎn)qinit，在此基礎(chǔ)上隨機(jī)出現(xiàn)一個(gè)新的節(jié)點(diǎn)qrand.然后，選定樹節(jié)點(diǎn)中qrand 距離最近的節(jié)點(diǎn)qnearest.判定若qnearest 節(jié)點(diǎn)出現(xiàn)在目標(biāo)位置范圍內(nèi)，得出結(jié)果.否則，在此基礎(chǔ)上尋找新節(jié)點(diǎn)qnew，直到逼近目標(biāo)節(jié)點(diǎn).

算法表示如下：

BuildRRT（qinit，K，Δq）

1）T.init（qinit）

2）for k=1 to K

3）qrand=Sample（）

4）qnearest=Nearest（T，qrand）

5）if Distance（qnearest，qgoal）

6）return true

7）qnew=Extend（qnearest，qrand，Δq）

8）if qnew≠NULL then

9）T.AddNode（qnew）

10）return false

3.2 機(jī)械臂避障運(yùn)動(dòng)規(guī)劃

啟動(dòng)MoveIt! 功能包中包含模型的launch 文件，進(jìn)入Rviz 仿真界面，顯示出由SolidWorks 構(gòu)建并簡(jiǎn)化的機(jī)械臂模型.在仿真環(huán)境中，創(chuàng)建一個(gè)長(zhǎng)方體作為障礙物，進(jìn)行軌跡規(guī)劃，并在Rviz 中發(fā)布到環(huán)境中.可設(shè)置特定的姿勢(shì)作為初始位置和目標(biāo)位置，也可通過拖動(dòng)機(jī)械臂末端直接改變姿態(tài)設(shè)置初始和目標(biāo)位置.選擇RRT 算法，在當(dāng)前環(huán)境開始規(guī)劃，此過程會(huì)在終端中顯示進(jìn)程，且Rviz 仿真窗口會(huì)顯示一條機(jī)械臂可行的避障路徑.圖5 為Rviz 仿真窗口，機(jī)械臂初始姿態(tài)置于障礙物右側(cè)，目標(biāo)姿態(tài)置于左側(cè).機(jī)械臂需要繞過豎在中間的長(zhǎng)方體才能到達(dá)目標(biāo)位姿，完成避障規(guī)劃.圖6 為機(jī)械臂避障軌跡.

圖6 機(jī)械臂避障仿真軌跡Fig.6 The trajectory of obstacle avoidance simulation of manipulator

通過獲取機(jī)械臂各關(guān)節(jié)位置變化的信息，得到機(jī)械臂在運(yùn)動(dòng)過程中各關(guān)節(jié)的位置隨時(shí)間變化的曲線，該曲線記錄了機(jī)械臂各關(guān)節(jié)在避障過程中實(shí)時(shí)的位置變化情況，如圖7 所示.

圖7 關(guān)節(jié)位置-時(shí)間曲線Fig.7 Joint position-time curve

4 結(jié) 論

本文對(duì)某液壓重載機(jī)械臂進(jìn)行了運(yùn)動(dòng)學(xué)分析，運(yùn)用SolidWorks 建立了機(jī)械臂空間結(jié)構(gòu)模型，利用ROS 平臺(tái)中的URDF 文件建立仿真模型.通過配置MoveIt！工具，采用快速擴(kuò)展隨機(jī)樹算法（RRT）對(duì)機(jī)械臂的運(yùn)動(dòng)路徑進(jìn)行規(guī)劃，得到機(jī)械臂各關(guān)節(jié)位移隨時(shí)間變化的曲線，實(shí)現(xiàn)了存在障礙物時(shí)機(jī)械臂的自動(dòng)避障.本文采用的方法為在復(fù)雜障礙環(huán)境下進(jìn)一步改進(jìn)規(guī)劃算法提供了直觀便捷的方法.