摘 要：設(shè)計(jì)一個(gè)以TMS320F2812DSP為控制核心、2個(gè)獨(dú)立驅(qū)動(dòng)的同軸直流電機(jī)為執(zhí)行機(jī)構(gòu)的兩輪自平衡機(jī)器人，其姿態(tài)傳感器包括傾角儀、速率陀螺和電機(jī)編碼器。依據(jù)經(jīng)典牛頓力學(xué)建立線性系統(tǒng)數(shù)學(xué)模型，采用LQR方法得到系統(tǒng)的反饋系數(shù)后，進(jìn)行系統(tǒng)仿真實(shí)驗(yàn)和實(shí)際物理系統(tǒng)實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明，該系統(tǒng)的建模和控制器的設(shè)計(jì)是合理和有效的，且所設(shè)計(jì)的DSP控制程序可以方便的實(shí)現(xiàn)其他控制算法，并且可得到系統(tǒng)運(yùn)行時(shí)各狀態(tài)的值，為數(shù)據(jù)分析和傳感器信號(hào)的處理提供方便。

關(guān)鍵詞：兩輪自平衡；TMS320F2812；LQR；陀螺儀

中圖分類號(hào)：TP183 文獻(xiàn)標(biāo)識(shí)碼：B 文章編號(hào)：1004373X(2008)1805704

System Design，Modeling and LQ Control of a Twowheeled Selfbalancing Robot

RUAN Xiaogang，LIU Jiang，DI Haijiang，LI Xinyuan，ZHAO Jianwei

(School of Electronics Information Control Engineering，Beijing University of Technology，Beijing，100022，China)

Abstract：A twowheeled selfbalancing robot is designed，which uses a TMS320F2812DSP as center controller，two coaxial DC motors as actuator independently.Gesture sensors include a gyroscope，an inclinometer and two motor encoders.Based upon Newton dynamics mechanics theory，a mathematical model of linear system is built up.A systematic simulation experiment and physics experiment are accomplished after the feedback coefficients is gained using LQR control strategy.The experiment result shows that the system model and the controller design are reasonable and effective.Besides，the DSP control program may be used for other control algorithms，and values of running states are accessible，which offers convenience for data analysis and sensor signal processing.

Keywords：twowheeled selfbalancing;TMS320F2812，LQR;gyroscope

倒立擺系統(tǒng)作為受控對(duì)象是非線性、強(qiáng)耦合、多變量和自然不穩(wěn)定的系統(tǒng)，是檢驗(yàn)各種控制理論的理想模型。在控制過(guò)程中倒立擺系統(tǒng)能有效地反映諸如穩(wěn)定性、魯棒性、隨動(dòng)性以及跟蹤性等許多控制中的關(guān)鍵問(wèn)題。本文涉及的兩輪自平衡機(jī)器人相比一級(jí)倒立擺系統(tǒng)，由于其兩輪分別驅(qū)動(dòng)，增加了控制難度，且使用了嵌入式的數(shù)字信號(hào)處理器作為控制器，使其可以脫離實(shí)驗(yàn)室的導(dǎo)軌在普通路面上巡航。國(guó)內(nèi)外研究學(xué)者對(duì)移動(dòng)輪式倒立擺模型及對(duì)兩輪行走平衡控制技術(shù)的進(jìn)行了大量的研究，分別出現(xiàn)了Segway，Quasimoro，Joe和Nbot等機(jī)器人。近兩年該項(xiàng)技術(shù)開(kāi)始成為全球機(jī)器人控制技術(shù)的研究熱點(diǎn)之一。與倒立擺類似，兩輪自平衡機(jī)器人在垂直位置，即傾角小于±10°時(shí)，可以近似認(rèn)為是線性系統(tǒng)，所以對(duì)兩輪自平衡機(jī)器人的控制可以采用傳統(tǒng)的線性控制方法。

本文主要介紹兩輪自平衡機(jī)器人的設(shè)計(jì)、數(shù)學(xué)模型的建立、LQR控制方法的使用并給出實(shí)驗(yàn)結(jié)果。

1 系統(tǒng)設(shè)計(jì)

