移動機器人動力學控制半物理仿真研究

葉錦華，鄭炳坤，李 迪，葉 峰

（華南理工大學 機械與汽車工程學院，廣東 廣州 510641）

0 引 言

移動機器人在探索人類無法到達的未知和危險地域優勢突出，已廣泛地應用于工農業、國防等各個領域，對其進行相關技術研究意義重大。運動控制是移動機器人的核心研究內容之一，為了讓其能更接近“完美的速度跟蹤”，應當考慮系統的動力學模型［1］。在控制方法驗證上，采用實際實驗時［2］，移動機器人動力學系統的相關參數往往不易測量和調整，不利于控制方法的快速驗證，延長了系統的開發周期，而采用純數值仿真［3］，由于無法獲得系統的精確數學模型，往往忽略了實際系統的某些特性和參數，設計的控制器在實際系統中不定可用，為了解決上述問題，本文引入半物理仿真的概念。半物理仿真直接將控制對象連接在仿真系統中，因此也稱之為硬件在環（Hardware－In－The－Loop，HIL）［4］仿真，它是對實際過程進行的實時仿真，由于物理實體被引入到仿真回路中，因此比純數值仿真的置信度高，同時通過合理設計物理實體和仿真系統，可以獲得較為精確的系統模型，并方便系統參數的調整和復雜控制方法的測試，仿真過程結果的驗證直觀，操作簡便。

本文結合Matlab／Simulink仿真軟件與Quanser公司的Q8卡、QuaRC 開發環境和工業機電驅動單元（Industrial Mechatronic Drives Unit，IMDU），以軌跡跟蹤控制為例，建立了（2，0）［5］型輪式移動機器人（WMR）的動力學控制半物理仿真系統，試驗結果表明所設計的系統滿足所需性能指標，達到預期的設計要求。

1 Quanser仿真平臺簡介

Quanser公司的Q8卡［6］是一塊集實時檢測和控制于一體的高性能板卡，它提供了豐富的硬件接口和完善的軟件支持，帶有多路的高速A／D 輸入、高精度D／A 輸出，編碼器輸入、擴展數字I／O和PWM 輸出。Q8卡使用PCI接口連接計算機，并通過終端接口卡連接各種外部設備，形成閉環控制結構。

QuaRC［7］是Quanser的下一代多功能快速控制開發環境，它無縫集成在Simulink中。利用QuaRC提供的框圖模塊，并結合Simulink，用戶可以方便快速地搭建自己的系統模型，然后通過QuaRC自動生成實時代碼，將工業級的實時應用程序下載到Windows或QNX 等操作系統中，還可以通過QuaRC提供的“外部模式”通訊模塊將實時代碼下載到目標板卡上，實現對Quanser板卡的軟件控制。借助Simulink中的各種監測模塊，用戶可以實時觀測整個系統的運行狀態，從而增強了工程的可管理性同時降低了開發成本與時間。

IMDU［8］是一個可模擬各種工業控制單元的裝置系統，每個IMDU 主單元共有四根輸出軸，其中兩根是直流電機驅動軸，其余為自由軸，利用單個或多個IMDU 主單元及其豐富的模塊化組件，可以搭建帶有卷繞控制、間隙與摩擦補償，高階耦合系統等的各種復雜工業控制系統。IMDU提供電機力矩（電流）環的控制和反饋，且每根軸都配有高精度編碼器，IMDU 與Q8終端接口卡使用配套標準數據線進行連接。

2 WMR的動力學模型與控制器

2.1 WMR 動力學模型分析

半物理仿真對象（2，0）型WMR的機械結構如圖1所示，它由車體及兩個獨立驅動輪組成，通過控制兩驅動輪的不同轉速來實現其各種運動形式。建立WMR的慣性坐標系為X0O0Y0，局部坐標系為XcOcYc，Oc位于WMR的質心，θ為WMR的導向角度，v和w分別為WMR的線速度和角速度。兩驅動輪間的距離為2b，輪子半徑為r。

定義WMR的位姿為q＝［xcycθc］T，右左輪的轉速為ψ＝［φr φl］T，不失一般性，假設WMR 各輪為無變形的剛體，且在水平面運動，同時忽略表面摩擦、向心力和哥氏力，由于各輪受非完整約束，即滿足純滾動和無滑動的條件，其完整運動學及動力學方程可表示為［9］

圖1 WMR的結構

式中：S——WMR的廣義Jacobian 矩陣；M ——系統慣性矩陣；V＝［v w］T——速度輸入矢量；τd——未知外界擾動；B和C——變換矩陣；τ＝［τrτl］T——兩輪控制力矩輸入矢量。各矩陣取為

式中：m——整車質量，I＝mb2／4——車體繞質心軸的轉動慣量。

2.2 WMR 軌跡跟蹤動力學控制器

