摘  要： 在機器人技術研究中，為了提高機器人控制算法的開發(fā)效率，提出移動機器人三維仿真軟件的設計方案并加以實現。該軟件采用ODE物理引擎生成動力學世界和實現碰撞檢測，提高了仿真速度和精確度，同時采用OpenGL繪制三維圖形，改善了圖形顯示效果。仿真實例證明，該軟件具有一定的實用價值。
關鍵詞： 移動機器人；三維仿真；ODE；OpenGL；控制算法

    在研究移動機器人控制算法和多機器人協(xié)作控制算法時，常采用仿真軟件來提高算法的開發(fā)效率并減少實驗成本。仿真軟件可直接利用物理引擎對剛體對象的運動進行仿真，物理引擎為剛體賦予真實的物理屬性，以此對物體的運動、旋轉和碰撞進行仿真計算。目前主流物理引擎有Havok(Havok Game Dynamics)、PhysX(Physics Engine Middleware)、Bullet（Bullet Physics Engine）和ODE（Open Dynamics Engine）等[1]，Havok是業(yè)界功能最全面的物理仿真解決方案，主要應用于實時碰撞計算、動力學約束求解和車輛綜合解決方案等領域；PhysX的特點是適合計算量比較大的物理模擬計算；Bullet的特點是具有最先進的碰撞檢測技術，主要用于柔體動力學和剛體動力學的仿真；ODE的特點是具有高級的關節(jié)類型和碰撞檢測整合系統(tǒng)，它適合于對機電一體化設備和移動機器人的模擬，并且易于與C/C++ API配合使用，具有快速性、強健性和可移植性等特點[2]。
    在采用物理引擎生成動力學世界之后，需利用圖形繪制程序將場景繪制到顯示終端[3]。常用的圖形繪制函數庫有OpenGL(Open Graphics Library)、DirectX(Direct Extension)和Irrlicht(Irrlicht Engine)等，OpenGL主要應用于專業(yè)高端繪圖領域[4]，具有使用簡便、效率高和可移植等特點；DirectX是微軟開發(fā)的三維繪圖軟件包，性能上已經超過OpenGL，但其接口采用COM方式；Irrlicht為高性能實時3D引擎，它可以提供在商業(yè)級3D引擎上才具有的藝術特性。
    綜上所述，在不影響軟件性能的前提下，為了縮短仿真軟件的開發(fā)周期，本文采用ODE物理引擎和OpenGL來開發(fā)移動機器人三維仿真軟件。
1 軟件特性
    (1)采用基于面向對象技術實現，軟件操作簡單，易于維護和功能擴展；
    (2)可以導入x格式和3ds格式的三維模型文件；
    (3)允許物體同時實現多個移動操作，在每個運動方向都有加速度、減速度和最大速度等運動屬性；實現碰撞檢測、移動機器人和虛擬場景的圖形化顯示；
    (4)支持實時調試功能； 3D動畫和仿真計算結果同步且真實對應;繪制仿真環(huán)境的二維地圖和物體運動軌跡。
    (5)提供與外部軟件連接的接口，即可以通過ODBC與外部數據庫相連，或通過Socket接口與外部設備相連，實現進程之間的通信；
    (6)軟件接口的多樣性和擴展性,即可通過游戲手柄、鍵盤來輸入控制信息、模型參數和仿真參數等；仿真數據的保存輸出。

其中，速度比例系數k=0.035，線速度單位為m/s，角速度單位為rad/s。

