張梅 王飛活 何婉紅 高紅霞

?

基于動(dòng)態(tài)引擎的控制理論3D仿真實(shí)驗(yàn)平臺(tái)設(shè)計(jì)*

張梅 王飛活 何婉紅 高紅霞

（華南理工大學(xué)自動(dòng)化科學(xué)與工程學(xué)院）

為更好地理解和驗(yàn)證控制理論知識(shí)，采用開源物理引擎和Visual Studio 2005設(shè)計(jì)并實(shí)現(xiàn)倒立擺3D虛擬仿真實(shí)驗(yàn)平臺(tái)。該平臺(tái)既可通過(guò)設(shè)置PID參數(shù)或配置極點(diǎn)等方法驗(yàn)證控制理論，也可利用Matlab或VC++自行編寫其他控制算法，并生成相應(yīng)的動(dòng)態(tài)鏈接庫(kù)以供平臺(tái)調(diào)用和測(cè)試。實(shí)驗(yàn)表明，該平臺(tái)能夠提供一個(gè)虛擬直觀、操作性強(qiáng)、兼容性和開放性好的控制系統(tǒng)實(shí)驗(yàn)環(huán)境。

倒立擺；開源物理引擎；Matlab；虛擬仿真實(shí)驗(yàn)平臺(tái)

0　引言

倒立擺系統(tǒng)是一種典型的高階次、多變量、強(qiáng)耦合且絕對(duì)不穩(wěn)定的非線性系統(tǒng)，對(duì)其控制的效果可以非常直觀地體現(xiàn)控制理論的有效性和優(yōu)缺點(diǎn)[1]，是驗(yàn)證各種控制理論的理想實(shí)驗(yàn)平臺(tái)。本文結(jié)合開源物理引擎（open dynamic engine，ODE[2-3]）的3D圖形技術(shù)和控制系統(tǒng)的仿真技術(shù)，利用Matlab與VC++混合編程，設(shè)計(jì)一款可以脫離Matlab運(yùn)行環(huán)境的倒立擺3D仿真控制實(shí)驗(yàn)平臺(tái)。該平臺(tái)具有良好人機(jī)界面，操作性強(qiáng)、靈活開放，兼?zhèn)淞藢?shí)體實(shí)驗(yàn)的直觀顯示倒立擺運(yùn)行狀況、降低實(shí)驗(yàn)成本和不受時(shí)間地點(diǎn)等因素制約等優(yōu)點(diǎn)。

1　開發(fā)環(huán)境與倒立擺系統(tǒng)介紹

1.1基于Visual Studio 2005和ODE的動(dòng)力學(xué)仿真

本文采用ODE和Visual Studio 2005進(jìn)行倒立擺仿真實(shí)驗(yàn)平臺(tái)的開發(fā)。ODE是一個(gè)可用于模擬關(guān)節(jié)連接的剛體動(dòng)力學(xué)庫(kù)，它有內(nèi)建的碰撞檢測(cè)系統(tǒng)，可快速靈活地模擬地面上的車輛、動(dòng)物和虛擬環(huán)境中的可移動(dòng)物體。

ODE使用Body、Geom和Joint 3個(gè)基本元素描述剛體，分別表示剛體本體、幾何形狀和剛體間的連接關(guān)系。其中剛體本體Body的屬性包括質(zhì)心位置、線速度、角速度、質(zhì)量、質(zhì)心和轉(zhuǎn)動(dòng)慣量等。1組Body構(gòu)成1個(gè)剛體世界（World）。Geom表示剛體的幾何形狀，包括球、圓柱、長(zhǎng)方形、平面、三角形網(wǎng)格等復(fù)雜形狀。1組Geom形成1個(gè)碰撞空間（Space）。將若干個(gè)簡(jiǎn)單的Geom以一定Joint（2個(gè)剛體間的關(guān)節(jié)，表示剛體間的約束關(guān)系）進(jìn)行連接，從而形成控制對(duì)象。1組Joint形成1個(gè)Joint群（Group）。最終，在一個(gè)虛擬空間中，可利用以上3要素構(gòu)建倒立擺系統(tǒng)和它所在的物理世界。

1.2Matlab與VC++混合編程

