五自由度機械臂運動分析及軌跡規(guī)劃

謝 嘉，竇良浩，李永國，梁錦濤

(上海海洋大學(xué) 工程學(xué)院，上海 201306)

0 引言

21世紀(jì)以來，社會發(fā)展愈來愈智能化信息化，人工成本的升高，以及社會生產(chǎn)生活對于效率和精度要求越來越高，以機器人操作代替人工操作的方式普遍存在于各行各業(yè)。

其中在服務(wù)行業(yè)，服務(wù)機器人憑借著廉價、優(yōu)秀的交互能力和良好的執(zhí)行能力在家庭服務(wù)，行業(yè)咨詢，消防救險，物資輸送等行業(yè)大放異彩。然而帶有機械臂的服務(wù)類機器人，除了能夠主人之間的語言交互，還能夠根據(jù)要求做出相應(yīng)的回應(yīng)，例如，端茶倒水，捏肩等。

本文在以設(shè)計家庭服務(wù)機器人的基礎(chǔ)上，針對性研究機器臂，探討研究多自由度狀態(tài)下機械臂運動與軌跡以及工作空間問題。

從人類手臂的結(jié)構(gòu)來說，三自由度手臂已滿足正常的工作需求，考慮到家庭服務(wù)環(huán)境可能較為狹窄三自由度手臂在人體關(guān)節(jié)正?；顒臃秶鷥?nèi)的工作空間無法滿足機器人復(fù)雜日常工作需求[1]。出于節(jié)約機器人工作空間以及適應(yīng)更加復(fù)雜工作要求的目的，本文設(shè)計一種五自由度家庭服務(wù)機器人的手臂。針對于機器人的手臂，完成了對于五自由度機器臂的運動學(xué)分析和軌跡規(guī)劃，為后續(xù)控制手臂完成捏肩倒水等工作提供理論依據(jù)。

1 機械手臂的結(jié)構(gòu)與運動學(xué)分析

1.1 機械手臂的結(jié)構(gòu)

本研究設(shè)計的機械臂具有5個關(guān)節(jié)自由度，可以很好的完成期待目標(biāo)動作。機械機構(gòu)如圖1所示，所有零件的設(shè)計裝配在SolidWorks中完成，參考人類手臂結(jié)構(gòu)，設(shè)計五自由度的機械臂機構(gòu)。肩舵機通過聯(lián)軸器與肩部直步進電機鏈接，完成肩部的旋轉(zhuǎn)與擺動的相關(guān)動作。同樣肘部擁有肘舵機與肘部步進電機，提供肘部的旋轉(zhuǎn)與彎曲，在手臂末端提供一個小型電機方便微調(diào)末端手腕的姿勢。

圖1 機械臂結(jié)構(gòu)示意圖

在機械臂的自由度選擇上，一般來說機械臂的自由度并不需要太多，但是考慮到在家庭環(huán)境中機械臂需要對主人的要求做出回應(yīng)。針對于較為狹窄的家庭環(huán)境來說，五個自由度的機械臂的靈活性更好，工作空間更大，適用于家庭環(huán)境內(nèi)。

依據(jù)本文機械臂的基本結(jié)構(gòu)，采用常用且經(jīng)典的D-H坐標(biāo)變換法，完成對機械臂的三維模型的建立。根據(jù)方法基本要求建立連桿坐標(biāo)系如圖2所示，5自由度機器臂的D-H參數(shù)及各關(guān)節(jié)運動范圍如表1所示。

表1 機器人D-H連桿參數(shù)表

圖2 機器人連桿坐標(biāo)系

其中D-H建模方法有兩種，一種是標(biāo)準(zhǔn)的D-H建模法，另一種是改進的D-H建模法，兩種方法最大的區(qū)別就是在于坐標(biāo)系{i}的位置不同。標(biāo)準(zhǔn)的D-H法的坐標(biāo)系{i}建立在i+1的關(guān)節(jié)之上而改進后的D-H法的坐標(biāo)系{i}建立在i+1上。對比兩種D-H法由于坐標(biāo)系建立的不同隨之帶來的則是D-H表參數(shù)的不同。兩種方法本質(zhì)上沒有太大區(qū)別，在運動學(xué)分析時采用標(biāo)準(zhǔn)的D-H法，在MATLAB中采用修改的D-H法做兩種方法的熟悉與運用。