2.2 三維物體建模
    ODE物理引擎提供球體、盒子、膠囊、平面和圓柱等幾何體。在創(chuàng)建出一個幾何體后，其中心一般落在仿真環(huán)境坐標系的原點上。在對移動機器人進行建模時，需要使用盒子和圓柱兩種幾何體，盒子需要指定3個參數，即長、高、寬;對于圓柱而言，則需指定長度和半徑2個參數。在動力學世界中，以Geom代表物體幾何體，以Body代表虛擬場景中的對象。軟件可以通過調用ODE內部函數來檢測幾何體和對象的對應關系，也有函數用于檢測對象之間是否存在連接。下面以創(chuàng)建盒子物體為例，說明單個物體的建模方法。
    首先調用dBodyCreate函數創(chuàng)建出給定空間中的剛體對象,再調用dBodySetPosition和dBodySetRotation兩個函數，調整該對象在空間中的位姿,接著調用dMassSetBoxTotal和dBodySetMass兩個函數設定該對象的質量屬性,最后調用dCreateBox函數創(chuàng)建相應尺寸的盒子幾何體,并調用dGeomSetBody函數將該幾何體與對象關聯起來。
    在創(chuàng)建出單個對象后，往往需要利用各種關節(jié)將不同對象連接起來。ODE物理引擎提供5種類型的關節(jié)，分別為鉸鏈型、球-球窩型、滑竿柱型、固定型和鉸鏈2型等。其中鉸鏈型為合頁關節(jié)，滑竿柱型為插銷關節(jié)，鉸鏈2型則是帶有軸的關節(jié)，這些關節(jié)都有內置的馬達。本文選用鉸鏈關節(jié)來連接驅動輪和機器人車體，采用固定關節(jié)構建機器人車體結構。下面以使用鉸鏈關節(jié)連接兩個對象為例，說明創(chuàng)建關節(jié)的方法。
    在調用dJointCreateHinge函數創(chuàng)建鉸鏈關節(jié)對象后，再調用dJointAttach函數指定用該關節(jié)連接的兩個物體對象，然后調用dJointSetHingeAnchor函數設定旋轉軸的中心點坐標，并調用dJointSetHingeAxis設定旋轉軸的方向。
    為了讓剛體對象能夠在仿真環(huán)境中運動起來，ODE提供了3種方法： (1)調用dBodyAddForce、dBodyAddTorque等函數給剛體施加力的作用; (2)調用dJointSetHingeParam
函數來改變內置馬達的轉速，同時需指定該函數的第二個輸入參數為dParamVel; (3)調用dBodySetLinearVel和dBodySetAngularVel兩個函數直接給物體設定線速度和角速度。
    此外，在ODE仿真環(huán)境中，可通過兩種方式來模擬彈簧-阻尼系統(tǒng): (1)通過設置ERP（Error Reduction Parameter）和CFM（Constraint Force Mixing）兩個參數來實現，ERP為每一仿真循環(huán)中的修正誤差，取值范圍為0~0.8，默認取值為0.2；CFM代表物理引擎的全局混合約束力，它反映物體表面的柔軟程度，其取值范圍為10e-9~1；(2)利用動力學方程來求解，即胡克定律:

其中γ為阻尼系數,它與物體的形狀以及周圍性質有關。
    綜上所述，典型ODE仿真過程為[6]：
    (1)生成一個動力學世界，并在該世界中創(chuàng)建物體；
    (2)設置物體狀態(tài)（如質量、質心位置和姿態(tài)等），并在動力學世界中創(chuàng)建關節(jié)；
    (3)將關節(jié)與物體綁定起來,為所有的關節(jié)設置參數；
    (4)生成碰撞檢測空間，并為需要作碰撞檢測的物體生成碰撞幾何體；創(chuàng)建一個容納關節(jié)的關節(jié)組；
    (5)循環(huán)處理過程:
　     ①在物體上施加力；
        ②根據需要調整關節(jié)參數；
        ③調用碰撞檢測，得到碰撞點和碰撞的物體；
        ④為每個碰撞點的碰撞創(chuàng)建一個接觸關節(jié)，并將其放入關節(jié)組；
        ⑤執(zhí)行一個仿真步驟；
        ⑥清空接觸關節(jié)組中的關節(jié)；
    (6)銷毀動力學世界和碰撞世界。