兩輪自平衡機(jī)器人系統(tǒng)的機(jī)械結(jié)構(gòu)采用層狀結(jié)構(gòu)，底層是兩個(gè)輪子，由安裝在機(jī)器人底部的兩個(gè)同軸的直流電機(jī)驅(qū)動(dòng)，直流電機(jī)裝有減速器及編碼器。輪子上方，即機(jī)器人的中層放置有機(jī)器人的電源、電機(jī)驅(qū)動(dòng)器、控制器等。在中層的前后側(cè)設(shè)計(jì)有一對(duì)護(hù)臂，當(dāng)機(jī)器人停機(jī)時(shí)方便機(jī)器人的存放；在調(diào)試過(guò)程中可以防止機(jī)器人摔倒造成內(nèi)部元件的損壞；運(yùn)行時(shí)可以起到防止機(jī)器人直接撞到障礙物而造成損壞。上層是機(jī)器人的機(jī)身及頭部，用來(lái)放置姿態(tài)傳感器以及未來(lái)的設(shè)計(jì)中其他部件的安裝。

兩輪自平衡機(jī)器人系統(tǒng)的電氣結(jié)構(gòu)如圖1所示。控制系統(tǒng)的核心是一塊TI公司的TMS320F2812DSP處理器，該芯片集實(shí)時(shí)處理能力與外設(shè)控制功能于一身，特別適合測(cè)量值多、程序大、實(shí)時(shí)性強(qiáng)的復(fù)雜運(yùn)動(dòng)控制。DSP的2路PWM信號(hào)分別給2個(gè)驅(qū)動(dòng)器提供輸入信號(hào)調(diào)節(jié)電機(jī)兩端的電壓，達(dá)到調(diào)節(jié)電機(jī)轉(zhuǎn)速的目的。DSP的JTAG接口通過(guò)仿真器與上位機(jī)連接，方便下載程序及在線調(diào)試。另外DSP還通過(guò)SCI串行通訊接口與上位機(jī)相連，將機(jī)器人運(yùn)行中的數(shù)據(jù)上傳到上位機(jī)中存儲(chǔ)，上位機(jī)也可以實(shí)時(shí)給機(jī)器人下達(dá)指令，完成規(guī)定動(dòng)作。姿態(tài)檢測(cè)傳感器包括傾角傳感器、速率陀螺以及安裝在電機(jī)上的光電編碼器，用來(lái)實(shí)時(shí)檢測(cè)機(jī)器人傾倒的角度與角速度以及兩個(gè)輪子轉(zhuǎn)過(guò)的角度與角速度。電源部分采用20節(jié)鎳氫電池組成電池組，提供直流電壓，并通過(guò)一塊電源分配電路板提供24 V，5 V的穩(wěn)定電壓以供各機(jī)器人各部分使用。此外還提供一塊電源監(jiān)測(cè)電路板以監(jiān)測(cè)電池組供電電壓，當(dāng)電壓低于24 V的時(shí)候會(huì)報(bào)警，提醒用戶及時(shí)充電。

圖1 兩輪自平衡機(jī)器人電氣結(jié)構(gòu)圖2 數(shù)學(xué)模型

2.1 系統(tǒng)數(shù)學(xué)模型的建立

假設(shè)車輪、機(jī)器人車體為剛體，如圖2、圖3所示，可以將機(jī)器人分為右輪、左輪、車體3部分。下面是變量的說(shuō)明：

m為一個(gè)車輪的質(zhì)量；M為機(jī)器人除兩個(gè)車輪外的質(zhì)量，即車體質(zhì)量；Jw為車輪對(duì)輪軸的轉(zhuǎn)動(dòng)慣量；Jθ為車體對(duì)z軸的轉(zhuǎn)動(dòng)慣量；Jδ為車體對(duì)y軸的轉(zhuǎn)動(dòng)慣量；R為車輪半徑；D為車輪之間的距離；L為z軸到車體的距離；θ為車體與豎直軸y軸的夾角；ω為車體與豎直軸y軸的夾角速度；δ為車體與x軸的夾角；xl，xr，xm為左右車輪位移，兩輪軸中點(diǎn)位移；fl，fr為左右輪與地面的靜摩擦力；Hl，Hr為左右輪與車體相互作用力的x軸分量；Vl，Vr為左右輪于車體相互作用力的y軸分量；Cl，Cr為左右電機(jī)的輸出轉(zhuǎn)矩；xp，yp為機(jī)器人車體質(zhì)心位移。