從首先確定的第一個關(guān)節(jié)到下一個關(guān)節(jié)的變換，實際上是兩個關(guān)節(jié)坐標(biāo)系之間的變換。因此通過這樣的坐標(biāo)系變換方式，可以推導(dǎo)機器人的總變換矩陣。例如確定基座作為起始，由基座變換至第一個關(guān)節(jié)，在此基礎(chǔ)上在變換至第二個關(guān)節(jié)，結(jié)合所有變換得到的變換矩陣為總變換矩陣。

而相鄰連桿坐標(biāo)系i相對于i-1的變換i+1iT的齊次坐標(biāo)變換矩陣通式為：

其中：sθi=sinθi，cθ1=cosθi。

由此i與i-1連桿坐標(biāo)系之間的齊次坐標(biāo)變換矩陣通式，可以得到01T12T23T34T45T的齊次變換矩陣。

1.2 運動學(xué)分析

作為家庭服務(wù)機器人的機械手臂，在工作時需要對主人的指令做出回應(yīng)，因此對于機械臂的運動學(xué)分析極其重要。機械臂運動學(xué)分析包含了機械臂正向運動學(xué)分析和逆向運動學(xué)分析[2]。

1.2.1 正運動學(xué)分析

采用D-H法根據(jù)表一中提供的相關(guān)數(shù)據(jù)，利用齊次變換矩陣通式可得到機器人基座坐標(biāo)系O0與末端執(zhí)行器坐標(biāo)系O5的正運動學(xué)方程[3]為：

經(jīng)過齊次變換矩陣得到的正運動學(xué)方程，其中未知數(shù)為各關(guān)節(jié)角度。則通過式(1)可推導(dǎo)末端關(guān)節(jié)位姿（n，o，a，p）與關(guān)節(jié)變量(θ1，θ2，θ3，θ4，θ5，)之間的關(guān)系。其中，n，o，a三個向量表示末端執(zhí)行器相對于基座參考坐標(biāo)系的姿態(tài)，p向量則表示末端執(zhí)行器在基座參考系中的位置[4]。

其中：

由位姿矩陣的定義，機器人末端的姿態(tài)和坐標(biāo)分別為：

因此當(dāng)給定確定的θ，機器臂位姿總是確定的。

1.2.2 逆運動學(xué)分析

現(xiàn)已知機器人的期望位姿，求解每個關(guān)節(jié)值，這就是逆運動學(xué)所要研究的，也是運動學(xué)中最重要的。

已知：

本文采用反變換法求解。

1）求解θ1同時將式(3)中左邊同乘以消去其中一個變量得：

另其中該式兩端(3，3)（3，4）元素相等可解的：

另其中(1，4)和(2，4)兩端元素相等解得：

4）求解θ4。對式(6)中(1，3)和(3，3)元素分別對應(yīng)相等得：

5）求解θ5。

2 基于MATLAB機械臂的運動學(xué)仿真

2.1 機械臂建模

對于機械臂的運動學(xué)仿真方法，本文在MATLAB軟件環(huán)境下，通過調(diào)用機器人工具箱Robotics Tool中的函數(shù)算法，可以得到符合研究目標(biāo)的運動學(xué)模型。

由表一中的機器臂的各個參數(shù)，編寫相應(yīng)程序，采用修改后的D-H法編寫的程序如下，并命名該手臂為mdl_p5dof。

依次建立五個桿件數(shù)學(xué)模型L1，L2，L3，L4，L5

連接各關(guān)節(jié)獲得的機械臂的模型如圖3所示。

圖3 五自由度機械臂運動學(xué)模型

2.2 機械臂的正逆運動學(xué)仿真分析

對于機器臂的正運動學(xué)的驗證，我們通過基于Robotics Tool機器人工具箱關(guān)于機械臂正運動學(xué)的算法，在每個目標(biāo)關(guān)節(jié)變量允許的變化范圍內(nèi)隨機選取任意角度，例如：