2.3 三維圖形的繪制
　 OpenGL繪制圖形的基本操作步驟[4]：
    (1)設置像素格式：設定OpenGL繪制風格、顏色模式和顏色位數等重要信息。
    (2)建立模型：根據基本圖元建立景物的三維模型，并對模型進行數學描述。
    (3)舞臺布置：把景物放置在三維空間的適當位置，設置視點、視角和投影模型等。
    (4)效果處理：設置物體的材質，加入光照及光照條件。
    (5)光柵化：把景物及其顏色信息轉化為可在計算機屏幕上顯示的像素信息。
    在繪制圖形時，需注意坐標系的變換，否則很容易導致繪制失敗。OpenGL定義了兩個坐標系：世界坐標系和當前繪圖坐標系。世界坐標系是固定不變的，規(guī)定以屏幕中心為原點，面對顯示終端，向右為x正軸，向上為y正軸，向終端外面為z正軸。當前繪圖坐標系是繪制物體時的參考坐標系。仿真軟件完成初始化后，世界坐標系與當前繪圖坐標系是重合的。在調用glTranslatef、glRotate等變換函數對繪圖坐標系進行平移和旋轉后，繪圖坐標系會在原來的基礎上做出相應改變。此時，若調用gluSphere、glVertex3f等繪圖函數，繪圖函數是在改變之后的繪圖坐標系上進行繪制。如若要讓繪圖坐標系與世界坐標系再次重合，可以調用glLoadIdentity函數。此外，可以調用glColor3f（r，g，b）函數設置繪圖函數所使用的顏色，如果沒有再次調用該函數，則繪制出的圖形顏色將保持原先顏色不變，rgb三個顏色分量的取值范圍為0.0~1.0。
3 軟件框架
    軟件框架及其處理流程如圖2所示，軟件仿真循環(huán)的處理流程如圖3所示，下面簡述主要處理過程的實現思路及方法：

    (1)軟件的初始化
    軟件的初始化工作主要包括：從配置文件讀入移動機器人的數量、軟件運行時所需文件的路徑和繪制窗口大小等參數；導入虛擬場景所需的素材，如聲音、紋理和3ds模型等，本文采用lib3ds庫來讀取3ds文件[6]；初始化ODBC 接口，獲取ODBC 環(huán)境句柄，實現進程與數據庫的數據交互；初始化游戲手柄和UDP通信網絡；初始化ODE環(huán)境，創(chuàng)建動力學環(huán)境和碰撞檢測空間，創(chuàng)建移動機器人和仿真場景。
    (2)物體運動參數的接收
    在仿真循環(huán)中，利用UDP協(xié)議實現本地進程與其他進程之間的通信，該軟件也可以從鍵盤、游戲手柄和網絡數據庫讀入機器人的動作指令。
    (3)物體碰撞檢測
    本文將與物理引擎有關的操作函數封裝成動態(tài)鏈接庫，通過使用動態(tài)鏈接庫技術，軟件可以實現模塊化，即由相對獨立的組件來組成整個軟件。這簡化了軟件項目的管理，而且能夠節(jié)省內存，也有助于資源共享和代碼更新移植。
    ODE 的碰撞檢測引擎需要給定兩個物體的形狀信息。在每一仿真循環(huán)中，調用dSpaceCollide函數獲取可能發(fā)生接觸的物體，再由該函數指定的碰撞回調函數nearCallback將接觸點信息傳給用戶。由此，用戶可根據自身需要建立物體間的碰撞連接點，每個連接點都有相應的dContactGeom 結構，該結構保存著碰撞點的位置和兩個物體互相進入對方的深度。為了提高仿真速度，軟件使用較少的關節(jié)和較少的接觸面，如果條件允許，也可以使用無摩擦或者粘性接觸面。下面給出碰撞回調函數中關于接觸面的參數設置：
    surface.mu=3.0；                         //庫侖摩擦力系數
    surface.mu2=0.0；                           //庫侖摩擦力系數2
    surface.slip1=0.05；        //摩擦力1方向的滑動摩擦力
    surface.slip2=0.05；        //摩擦力2方向的滑動摩擦力
    surface.bounce=0.9；                                 //反彈系數
    surface.bounce_vel=1.0；  //反彈所需要的最小碰撞速度
    surface.soft_erp=0.2；      //接觸點法線方向“柔軟”參數
    surface.soft_cfm=1e-4；   //接觸點法線方向"柔軟"參數

    (4)三維圖形繪制
    在仿真軟件中，與繪制圖形有關的操作函數都被封裝成動態(tài)鏈接庫，該動態(tài)鏈接庫只向仿真軟件提供若干個接口，如在繪制機器人時，只需根據機器人的不同部位，設定相應的繪制參數，然后輪流調用dsDrawCylinder、dsDrawBox兩個函數即可實現，dsDrawCylinder函數的輸入參數分別為物體位置、朝向、長度和半徑，而dsDrawBox函數的輸入參數為位置、朝向和尺寸；在繪制從x文件和3ds文件導入的三維模型時，只需調用dsDrawTriangleD函數即可。
