趙丕洋,洪榮晶,方成剛

(1.南京工業(yè)大學(xué) 機(jī)械與動力工程學(xué)院,江蘇 南京 211800; 2.江蘇省工業(yè)裝備數(shù)字制造及控制技術(shù)重點(diǎn)實(shí)驗(yàn)室,江蘇 南京 211800)

目前,人力加工仍然是國內(nèi)機(jī)械制造行業(yè)的主流方式,但其生產(chǎn)效率低,機(jī)械加工精度較差,工人熟練度對產(chǎn)品質(zhì)量影響較大[1],且工廠的惡劣環(huán)境極大地?fù)p傷工人健康。工業(yè)機(jī)器人集電控、機(jī)械和自動化等多種學(xué)科特點(diǎn)于一體,工件裝夾方便、工作空間大、經(jīng)濟(jì)效益高,可以獨(dú)立完成機(jī)械生產(chǎn)中的各個階段,提高了加工設(shè)備的柔性。開發(fā)者們著力于提高機(jī)器人的控制精度和柔性,以提升產(chǎn)品質(zhì)量、經(jīng)濟(jì)效益和解放人力[2]。

機(jī)器人硬件不斷升級,而控制系統(tǒng)和軟件也亟需發(fā)展以適應(yīng)其要求,因此實(shí)現(xiàn)加工代碼的復(fù)用性、系統(tǒng)的模塊化和通用性已成為熱點(diǎn)問題,機(jī)器人開發(fā)者們不斷嘗試,以實(shí)現(xiàn)仿真模型和軟件系統(tǒng)的功能共享。開源機(jī)器人操作系統(tǒng)(ROS)具有獨(dú)特的優(yōu)點(diǎn),它集成了多種軟件工具包,兼容主流編程語言,為機(jī)器人開發(fā)提供了更廣闊的空間,越來越多的國內(nèi)外專家對其開展了研究。ROS 具有分布式的系統(tǒng)架構(gòu)[3], 具有支持程序的管理與信息的傳遞等功能[4],使機(jī)器人控制的加工任務(wù)和運(yùn)動更加簡化[5-6]。ROS能通過編寫底層程序,將多種不同的傳感器(力覺傳感器、相機(jī)等)的通信集成在一起,極大地推動了系統(tǒng)的集成化和模塊化開發(fā)。

1 機(jī)械臂模型建立及運(yùn)動學(xué)分析

1.1 機(jī)器人Denavit-Hartenberg(D-H)參數(shù)

以丹麥公司的經(jīng)典機(jī)械臂UR5為例,機(jī)械臂各個關(guān)節(jié)的坐標(biāo)系可以通過D-H參數(shù)法確定,根據(jù)Pieper法則,將基座、肩部和手肘3個關(guān)節(jié)建立到同一個軸線上,以簡化D-H參數(shù)[7]。查閱UR5相關(guān)使用手冊,得到UR5的相關(guān)D-H參數(shù)(表1)。搭建機(jī)器人控制平臺,對UR5機(jī)械臂進(jìn)行運(yùn)動學(xué)分析[8],建立實(shí)體模型及運(yùn)動結(jié)構(gòu),如圖1所示。

表1 UR5機(jī)械臂的D-H參數(shù)

圖1 UR5機(jī)械臂運(yùn)動模型Fig.1 Motion model of UR5 robotic arm

1.2 正運(yùn)動學(xué)分析

(1)

將各關(guān)節(jié)參數(shù)分別代入式(1),計(jì)算得到各坐標(biāo)系之間的變換矩陣,如式(2)所示。

(2)

末端坐標(biāo)變換矩陣T如式(3)所示。

(3)

在式(3)中,通過輸入6個關(guān)節(jié)角度θi(i=1～6),經(jīng)過計(jì)算可得到末端坐標(biāo)變換矩陣T為

(4)

推導(dǎo)出T中的元素值,如式(5)所示。

(5)

式中:ci=cosθi,si=sinθi,c23=cos(θ2+θ3),s23=sin(θ2+θ3),s234=sin(θ2+θ3+θ4),c234=cos(θ2+θ3+θ4)。