選取(θ1，θ2，θ3，θ4，θ5)=(π/6，π/3，-π/6，π/3，π/3)，將這些數(shù)據(jù)代入式(2)中得到機器人末端位姿矩陣為：

本文通過對比MATLAB中基于fkine函數(shù)編程求解得到的機器人末端位姿矩陣與前文由標(biāo)準(zhǔn)D-H法得到的末端位姿矩陣驗證正運動學(xué)的正確性。由于在計算過程中舍入位的差異，導(dǎo)致最后二者求解末端位姿矩陣由些許差異，但并不影響結(jié)論的正確性。

在逆運動學(xué)求解過程中Robotics Toolbox機器人工具箱提供兩種求逆向運動學(xué)的函數(shù)。其中ikine6s()函數(shù)一般求解于6軸機械臂而ikine()可默認(rèn)6軸機器臂也可用于非6軸機械臂的求解。本文設(shè)計為五軸機械臂，將采用ikine()函數(shù)，已知量設(shè)置為正運動求解得到的位姿矩陣，在MATLAB通過ikine()函數(shù)求解逆運動。求解得到的關(guān)節(jié)變量為：

在MATLAB中求解的結(jié)果與關(guān)節(jié)已知量之間不完全相同，這是因為機器人逆運動計算過程中存在多重的計算結(jié)果，而ikine()只會自動選取其中一組逆解[5]。由MATLAB得到的逆解不一定是最適合于該設(shè)計情況下的最優(yōu)解，現(xiàn)實設(shè)計時需要結(jié)合工作任務(wù)及各關(guān)節(jié)變量范圍取最適合逆解。

3 軌跡規(guī)劃

軌跡規(guī)劃指選取機器人手臂末端執(zhí)行端在工作軌跡上的若干點，通過運動學(xué)逆解得出各個關(guān)節(jié)的運動規(guī)律，在關(guān)節(jié)空間內(nèi)選取相應(yīng)點進行插值并建立運動方程，得到光滑的連續(xù)擬合曲線[6]。對機械臂進行軌跡規(guī)劃的時通常分為兩種方法：關(guān)節(jié)空間軌跡規(guī)劃和笛卡爾空間軌跡規(guī)劃[7]。關(guān)節(jié)空間軌跡規(guī)劃針對于各個關(guān)節(jié)，通過多各個關(guān)節(jié)進行插值擬合，保證各個關(guān)節(jié)的連續(xù)性與穩(wěn)定性，但并不直接對末端執(zhí)行器的軌跡直接進行規(guī)劃。互補的，笛卡爾空間軌跡規(guī)劃并沒有對機器臂關(guān)節(jié)規(guī)劃分析，直接給出末端執(zhí)行器的軌跡，目的在于確保機械臂末端執(zhí)行器在規(guī)定的軌跡上運動，但是容易出現(xiàn)特殊關(guān)節(jié)。本文通過模型分析各關(guān)節(jié)運動規(guī)律，求解關(guān)節(jié)角度給出關(guān)節(jié)空間的軌跡規(guī)劃，并分析笛卡爾空間軌跡規(guī)劃獲得該機器臂的位移軌跡。

3.1 關(guān)節(jié)空間軌跡規(guī)劃

關(guān)節(jié)空間軌跡規(guī)劃通過設(shè)置起始點與終點，在其之間插入相當(dāng)關(guān)節(jié)數(shù)的軌跡點，由于約束條件限制選擇最優(yōu)的插值函數(shù)，求解各個關(guān)節(jié)的變量并獲得光滑的擬合曲線。在Robotics Toolbox機器人工具箱中利用jtraj()函數(shù)，設(shè)置關(guān)節(jié)起始點為（[40 50 -30 -30 -10]*du），終點處設(shè)置為（[40 10 -60 -30 30]*du），這樣機械臂的末端執(zhí)行器起點與終點設(shè)置完畢。將關(guān)節(jié)空間軌跡規(guī)劃時間設(shè)置為8秒，步長為0.05，得到軌跡規(guī)劃圖如圖4所示。

圖4 關(guān)節(jié)空間運動軌跡圖