4 仿真實例
    本文在VS2008平臺上開發(fā)仿真軟件，ODE版本為0.9，OpenGL版本為1.0，軟件為控制臺程序。
　 在檢驗路徑規(guī)劃算法或避障算法時，可先創(chuàng)建一個虛擬足球場,再設定機器人初始位置的絕對坐標為(-9.0，-5.0)，目的地絕對坐標為(5.5，5.0)，并在路徑的中間布置8個或更多的障礙物，障礙物直徑為50 cm，其位置參數可以人為指定，也可以隨機產生，然后在虛擬場景的二維地圖上，以宏觀鳥瞰的遠程視野，對避障算法的仿真結果進行觀察、比較和分析，仿真效果如圖4所示。若需了解算法運行的細節(jié)，可以直接觀察3D環(huán)境的仿真過程，或通過分析記錄下的仿真數據來比較算法的優(yōu)劣。而在檢驗多機器人協(xié)作算法時，可同時利用三維環(huán)境和二維全局地圖來對算法性能進行比較，以由多個移動機器人組成的足球隊為例，移動機器人需按照協(xié)作算法來實現站位[7]。

    此外，在采用PID控制算法對機器人位置進行控制時，有比例增益、微分增益和積分增益三個參數需要整定測試[8]，此時,可以利用仿真軟件對參數進行步估計。實驗方法是：先設定初始位置和目的地位置，然后讓機器人以最快速度向目的地移動，當到達目的地后，就停止不動。在機器人移動過程中，記錄下機器人在每一仿真步驟中的位姿(x,y,θ)T，并將位姿數據歸一化，以便能夠將三維的向量繪制在同一坐標系下，同時記錄機器人走完設定路徑所需的時間。圖5(a)為采用比例控制算法的機器人位姿變化軌跡，機器人走完該路徑所需時間為16.239 s，圖5(b)為采用比例-微分控制算法的變化軌跡，所需時間為14.026 s。從圖5的結果可看出，采用比例控制算法的系統(tǒng)輸出無超調，而采用比例-微分控制算法的系統(tǒng)，其控制目標會出現微小超調，但機器人的響應速度明顯提高，尤其是機器人朝向角的控制，能夠更加快速準確地跟隨給定值。

    在移動機器人控制技術和多機器人協(xié)作技術的研究中，為了能夠對算法參數進行有效檢驗和測試，本文利用ODE、OpengGL和VS2008開發(fā)出移動機器人仿真軟件。該軟件采用ODE生成動力學世界和模擬物體碰撞，充分利用了ODE的快速性和精確性，仿真軟件還采用高效的圖形接口OpenGL來繪制圖形，提高軟件的圖形處理能力，改善圖形顯示效果。仿真實例證明，該軟件具有較好的擴展性和實用性。
參考文獻
[1] 胡宇慧. 3D游戲引擎中若干關鍵技術及算法的研究與分析[D]. 上海:上海師范大學, 2006.
[2] 高嵩. 基于OGRE和ODE的駕駛模擬系統(tǒng)的設計與實現[D]. 武漢:武漢理工大學, 2006.
[3] 歐陽慧琴, 陳福民. 物理引擎與圖形渲染引擎綁定的研究與實現[J]. 計算機工程與設計, 2008,29(21):5580-5620.
[4] 蔣東翔. 基于OpenGL的3D網絡游戲開發(fā)中關鍵技術的研究[D]. 哈爾濱:哈爾濱工程大學, 2008.
[5] TAKEMURA Y. Development of “Hibikino-Musashi” omnidirectional mobile robot[J/OL]. http://robocup.ksrp.or.jp/hibikino-musashi/wp-content/uploads/2010/05/BrainIT2006_ Ronbun.pdf.
[6] DEMURA K. 3DS Loader for open dynamics engine-0.0[CP/OL]. http://demura.net/9ode/460.html.
[7] 邢關生, 張建勛. 移動機器人網絡運動協(xié)調的控制與仿真[J]. 系統(tǒng)仿真學報, 2010,22(03).
[8] 楊智, 朱海鋒, 黃以華. PID控制器設計與參數整定方法綜述[J]. 化工自動化及儀表, 2005,32(05):1-7.