莊衡衡，丁 飛，章華濤，張海濤，暴建民

（1.南京郵電大學 江蘇省寬帶無線通信和物聯(lián)網(wǎng)重點實驗室，江蘇 南京 210003; 2.南京郵電大學物聯(lián)網(wǎng)學院，江蘇 南京 210003; 3.中國科學院國家天文臺南京天文光學技術(shù)研究所，江蘇 南京 210042; 4.中國科學院天文光學技術(shù)重點實驗室，江蘇 南京 210042）

0 引 言

隨著我國先進制造技術(shù)的發(fā)展，多自由度機器人以其自動化、高效化和智能化優(yōu)勢，將在高端制造、空間遙操作和智能自主系統(tǒng)等領(lǐng)域獲得廣泛應用[1-2]。多自由度機器人的運動控制影響位姿的可達工作空間與目標指向精度[3]，兩者之間的有效協(xié)同將直接決定多自由度機器人的工作性能[4-5]。因此，研究并建立多自由度機器人的運動控制和空間位姿的協(xié)同分析與仿真系統(tǒng)具有重要意義。

作為機器人運動控制、位姿調(diào)整[6]的核心計算模塊，對正向運動學和逆向運動學的求解是關(guān)鍵。郭萬金等[7]針對一種五自由度機器人提出了逆運動學的封閉解法。張道義等[8]用矢量法和求導法分別對五自由度雕刻機末端運動學進行正向和逆向速度的分析。Cao F等[9]提出了一種基于SCARA的新型五自由度焊接機器人，采用DH模型得到正、逆運動學的一般公式，同時利用ANSYS軟件對其進行了模態(tài)分析。Lee J W等[10]指出工業(yè)機器人制造商和用戶大都專注于使用激光跟蹤器對機器人進行空間位姿精度測試。Zheng K等[11]對5-DOF混聯(lián)乒乓球機器人的機械臂進行空間位姿分析，將工作空間中的球拍姿勢轉(zhuǎn)換為聯(lián)合空間中驅(qū)動軸的參數(shù)，并使用ADAMS軟件進行運動模擬。

上述研究為多自由度機器人的運動控制與空間位姿分析奠定了良好基礎(chǔ)。本文提出一種五自由度機器人運動控制與空間位姿仿真方法，首先對五自由度機器人進行運動學建模，然后推導了其正向運動學和逆向速度運動求解方法，最后設(shè)計并實現(xiàn)了其運動控制與空間位姿協(xié)同仿真系統(tǒng)，為后續(xù)多自由度機器人的結(jié)構(gòu)優(yōu)化和可達工作空間分析提供分析平臺和理論支撐。

1 運動學建模與求解

1.1 五自由度機器人結(jié)構(gòu)模型

本文的五自由度機器人結(jié)構(gòu)模型如圖1所示，該機器人由三自由度的平動機構(gòu)和兩自由度的轉(zhuǎn)動機構(gòu)共同構(gòu)成，其中前3個自由度分別定義為沿著X軸，Y軸和Z軸向的平動，O1、O2和O3分別為平動關(guān)節(jié)軸X軸，Y軸和Z軸上的關(guān)節(jié)節(jié)點，Ox,Oy和Oz分別為O1,O2和O3在各自平動軸上運動的起始點；根據(jù)平動關(guān)節(jié)軸的定義方向，后兩個自由度分別定義為沿著X軸和Y軸向的轉(zhuǎn)動，O4和O5分別為轉(zhuǎn)動關(guān)節(jié)軸X軸和Y軸向上的關(guān)節(jié)節(jié)點，c2，a4和b5分別為O1與O2，O3與O4和O4與O5之間的相鄰關(guān)節(jié)點的結(jié)構(gòu)偏置，末端執(zhí)行器安裝在關(guān)節(jié)節(jié)點O5上，在其上定義指向節(jié)點P，且兩個轉(zhuǎn)動關(guān)節(jié)軸的起始角度如圖1（a）所示。