Matlab是一款強(qiáng)大的數(shù)學(xué)分析軟件，在數(shù)值計(jì)算、矩陣分析以及非線性系統(tǒng)建模與仿真等方面具有優(yōu)勢(shì)，但程序執(zhí)行速度慢。VC++ 2005開發(fā)環(huán)境強(qiáng)大，程序執(zhí)行速度快，具有友好的編程界面，但相對(duì)而言，其科學(xué)計(jì)算能力、圖形顯示功能不強(qiáng)。因此，本文結(jié)合這兩款開發(fā)工具的優(yōu)勢(shì)，先利用Matlab編寫例如求解狀態(tài)反饋矩陣等數(shù)學(xué)計(jì)算方面的函數(shù)，生成“*.m”文件，然后通過(guò)Matlab編譯器生成相應(yīng)的動(dòng)態(tài)鏈接庫(kù)文件“*.dll”，以供VC調(diào)用。此方法設(shè)計(jì)的仿真控制平臺(tái)可脫離Matlab環(huán)境，獨(dú)立運(yùn)行，操作簡(jiǎn)單、方便[4-6]。

1.3倒立擺系統(tǒng)的仿真問(wèn)題

倒立擺系統(tǒng)的控制是使小車和擺桿盡快達(dá)到預(yù)期的平衡位置，同時(shí)要求它們不會(huì)有大的振蕩幅度、速度和角速度，并且當(dāng)?shù)沽[系統(tǒng)達(dá)到期望位置后，系統(tǒng)能克服一定范圍的擾動(dòng)而保持平衡。單級(jí)倒立擺系統(tǒng)示意圖如圖1所示。倒立擺系統(tǒng)的輸入為小車的位移（即位置）和擺桿的傾斜角度期望值。計(jì)算機(jī)在每一個(gè)采樣周期中，采集來(lái)自傳感器的小車與擺桿的實(shí)際位置信號(hào)，與期望值比較后，通過(guò)控制算法得到控制量，在實(shí)際控制系統(tǒng)中將控制量通過(guò)數(shù)模轉(zhuǎn)換驅(qū)動(dòng)直流電動(dòng)機(jī)實(shí)現(xiàn)倒立擺的實(shí)時(shí)控制。

圖1單級(jí)倒立擺系統(tǒng)示意圖

為了在ODE中將單級(jí)倒立擺系統(tǒng)表示出來(lái)，可將其看成由1個(gè)底盤、4個(gè)輪子和1個(gè)擺桿組成，這些物體有不同質(zhì)量和物理屬性，它們之間由一定的聯(lián)接方式連接。

基于ODE設(shè)計(jì)單級(jí)倒立擺的仿真平臺(tái)，就是將單級(jí)倒立擺實(shí)物模型分解為ODE的Geom、Body和Joint等基本元素。根據(jù)ODE的特點(diǎn)，所涉及的各Body、Joint等，只需定義其初始狀態(tài)，運(yùn)動(dòng)過(guò)程中的狀態(tài)變化由引擎自動(dòng)計(jì)算獲得。構(gòu)建仿真平臺(tái)的關(guān)鍵是Body和Joint在World中的構(gòu)建和狀態(tài)設(shè)置。

2　基于ODE的倒立擺仿真平臺(tái)設(shè)計(jì)

基于ODE的倒立擺仿真平臺(tái)由ODE引擎、倒立擺本體、3D顯示模塊和控制模塊組成。各部分分別負(fù)責(zé)倒立擺動(dòng)力學(xué)仿真、倒立擺仿真實(shí)物（關(guān)節(jié)）等的定義和封裝、3D仿真圖像場(chǎng)景顯示、外接控制器接口。ODE開發(fā)demo會(huì)提供ODE引擎和一個(gè)封裝了OpenGL的3D顯示函數(shù)庫(kù)drawstuff。ODE引擎可完成仿真系統(tǒng)的動(dòng)力學(xué)、碰撞檢測(cè)和仿真循環(huán)等。

平臺(tái)設(shè)計(jì)主要是完成倒立擺本體的模塊建立，包括利用ODE定義倒立擺本體和3D顯示；構(gòu)建1個(gè)控制接口模塊，實(shí)現(xiàn)倒立擺系統(tǒng)的控制。倒立擺本體是由擺桿、小車和車輪等Body通過(guò)Joint關(guān)節(jié)連接而成。這里使用的是合頁(yè)約束Joint（即Hinge Joint）。不同的Body之間用Joint連接，使小車Body、車輪Body和擺桿Body連接成為一個(gè)整體。

仿真平臺(tái)結(jié)構(gòu)如圖2所示，該仿真平臺(tái)的控制結(jié)構(gòu)如圖3所示。

圖2　倒立擺仿真平臺(tái)結(jié)構(gòu)

圖3　基于ODE的倒立擺控制系統(tǒng)結(jié)構(gòu)

倒立擺控制系統(tǒng)一般裝有很多傳感器，如：水平儀、陀螺儀、碼盤和加速度傳感器等，可分別用于檢測(cè)擺桿傾角、角速度、電機(jī)轉(zhuǎn)角和倒立擺運(yùn)動(dòng)的加速度，為控制系統(tǒng)提供反饋信號(hào)[7]。而在本仿真平臺(tái)中這些傳感器信息可以通過(guò)ODE引擎本身已封裝好的函數(shù)進(jìn)行查詢Joint和Body的狀態(tài)得到，例如利用dBodyGetPosition、dBodyGetLinearVel、dJointGet- HingeAngle、dJointGetHingeAngleRate等獲得倒立擺系統(tǒng)當(dāng)前狀態(tài)下的小車位移、速度、擺桿的角度、角加速度等，并作為倒立擺系統(tǒng)的反饋量添加到控制算法中，以實(shí)現(xiàn)倒立擺系統(tǒng)的實(shí)時(shí)控制。

控制接口模塊接收4個(gè)輸入信息，包括小車位移、速度、擺桿的角度和角加速度。通過(guò)選擇平臺(tái)事先設(shè)計(jì)好的控制器（如PID控制[8]、狀態(tài)反饋[9]或滑模控制[10]等）并配置相關(guān)參數(shù)后，該控制器可根據(jù)接口模塊獲得的小車位移、速度、擺桿的角度和角加速度等的偏差，計(jì)算輸出控制器的控制量，即加在小車上的外力，從而實(shí)現(xiàn)倒立擺的穩(wěn)定和相關(guān)性能指標(biāo)的要求。

3　軟件設(shè)計(jì)與實(shí)現(xiàn)

3.1總體思想

該仿真平臺(tái)擁有2條線程：主線程和仿真線程。

主線程建立程序的主窗口，即仿真平臺(tái)的界面。該線程由MFC創(chuàng)建，主要用于MFC的菜單繪制、界面創(chuàng)建與顯示、消息響應(yīng)、消息處理、用戶數(shù)據(jù)處理和用戶函數(shù)調(diào)用等。

仿真線程由ODE創(chuàng)建，主要用于實(shí)時(shí)計(jì)算各剛體的動(dòng)力學(xué)屬性和數(shù)據(jù)、計(jì)算關(guān)節(jié)點(diǎn)的狀態(tài)、碰撞檢測(cè)與仿真、繪制倒立擺的實(shí)時(shí)3D狀態(tài)、調(diào)整用戶視區(qū)和角度等。

2條線程共享同一個(gè)倒立擺資源，為了保證數(shù)據(jù)同步，需要對(duì)線程進(jìn)行同步處理。主線程在創(chuàng)建、銷毀、重置倒立擺、切換控制算法的過(guò)程中，倒立擺資源呈掛起狀態(tài)，此時(shí)仿真線程不得對(duì)倒立擺資源進(jìn)行訪問(wèn)。

3.2控制方法切換

所有添加的控制算法信息（包括控制算法的名稱、所在DLL名稱、算法函數(shù)名稱、需要參數(shù)個(gè)數(shù)、需要參數(shù)名稱和狀態(tài)等）均保存在平臺(tái)安裝目錄下的CtrInfo.ini文件中，每次軟件啟動(dòng)后，會(huì)讀取并解釋這個(gè)文件的所有算法狀態(tài)。若有被選中的控制算法，會(huì)從該算法所在的DLL中加載該算法的函數(shù)指針，通過(guò)該函數(shù)指針訪問(wèn)對(duì)應(yīng)控制算法。若用戶更改控制算法，會(huì)將舊控制算法的狀態(tài)更改為“未被使用”，而新選中的控制算法狀態(tài)更改為“正在使用”，并重新加載新控制算法的函數(shù)指針，替換舊的函數(shù)指針，從而實(shí)現(xiàn)算法的切換。

3.3ODE仿真的主要步驟[2-3]

步驟1：創(chuàng)建一個(gè)仿真世界dynamics World；

步驟2：在World中創(chuàng)建物體Bodies，設(shè)置所有物體的狀態(tài)，如位置等；

步驟3：在World中創(chuàng)建聯(lián)結(jié)Joints，將Joint與body綁定，為所有的聯(lián)結(jié)設(shè)置參數(shù)；

步驟4：按需要?jiǎng)?chuàng)建一個(gè)碰撞世界Space和碰撞幾何體Geom;