qr滿足如下等式

式中：Vr＝［vrwr］T——參考速度輸入矢量，定義跟蹤誤差為

基于反演（Backstepping）方法取得運動學控制器為［10］

式中：kx，ky，kθ——正的常數，采用非線性反饋設計加速度控制輸入u為

式中：kv——正的常數矩陣。最后應用計算力矩法取得兩輪的控制力矩為

控制力矩再轉換為IMDU 直流電機的驅動電流Ic，轉換關系式為

式中：kg——電機輸出到輪軸的減速比，kt——電機力矩常數。

3 半物理仿真系統的軟硬件實現

3.1 系統總體結構

WMR 動力學控制半物理仿真系統總體結構如圖2所示。

圖2 系統總體結構

系統使用IMDU 構建WMR的動力學系統，WMR 軌跡跟蹤的軟件控制在Simulink上實現，并通過QuaRC實現計算機與Q8卡的實時數據交互。Simulink將實時控制過程中計算得到的相關數據傳遞給Q8卡，Q8卡再通過終端接口卡輸出驅動電流給IMDU，實現WMR 兩輪的力矩控制，同時IMDU 通過終端接口卡將電機的實際驅動電流與增量式編碼器信號反饋給Q8卡，經Q8卡處理后，得到WMR力矩和速度反饋值返回給計算機，從而實現WMR 軌跡跟蹤的閉環動力學控制。

3.2 硬件實現

基于IMDU 構建的WMR 動力學系統如圖3所示。

圖3 基于IMDU 構建的WMR 動力學系統

IMDU 兩驅動軸1、2為WMR的轉動輪軸，剎車鼓組件3作為WMR的轉動輪，摩擦制動單元4模擬WMR 車身重量對輪子的壓力，并形成WMR 運動的摩擦阻力，壓力值可通過旋轉組件4的螺絲釘來調節，并可用測力計測得具體數值，定義壓力值為T，則WMR 車身的質量為m＝2T／g，式中，g為重力加速度。

3.3 軟件實現

動力學模型下，WMR 軌跡跟蹤控制的總體控制結構如圖4所示，它由三個控制環組成，即位置控制環，速度控制環和電流控制環。

圖4 WMR 軌跡跟蹤控制的總體控制結構

系統運行時，位置控制環根據WMR 反饋位姿取得位置控制偏差ep，并結合參考速度Vr，由運動控制器式（6）得到速度環的參考輸入Vc，然后速度控制環根據IMDU 編碼器反饋獲得的速度反饋值V ，取得速度控制偏差ec，由速度控制器式（7）和計算力矩控制器式（8）得到電流環的參考輸入力矩τ，并經過轉換式（9）得到IMDU的參考輸入電流Ic，最后電流控制環根據IMDU的電流反饋值取得電流控制偏差，并通過PI控制器得到最終IMDU電機的控制輸入電流。IMDU的輸出為兩電機軸即WMR兩輪的轉速ψ，應用式（3）ψ 可轉換為V ，再通過式（1）取得WMR 位姿的變化，最后使用積分運算獲得WMR的當前位姿反饋值。

基于上述控制過程，在Simulink 仿真環境下創建WMR 軌跡跟蹤的半物理仿真模型如圖5所示，它由參考軌跡生成單元、位置控制器、速度控制器、WMR 動力學系統（含電流環）及反饋值計算單元五個部分組成。

系統運行過程中，動態參數如WMR 時變導向角θ 作為仿真模塊的輸入影響整個系統的輸出，不方便使用全局變量來進行傳遞，為了保證動態參數的實時傳遞，實現時變矩陣的動態運算，本文采用S－Function來實現，S－Function提供給用戶自己編寫程序來滿足自己要求模型的接口，可以實現復雜函數信號的生成及Simulink自帶模塊難以實現的信號運算，利用Simulink提供的各種S－Function模板，我們可以很方便實現所需功能的S－Function模塊，并有利于提高程序的運行效率。

圖5 WMR 軌跡跟蹤的半物理仿真模型

仿真模型運行在RTW 環境下，安裝QuaRC 后，在Simulink參數配置選項卡中，選擇Real－Time WorkShop中的“system target file”為“quarc.windows.tlc”，即可實現以QuaRC Win32為目標的實時仿真。C－MEX S函數是實現RTW的接口，因此在仿真模型中我們使用C 語言形式 的S－Function，完 成 后的S－Function 再 通 過“mex filename.c”方法編譯成filename.mexw32形式的mex文件供系統使用。

4 仿真試驗