1.3 逆運(yùn)動學(xué)分析

根據(jù)逆運(yùn)動學(xué)理論,推算θi,為了得到全部逆解,使用數(shù)學(xué)解析法進(jìn)行公式推導(dǎo)[10],由式(4)可得

(6)

其中,中間矩陣為

(8)

(9)

(10)

將式(6)—(9)代入式(5)中,解得

(11)

接著,由式(4)推導(dǎo)得

(12)

與前面推導(dǎo)過程類似,經(jīng)過計(jì)算求出

(13)

通過分析上述逆解公式結(jié)果,關(guān)節(jié)1、3、5都分別有正負(fù)兩組解,因此該機(jī)器人一共存在2×2×2=8組解。在實(shí)際機(jī)械臂的關(guān)節(jié)指令發(fā)布時,應(yīng)以逆運(yùn)動學(xué)的解為限制依據(jù),計(jì)算機(jī)器人處于奇異位姿時的數(shù)據(jù),避免機(jī)器人運(yùn)行到奇異位姿而損害機(jī)械結(jié)構(gòu)。

2 基于ROS的仿真系統(tǒng)設(shè)計(jì)

圖2為ROS系統(tǒng)的架構(gòu),按照功能分為OS層、中間層和應(yīng)用層。由圖2可知：OS層安裝了ROS依附性最好的Ubuntu操作系統(tǒng);中間層基于TCPROS/UDPROS通信協(xié)議,實(shí)現(xiàn)數(shù)據(jù)傳輸和相應(yīng)模型的構(gòu)造;應(yīng)用層中運(yùn)行Master節(jié)點(diǎn)進(jìn)行管理,集成了多種功能應(yīng)用包,提高了系統(tǒng)開發(fā)效率[11]。

圖2 ROS架構(gòu)Fig.2 ROS architecture

2.1 創(chuàng)建機(jī)械臂模型

以D-H參數(shù)為依據(jù),建立UR5模型,添加約束條件。為導(dǎo)出統(tǒng)一機(jī)器人描述格式(URDF)文件,安裝sw_urdf_exporter插件,設(shè)置好相關(guān)參數(shù),生成功能包后導(dǎo)入到ROS系統(tǒng)中[12]。

2.2 模型可視化

成功導(dǎo)入機(jī)械臂URDF文件后,配置軟件相關(guān)參數(shù),例如機(jī)器人坐標(biāo)、轉(zhuǎn)動關(guān)節(jié)等,在rviz可視化軟件中完成機(jī)器人模型的運(yùn)動驅(qū)動設(shè)置,并規(guī)劃了隨機(jī)點(diǎn)。仿真中觀察到UR5模型穩(wěn)定無異常抖動,規(guī)劃目標(biāo)點(diǎn)后可以準(zhǔn)確運(yùn)動。

2.3 MoveIt規(guī)劃仿真分析

MoveIt被廣泛應(yīng)用在多種軌跡規(guī)劃中,其控制結(jié)構(gòu)如圖3所示。由圖3可知：針對move_group進(jìn)行控制,使機(jī)器人的軌跡控制更加簡便、高效,通過話題和服務(wù)通訊與其他獨(dú)立的功能組件交互,為開發(fā)者提供ROS中的運(yùn)動指令和服務(wù)[13]。

move_group節(jié)點(diǎn)兼容主流編程語言,可進(jìn)一步改進(jìn)機(jī)器人運(yùn)動控制系統(tǒng)的開發(fā)。將URDF文件導(dǎo)入rviz軟件中,簡單設(shè)置后,生成SRDF控制文件后，再創(chuàng)建一個包含MoveIt配置的功能包[14]。配置完成后,以點(diǎn)對點(diǎn)的方式測試機(jī)械臂軌跡規(guī)劃效果,如圖4所示。由圖4可知：機(jī)械臂始末位姿之間的控制規(guī)劃成功且路徑平滑,表明所推導(dǎo)的UR5運(yùn)動學(xué)正逆解公式正確。

圖3 move_group架構(gòu)Fig.3 Architecture of move_group