步驟5：創(chuàng)建一個(gè)聯(lián)結(jié)組Joints Group管理Joints；

步驟6：Loop

1) 按需給物體施加力，

2) 按需調(diào)整Joint的參數(shù)，

3) 調(diào)用碰撞檢測(cè)，

4) 為每個(gè)碰撞點(diǎn)collision point創(chuàng)建一個(gè)接觸聯(lián)結(jié)，并將其放入到contact Joint Group，

5) 執(zhí)行一個(gè)仿真步，刷新仿真系統(tǒng)，

6) 移除在contact Joint Group中所有聯(lián)結(jié)Joints，

步驟7：銷毀動(dòng)態(tài)World和碰撞世界Space。

3.4倒立擺控制函數(shù)

用全局變量contfunc、pn、an、fs分別代表控制方法的函數(shù)指針、位置噪聲、角度噪聲、輸出力飽和標(biāo)識(shí)。它們都是在主線程中賦值，在仿真線程中讀取和使用。

通過(guò)contfunc調(diào)用當(dāng)前的控制算法，由控制算法計(jì)算并返回控制力的值；通過(guò)判斷pn、an決定是否需要施加位置和角度干擾；通過(guò)判斷fs決定是否需要設(shè)置力飽和輸出的值。倒立擺控制函數(shù)流程如圖4所示。

