屈力剛,高凱,邢宇飛,張丹雅
(沈陽航空航天大學機電工程學院,遼寧沈陽 110136)
隨著機器人平臺與硬件設備日益豐富,機器人系統(tǒng)變得更加復雜,軟件的編寫難度也在不斷增加。當前機器人技術面臨的問題有:不同軟件之間難以互通互聯(lián);硬件不斷豐富,軟件代碼復雜繁瑣;不同機器人之間軟件代碼不能復用和模塊化[1]。為解決以上問題,科學工作者研發(fā)了多種優(yōu)秀的軟件框架:ROS(機器人操作系統(tǒng))界面簡單易操作,適配多種機器人模型,采用統(tǒng)一的格式進行描述,可以控制多種型號機器人,系統(tǒng)設計結構簡單,支持多種語言;OpenRTM機器人平臺是面向組件的開發(fā),通過為各功能元素創(chuàng)建組件來構建系統(tǒng);Orocos是用于機器人控制的C語言庫,允許使用C語言編寫實時組件;其中ROS具有多語言支持、架構精簡,集成度高、組件化工具包豐富、免費開源等多個優(yōu)點被廣泛使用[2-3]。
本文作者基于ROS實驗平臺,采用ABB irb-2400型號機器人為研究主體,進行正逆運動學分析。隨后將機器人的三維實體模型設置為URDF文件,采用Setup Assistant配置包,根據(jù)URDF模型文件完成機器人的配置和Rviz可視化,并對機器人進行笛卡爾空間規(guī)劃,最后進行避障仿真。
ROS使用統(tǒng)一的機器人描述格式(Unified Robot Description Format,URDF)來構建機器人模型,可以直接控制移動或者程序驅動機器人以及添加其他環(huán)境信息[4]。URDF文件采用XML標簽,以連桿(link)和關節(jié)(joint)為基本結構,描述了機器人的各個部件及其位置關系。并且可以儲存機器人的基本物理屬性,以及機器人的運動學、動力學屬性。
URDF文件主要由連桿(link)和關節(jié)(joint)2個基本結構組成。如圖1所示:連桿分別通過visual描述外觀參數(shù),inertial描述慣性參數(shù),以及collision描述碰撞屬性;關節(jié)的作用是連接2個連桿,稱為父連桿和子連桿。復雜的機械臂模型可以簡化為若干連桿,通過關節(jié)按照固定順序連接起來。每個連桿和關節(jié)的屬性都是固定的,因此,只要確定基座的坐標系就可以推出整個模型的坐標關系[5]。
圖1 URDF關節(jié)、連桿結構
依據(jù)上述原理創(chuàng)建好ABB irb-2400型號機器人完整的URDF文件后,使用MoveIt Setup Assistant生成irb-2400機器人的MoveIt配置功能包,從而完成機器人運動規(guī)劃的初始配置、可視化和仿真等工作[6]。
隨后運行其中的launch文件,通過Rviz實時顯示機器人模型以及狀態(tài),以便于直觀地配置和修改模型文件。機器人模型如圖2所示[7]。
圖2 機器人模型
機器人的正向運動學分析是根據(jù)機械臂各關節(jié)類型,相鄰關節(jié)尺寸及運動量大小來確定末端執(zhí)行器的位置和位姿關系。運用D-H參數(shù)法對irb-2400型號機械臂進行正運動學求解,即通過坐標轉換把末端執(zhí)行器的位姿在坐標系里表示出來。該參數(shù)法由DENAVIT和HARTENBERG提出[8],現(xiàn)已成為常用的機器人連桿建模方法[9]。根據(jù)D-H參數(shù)法建立的irb-2400 機器人的坐標系如圖3所示。
圖3 ABB irb-2400機器人連桿坐標系
圖3中基坐標系即為x0y0z0構成的坐標系,經(jīng)過坐標變換得到末端執(zhí)行器坐標系即為x6y6z6所構成的坐標系。機器人連桿長度和關節(jié)轉角范圍的相關參數(shù)描述可以通過查閱URDF文件獲取。根據(jù)相關參數(shù)建立機械臂D-H參數(shù),如表1所示。其中,i表示連桿,ai表示連桿長度,αi表示連桿扭曲角,di表示連桿偏置,θi表示變量關節(jié)角度[10]。
表1 ABB irb-2400機器人D-H參數(shù)
在建立各個關節(jié)的坐標系以后,根據(jù)D-H法則經(jīng)過2次旋轉和2次平移后確立相鄰兩連桿i-1和i之間的相對關系:
(1)
(2)
在實際情況中,大多數(shù)是已知機器人末端執(zhí)行器的目標位姿矩陣,求解各關節(jié)的角度變化,即求θ1、θ2、θ3、θ4、θ5、θ6的值。通過代數(shù)法對機器人進行逆運動學求解,ABB irb-2400機器人具有6個自由度,其運動學方程可寫成:
(3)
令矩陣方程(3)兩端元素2、4行對應相等得:
-sinθ1px+cosθ1py=d2
(4)
利用三角代換得:
(5)
式中正負號對應于θ1的2種可能解。選定其中一個解后,再令式(3)兩端元素1、4行和3、4行分別對應相等,得到2個方程,繼續(xù)使用三角代換可解出θ3。以此類推求出其余每個關節(jié)的解。
機器人在進行連續(xù)作業(yè)時,為保證末端執(zhí)行器按照既定軌跡進行運動,則需要在笛卡爾空間中進行軌跡規(guī)劃。笛卡爾空間軌跡規(guī)劃就是已知機器人末端的初始點位置和終止點位置,將軌跡劃分為有限個點,通過插補算法求解出中間點的位置,然后使用逆運動學求解出各中間點所對應的關節(jié)角度值。
對于直線軌跡規(guī)劃,已知機器人末端執(zhí)行器起始點Q0(x0,y0,z0)和終止點Q1(x1,y1,z1),設插補次數(shù)為N,則中間插補點坐標為
(6)
在MoveIt中采用前面的直線軌跡規(guī)劃進行運算,完成軌跡點之間的插補,運動實驗如圖4所示,實體為機器人起始狀態(tài),虛體為結束狀態(tài),給定起始點和終止點坐標,運行程序。
圖4 直線軌跡規(guī)劃
機器人末端從Q1(x1,y1,z1)經(jīng)過第二點Q2(x2,y2,z2)到達終點Q3(x3,y3,z3),此圓弧即為機械臂末端的運動軌跡。由Q1、Q2、Q3三點所構成的平面方程為
Ax+By+Cz+D=0
(7)
取直線Q1Q2和Q2Q3垂直平分線,交點即為圓心O(x0,y0,z0)坐標,進而求得圓弧半徑R。
以圓心O為坐標原點,構建新坐標系o-uvw。令Q1O連線方向為u軸方向,Q1Q2連線垂直方向為w軸方向,v軸方向通過u、w軸向量的叉乘得到[11]。則可建立起基座標系到新坐標系的變換矩陣為
(8)
設圓弧上有一點Qi在新坐標系下坐標為(ui,vi,wi),設Q0O和Q1O夾角為θ,則Q0坐標為(Rcosθ,Rsinθ,0),將插補結果得到坐標映射至基座標系上。設點Q0在基坐標系的坐標值為(xi,yi,zi),則有:
(9)
在MoveIt中對機器人進行圓弧軌跡規(guī)劃,運動軌跡如圖5所示。
圖5 圓弧軌跡規(guī)劃
move_group是MoveIt中的核心節(jié)點,主要是將各功能包和插件集成起來為用戶提供動作指令和服務。自定義規(guī)劃算法的實現(xiàn)如圖6所示,將設計好的算法與MoveIt標準接口組成一個插件,添加注冊文件使得move_group能夠感知算法,之后通過yaml、launch文件調(diào)用算法,從而使機器人在運動規(guī)劃過程中采用自定義算法進行軌跡規(guī)劃。
圖6 自定義規(guī)劃算法實現(xiàn)原理
線性插補是機器人在運動規(guī)劃過程中從起始點到終止點各軸均保持線性均速移動。對于線性插補規(guī)劃,已知起始點Q0(x0,y0,z0)和終止點Q1(x1,y1,z1),設插補次數(shù)為N,各軸中心點初始坐標為bi,插補后坐標為Zi(i=1,…,6),斜率為k,則中間各軸坐標滿足:
Zi=(k×N+1)bi
(10)
將算法插件接入MoveIt,運行ABB機械臂launch文件,打開Rviz界面后,即可看到運動規(guī)劃器已換成自定義算法。給定起始點和終止點,點擊運行,機器人即完成線性插補。
將運行過程中各軸插補點坐標導入Excel表格,結果如圖7所示,各軸均保持線性運動,證明該算法的可行性。
圖7 線性插補過程各軸位置
在很多應用場景下,機器人的工作環(huán)境中有其他物體,在機器人運動過程中這些物體可能成為其運動規(guī)劃的障礙物。為避免與物體發(fā)生碰撞,運動規(guī)劃需要考慮避障問題。MoveIt中的OMPL是包含許多運動規(guī)劃算法的開源采樣運動規(guī)劃庫并且支持避障規(guī)劃,文中采用其中的RRT-Connect算法對機器人進行避障仿真分析。
傳統(tǒng)RRT(Rapidly Exploring Random Tree)算法是一種快速搜索算法,從起始點開始,以樹狀分支的路徑形式快速擴展來填充空間的大部分區(qū)域,直到尋找到目標區(qū)域,原理如圖8所示[12]。RRT-Connect算法在RRT的基礎上進行改進,在目標點區(qū)域建立第二棵樹,2棵樹不斷朝向對方交替擴展,顯著提高了搜索速度和效率[13]。
圖8 RRT算法原理
通過在MATLAB編程,加載障礙物地圖,設定起始點[10,10]和目標點[490,490],迭代次數(shù)5 000,迭代步長為20,使用RRT和RRT-Connect算法搜索出到達目標點的無碰撞路徑,所用時間和路徑長度如圖9所示。
圖9 RRT算法與RRT-Connect算法結果對比
可見,RRT-Connect算法所用時間和規(guī)劃路徑長度均短,因此,為規(guī)劃提高效率,在仿真實驗中采用RRT-Connect算法進行路徑規(guī)劃。
在終端啟動配置的irb-2400機器人文件,進入Rviz界面,選擇RRT-Connect算法,在Motion Planning里將機器人末端運動軌跡顯示出來,以便于分析和觀察。令初始位置為起點位置,設定機器人的終點位置。另起一個終端,將編寫的Python程序運行,可以看到障礙物添加到環(huán)境中。將障礙物發(fā)布于環(huán)境,機器人在運動過程中會躲避障礙物到達終點。當機器人從終點返回到起點時,仍然會規(guī)劃一條避障路徑,運動軌跡如圖10、圖11所示。
圖10 機械臂到達終點 圖11 機械臂返回起點
Rqt_plot是ROS平臺的二維數(shù)值曲線繪制工具,可以將需要顯示的數(shù)據(jù)用平面坐標圖描繪。在終端輸入Rqt打開界面,輸入/joint_states/position,即可得到實時關節(jié)變化曲線。如圖12所示,在機器人進行避障規(guī)劃過程中,各關節(jié)平穩(wěn)運行。
圖12 關節(jié)變化曲線
文中基于ROS平臺對機器人進行運動規(guī)劃研究,包括軌跡規(guī)劃及避障仿真實驗。通過各關節(jié)位置變化曲線可以看出:機器人運動過程軌跡平滑連續(xù),穩(wěn)定性強。既可以按照既定直線和圓弧軌跡運動,又可以靈活進行避障,并且可以根據(jù)實際需求自定義運動規(guī)劃算法,對機器人的研究具有一定的參考價值。