圖4 rviz機(jī)械臂運(yùn)動仿真測試Fig.4 Motion simulation test of rviz robotic arm

3 基于快速拓展隨機(jī)樹(RRT)算法的運(yùn)動規(guī)劃分析及改進(jìn)

MoveIt提供開放用戶接口,可編寫和改進(jìn)算法作為運(yùn)動規(guī)劃器,并可根據(jù)自身需求對機(jī)器人進(jìn)行合理運(yùn)動規(guī)劃,同時滿足最大速度限制、關(guān)節(jié)限位和最大加速度限制等要求。

3.1 RRT算法基本原理

RRT算法的基本搜尋策略:從初始位置開始,隨機(jī)在可行空間中采樣。以樹的形式進(jìn)行探索,對路徑進(jìn)行隨機(jī)拓展,直到覆蓋整個目標(biāo)區(qū)域,最后在狀態(tài)空間內(nèi)選取最短的一條路徑作為結(jié)果輸出[15]。與其他運(yùn)動規(guī)劃算法相比,RRT算法的實(shí)時性和可擴(kuò)展性更好,且不需要對碰撞模型進(jìn)行精確建模[16]。路徑尋優(yōu)時快速有效,可應(yīng)用于機(jī)械臂的復(fù)雜任務(wù)規(guī)劃[17]。

RRT規(guī)劃目標(biāo)點(diǎn)的原理如圖5所示[18]。圖1所示是一個二維空間,假設(shè)生成一個含有k個節(jié)點(diǎn)的隨機(jī)樹。由圖5可知：該隨機(jī)樹的根節(jié)點(diǎn)用qinit表示，新隨機(jī)搜索的節(jié)點(diǎn)用qrand表示，qnear表示生成隨機(jī)樹中最接近qrand的節(jié)點(diǎn)，d是搜索步長，L是節(jié)點(diǎn)qrand和qnear的連線距離。算法運(yùn)行后,在狀態(tài)空間內(nèi)生成隨機(jī)采樣點(diǎn)qrand,接著將qrand與qnear連線,作為節(jié)點(diǎn)生成方向,然后將兩者連接起來,若連線與障礙物產(chǎn)生了干涉,則該節(jié)點(diǎn)不會成為隨機(jī)樹的節(jié)點(diǎn),而是在空間內(nèi)重新隨機(jī)搜尋,進(jìn)行下一次擴(kuò)展;若連線與障礙物無干涉，則判斷L與d的大小關(guān)系,若L>d, 則在qnear和qrand連線上選擇距離qnear長度為d的點(diǎn)作為新節(jié)點(diǎn)qnew,若L≤d,則該qrand成為新節(jié)點(diǎn)qnew。

在樹節(jié)點(diǎn)擴(kuò)展到目標(biāo)點(diǎn)之后,RRT算法從終點(diǎn)開始反向搜索,依次查詢父節(jié)點(diǎn),如果最終查詢到起始點(diǎn),那么路徑規(guī)劃結(jié)束,并連接起始點(diǎn)和目標(biāo)點(diǎn)生成一條最優(yōu)路徑。

圖5 RRT算法拓展策略Fig.5 RRT algorithm expansion strategy

3.2 RRT算法改進(jìn)

RRT是概率完備的算法,由于其具有隨機(jī)性特征,對于相同的始末位姿,采用RRT進(jìn)行規(guī)劃得到的路徑不一定相同,因此具有很差的路徑可重復(fù)性。新節(jié)點(diǎn)是在工作空間中隨機(jī)搜索生成的,各節(jié)點(diǎn)的連線可能會出現(xiàn)劇烈的轉(zhuǎn)折,生成的規(guī)劃路徑圓滑度較低,因此RRT算法的規(guī)劃結(jié)果并不最優(yōu)，因此不能適應(yīng)實(shí)際加工的需要[19]。

