一階直線倒立擺控制器MBD設(shè)計(jì)方法研究

常廣暉，張亞超，蘇 攀

(海軍工程大學(xué)動(dòng)力工程學(xué)院， 湖北 武漢 430033)

1 引言

基于模型的設(shè)計(jì)(MBD)方法被證實(shí)是一種有效的控制器設(shè)計(jì)方法?；贛BD方法設(shè)計(jì)控制系統(tǒng)，需要遵循四個(gè)步驟：對(duì)系統(tǒng)進(jìn)行建模，為模型設(shè)計(jì)控制器，對(duì)控制器進(jìn)行仿真驗(yàn)證，最后將設(shè)計(jì)好的控制器部署到真實(shí)系統(tǒng)上的控制器上。在MBD方法中，最適合使用預(yù)定義的連續(xù)時(shí)間和離散時(shí)間各類模塊對(duì)系統(tǒng)系統(tǒng)進(jìn)行建模，而不是使用復(fù)雜的軟件代碼。針對(duì)硬件在環(huán)仿真，系統(tǒng)模型參數(shù)可以方便的進(jìn)行適應(yīng)性調(diào)整。此功能使得在系統(tǒng)以及控制器設(shè)計(jì)中比傳統(tǒng)的建模技術(shù)可以更有效的測試設(shè)備的動(dòng)態(tài)行為。

倒立擺是一個(gè)典型的不穩(wěn)定非線性系統(tǒng)，很多技術(shù)原型都來源于此，比如平衡車、雙足步行機(jī)器人等?？刂频碾y度使得倒立擺成為控制領(lǐng)域經(jīng)典案例，圍繞倒立擺已經(jīng)有設(shè)計(jì)了很多類型的控制器，例如神經(jīng)網(wǎng)絡(luò)、自適應(yīng)控制、極點(diǎn)配置、PID控制、LQR控制甚至模糊控制等。倒立擺控制的難度主要來源于結(jié)構(gòu)性和非結(jié)構(gòu)性的不確定性，結(jié)構(gòu)性不確定性主要由模型不確定和未知的負(fù)載變化引起，非結(jié)構(gòu)性不確定性是由于傳感器噪聲、外部干擾等引起。隨著計(jì)算機(jī)仿真技術(shù)的進(jìn)步，開發(fā)精確倒立擺模型成為可能，這有助于通過仿真分析系統(tǒng)，在開展物理實(shí)驗(yàn)之前就能設(shè)計(jì)出更穩(wěn)定的控制系統(tǒng)。

本文提出了一種應(yīng)用于直線倒立擺控制的MBD設(shè)計(jì)實(shí)現(xiàn)方法。首先，建立了一階直線倒立擺的精確多體動(dòng)力學(xué)非線性模型，該模型通過滑動(dòng)和旋轉(zhuǎn)鉸鏈可以模擬空氣阻尼，滑動(dòng)摩擦等不確定因素。遵循MBD方法，非線性模型方程在平衡點(diǎn)被線性化，根據(jù)線性化狀態(tài)方程設(shè)計(jì)了LQR控制器，為了克服系統(tǒng)噪聲和傳感器噪聲并設(shè)計(jì)了卡爾曼濾波器，并在Matlab虛擬現(xiàn)實(shí)中動(dòng)態(tài)模擬了擾動(dòng)情況下的倒立擺穩(wěn)擺控制效果。最后，通過自動(dòng)代碼生成和軟件集成將設(shè)計(jì)好的控制器部署在倒立擺嵌入式控制電路板上。通過實(shí)際倒立擺測試實(shí)驗(yàn)，證明了倒立擺控制的MBD設(shè)計(jì)實(shí)現(xiàn)方法的有效性和便捷性。

2 倒立擺建模及仿真

2.1 倒立擺物理模型

一級(jí)直線倒立擺原理如圖1所示，倒立擺的模型通常采用Lagrange方程進(jìn)行建模，忽略小車和擺桿的阻尼，取小車的位移x和擺桿旋轉(zhuǎn)角度θ為廣義坐標(biāo)則其Lagrange函數(shù)為

圖1 倒立擺示意圖

L

=

T

-

V

(1)

其中

T

和

V

為小車和擺桿的動(dòng)能和使能。在廣義坐標(biāo)

x

上小車所受的外力

F

即為控制量

u

為非保守力，所以拉格朗日方程可以寫成如下形式：

(2)

(3)

其中

I

為擺桿的轉(zhuǎn)動(dòng)慣量。