圖2 右輪受力圖圖3 車體受力圖根據(jù)經(jīng)典牛頓定律進(jìn)行受力分析，得到如下的方程組：對(duì)右輪：rm=fr-Hr(1)

rJwR=Cr-fr(2)

對(duì)左輪：lm=fl-Hl(3)

lJwR=Cl-fl(4)

對(duì)車體：pM=Hr+Hl(5)

Jp=(Vr+Vl)Lsin θ-(Hr+Hl)Lcos θ-(Cr+Cl)(6)

pM=Vr+Vl-Mg(7)

Jδ=(Hl-Hr)D2(8)

幾個(gè)物理量之間的轉(zhuǎn)化關(guān)系：xm=(xr+xl)2(9)

xp=xm+Lsin θ(10)

yp=Lcos θ-L(11)

δ=(xl-xr)D(12)

根據(jù)這12個(gè)動(dòng)力學(xué)方程，且在θ=0線性化即sin θθ，cos θ1，并略去θ的高次項(xiàng)，經(jīng)整理可得到系統(tǒng)的狀態(tài)空間表達(dá)式：

原來(lái)的多輸入系統(tǒng)分解成2個(gè)單輸入單輸出系統(tǒng)。分別對(duì)這2個(gè)子系統(tǒng)進(jìn)行控制，計(jì)算出兩個(gè)子系統(tǒng)的控制量，再經(jīng)過(guò)變換得到系統(tǒng)兩個(gè)電機(jī)的輸出轉(zhuǎn)矩Cl和Cr。

3 系統(tǒng)仿真

將參數(shù)帶入系統(tǒng)狀態(tài)方程后，經(jīng)計(jì)算2個(gè)系統(tǒng)都分別能控。針對(duì)兩個(gè)子系統(tǒng)分別采用LQR方法對(duì)系統(tǒng)進(jìn)行控制。?。篔(u)=11∫∞t0xT(t)Qx(t)+uT(T)Ru(t)〗dt其中Q為單位矩陣，R為1；得到子系統(tǒng)1的反饋系數(shù)矩陣為：K1=\\-1.000 0-1.710 8-1.990 4\\〗

子系統(tǒng)2的反饋矩陣為：K2=\\-1.000 0-1.026 7\\〗

得到兩個(gè)系統(tǒng)的階躍響應(yīng)曲線分別如圖4，圖5所示。

圖4 子系統(tǒng)1的階躍響應(yīng)圖5 子系統(tǒng)2的階躍響應(yīng)仿真效果表明所設(shè)計(jì)的LQR控制器可以很好地控制系統(tǒng)穩(wěn)定。系統(tǒng)各狀態(tài)量在5個(gè)控制周期后都趨于穩(wěn)定狀態(tài)。

4 機(jī)器人物理系統(tǒng)實(shí)驗(yàn)

控制器的軟件設(shè)計(jì)采用TI公司的CCS軟件開(kāi)發(fā)系統(tǒng)。使用25 ms周期中斷對(duì)系統(tǒng)進(jìn)行實(shí)時(shí)控制。在中斷中完成對(duì)傳感器輸入的模擬信號(hào)和數(shù)字信號(hào)的處理，實(shí)時(shí)計(jì)算電機(jī)所需控制量實(shí)現(xiàn)反饋控制。在主程序中對(duì)用戶想了解的狀態(tài)變量通過(guò)串行電纜將向上位機(jī)傳輸。并且在上位機(jī)用VC++開(kāi)發(fā)數(shù)據(jù)接收程序。

將仿真得到的兩組參數(shù)在兩輪直立式自平衡機(jī)器人上做了狀態(tài)反饋控制。因?yàn)樗〝?shù)學(xué)模型和真實(shí)的物理系統(tǒng)有一定的差異，故針對(duì)機(jī)器人實(shí)際運(yùn)行情況對(duì)反饋系數(shù)進(jìn)行一些微調(diào)，得到比較滿意的效果。由于機(jī)器人可以向上位機(jī)傳輸運(yùn)行的狀態(tài)量。這里對(duì)得到的數(shù)據(jù)用Matlab繪制圖像，得到系統(tǒng)穩(wěn)態(tài)運(yùn)行時(shí)的各狀態(tài)如圖6所示。