在六自由度機(jī)器人工作空間的運(yùn)動規(guī)劃中,若關(guān)節(jié)運(yùn)動軌跡夾角過小,則路徑曲率半徑很小,機(jī)器人就會發(fā)生“急轉(zhuǎn)彎”，進(jìn)而引起機(jī)械臂加速度和速度的突變,不能保證機(jī)器人的平穩(wěn)運(yùn)動,且極易對機(jī)械結(jié)構(gòu)產(chǎn)生損害[20]。為提高機(jī)器人在避障過程中軌跡的圓滑性,并保證機(jī)器人在運(yùn)行過程中加速度產(chǎn)生的突變較小,縮短搜索生成路徑的時間,提出判定函數(shù)F。F將相鄰節(jié)點(diǎn)間的轉(zhuǎn)角考慮在內(nèi),拓展方式如圖6所示,表達(dá)式[20]如式(14)所示。

(14)

式中:si為節(jié)點(diǎn)qi與其父節(jié)點(diǎn)的連線位姿；sk為節(jié)點(diǎn)qi與新的隨機(jī)節(jié)點(diǎn)的連線位姿；H(si,sk)為兩種位姿的夾角，夾角越小表示機(jī)器人運(yùn)動時轉(zhuǎn)動弧度大且平緩,運(yùn)動就越平穩(wěn);D(qi,qk)為機(jī)械臂執(zhí)行裝置的位置qi到目標(biāo)位置qk的距離代價(jià),距離越短表示路徑規(guī)劃越優(yōu);Nd和Nh為歸一化函數(shù),將距離和角度統(tǒng)一量綱以進(jìn)行比較;Wd和Wh分別為節(jié)點(diǎn)間距離和節(jié)點(diǎn)間角度在綜合因素考慮中所占權(quán)重。

由圖6可知：子節(jié)點(diǎn)到q2的距離(d2)小于到q1的距離(d1),但轉(zhuǎn)角θ2遠(yuǎn)大于θ1,綜合考慮后，算法選擇q1作為回溯父節(jié)點(diǎn)并生成軌跡。

圖6 改進(jìn)RRT算法Fig.6 Improved RRT algorithm

在MATLAB軟件中編寫M文件,對比分析改進(jìn)RRT算法相較于基本RRT算法的優(yōu)越性,在仿真中設(shè)置參數(shù):生成一個100×100的搜索空間,起始點(diǎn)為(0,0),目標(biāo)點(diǎn)為(95,95),搜索步長為5,障礙物是一個20×20的方塊,設(shè)定隨機(jī)生成種子數(shù)為1 000,在改進(jìn)RRT算法中為了綜合考慮角度和距離因素,設(shè)置Wd=Wh=0.5,運(yùn)行程序并繪制路徑圖,如圖7所示。

圖7 基本RRT及改進(jìn)RRT的搜索路徑Fig.7 Basic RRT and improved RRT search path maps

由圖7可知：基本RRT算法生成的軌跡轉(zhuǎn)角較大,在機(jī)器人加工中會導(dǎo)致加速度和速度突變,造成機(jī)械結(jié)構(gòu)的損傷;改進(jìn)RRT算法綜合考慮了相鄰節(jié)點(diǎn)的角度和距離因素,明顯提高了軌跡的圓滑度,并縮短了路徑總長度,證明了理論的可行性。

4 試驗(yàn)結(jié)果分析

運(yùn)行機(jī)械臂配置文件,配置機(jī)械臂路徑規(guī)劃環(huán)境,通過MoveIt配置UR5運(yùn)動的相關(guān)驅(qū)動功能包,導(dǎo)入ROS中并進(jìn)行編譯。啟動rviz仿真軟件,對規(guī)劃過程中的參數(shù)進(jìn)行配置,利用改進(jìn)RRT算法程序規(guī)劃機(jī)械臂路徑,目標(biāo)偏差參數(shù)若設(shè)置太大則精度不夠,過小則會導(dǎo)致運(yùn)動器循環(huán)求解,因此設(shè)置目標(biāo)偏差為0.01。

在rviz中導(dǎo)入正方體模型作為障礙物,設(shè)置起始位姿進(jìn)行運(yùn)動規(guī)劃仿真,具體運(yùn)動路徑如圖8所示,規(guī)劃路徑完美避過了正方體障礙,且軌跡平滑,路徑距離也較優(yōu)。