這樣建模更多是理論指導(dǎo)性的，對(duì)于一個(gè)實(shí)際倒立擺系統(tǒng)或者更為復(fù)雜形狀的機(jī)械結(jié)構(gòu)而言，其質(zhì)心及轉(zhuǎn)動(dòng)慣量等是很難確定的。對(duì)于一個(gè)已知的實(shí)際倒立擺機(jī)構(gòu)，更希望建立一個(gè)具有3D特性實(shí)體物理模型。Matlab軟件中的SimscapeMultibody工具箱提供了這種可能，它可以使用代表實(shí)體的模塊，關(guān)節(jié)、約束、力元素和傳感器等為多體系統(tǒng)建模并可求解完整機(jī)械系統(tǒng)的運(yùn)動(dòng)方程。首先在SolidWork環(huán)境下建立了包含實(shí)際倒立擺幾何尺寸、質(zhì)量、關(guān)系、約束等參數(shù)信息的的3D裝配體模型，然后通過SimscapeMultibody將其導(dǎo)入到Simulink環(huán)境下，這樣仿真時(shí)可以將倒立擺的動(dòng)態(tài)可視化，最終建立的倒立擺Simscape模型如圖2所示。

圖2 倒立擺的Simscape模型

(4)

2.2 直流電機(jī)模型

直流電機(jī)的電樞回路電壓平衡方程為

(5)

式中

E

=

K

ω

為反電動(dòng)勢(

K

為電機(jī)電勢常數(shù))，

R

為電樞阻值，

L

為電樞線圈電感，

u

為電樞外加電壓。直流電機(jī)電感量

L

很小，通常忽略不計(jì)則，電機(jī)產(chǎn)生的力矩為

(6)

式中

K

為力矩系數(shù)，通常有

K

=

K

。

電機(jī)通過減速齒輪箱同小車皮帶輪連接，故小車速度和電機(jī)角速度之間滿足

(7)

式中，

r

為皮帶輪半徑，

K

為減速比。同理小車驅(qū)動(dòng)力

F

與電機(jī)轉(zhuǎn)動(dòng)力矩

M

之間存在著如下關(guān)系

(8)

聯(lián)立式 (5)、(6)、(8)可得直流電機(jī)模型

(9)

本例驅(qū)動(dòng)電機(jī)參數(shù)如表2所示。

表2 實(shí)際電機(jī)參數(shù)

2.3 基于卡爾曼濾波的LQR控制算法