圖6 系統(tǒng)在5 000 ms處加擾動(dòng)后恢復(fù)平衡5 結(jié) 語(yǔ)

設(shè)計(jì)一個(gè)兩輪自平衡機(jī)器人，包括機(jī)械系統(tǒng)、電氣系統(tǒng)和控制系統(tǒng)，建立它的數(shù)學(xué)模型并用LQR算法在仿真環(huán)境下對(duì)其進(jìn)行了控制，利用仿真得到的反饋參數(shù)，在DSP中實(shí)現(xiàn)了機(jī)器人系統(tǒng)的實(shí)時(shí)控制。

實(shí)驗(yàn)結(jié)果表明，該系統(tǒng)的建模和控制器的設(shè)計(jì)合理和有效，且所設(shè)計(jì)的DSP軟件可以方便地實(shí)現(xiàn)其他控制算法，并且可得到系統(tǒng)運(yùn)行時(shí)各狀態(tài)的值，為數(shù)據(jù)分析和傳感器信號(hào)的處理提供了方便。

本系統(tǒng)的搭建，為兩輪自平衡機(jī)器人的控制提供很好的開(kāi)發(fā)環(huán)境。后續(xù)工作在于尋求其他的控制理論如模糊控制，非線性控制等，以求進(jìn)一步改善和提高兩輪自平衡機(jī)器人的瞬態(tài)特性和魯棒性，為實(shí)現(xiàn)機(jī)器人的系統(tǒng)參數(shù)識(shí)別和自學(xué)習(xí)平衡積累數(shù)據(jù)和經(jīng)驗(yàn)。

參 考 文 獻(xiàn)

［1］段旭東，魏衡華，陳星.基于DSP的二輪小車倒立擺系統(tǒng)［J］.控制工程，2004，11(11):521524.

［2］陳星，魏衡華，張玉斌.二輪行走倒立擺系統(tǒng)建模與魯棒方差控制［J］.計(jì)算機(jī)仿真，2006，23(3):269272.

［3］Segway Human Transporter (2004).\\.http://www.segway.com.

［4］NBot Balancing Robot.A Two Wheel Balancing Robot.\\.http://www.geology.smu.edu/~dpa-www/robo/nbot/index.html，2003.

［5］Salerno A，Angeles J.On the Nonlinear Controllability of a Quasiholonomic Mobile Robot［C］.In Proc.IEEE ICRA，Taiwan，2003:3 3793 384.

［6］Grasser F，Arrigo A D，Colombi S，et al.Joe:A mobile，inverted Pendulum［C］.IEEE Trans.Ind.Electron.，2002，49(1):107114.

［7］蘇奎峰，呂強(qiáng)，耿慶鋒，等.TMS320F2812原理與開(kāi)發(fā)［M］.北京:電子工業(yè)出版社，2005.

［8］鄭大中.線性系統(tǒng)理論［M］.北京:清華大學(xué)出版社，2002.

［9］張培仁，張志堅(jiān).基于16/32位DSP機(jī)器人控制系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)［M］.北京:清華大學(xué)出版社，2006.

［10］李航， 孫厚芳，韓建海.兩輪機(jī)器人行走機(jī)構(gòu)的建模與實(shí)驗(yàn)\\.北京理工大學(xué)學(xué)報(bào)，2004(12):2831.

［11］劉金錕.先進(jìn)PID控制Matlab仿真［M］.2版.北京:電子工業(yè)出版社，2004.

［12］劉勇，蔡延財(cái)，陳永冰.基于TMS320F2812CPLD的液晶顯示的驅(qū)動(dòng)設(shè)計(jì)\\.現(xiàn)代電子技術(shù)，2006，29（3）：142144.

注：本文中所涉及到的圖表、注解、公式等內(nèi)容請(qǐng)以PDF格式閱讀原文