3.5仿真線程函數(shù)

設(shè)置變量renderer_run、renderer_ss和renderer_pause，分別控制仿真的運(yùn)行、單步和暫停。這3個(gè)參數(shù)在主線程中改變，在仿真線程中使用。通過(guò)判斷其表示運(yùn)行狀態(tài)的數(shù)值來(lái)控制程序的運(yùn)行。另外，在倒立擺的角度超過(guò)±60o時(shí)，renderer_run變成0，此時(shí)系統(tǒng)已經(jīng)超出控制范圍，仿真線程退出。仿真線程函數(shù)流程如圖5所示。

圖4　倒立擺控制函數(shù)流程

圖5　仿真線程函數(shù)流程

3.6用戶自定義算法的實(shí)現(xiàn)

為提高本實(shí)驗(yàn)平臺(tái)的開放性，能夠自主編寫不同的控制算法，本仿真平臺(tái)提供相應(yīng)接口，用戶可添加自定義的新控制算法對(duì)倒立擺進(jìn)行控制。其編寫方法有2種：1) 通過(guò)Matlab編寫控制算法并生成相應(yīng)的C++動(dòng)態(tài)鏈接庫(kù)供Visual Studio 2005直接調(diào)用；2) 直接用VC++編寫控制算法并生成DLL文件，通過(guò)調(diào)用DLL動(dòng)態(tài)鏈接庫(kù)技術(shù)實(shí)現(xiàn)對(duì)用戶算法的調(diào)用。

其中，VC++與Matlab的混合編程步驟[4-6]（以計(jì)算極點(diǎn)配置下的狀態(tài)反饋矩陣的算法為例）如下：

1) 利用Matlab編譯器生成“.dll”文件

①M(fèi)atlab編寫計(jì)算反饋增益矩陣的m文件Pole.m；

②設(shè)置Matlab編譯器，在Matlab命令框中先后執(zhí)行下列語(yǔ)句

>>mex -setup

>>mbuild -setup

③對(duì)m文件進(jìn)行編譯，在Matlab命令框中先后執(zhí)行下列語(yǔ)句

>> cd “m文件所在路徑”

>>mcc -W cpplib:Pole -T link:libPole.m

2) Visual Studio 2005環(huán)境設(shè)置