LQR控制算法是一種普遍采用的最優(yōu)控制系統(tǒng)設(shè)計(jì)方法，當(dāng)?shù)沽[擺桿受擾偏離原平衡狀態(tài)時(shí)，通過LQR控制可以使擺桿保持在豎直狀態(tài)，并使控制過程中的動(dòng)態(tài)誤差和能量消耗綜合最優(yōu)即，通過求解Riccati代數(shù)方程，可以求得倒立擺的最優(yōu)控制律：

u

=-

Kx

=-

R

B

Px

(10)

式中，

K

為

LQR

控制增益矩陣，

R

為正定實(shí)對(duì)稱常數(shù)矩陣，

P

為常值正定矩陣，必須滿足Riccati代數(shù)方程，即

A

P

+

PA

+

Q

-

PBR

B

P

=0

(11)

式中，

A

，

B

分別為倒立擺模型的系統(tǒng)矩陣和控制矩陣。通過MATLAB自帶的lqr函數(shù)可以求解Riccati代數(shù)方程從而求得LQR控制增益矩陣K。

由于在實(shí)際倒立擺控制時(shí)，系統(tǒng)本身存在隨機(jī)噪聲干擾，同時(shí)倒立擺的四個(gè)狀態(tài)量中只能方便的測量小車的位移和擺桿的角度，測量過程也存在測量噪聲干擾，很難獲得精確的全部狀態(tài)量x，在LQR控制器前端增加卡爾曼濾波狀態(tài)觀測器可以解決這一問題。卡爾曼濾波器采用控制工具箱中的KalmanFilter模塊實(shí)現(xiàn)。

2.4 倒立擺系統(tǒng)仿真

將上述倒立擺各部分模型及控制器模型在Simulink環(huán)境下集成，得到的系統(tǒng)仿真模型如圖3所示。

圖3 倒立擺仿真模型

為了考慮后續(xù)代碼自動(dòng)生成在嵌入式平臺(tái)運(yùn)行的需要仿真設(shè)置為定步長，步長為0.005(即5ms)，積分算法選擇為ode4。倒立擺系統(tǒng)過程噪聲方差Q設(shè)為0.01，位移和角度測量噪聲方差R設(shè)為0.001。仿真時(shí)間設(shè)為5s，未使用卡爾曼濾波器狀態(tài)觀測器，單純采用LQR的控制效果如圖4所示。基于卡爾曼濾波器的LQR的控制效果如圖5所示。小車位移及擺桿角度信號(hào)原始值、測量值及卡爾曼濾波器估計(jì)值的對(duì)比曲線如圖6、7所示。

圖4 LQR控制輸出曲線

圖5 LQR加Kalman濾波器的控制輸出曲線

圖6 位移原始值、測量值及估計(jì)值的對(duì)比曲線

圖7 角度原始值、測量值及估計(jì)值的對(duì)比曲線

從圖4、5可以看出，通過卡爾曼濾波減少了噪聲干擾，增強(qiáng)了LQR控制精度。從圖6，7可以看出經(jīng)過卡爾曼濾波大大減少了狀態(tài)向量的幅值，經(jīng)LQR運(yùn)算后會(huì)減少控制量波動(dòng)的幅值，減少對(duì)驅(qū)動(dòng)芯片的沖擊，增長其工作壽命。

3 代碼生成及硬件部署

3.1 嵌入式控制電路板

倒立擺的嵌入式控制電路板采用核心板+底板結(jié)構(gòu)的模塊化設(shè)計(jì)方案，電路圖如圖8、9所示。底板主要是電源電路、電機(jī)驅(qū)動(dòng)電路、位移及角度測量接口等，核心板電路如圖4所示，核心板采用ST公司的32bit Cortex-M3內(nèi)核ARM STM32F103ZET6作為MCU，在此基礎(chǔ)上集成DM9000BI以太網(wǎng)通信電路和FLASH存儲(chǔ)電路等。

圖8 核心板電路圖

圖9 底板電路圖

3.2 代碼自動(dòng)生成

代碼自動(dòng)生成的目的就是將上述基于Kalman濾波的LQR控制模塊生成為直接可以控制實(shí)際倒立擺裝置的C代碼?？刂颇K的輸出量通常為為控制量(即對(duì)小車的作用力)，而實(shí)際控制如圖2所示，控制器輸出信號(hào)應(yīng)為PWM驅(qū)動(dòng)信號(hào)，該信號(hào)輸出給驅(qū)動(dòng)電路從而驅(qū)動(dòng)電機(jī)動(dòng)作產(chǎn)生作用在小車上的作用力，因此LQR輸出端口應(yīng)連接到電機(jī)的逆模型，這樣可以將LQR輸出的力信號(hào)形式的控制量轉(zhuǎn)換為電壓信號(hào)形式的控制量提供給電機(jī)?？刂颇K的輸入量為小車位移和擺桿的角度，而實(shí)際控制中如圖2所示，輸入量為編碼器形式的位移信號(hào)和旋轉(zhuǎn)電位器形式的角度信號(hào)，同理LQR控制模塊應(yīng)包含控制用編碼器和電位器的逆模型。

自動(dòng)生成的控制代碼同底層驅(qū)動(dòng)代碼數(shù)據(jù)交互采用全局變量的方式實(shí)現(xiàn)，如圖2所示，將控制模塊的輸入輸出信號(hào)取名為Measured＿x，Measured＿theta， pwm＿set，并將信號(hào)設(shè)置為Test Point，儲(chǔ)存類型設(shè)置為ExportedGlobal，這樣在驅(qū)動(dòng)程序中就可以方便的調(diào)用這3個(gè)變量了。

接下來在Simulink配置對(duì)話框中，選用ert.tlc系統(tǒng)目標(biāo)文件，硬件類型選擇ARM Cortex類型，設(shè)置好之后就可以將使用build命令利用Embedded Coder將控制模塊生成高效率的嵌入式C代碼，生成的代碼如圖10所示。

圖10 生成代碼圖

3.3 代碼軟件集成

倒立擺控制軟件采用實(shí)時(shí)操作系統(tǒng)uCOS-II進(jìn)行模塊嵌入式系統(tǒng)軟件設(shè)計(jì)，基礎(chǔ)是線程設(shè)計(jì)?？刂栖浖?yīng)實(shí)現(xiàn)編碼器形式的小車位置測量和電位器形式的擺桿角度測量、倒立擺的穩(wěn)擺控制以及以太網(wǎng)通信程序(向Simulink實(shí)時(shí)上傳控制數(shù)據(jù))等功能，因此模塊驅(qū)動(dòng)軟件應(yīng)包含1個(gè)ISR和4個(gè)線程分別是：DM9000數(shù)據(jù)接收中斷服務(wù)程序、信號(hào)測量線程、穩(wěn)擺控制線程、以太網(wǎng)通信線程。為了實(shí)現(xiàn)自動(dòng)代碼和控制軟件的集成，將上節(jié)自動(dòng)生成的控制代碼中的LQRwithKalman＿step()函數(shù)在穩(wěn)擺控制線程的線程函數(shù)中進(jìn)行調(diào)用，并將線程的任務(wù)延時(shí)設(shè)定為5ms(與仿真步長一致)，這樣就實(shí)現(xiàn)了倒立擺的實(shí)時(shí)控制。擺桿角度、小車位移、PWM控制等信號(hào)需要在測量線程、穩(wěn)擺控制線程、以太網(wǎng)通信線程三個(gè)線程之間共享，為了防止出現(xiàn)沖突，三個(gè)線程通過互斥信號(hào)量來防止共享沖突。由于uC/OS-II實(shí)時(shí)操作系統(tǒng)還保留對(duì)最高的四個(gè)優(yōu)先級(jí)0-3的使用權(quán)，系統(tǒng)啟動(dòng)任務(wù)優(yōu)先級(jí)最高設(shè)為4。為了保證信號(hào)采集及穩(wěn)擺控制的實(shí)時(shí)性，各線程優(yōu)先級(jí)設(shè)置如表2所示。

