軟實(shí)時環(huán)境下機(jī)器人運(yùn)動學(xué)逆解研究

李啟源 裴海龍

(華南理工大學(xué)自動化科學(xué)與工程學(xué)院，廣東 廣州 510640)

0 引言

典型機(jī)器人控制系統(tǒng)一般采用分層遞階結(jié)構(gòu)［1］，即將一個復(fù)雜的任務(wù)層分解落實(shí)，最終形成一系列底層硬件系統(tǒng)可高速串行執(zhí)行的簡單“原子級動作”。這樣的控制系統(tǒng)適用于大型、復(fù)雜、多任務(wù)環(huán)境下的機(jī)器人系統(tǒng)，然而當(dāng)考慮到空間安裝尺寸、運(yùn)算速度和價格等因素時，上述傳統(tǒng)控制系統(tǒng)架構(gòu)就有了其“力不從心”的地方。在這種情形下，開源軟實(shí)時操作系統(tǒng)架構(gòu)下的機(jī)器人控制系統(tǒng)應(yīng)運(yùn)而生。

1 機(jī)器人控制系統(tǒng)的組成

機(jī)器人控制系統(tǒng)采用基于PC的技術(shù)路線。硬件結(jié)構(gòu)以PC104架構(gòu)為基礎(chǔ)，通過PCI/ISA總線連接以FPGA為核心的底層硬件電路;軟件結(jié)構(gòu)在RTLinux上進(jìn)行二次開發(fā)，利用其實(shí)時性和多任務(wù)調(diào)度功能實(shí)現(xiàn)復(fù)雜的機(jī)器人任務(wù)［2］。

2 RTLinux及應(yīng)用系統(tǒng)

RTLinux是在Linux內(nèi)核下層添加了一個簡單的實(shí)時內(nèi)核［3］，將Linux本身作為這個實(shí)時內(nèi)核的優(yōu)先級最低的任務(wù)，即所有的實(shí)時任務(wù)的優(yōu)先級都高于Linux系統(tǒng)本身的以及Linux系統(tǒng)下的一般任務(wù)［4］。RTLinux結(jié)構(gòu)如圖1所示。

圖1 RTLinux結(jié)構(gòu)圖Fig.1 Structure of RTLinux

正常的Linux進(jìn)程仍可以在Linux內(nèi)核上運(yùn)行，這樣既可以使用標(biāo)準(zhǔn)分時操作系統(tǒng)即Linux的各種服務(wù)，又能提供低延時的實(shí)時環(huán)境［5］。

機(jī)器人控制系統(tǒng)的軟件部分采用RTLinux平臺，以C、C++語言編寫，可以分為四部分:①人機(jī)接口(示教盒);②任務(wù)調(diào)度與分解進(jìn)程，負(fù)責(zé)從用戶界面進(jìn)程獲得指令輸入，結(jié)合從運(yùn)動規(guī)劃與控制進(jìn)程和傳感器管理進(jìn)程獲得的當(dāng)前系統(tǒng)狀態(tài)，作出系統(tǒng)動作決策，并將當(dāng)前允許執(zhí)行的指令通過共享內(nèi)存消息機(jī)制發(fā)送到下一層的進(jìn)程執(zhí)行;③運(yùn)動規(guī)劃與控制，根據(jù)任務(wù)調(diào)度與分解模塊發(fā)出的運(yùn)動指令，實(shí)現(xiàn)機(jī)器人正逆學(xué)分解及運(yùn)動規(guī)劃/插補(bǔ)計算、位置控制等;④傳感器信息處理，接收外界信息后進(jìn)行預(yù)處理，然后傳遞給任務(wù)調(diào)度與分解進(jìn)程。這四部分各自以進(jìn)程的形式獨(dú)立運(yùn)行，通過共享內(nèi)存方式進(jìn)行交互通信，其中運(yùn)動規(guī)劃與控制進(jìn)程是實(shí)時進(jìn)程，其他為常規(guī)進(jìn)程。

按照功能劃分，機(jī)器人控制系統(tǒng)結(jié)構(gòu)如圖2所示。

圖2 機(jī)器人控制系統(tǒng)結(jié)構(gòu)圖Fig.2 Structure of robot control system

3 硬件接口設(shè)計

本文設(shè)計的機(jī)器人控制系統(tǒng)由上位機(jī)、運(yùn)動控制接口電路和被控對象構(gòu)成。上位機(jī)使用桑達(dá)的PC104，接口電路的核心部件是FPGA，被控對象為安川公司的交流伺服電機(jī)。機(jī)器人控制系統(tǒng)硬件電路結(jié)構(gòu)如圖3所示。

圖3 硬件電路結(jié)構(gòu)圖Fig.3 Schematic of the hardware circuit

