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

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

0 引 言

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

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

1 Quanser仿真平臺簡介

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

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

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

2 WMR的動力學(xué)模型與控制器

2.1 WMR 動力學(xué)模型分析

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

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

圖1 WMR的結(jié)構(gòu)

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

式中：m——整車質(zhì)量，I＝mb2／4——車體繞質(zhì)心軸的轉(zhuǎn)動慣量。

2.2 WMR 軌跡跟蹤動力學(xué)控制器

qr滿足如下等式

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

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

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

式中：kv——正的常數(shù)矩陣。最后應(yīng)用計算力矩法取得兩輪的控制力矩為

控制力矩再轉(zhuǎn)換為IMDU 直流電機的驅(qū)動電流Ic，轉(zhuǎn)換關(guān)系式為

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

3 半物理仿真系統(tǒng)的軟硬件實現(xiàn)

3.1 系統(tǒng)總體結(jié)構(gòu)

WMR 動力學(xué)控制半物理仿真系統(tǒng)總體結(jié)構(gòu)如圖2所示。

圖2 系統(tǒng)總體結(jié)構(gòu)

系統(tǒng)使用IMDU 構(gòu)建WMR的動力學(xué)系統(tǒng)，WMR 軌跡跟蹤的軟件控制在Simulink上實現(xiàn)，并通過QuaRC實現(xiàn)計算機與Q8卡的實時數(shù)據(jù)交互。Simulink將實時控制過程中計算得到的相關(guān)數(shù)據(jù)傳遞給Q8卡，Q8卡再通過終端接口卡輸出驅(qū)動電流給IMDU，實現(xiàn)WMR 兩輪的力矩控制，同時IMDU 通過終端接口卡將電機的實際驅(qū)動電流與增量式編碼器信號反饋給Q8卡，經(jīng)Q8卡處理后，得到WMR力矩和速度反饋值返回給計算機，從而實現(xiàn)WMR 軌跡跟蹤的閉環(huán)動力學(xué)控制。

3.2 硬件實現(xiàn)

基于IMDU 構(gòu)建的WMR 動力學(xué)系統(tǒng)如圖3所示。

圖3 基于IMDU 構(gòu)建的WMR 動力學(xué)系統(tǒng)

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

3.3 軟件實現(xiàn)

動力學(xué)模型下，WMR 軌跡跟蹤控制的總體控制結(jié)構(gòu)如圖4所示，它由三個控制環(huán)組成，即位置控制環(huán)，速度控制環(huán)和電流控制環(huán)。

圖4 WMR 軌跡跟蹤控制的總體控制結(jié)構(gòu)

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

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

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

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

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

4 仿真試驗

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

直線參考軌跡為：

圓參考軌跡為：

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

圖6 WMR 半物理仿真系統(tǒng)的硬件實物

圖7 直線軌跡跟蹤

5 結(jié)束語

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

圖11 圓弧軌跡跟蹤左輪驅(qū)動力矩

［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）.［王宗義，李艷東，朱玲.非完整移動機器人的雙自適應(yīng) 神 經(jīng) 滑模 控 制［J］.機 械 工 程 學(xué) 報，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īng)網(wǎng)絡(luò)的輪式移動機器人軌跡跟蹤控制［J］.計算機工程與設(shè)計，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）.［葉錦華，吳海彬，陳天炎.移動機器人運動控制系統(tǒng)研究［J］.機械設(shè)計與制造，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的自適應(yīng)反演滑?？刂疲跩］.華南理工大學(xué)學(xué)報（自 然科學(xué)版），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.