表2 各線程屬性設(shè)置

4 實(shí)驗(yàn)驗(yàn)證

圖11 倒立擺控制實(shí)驗(yàn)驗(yàn)證

為了驗(yàn)證上述基于模型設(shè)計(jì)的有效性，將集成后的軟件通過keil5編譯后下載到倒立擺嵌入式控制器中進(jìn)行了控制測試試驗(yàn)，試驗(yàn)如圖11所示。試驗(yàn)的同時(shí)上位計(jì)算機(jī)通過Simulink的TCP/IP Receive模塊接受嵌入式控制電路板實(shí)時(shí)上傳的小車位移和擺桿角度兩個(gè)數(shù)據(jù)并顯示。試驗(yàn)結(jié)果表明，采用基于模型設(shè)計(jì)的控制算法通過自動(dòng)代碼生成直接移植到嵌入式控制器，取得了同仿真一樣的控制效果，通過卡爾曼濾波減少了小車位移以及擺桿角度在穩(wěn)定點(diǎn)附近的波動(dòng)，彌補(bǔ)了實(shí)際測量噪聲對(duì)控制的干擾。

5 結(jié)論

本文提出了基于模型來設(shè)計(jì)直線倒立擺控制器的設(shè)計(jì)方法，實(shí)現(xiàn)了基于卡爾曼濾波的LQR穩(wěn)擺控制。首先，使用Solikwork 建立了與實(shí)際倒立擺裝置一致的3D裝配體，通過SimscapeMultiboy Link將裝配體轉(zhuǎn)換為Simscape非線性物理模型，該模型的物理參數(shù)和運(yùn)行特性與實(shí)際倒立擺機(jī)構(gòu)一致，這樣就可以方便的在Simulink環(huán)境下設(shè)計(jì)合適的穩(wěn)擺控制器，而且控制效果可以實(shí)現(xiàn)動(dòng)態(tài)可視化。設(shè)計(jì)完成后通過Embedded Coder將控制器生成高效率的嵌入式C代碼與底層驅(qū)動(dòng)代碼以實(shí)時(shí)操作系統(tǒng)多線程的方式進(jìn)行集成并最終部署于基于STM32的倒立擺嵌入式控制電路板中。通過倒立擺實(shí)際穩(wěn)擺控制試驗(yàn)，驗(yàn)證了使用MBD方法簡化了嵌入式控制器的設(shè)計(jì)過程，減少了設(shè)計(jì)失敗的風(fēng)險(xiǎn)，相比通過手動(dòng)編寫代碼設(shè)計(jì)控制其而言，縮短了設(shè)計(jì)時(shí)間提高了設(shè)計(jì)效率。該MDB方法流程為其它復(fù)雜機(jī)械系統(tǒng)控制器的設(shè)計(jì)提高了很好的借鑒。