圖1 五自由度機器人的結(jié)構(gòu)模型

為了方便描述經(jīng)過五自由度的平動、轉(zhuǎn)動變后末端執(zhí)行器的指向，在機器人的結(jié)構(gòu)模型中定了2個坐標系，一個是基礎(chǔ)坐標系XYZ，另一個是過五自由度變換后的末端坐標系X′Y′Z′。其中義末端坐標系各坐標軸的正方向為基礎(chǔ)坐標系各標軸的正方向經(jīng)過五自由度平動、轉(zhuǎn)動變換之后指向，且在末端執(zhí)行器的初始位姿下，末端坐標與基礎(chǔ)坐標系的各坐標軸的正方向保持一致。關(guān)節(jié)點O1、O2、O3、O4、O5和P的位置均用基礎(chǔ)坐系描述，定義末端執(zhí)行器的指向沿著末端坐標X′Y′Z′中Z′軸的正方向，因此指向節(jié)點P也定義在軸上，則末端執(zhí)行器的指向沿著O5點到P點的方五自由度機器人的結(jié)構(gòu)調(diào)整分別由各自運動上的高精度伺服電機驅(qū)動，均可在各自的運動軸沿著正、負兩個方向進行運動，且運動的正方向圖1（a）所示。通過對5個伺服電機的高精度協(xié)控制[12]，調(diào)整五自由度上機械臂的位姿，從而實機器人末端執(zhí)行器的位姿控制與精確對準。

1.2 正向運動學求解

正向運動學是在已知各關(guān)節(jié)點的初始位置、相鄰關(guān)節(jié)點間的結(jié)構(gòu)偏置等參數(shù)的前提下，結(jié)合相關(guān)節(jié)點間的相對運動量，求解末端執(zhí)行器位姿的程[13]。在平動關(guān)節(jié)軸X軸，Y軸和Z軸上分別調(diào)整q2和q3的位移量，轉(zhuǎn)動關(guān)節(jié)軸X軸和Y軸上分別調(diào)q4和q5的旋轉(zhuǎn)量，經(jīng)過五自由度的平動和轉(zhuǎn)動變調(diào)整后的狀態(tài)模型如圖1（b）所示。

將上述矩陣依次相乘，得到末端執(zhí)行器相對于基礎(chǔ)坐標系XYZ的等價齊次變換矩陣：

其中，相鄰關(guān)節(jié)點間的相對運動量向量為：q=（q1,q2,q3,q4,q5）T；旋轉(zhuǎn)矩陣為：

而末端執(zhí)行器上的指向節(jié)點:

根據(jù)O5點的坐標和O5點到P點的指向，即可確定末端執(zhí)行器相對于基礎(chǔ)坐標系的位姿：

當使用ZYZ型歐拉角來描述末端執(zhí)行器的指向時，在不考慮最后一個旋轉(zhuǎn)軸Z軸自轉(zhuǎn)的情況下，目標指向在地平式坐標內(nèi)的方位角如公式（2），Z軸沿逆時針方向旋轉(zhuǎn)為正；目標指向在地平式坐標內(nèi)的天頂距如公式（3），Y軸沿順時針方向旋轉(zhuǎn)為正。

1.3 逆向速度運動學求解

逆向運動學是通過末端執(zhí)行器的當前位姿和目標位姿，反解出相鄰節(jié)點間相對運動量的過程[14-16]。與速度運動學相結(jié)合[17]，就是在已知末端執(zhí)行器當前位姿和目標位姿的前提下，將末端執(zhí)行器由當前位姿向目標位姿運動時的線速度和角速度與關(guān)節(jié)速度矢量聯(lián)系起來，并把關(guān)節(jié)速度矢量乘以電機的掃描時間間隔作為各個關(guān)節(jié)的調(diào)整量。

