五自由度機(jī)器人運(yùn)動(dòng)控制與空間位姿仿真系統(tǒng)

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

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

0 引 言

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

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

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

1 運(yùn)動(dòng)學(xué)建模與求解

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

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

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

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

1.2 正向運(yùn)動(dòng)學(xué)求解

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

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

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

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

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

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

1.3 逆向速度運(yùn)動(dòng)學(xué)求解

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

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

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

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

綜上，得到表達(dá)式：

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

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

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

綜上，根據(jù)速度運(yùn)動(dòng)學(xué)公式得：

其中J?為雅可比矩陣的偽逆矩陣，代入計(jì)算出，即五自由度上的速度調(diào)整量，用乘以電機(jī)的掃描時(shí)間間隔得到5個(gè)電機(jī)對(duì)應(yīng)的運(yùn)動(dòng)調(diào)整量。

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

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

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

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

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

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

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

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

1）控制程序讀入末端執(zhí)行器的目標(biāo)位姿，調(diào)用逆向速度運(yùn)動(dòng)學(xué)算法，計(jì)算出五自由度上電機(jī)各自的調(diào)整值。

2）通過標(biāo)準(zhǔn)化控制接口，傳輸控制信號(hào)給5個(gè)伺服電機(jī)。

3）電機(jī)運(yùn)動(dòng)完成后，進(jìn)行位姿校準(zhǔn)，程序讀取電機(jī)的輪上碼值，調(diào)用正向運(yùn)動(dòng)學(xué)算法，確定末端執(zhí)行器調(diào)整后的位姿。

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

3 實(shí)驗(yàn)與分析

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

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

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

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

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

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

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

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

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

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

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

圖6 五自由度機(jī)器人空間位姿仿真圖

4 結(jié)束語

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