伺服電機(jī)自帶光電編碼器，能自動檢測并反饋電機(jī)的位置值，反饋信號為相位差90°的A/B兩相信號。A/B兩相信號進(jìn)入FPGA后，通過FPGA內(nèi)部設(shè)計的計數(shù)器模塊對A/B信號進(jìn)行計數(shù)，獲得各個電機(jī)的位置信息并存儲在FPGA的內(nèi)部寄存器中。PC104通過PCI/ISA總線，從FPGA中讀取該寄存器的值，從而得到電機(jī)的位置值。將該電機(jī)的實(shí)際位置值與系統(tǒng)傳輸?shù)睦硐胛恢弥当容^，得到一個偏差值，再根據(jù)一定的算法對偏差進(jìn)行插補(bǔ)、PID運(yùn)算等處理，得到一個新的控制信號。通過PCI/ISA總線，將控制信號發(fā)送給伺服控制板，最后通過FPGA中的pulse模塊發(fā)送一定脈寬的脈沖，并通過伺服控制器驅(qū)動電機(jī)達(dá)到想要的速度和位置［6］。從上述原理來看，本設(shè)計構(gòu)成對電機(jī)的雙閉環(huán)控制，外部為位置環(huán)、內(nèi)部為速度環(huán)，確保了電機(jī)運(yùn)動的速度與精度。

4 機(jī)器人建模

現(xiàn)有機(jī)器人本體屬于關(guān)節(jié)式機(jī)器人，其坐標(biāo)系如圖 4 所示［7］。

圖4 機(jī)器人坐標(biāo)系統(tǒng)Fig.4 Robot coordinate system

圖4中，機(jī)器人的六個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié)。前三個關(guān)節(jié)確定手腕參考點(diǎn)的位置，后三個關(guān)節(jié)確定手腕的方位。與大多數(shù)的工業(yè)機(jī)器人一樣，最后三個關(guān)節(jié)軸交于一點(diǎn)。將該點(diǎn)選作手腕的參考點(diǎn)，同時選作為連桿坐標(biāo)系{4}、{5}、{6}的原點(diǎn)。關(guān)節(jié)1的軸線為鉛直方向，關(guān)節(jié)1和關(guān)節(jié)2的軸線垂直交錯，距離為a1;關(guān)節(jié)2和關(guān)節(jié)3的軸線水平且平行，距離為a2;關(guān)節(jié)3和關(guān)節(jié)4的軸線垂直交錯，距離為a3;關(guān)節(jié)3和關(guān)節(jié)4沿z軸方向的距離為d4。由圖4可知，a1=150 mm、a2=570 mm、a3=150 mm、d4=650 mm。相應(yīng)機(jī)器人的連桿參數(shù)如表1所示。

表1 機(jī)器人的連桿參數(shù)表Tab.1 Link parameters of robot

5 求解過程

基于現(xiàn)有機(jī)器人D-H模型，使用反變換法+Pieper方法討論機(jī)器人的逆運(yùn)動學(xué)問題，具體為利用Pieper方法求解 θ2、θ3，其他利用反變換法求解［8］。

在此已知的是世界坐標(biāo)值 x、y、z、a、b、c，在求解過程中，首先要將世界坐標(biāo)值轉(zhuǎn)換為奇次變換矩陣。奇次變換矩陣包含平移和旋轉(zhuǎn)兩部分。平移部分即奇次變換矩陣的第四列的前三行的三個元素，它們是x、y、z的值;而旋轉(zhuǎn)部分是前三行和前三列部分的九個元素，它們是 r、p、y(即 a、b、c)三個角度的正余弦組合。即:

式中:γ=a、β=b、α=c。這樣，由已知的世界坐標(biāo)系中的a、b、c值就可以求得旋轉(zhuǎn)矩陣06Rxyz(γ，β，α)。

θ1求解過程即利用反變換法求解θ1，由于上述模型中d2=0，故:

利用Pieper方法求解θ2和θ3，分別有:

至此，可以求出 θ1、θ2、θ3的表達(dá)式，代入相應(yīng)的參數(shù)即可求出其值。下面根據(jù)手腕方位來反解出θ4、θ5和θ6。在此選用反變換法，由于現(xiàn)有的機(jī)器人D-H模型與PUMA560的差別僅在于12T的第四列不同，即奇次變換矩陣12T中關(guān)于平移的部分不同，關(guān)于旋轉(zhuǎn)的部分是完全一樣的。

由于上文已經(jīng)求出了θ1、θ2和θ3，則經(jīng)推導(dǎo)可知:

只要 s5≠0，便可求出 θ4，即:

當(dāng)s5=0時，操作臂處于奇異形位。此時，關(guān)節(jié)4與關(guān)節(jié)6重合，只能解出θ4與θ6的和或者差。奇異形位可以由式(7)中Atan2的兩個變量是否都接近0來判斷。若都接近0，則為奇異形位，否則不是奇異形位。在奇異形位時，可以任意選取θ4的值，再計算相應(yīng)的θ6的值。根據(jù)求得的θ4，便可以進(jìn)一步求解θ5，方法和求解θ4是一樣的。θ5的封閉解為:

有了上述解，可求出θ6的封閉解為:

6 試驗驗證