圖8 機(jī)械臂避障路徑規(guī)劃路線Fig.8 Robotic arm obstacle avoidance path planning route

選擇PC端作為上位機(jī),通過TCP協(xié)議作為通訊方式,連接機(jī)器人控制柜,設(shè)置PC網(wǎng)址為192.168.1.101,機(jī)器人網(wǎng)址為192.168.1.103,輸入命令行:

＄ roslaunch ur_modern_driver ur5_ros_control.launch robot_ip:=192.168.1.103

＄ roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch

＄ roslaunch ur5_moveit_config moveit_rviz.launch config:=true。

在rvzi仿真可視化界面中設(shè)置起始位姿進(jìn)行路徑規(guī)劃,規(guī)劃成功后,控制器執(zhí)行路徑驅(qū)動機(jī)器人運(yùn)動(圖9),藍(lán)色部分為機(jī)器人當(dāng)前位姿,黃色部分為目標(biāo)位姿,可以觀測到運(yùn)行過程中實(shí)體機(jī)器人與rviz模型運(yùn)行同步,機(jī)械臂平穩(wěn)無抖動,位置控制精度較高。

圖9 PC機(jī)控制UR5實(shí)體運(yùn)動Fig.9 PC control UR5 physical movement

ROS中集成了應(yīng)用程序開發(fā)框架qt工具包,其具有人性化、操作簡便和集成度高的優(yōu)點(diǎn)。啟動軟件的界面設(shè)計(jì)(UI)功能,可以實(shí)時監(jiān)測機(jī)械臂在運(yùn)行過程中的角速度、角加速度等信息。輸入命令行:

＄ rosrun rqt_plot rqt_plot。

選擇繪制機(jī)器人關(guān)節(jié)角速度反饋(joint_velocity)的數(shù)據(jù)點(diǎn)線圖,導(dǎo)出采集的數(shù)據(jù)點(diǎn),經(jīng)過處理得到角速度曲線，如圖10所示。

圖10 機(jī)械臂運(yùn)行各關(guān)節(jié)角速度Fig.10 Angular velocity of each joint of the robotic arm

監(jiān)測機(jī)械臂從起始點(diǎn)到目標(biāo)點(diǎn)一個來回的數(shù)據(jù),可以看出：6個關(guān)節(jié)角速度在起始時向著正負(fù)方向平穩(wěn)變大,到達(dá)2.5 s時角速度恒定,勻速運(yùn)動期間角速度波動很小,在接近停止點(diǎn)，即在4.8 s時角速度平穩(wěn)減少。分析曲線斜率可知:在機(jī)械臂加速到勻速過程和開始減速過程中,角加速度有輕微突變,但是機(jī)械臂無明顯抖動,整個過程運(yùn)行比較平穩(wěn),能夠滿足機(jī)械加工需要。

5 結(jié)論

1)根據(jù)機(jī)器人學(xué)相關(guān)知識,推導(dǎo)UR5機(jī)器人正逆解公式,代入D-H參數(shù)并得到奇異位姿。在ROS中進(jìn)行仿真測試,觀測到機(jī)器人軌跡規(guī)劃正確,表明計(jì)算公式和機(jī)器人模型的正確性。

2)分析了基本RRT算法,并綜合考慮角度和距離兩個因素進(jìn)行算法改進(jìn),試驗(yàn)結(jié)果證明，相較于基本RRT算法,改進(jìn)RRT算法縮短了尋優(yōu)時間,提高了軌跡規(guī)劃路徑的平滑度,證明了該算法的理論可行性。

3)通過實(shí)體機(jī)器人控制試驗(yàn),監(jiān)測到運(yùn)動過程中6個關(guān)節(jié)角速度平穩(wěn),在2.5到4.8 s的勻速運(yùn)動過程中觀測到的角速度波動較小,機(jī)械臂運(yùn)行平穩(wěn)無抖動,表明設(shè)計(jì)的ROS機(jī)器人系統(tǒng)控制性能較好,能夠滿足機(jī)械加工的位置精度和穩(wěn)定性要求。