軟實時環(huán)境下機器人運動學(xué)逆解研究

李啟源 裴海龍

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

0 引言

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

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

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

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

RTLinux是在Linux內(nèi)核下層添加了一個簡單的實時內(nèi)核［3］，將Linux本身作為這個實時內(nèi)核的優(yōu)先級最低的任務(wù)，即所有的實時任務(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進程仍可以在Linux內(nèi)核上運行，這樣既可以使用標準分時操作系統(tǒng)即Linux的各種服務(wù)，又能提供低延時的實時環(huán)境［5］。

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

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

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

3 硬件接口設(shè)計

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

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

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

4 機器人建模

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

圖4 機器人坐標系統(tǒng)Fig.4 Robot coordinate system

圖4中，機器人的六個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié)。前三個關(guān)節(jié)確定手腕參考點的位置，后三個關(guān)節(jié)確定手腕的方位。與大多數(shù)的工業(yè)機器人一樣，最后三個關(guān)節(jié)軸交于一點。將該點選作手腕的參考點，同時選作為連桿坐標系{4}、{5}、{6}的原點。關(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)機器人的連桿參數(shù)如表1所示。

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

5 求解過程

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

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

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

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

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

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

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

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

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

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

6 試驗驗證

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

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

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

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

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

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

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

7 結(jié)束語

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

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

［2］涂剛.嵌入式操作系統(tǒng)綜述［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的實時性能測試［J］.微電子學(xué)與計算機，2004，21(11):85 －88.

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

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

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