通過上述軟件實(shí)時系統(tǒng)和機(jī)器人坐標(biāo)系統(tǒng)運(yùn)動學(xué)系統(tǒng)的構(gòu)建，即可形成真正的機(jī)器人控制系統(tǒng)。為驗證機(jī)器人控制系統(tǒng)軟件算法的正確性和可靠性，需要對工業(yè)機(jī)器人在伺服控制、運(yùn)動控制板卡控制以及軟件控制三種控制模式下的運(yùn)動性能進(jìn)行對比測試。對機(jī)器人各軸在同一控制模式下不同速度運(yùn)行進(jìn)行對比測試，可以揭示不同速度時的運(yùn)動性能。

本方案采用激光測振儀對工業(yè)機(jī)器人各軸進(jìn)行運(yùn)動測試，給出各軸運(yùn)動速度曲線和相關(guān)參數(shù)，從以上三個角度對機(jī)器人的運(yùn)動性能進(jìn)行對比分析。

測試儀器為Polytec激光測振儀和Tektronix四通道數(shù)字示波器。測試方案具體介紹如下。

①測試點(diǎn)布置:取點(diǎn)原則是盡量反映各軸的運(yùn)動狀態(tài)，以便于測量。機(jī)器人姿態(tài)為原點(diǎn)。

②測試方法:在三種控制模式下對各軸的運(yùn)動性能對比測試。

③測試步驟:第一步是在機(jī)器人本體各測試點(diǎn)粘貼反光紙，調(diào)試儀器;第二步是在伺服控制模式下，將第一軸的運(yùn)行速度分別設(shè)置為5 °/s、10 °/s、20 °/s，從初始位置單獨(dú)進(jìn)行小范圍運(yùn)動(－10°～+10°)，測量測試點(diǎn)的運(yùn)動速度曲線和相應(yīng)參數(shù);第三步是在板卡控制模式下，將第一軸的運(yùn)行速度分別設(shè)置為5°/s、10°/s、20 °/s，從初始位置單獨(dú)進(jìn)行小范圍運(yùn)動(－10°～+10°)，測量測試點(diǎn)的運(yùn)動速度曲線及相應(yīng)參數(shù);第四步是在軟件控制模式下，將第一軸的運(yùn)行速度分別設(shè)置為 5 °/s、10 °/s、20 °/s，從初始位置單獨(dú)進(jìn)行小范圍運(yùn)動(－10°～+10°)，測量測試點(diǎn)的運(yùn)動速度曲線和相應(yīng)參數(shù);第五步是使用第二步至第四步的方法，對2～6軸進(jìn)行測試。

測試結(jié)果如圖5所示，限于篇幅，只表現(xiàn)第六軸。與圖5(a)和圖5(c)相比，圖5(b)是經(jīng)過軟件運(yùn)動規(guī)劃/伺服控制處理后系統(tǒng)第六軸以20°/s的速度運(yùn)行得到的曲線，運(yùn)行平滑穩(wěn)定，毛刺較少。該結(jié)果充分說明了以RTLinux為核心的軟實(shí)時控制系統(tǒng)在穩(wěn)定性以及串聯(lián)機(jī)構(gòu)末端振動的抑制方面具有較好的效果。

7 結(jié)束語

本測試采用激光測振儀對工業(yè)機(jī)器人各軸進(jìn)行小范圍運(yùn)動測試，得到各軸運(yùn)動速度曲線和相關(guān)參數(shù)，并通過對比的方法，從三個角度對機(jī)器人的運(yùn)動性能進(jìn)行詳細(xì)分析。通過對機(jī)器人1～6軸三種控制模式下的運(yùn)動性能進(jìn)行對比測試，結(jié)果表明，軟件控制模式的效果優(yōu)于伺服控制和板卡控制，排除了軟件控制引起振動的可能;軟件方式效果明顯優(yōu)于前兩種，充分說明了基于RTLinux的機(jī)器人控制系統(tǒng)的有效性。

［1］國家863計劃智能機(jī)器人專家組.機(jī)器人博覽［M］.北京:中國科學(xué)技術(shù)出版社，2001.

［2］涂剛.嵌入式操作系統(tǒng)綜述［J］.計算機(jī)應(yīng)用研究，2000(11):4－6.

［3］周德新，張向利.Linux和嵌入式操作系統(tǒng)［J］.桂林電子工業(yè)學(xué)院學(xué)報:自然科學(xué)版，2000，20(4):21 －23.

［4］衷克定.大學(xué)信息技術(shù)(Linux操作系統(tǒng)及其應(yīng)用)［M］.北京:清華大學(xué)出版社，2005.

［5］朱響斌，涂時亮.Linux的實(shí)時性能測試［J］.微電子學(xué)與計算機(jī)，2004，21(11):85 －88.

［6］劉韜，樓興華.FPGA數(shù)字電子系統(tǒng)設(shè)計與開發(fā)實(shí)例導(dǎo)航［M］.北京:人民郵電出版社，2005.

［7］日本機(jī)器人學(xué)會.機(jī)器人技術(shù)手冊［M］.宗光華，程君實(shí)，譯.北京:科學(xué)出版社，2007.

［8］熊有倫.機(jī)器人技術(shù)基礎(chǔ)［M］.武漢:華中科技大學(xué)出版社，1992.