各個關(guān)節(jié)隨時間變化的角位移，角速度和角加速度的曲線如圖5所示。

圖5 各個關(guān)節(jié)角度，角速度，角加速度曲線

圖五中各個關(guān)節(jié)角度，角速度和角加速度隨時間變化曲線圖光滑連續(xù)無凸起，關(guān)節(jié)角度在變量允許范圍內(nèi)連續(xù)變化，角速度起始點為0且在四秒時角速度達到最大。而角加速度在2秒時達到最大并在4秒時為0，此時速度達到最大在之后4秒做減速運動。在軌跡規(guī)劃過程中角速度與加速度曲線沒有突然變化，表示該機械臂在運行過程中相當(dāng)平穩(wěn)沒有出現(xiàn)劇烈震顫，同時說明該機械臂設(shè)計較為合理。

特殊的，關(guān)節(jié)4個變量隨時間沒有明顯變化。但是并不意味著該關(guān)節(jié)在實際工作中喪失功能性，在其余逆解超出關(guān)節(jié)變量范圍時可通過關(guān)節(jié)4的變化實現(xiàn)功能。

3.2 笛卡爾空間軌跡規(guī)劃

依據(jù)關(guān)節(jié)空間軌跡規(guī)劃的起始點與終點得到的機械臂的末端執(zhí)行器的位姿分別為T1=transl(0.2，0.2，0.2)，T2=transl(0.2，-0.1，0.1)以T1與T2為始末狀態(tài)調(diào)用ctraj函數(shù)對其進行位移軌跡空間規(guī)劃，得到圖6和圖7。

圖6 x-y平面的笛卡爾位移軌跡圖

圖7 笛卡爾空間軌跡圖

4 工作空間

工作空間是衡量機器臂性能的重要指標(biāo)，工作空間的大小決定了機械臂逆解的可行性，是末端執(zhí)行器所有能夠達到的目標(biāo)點的合集。

目前求解工作空間的方法較多，但蒙特卡洛作為優(yōu)秀的一種數(shù)值方法廣泛求解于隨機事件的問題。本文采用此方法求解五自由度機器人的工作空間。

通過導(dǎo)入上文建立的機械臂模型，設(shè)置各個關(guān)節(jié)的關(guān)節(jié)限制角度與計算點數(shù)n=5000，結(jié)合正運動學(xué)仿真函數(shù)，繪制機械臂在XYZ坐標(biāo)空間的工作空間點云圖。如圖8～圖11所示，為在基坐標(biāo)下工作空間以及在各直角平面的投影。

圖8 機械臂可達到工作空間

圖9 工作空間XOY平面的投影

圖10 工作空間在XOZ平面的投影

圖11 工作空間YOZ平面的投影

在仿真實驗中我們可以通過改變計算點數(shù)n的大小獲得更加接近真實工作空間的仿真數(shù)據(jù)。實驗發(fā)現(xiàn)，當(dāng)計算點數(shù)n分別取1000，5000和9000時獲得的工作空間有所不同，但是當(dāng)n值取越大越符合機械臂的真實工作空間。

5 結(jié)語

本文基于設(shè)計的一種家庭服務(wù)類機器人，舍棄傳統(tǒng)類人三自由度手臂的設(shè)計，采用五自由度機械臂的設(shè)計。通過在SolidWorks中建模設(shè)計構(gòu)建了5自由度機械臂的模型。運用D-H法建立了機械臂的數(shù)學(xué)模型，得到相關(guān)幾何參數(shù)后分析計算了其正逆運動的算法。通過MATLAB軟件仿真分析了其正逆運動的正確性，在可允許的工作空間內(nèi)，對五自由度手臂的關(guān)節(jié)空間進行分析，確保關(guān)節(jié)在運動過程中的平滑穩(wěn)定并給出了末端執(zhí)行器的位移軌跡，驗證建立的機器人手臂模型設(shè)計的合理性。相比于三自由度機械臂，五自由度手臂在任務(wù)執(zhí)行過程中，關(guān)節(jié)角變化更小，所需活動空間更小，同時能達到的工作空間更優(yōu)于三自由度，為之后機器人手臂的控制提供現(xiàn)實基礎(chǔ)。