圖6所示為整個系統的硬件實物圖，移動機器人的機械模型參數為：r＝0.03m，b＝0.12m，m＝4kg，齒輪減速比為：kg＝3，電機力矩常數為：kt＝0.0612 N·m／A。本文針對直線和圓弧兩類參考軌跡進行跟蹤試驗。在文中設計的WMR 半物理仿真模型下，我們可以很方便地調整各個參數和控制模塊，并快速取得較為理想的控制參數，試驗取得位置控制器參數為：kx＝0.8，ky＝1.2，kθ＝1；速度控制器參數為：kv＝40；直流電機PI控制器參數為：kI＝4，kP＝2。系統采樣周期為：Δt＝0.001s，仿真時間為：t＝80s，

直線參考軌跡為：

圓參考軌跡為：

移動機器人起始位姿取：q＝（1，4，0）。通過多次試驗，發現系統存在一定的固有誤差，我們直接進行了補償。直線和圓軌跡跟蹤的仿真結果如圖7、圖8所示，圓軌跡跟蹤誤差如圖9 所示，右左輪控制力矩曲線圖分別如圖10、圖11所示，由圖可知，半物理仿真系統較好地反映實際WMR 系統的運行特性，控制器達到較好的控制效果。

圖6 WMR 半物理仿真系統的硬件實物

圖7 直線軌跡跟蹤

5 結束語

本文針對WMR 動力學控制實驗研究使用實際系統參數不易測量和調整，而純數值仿真無法完全反映系統特性的不足，提出一種基于Quanser的半物理實時仿真實現方法，該方法使用IMDU 構建WMR 物理系統，利用Simulink和QuaRC實現仿真系統建模，以Q8卡作為通信中介，進行反饋數據采集和物理對象的實時控制，從而實現半物理仿真系統的閉環控制，對WMR 軌跡跟蹤控制試驗表明該方法下，系統較好反映系統真實特性，參數調整方便，操作簡便，驗證過程直觀，有利于各種控制器的快速構建和驗證，方便進行WMR的動力學控制研究。值得一提的是基于本文方法，通過增加IMDU 主單元的個數，還可以實現多機器人的半物理仿真，并可推廣應用到其他多自由度的機器人系統。

圖11 圓弧軌跡跟蹤左輪驅動力矩

［1］WANG Zongyi，LI Yandong，ZHU Ling.Dual adaptive neural sliding mode control of nonholonomic mobile robot［J］.Journal of Mechanical Engineering，2010，46（23）：16－21（in Chinese）.［王宗義，李艷東，朱玲.非完整移動機器人的雙自適應 神 經 滑模 控 制［J］.機 械 工 程 學 報，2010，46（23）：16－21.］

［2］Dax T，Kar I N.Design and implementation of an adaptive fuzzy logic－based controller for wheeled mobile robots［J］.IEEE Transactions on Control Systems Technology，2006，14（3）：501－510.

［3］LIU Yu，ZHOU Chuan，ZHANG Yan，et al.Tracking control of wheeled mobile robots based on RBF neural networks［J］.Computer Engineering and Design，2011，32（5）：1804－1806（in Chinese）.［劉鈺，周川，張燕，等.基于RBF神經網絡的輪式移動機器人軌跡跟蹤控制［J］.計算機工程與設計，2011，32（5）：1804－1806.］

［4］Shahram K，Philippe P，Shahrokn S.An HIL－based reconfigureable platform for design，implementation and verifycation of electrical system digital controllers［J］.IEEE Transactions on Industrial Electronnics，2010，57（4）：1226－1236.

［5］YE Jinhua，WU Haibin，CHEN Tianyan.The research on motion controller of mobile robot［J］.Machinery Design ＆ Manufacture，2008，6（6）：145－147（in Chinese）.［葉錦華，吳海彬，陳天炎.移動機器人運動控制系統研究［J］.機械設計與制造，2008，6（6）：145－147.］

［6］Quanser Consultina.Inc Q8data acquisition system user＇s guide version 1.2.［Z］.Markham，Ontario，Canada：Quanser Consulting，Inc，2007.

［7］Quanser Consultina，Inc Quanser Consultina，Inc.Qua－Rc 1.2User Manual［Z］.Markham，Ontario，Canada：Quanser Consulting，Inc，2007.

［8］Industrial mechatronic drives unit（IMDU）user mannual［Z］.Markham，Ontario，Canada：Quanser Consulting，Inc，2007.

［9］YE Jinhua，LI Di，YE Feng，et al.Adaptive backstepping sliding mode control of uncertain Nonholonomic AGV［J］.Journal of South China University of Technology：Natural Science Edition，2011，39（12）：32－37（in Chinese）.［葉錦華，李迪，葉峰，等.不確定非完整AGV的自適應反演滑模控制［J］.華南理工大學學報（自 然科學版），2011，39（12）：32－37.］

［10］CAO Zhengcai，ZHAO Yingtao，WU Qidi.Adaptive trajectory tracking control for a Nonholonomic mobile robot［J］.Chinese Journal of Mechanical Engineering，2011，24（3）：1－7.