已知當機器人移動時，關(guān)節(jié)變量和末端執(zhí)行器的位置都將是時間的函數(shù)。而角速度只要是相對于同一個坐標系來描述，就可以作為自由矢量相加，因此通過表示每個關(guān)節(jié)在基礎(chǔ)坐標系上的角速度，相加來確定末端執(zhí)行器相對于基礎(chǔ)坐標系的角速度。結(jié)合本文的五自由度機器人結(jié)構(gòu)，在使用ZYZ型歐拉角的旋轉(zhuǎn)軸定義，且不考慮最后一個旋轉(zhuǎn)軸 Z軸自轉(zhuǎn)的情況下，末端執(zhí)行器相對于基礎(chǔ)坐標系的總的角速度矢量：

當末端執(zhí)行器的目標位姿是固定值時，目標指向的速度方向向量恒為零向量，則當前指向調(diào)整到目標指向所需要的方向向量就是目標指向和當前指向間的方位角誤差，且在一定的時間間隔內(nèi)計算時，可以把所有的距離變量當作速度值。

那么末端執(zhí)行器的線速度：

綜上，得到表達式：

其中Jv和Jω都是3×5的矩陣，5表示機器人結(jié)構(gòu)中共有5個自由度，因此得到速度運動學方程[18]：

雅可比矩陣的下半部分Jω由下式給出：

其中前3個平動關(guān)節(jié)對應的Jωi=0，后2個轉(zhuǎn)動關(guān)節(jié)對應其各自運動軸的單位方向向量。

綜上，根據(jù)速度運動學公式得：

其中J?為雅可比矩陣的偽逆矩陣，代入計算出，即五自由度上的速度調(diào)整量，用乘以電機的掃描時間間隔得到5個電機對應的運動調(diào)整量。

2 空間位姿仿真設(shè)計

五自由度機器人運動控制與空間位姿仿真系統(tǒng)的邏輯框架如圖2所示，該系統(tǒng)從功能邏輯上分為控制側(cè)與仿真?zhèn)?，其中控制?cè)的主要功能為多自由度驅(qū)動單元的協(xié)同控制，核心的計算模塊是對正向運動學和逆向速度運動學的求解，從而確定末端執(zhí)行器的當前位姿和向目標位姿調(diào)整時多自由度驅(qū)動單元各自的調(diào)整量，最后當調(diào)整后的位姿與目標位姿之間的相對誤差超過預設(shè)的誤差量級時，觸發(fā)位姿反饋。仿真?zhèn)鹊闹饕δ苁菍刂苽?cè)的特定業(yè)務(wù)流程進行數(shù)據(jù)同步，再對末端執(zhí)行器當前的初始位姿進行計算，同時對動態(tài)調(diào)整過程中的階段性位姿進行在線仿真，最后對目標位姿進行誤差校驗與評定。

圖2 仿真系統(tǒng)的邏輯框架

五自由度機器人位姿初始化調(diào)整的業(yè)務(wù)流程如下：

1）控制程序調(diào)用各電機驅(qū)動器的驅(qū)動接口，與5個高精度伺服電機建立通信連接。

2）連接完成后，程序讀取伺服電機的輪上碼值，并進行硬件定義上的回原點，調(diào)整后通過正向運動學算法，計算出末端執(zhí)行器的當前位姿。

3）程序調(diào)用逆向運動學算法，控制末端執(zhí)行器運動到初始工作位姿（軟件定義上的回原點）。

五自由度機器人位姿仿真過程如下：

1）控制程序讀入末端執(zhí)行器的目標位姿，調(diào)用逆向速度運動學算法，計算出五自由度上電機各自的調(diào)整值。

2）通過標準化控制接口，傳輸控制信號給5個伺服電機。

3）電機運動完成后，進行位姿校準，程序讀取電機的輪上碼值，調(diào)用正向運動學算法，確定末端執(zhí)行器調(diào)整后的位姿。

在上面的執(zhí)行過程中，如調(diào)整后的位姿與目標位姿之間的相對誤差超過預設(shè)的誤差量級，此時將觸發(fā)位姿反饋，重復上述操作直至調(diào)整后末端執(zhí)行器的位姿符合預設(shè)。

3 實驗與分析

