六自由度機(jī)械臂軌跡規(guī)劃與仿真研究
孫亮，馬江，阮曉鋼
（北京工業(yè)大學(xué)電子信息與控制工程學(xué)院，北京100124）
摘 要：針對(duì)六自由度鏈?zhǔn)綑C(jī)械臂在進(jìn)行正運(yùn)動(dòng)學(xué)、逆運(yùn)動(dòng)學(xué)以及軌跡規(guī)劃仿真時(shí)，不易直觀地驗(yàn)證運(yùn)動(dòng)學(xué)算法的正確性和軌跡規(guī)劃的效果，在正確建立機(jī)械臂數(shù)學(xué)模型的基礎(chǔ)上，重點(diǎn)分析了機(jī)械臂在關(guān)節(jié)空間中軌跡規(guī)劃的兩種實(shí)現(xiàn)方法，并采用三雛運(yùn)動(dòng)仿真進(jìn)行了驗(yàn)證。開發(fā)了一套六自由度機(jī)械臂三雛仿真軟件，該仿真軟件在vc++6 0開發(fā)平臺(tái)上，首先利用分割類將基于nifc框架的窗口分割成為控制霄口和視圖窗口兩部分，然后利用openci的圖形厙對(duì)機(jī)械臂進(jìn)行建模，首次將正運(yùn)動(dòng)學(xué)、逆運(yùn)動(dòng)學(xué)以及軌跡規(guī)劃算法融入其中開發(fā)而成一該仿真軟件有效地驗(yàn)證了機(jī)械臂運(yùn)動(dòng)學(xué)模型建立的正確性，同時(shí)也對(duì)三次多項(xiàng)式和五次多項(xiàng)式兩種軌跡規(guī)劃方法做了直觀的比較，結(jié)果表明后一種軌跡規(guī)劃效果明顯優(yōu)于前一種。
關(guān)鍵詞：機(jī)械臂；運(yùn)動(dòng)學(xué)分析；軌跡規(guī)劃；運(yùn)動(dòng)仿真
中圖分類號(hào)：tp 241：tp 391 文獻(xiàn)標(biāo)識(shí)碼：a
1引言
六自由度鏈?zhǔn)?6r)機(jī)械臂的軌跡規(guī)劃既可以在關(guān)節(jié)空間，也可以在直角坐標(biāo)空間中進(jìn)行。由于在關(guān)節(jié)空聞中進(jìn)行軌跡規(guī)劃是直接用運(yùn)動(dòng)時(shí)的受控變量規(guī)劉軌跡，有著計(jì)算量小，容易實(shí)時(shí)控制，而且不會(huì)發(fā)生機(jī)構(gòu)奇異性ji等優(yōu)點(diǎn)，所以經(jīng)常被采用。但是這種方法難以確定各桿和末端抓持器位置，所以開發(fā)一套能夠直觀地監(jiān)視機(jī)械臂各個(gè)部分運(yùn)動(dòng)的三維仿真軟件變得極為重要。文獻(xiàn)[2]在平臺(tái)下開發(fā)了robotics工具箱，能夠通過函數(shù)實(shí)現(xiàn)對(duì)機(jī)器臂進(jìn)行正、逆運(yùn)動(dòng)學(xué)以及軌跡規(guī)劃分析，實(shí)現(xiàn)了簡(jiǎn)單的運(yùn)動(dòng)學(xué)仿真。文獻(xiàn)[3]基于opencl圖形庫開發(fā)了一套機(jī)械臂仿真系統(tǒng)，實(shí)現(xiàn)了機(jī)械臂的正、逆運(yùn)動(dòng)學(xué)仿真。文獻(xiàn)[4]利用文獻(xiàn)[3]的方法，且通過定時(shí)器，不斷刷新視圖，達(dá)到了動(dòng)畫的效果，但是并沒有提供具體的軌跡規(guī)劃算法。本文首次將軌跡規(guī)劃算法融人開發(fā)的六自由度機(jī)械臂三維仿真軟件中，有效直觀地驗(yàn)證了兩種插值函數(shù)軌跡規(guī)劃的效果。
2運(yùn)動(dòng)學(xué)分析
機(jī)械臂坐標(biāo)系，如圖l所示。
6r機(jī)器臂是具有6個(gè)關(guān)節(jié)的空間機(jī)構(gòu)，為描述末端執(zhí)行器在空間的位置和姿態(tài)，可以在每個(gè)關(guān)節(jié)上建立一個(gè)坐標(biāo)系，利用坐標(biāo)系之間的關(guān)系來描述末端執(zhí)行器的位置。
1）運(yùn)動(dòng)學(xué)正解正運(yùn)動(dòng)學(xué)的求解過程是根據(jù)已知關(guān)節(jié)變量θ1，θ2，θ3，θ4，θ5，θ6，求末端抓持器相對(duì)于參考坐坐標(biāo)系的位姿的過程。使用標(biāo)準(zhǔn)的上關(guān)節(jié)d-h法，將參考坐標(biāo)系設(shè)在6r機(jī)械臂的基座上，從基座開始變換到第一關(guān)節(jié)，然后到第二關(guān)節(jié)，最后變化到來端抓持器。6r機(jī)器臂的基座和手之間的總變換為
式中，a0為基座坐標(biāo)系到坐標(biāo)系0（關(guān)節(jié)一）之間的變換矩陣；a1為坐標(biāo)系0到坐標(biāo)系1之間的變換矩陣；ah為坐標(biāo)系5到坐標(biāo)系日之間的變換矩陣；s12= sin（θ1+θ2）；cl2=cos(θ1+θ2)；s123=sin（θ1+θ2+θ3）。
可以制作關(guān)節(jié)和連桿參數(shù)的表格，見表1。
2)運(yùn)動(dòng)學(xué)逆解逆運(yùn)動(dòng)學(xué)的求解過程是根據(jù)已知的末端抓持器相對(duì)于參考坐標(biāo)系的位姿，求關(guān)節(jié)變量θ1，θ2，θ3，θ4，θ5，θ6的過程，它是機(jī)器人運(yùn)動(dòng)規(guī)劃和軌跡控制的基礎(chǔ)，也是運(yùn)動(dòng)學(xué)最重要的部分。先給出機(jī)器人的期望位姿表達(dá)式：
式(2)為式(1)另一表達(dá)形式，向量n，o，a分別表示法線、指向和接近向量；p向量為末端抓持器坐標(biāo)系原點(diǎn)相對(duì)于基座坐標(biāo)系的位置向量。通常，p可以根據(jù)工件位置給出，而n，0，a這三個(gè)向量可以通過rpy（滾動(dòng)角φn，俯仰角φ0和偏航角φa）旋轉(zhuǎn)給出。由式(3)表示：
| 