①?gòu)腗atlab編譯生成的文件中選出Pole.h、Pole.dll和Pole.lib 3個(gè)文件，并復(fù)制到Visual Studio 2005工程目錄下；

②將Matlab安裝文件里的“extern”文件也復(fù)制到工程目錄下；

③選擇菜單“Project->Property Pages”

a) 在C/C++ ->General ->Directories下添加

“....externlibwin32microsoft”“....projectproject”（project為工程文件的名字）；

b) 在Linker->General ->Library Directories中添加

“....externlibwin32microsoft”

“....externinclude”

“....projectproject”

c) 在Linker-> Input -> Dependencies中添加

“mclmcrrt.lib”

“mclmcr.lib”

“Pole.lib”

④編寫Visual Studio 2005調(diào)用Matlab函數(shù)的接口程序，讀取狀態(tài)反饋增益矩陣。

4　倒立擺系統(tǒng)仿真與實(shí)驗(yàn)

4.1仿真平臺(tái)介紹

仿真平臺(tái)主要完成界面編程、倒立擺本體程序建立和控制器接口模塊實(shí)現(xiàn)等。

本平臺(tái)使用一個(gè)虛擬場(chǎng)景演示自動(dòng)控制實(shí)驗(yàn)中倒立擺的控制。這個(gè)控制實(shí)驗(yàn)在傳統(tǒng)的教學(xué)中需要利用復(fù)雜和昂貴的設(shè)備來(lái)完成，實(shí)驗(yàn)資源有限，在一定程度上影響了教學(xué)效果。而利用該基于3D游戲引擎的控制系統(tǒng)實(shí)驗(yàn)平臺(tái)控制系統(tǒng)的設(shè)計(jì)與仿真，不僅能讓學(xué)生很好地理解理論知識(shí)，而且能夠使學(xué)生感受到逼真的實(shí)驗(yàn)效果。

基于動(dòng)態(tài)引擎的控制理論3D仿真實(shí)驗(yàn)平臺(tái)界面如圖6所示?？刂颇康氖鞘剐≤嚭蛿[桿盡快到達(dá)平衡位置。系統(tǒng)在利用動(dòng)態(tài)引擎中的狀態(tài)測(cè)取函數(shù)獲得小車實(shí)時(shí)狀態(tài)值后，利用事先設(shè)定好的控制算法計(jì)算出控制量，然后作用于小車，從而保證小車位置及擺桿穩(wěn)定。

圖6　基于動(dòng)態(tài)引擎的控制理論3D仿真實(shí)驗(yàn)平臺(tái)界面

4.2系統(tǒng)功能介紹

本套仿真系統(tǒng)作為驗(yàn)證控制理論相關(guān)知識(shí)的實(shí)驗(yàn)平臺(tái)，可通過(guò)如下2種方法實(shí)現(xiàn)控制算法的主要功能：1) 用戶可通過(guò)選擇系統(tǒng)自帶的控制方式（如PID控制）設(shè)置控制參數(shù)；2) 可自己編寫控制算法，達(dá)到控制目的，如圖7所示。

圖7　添加自定義控制算法

4.3仿真平臺(tái)測(cè)試

仿真平臺(tái)測(cè)試采用極點(diǎn)配置控制方法，動(dòng)態(tài)仿真過(guò)程中某一時(shí)刻的仿真界面截圖如圖8所示。

圖8　極點(diǎn)配置控制實(shí)驗(yàn)

圖8中上下2條響應(yīng)曲線分別為擺桿角度響應(yīng)曲線和小車速度響應(yīng)曲線。由響應(yīng)曲線和系統(tǒng)動(dòng)態(tài)仿真模型可見，在仿真開始后添加干擾，角度和速度曲線均出現(xiàn)較大幅度的震蕩，但是在相應(yīng)的狀態(tài)反饋控制器的作用下，倒立擺系統(tǒng)逐漸趨于穩(wěn)定狀態(tài)，實(shí)現(xiàn)相應(yīng)的控制效果。

5　結(jié)語(yǔ)