五自由度機器人的運動控制軟件采用Visual Studio開發(fā)環(huán)境，集成了Matlab軟件開發(fā)的末端執(zhí)行器的位姿在線仿真平臺，兩者之間協(xié)同完成仿真任務(wù)。仿真實驗時，可以給定一組結(jié)構(gòu)參數(shù)：c2,a4,b5分別取1.115 m,0.340 m，?0.145 m，選取一個馬鞍面作為五自由度機器人的作業(yè)任務(wù)面進行運動控制與位姿仿真，其數(shù)學方程為：

其中，取值范圍：?0.400 m≤x,y≤0.400 m，Δx=Δy=0.020 m。

圖3為根據(jù)馬鞍面數(shù)學方程得到末端執(zhí)行器的目標位置仿真圖。末端執(zhí)行器的指向取目標位置所在XY平面內(nèi)的原點到目標位置的方向，此時末端執(zhí)行器的位姿空間分布如圖4所示，俯視圖如圖5所示，其中相鄰且顏色不同的兩個點間的方向構(gòu)成了末端執(zhí)行器的指向。

圖3 末端執(zhí)行器的目標位置仿真圖

圖4 末端執(zhí)行器的空間位姿仿真圖

圖5 末端執(zhí)行器位姿仿真俯視圖

定義末端執(zhí)行器目標位姿的位置（Tx,Ty,Tz）和指向（Tφ,Tθ），調(diào)整后末端執(zhí)行器位姿的位置（Cx,Cy,Cz）和指向（Cφ,Cθ）。當末端執(zhí)行器調(diào)整后的位姿與目標位姿之間的誤差≤10?4m時，視為調(diào)整完成，此時定義對應點的位置誤差Δd和指向誤差Δδ分別為：

計算得到仿真位姿最大的位置誤差為：Δd=3.624 159 774 613 950×10?13m，指向誤差為：Δδ=1.998 894 815 878 120×10?14rad，發(fā)現(xiàn)兩者相對誤差非常小，因此該數(shù)值驗證了所提出的正向運動學和逆向運動學解法正確有效。

對五自由度機器人進行運動控制與空間位姿仿真，通過控制程序，調(diào)整末端執(zhí)行器到達預設(shè)的起始工作點，末端執(zhí)行器起始位姿和目標位姿的參數(shù)如表1所示。

表1 末端執(zhí)行器起始位姿和目標位姿的參數(shù)

五自由度機器人的空間位姿仿真結(jié)果如圖6所示，圖中展示了對機器人三次靜態(tài)位姿下各個關(guān)節(jié)節(jié)點和指向節(jié)點的位置，其中指向節(jié)點上的數(shù)字0、1和2分別表示位姿調(diào)整的次數(shù)，第0次表示機器人的起始結(jié)構(gòu)位姿，則末端執(zhí)行器由起始位姿向目標位姿共調(diào)用運動學算法并調(diào)整了2次。計算第2次調(diào)整后相應的位置誤差與指向誤差，誤差結(jié)果均為0，結(jié)果表明該機器人運動控制與空間位姿仿真系統(tǒng)準確有效。

圖6 五自由度機器人空間位姿仿真圖

4 結(jié)束語

本文設(shè)計并實現(xiàn)了一款五自由度機器人運動控制與空間位姿仿真系統(tǒng)，針對五自由度機器人進行運動學建模，提出了正向運動學和逆向速度運動學求解方法，并選取馬鞍面為作業(yè)任務(wù)面進行在線仿真，數(shù)值計算結(jié)果表明末端執(zhí)行器的仿真位姿與目標位姿具有一致性，驗證了仿真系統(tǒng)的可行性與工作穩(wěn)定性。該仿真系統(tǒng)可實現(xiàn)五自由度機器人的運動控制與位姿仿真的一體化，為后續(xù)多自由度機器人的結(jié)構(gòu)優(yōu)化、可達工作空間分析與性能評估提供平臺支撐與技術(shù)基礎(chǔ)。