移動(dòng)機(jī)器人動(dòng)力學(xué)控制半物理仿真研究

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

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

0 引 言

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

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

1 Quanser仿真平臺(tái)簡(jiǎn)介

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

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

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

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

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

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

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

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

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

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

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

qr滿足如下等式

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

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

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

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

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

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

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

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

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

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

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

3.2 硬件實(shí)現(xiàn)

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

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

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

3.3 軟件實(shí)現(xiàn)

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

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

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

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

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

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

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

4 仿真試驗(yàn)

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

直線參考軌跡為：

圓參考軌跡為：

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

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

圖7 直線軌跡跟蹤

5 結(jié)束語(yǔ)

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

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

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