本文采用ODE和Visual Studio 2005實(shí)現(xiàn)倒立擺控制的3D仿真系統(tǒng)的設(shè)計(jì)。該平臺(tái)可提供直觀的3D動(dòng)態(tài)仿真效果和操作性強(qiáng)的實(shí)驗(yàn)環(huán)境，可在虛擬實(shí)驗(yàn)平臺(tái)上對(duì)控制理論知識(shí)進(jìn)行驗(yàn)證，同時(shí)還可自行定制更多的控制算法來(lái)加強(qiáng)對(duì)控制理論知識(shí)的理解和掌握，極大地激發(fā)學(xué)習(xí)的積極性和主動(dòng)性，有效增強(qiáng)動(dòng)手實(shí)踐和大膽創(chuàng)新的能力。這是將三維動(dòng)態(tài)引擎技術(shù)以及虛擬實(shí)驗(yàn)平臺(tái)應(yīng)用到控制系統(tǒng)理論的創(chuàng)新性教學(xué)中的一個(gè)嘗試。

參考文獻(xiàn)

[1] 孔凡國(guó),劉錫林,鐘廷志.平面一級(jí)倒立擺的滑?？刂蒲芯縖J].機(jī)械設(shè)計(jì)與制造,2011(3):133-135.

[2] Russell Smith. Open Dynamics Engine v0.5 user guide [EB/OL]. http://ode.org, Monday May 28, 2007.

[3] 薛方正,劉成軍,李楠,等.基于ODE引擎的開放式仿人機(jī)器人仿真[J].機(jī)器人,2011,33(1):84-89.

[4] 劉維.精通Matlab與C/C++混合程序設(shè)計(jì)[M].3版.北京:北京航空航天大學(xué)出版社,2012.

[5] 田紅霞.基于VC與Matlab的電力電子技術(shù)軟件實(shí)驗(yàn)平臺(tái)設(shè)計(jì)[J].實(shí)驗(yàn)室科學(xué),2011,14(1):116-118.

[6] 趙志強(qiáng),劉志成.VC++與Matlab混合編程技術(shù)應(yīng)用分析[J].實(shí)驗(yàn)技術(shù)與管理,2009,26(1):99-101.

[7] 楊興明,丁學(xué)明,張培仁,等.兩輪移動(dòng)式倒立擺的運(yùn)動(dòng)控制[J].合肥工業(yè)大學(xué)學(xué)報(bào):自然科學(xué)版,2005,28(11):1485-1488.

[8] Lal Bahadur Prasad, Barjeev Tyagi, Hari Om Gupta. Optimal control of nonlinear inverted pendulum system using PID contorller and LQR: Performance analysis without and with disturbance input[J].International Journal of Automation and Computing, 2014,11(6):661-670.

[9] 劉秀文,郭偉,余波年.倒立擺狀態(tài)反饋極點(diǎn)配置與LQR控制Matlab實(shí)現(xiàn)[J].現(xiàn)代電子技術(shù),2011,34(10):88-90.

[10] Manar Lashin, Ahmed Ramadan, Hossam S Abbass, et al. Design of an optimized sliding mode control for loaded double inverted pendulum with mismatched uncertainties [C] // 19th IEEE International Conference On Methods and Models in Automation and Robotics, 2014:270-275.

The Design of 3D Simulation Platform for Control Theory Based on Open Dynamic Engine

Zhang Mei Wang Feihuo He Wanhong Gao Hongxia

(College of Automatic Science and Engineering, South China University of Technology)

To make users understand and verify the control theory, a virtual simulation experimental platform of single inverted pendulum is developed based on ODE(Open Dynamic Engine) and Visual Studio 2005. Users can verify the control theory by setting the PID parameter, configuration of poles or other ways. And they can also program control algorithm based on Matlab and C++, and then generate a dynamic link library for the platform to call it, which improve the openness of the experimental platform. The realization of this platform shows that it can provide a virtual but intuitive experimental environment of control system with strong operability and good compatibility and openness for the users. So that it can consolidate the theoretical knowledge of the users and make the effects of simulation be more realistic quality.

Inverted Pendulum; ODE; Matlab; Virtual Simulation Experimental Platform

張梅，女，1977年生，博士，副教授，研究方向：控制理論與控制工程。E-mail:zhangmei@scut.edu.cn

廣東省精品課程“自動(dòng)控制原理精品課程建設(shè)”項(xiàng)目；華南理工大學(xué)教學(xué)改革項(xiàng)目“基于設(shè)計(jì)的現(xiàn)代控制理論課程改革”。