基于LQR控制的機(jī)器人自行車(chē)靜態(tài)平衡研究

古文生 王峰 周全 熊超偉

摘要：針對(duì)本團(tuán)隊(duì)目前研究的機(jī)器人自行車(chē)靜態(tài)平衡控制，使機(jī)器人自行車(chē)達(dá)到自動(dòng)靜止的目的。本文在對(duì)該系統(tǒng)進(jìn)行動(dòng)力學(xué)建模的基礎(chǔ)上，設(shè)計(jì)了一種基于LQR控制的算法，完成機(jī)器人自行車(chē)靜態(tài)平衡系統(tǒng)。該機(jī)器人自行車(chē)是在一輛自行車(chē)的基礎(chǔ)上改裝的，要使它能夠靜止不倒下，我們先讓前輪擺到90度。利用陀螺儀等傳感器進(jìn)行實(shí)時(shí)檢測(cè)系統(tǒng)的狀態(tài)，然后STM32程序通過(guò)LQR算法輸出控制力矩給前輪電機(jī)，改變當(dāng)前機(jī)器人自行車(chē)的狀態(tài)，使系統(tǒng)達(dá)到靜止平衡的狀態(tài)。

關(guān)鍵詞：機(jī)器人自行車(chē);靜態(tài)平衡;動(dòng)力學(xué)建模;LQR控制算法;STM32;陀螺儀

中圖分類(lèi)號(hào)：TP2

文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1009-3044（2020）04-0192-03

收稿日期：2019-11-21

作者簡(jiǎn)介：古文生（1993—），男，廣東韶關(guān)人，碩士研究生，研究方向?yàn)闄C(jī)器人和智能車(chē)輛動(dòng)力學(xué)建模控制;王峰（1961—），男，美籍華人，特聘教授，研究生導(dǎo)師，主要研究方向?yàn)闄C(jī)器人和智能車(chē)輛建?？刂?周全（1995—），男，湖北黃岡人，碩士研究生，研究方向?yàn)闄C(jī)器人和智能車(chē)輛動(dòng)力學(xué)建?？刂?熊超偉（1995—），男，江西豐城人，碩士研究生，研究方向?yàn)闄C(jī)器人和智能車(chē)輛動(dòng)力學(xué)建?？刂?。

Research on Static Balance of Robot Bicycle Based on LQR Control GU Wen-sheng，Everett Xiaolin Wang，ZHOU Quan，XIONG Chao-wei

（School of Information Engineering，Guangdong University of Technology，Guangzhou 510006，China）

Abstract：Aiming at the static balance control of the robot bicycle studied by our team，the robot bicycle can achieve the purpose of au-tomatic static.On the basis of dynamic modeling of the system，an algorithm Based on LQR control is designed to complete the static bal-ance system of the robot bicycle.The robot bike is a modified version of a bicycle.To make it stationary and not fall down，we first put the front wheel at a 90-degree Angle.Using gyroscope and other sensors to detect the state of the system in real time，STM32 program outputs control torque to the front wheel motor through LQR algorithm，changes the current state of the robot bicycle，and makes the sys-tem reach the state of static balance.

Key words：Robot Bicycle;Static Balance;Dynamic Modeling;LQR Control Algorithm;STM32;Gyroscope.

機(jī)器人自行車(chē)是一種典型的欠驅(qū)動(dòng)系統(tǒng)，要實(shí)現(xiàn)靜態(tài)平衡，必須要通過(guò)某一個(gè)控制量改變當(dāng)前自行車(chē)的狀態(tài)，以抵消某個(gè)自由度產(chǎn)生的偏移，從而達(dá)到平衡。之前流行著一種實(shí)現(xiàn)機(jī)器人自行車(chē)靜態(tài)平衡的方法，就是在自行車(chē)上安裝一個(gè)高速運(yùn)轉(zhuǎn)的飛輪，通過(guò)這個(gè)飛輪達(dá)到平衡的目的。但是這種飛輪需要轉(zhuǎn)很快才能達(dá)到平衡，需要消耗巨大能量。而本文介紹的方法是，機(jī)器人自行車(chē)切換到靜態(tài)時(shí)候，直接把前輪轉(zhuǎn)到90度，使前輪和車(chē)主軸呈現(xiàn)垂直的狀態(tài)。通過(guò)控制前輪的電機(jī)，改變車(chē)的狀態(tài)，達(dá)到靜態(tài)平衡。這種方法解決了飛輪消耗能量的缺點(diǎn)[1]。

線性二次型[4]最優(yōu)控制設(shè)計(jì)是基于狀態(tài)空間技術(shù)設(shè)計(jì)一個(gè)優(yōu)化的動(dòng)態(tài)控制器，線性二次型（LQR）算法應(yīng)用對(duì)象是現(xiàn)代控制理論中以狀態(tài)空間形式給出的線性系統(tǒng)，而目標(biāo)函數(shù)為對(duì)象狀態(tài)和控制輸入的二次型函數(shù)。LQR最優(yōu)設(shè)計(jì)是指設(shè)計(jì)出的狀態(tài)反饋控制矩陣K要使得二次型目標(biāo)函數(shù)輸出最小值，而K矩陣由Q，R唯一確定。所以在選取這兩個(gè)矩陣的時(shí)候要特別注意。LQR可得到狀態(tài)線性反饋的最優(yōu)控制規(guī)律，易于構(gòu)成閉環(huán)最優(yōu)控制。

針對(duì)線性系統(tǒng)的控制算法設(shè)計(jì)問(wèn)題[5-7]，如果該系統(tǒng)的性能指標(biāo)是狀態(tài)變量或者控制變量的二次型函數(shù)積分，那么這種動(dòng)態(tài)系統(tǒng)的最優(yōu)化問(wèn)題稱為線性系統(tǒng)二次型性能指標(biāo)的最優(yōu)控制問(wèn)題，也就是線性二次問(wèn)題。線性二次型求優(yōu)問(wèn)題可以寫(xiě)成統(tǒng)一的解析表達(dá)式和實(shí)現(xiàn)求解過(guò)程的規(guī)范化，并可簡(jiǎn)單地采用狀態(tài)線性反饋控制律構(gòu)成閉環(huán)最優(yōu)控制系統(tǒng)，能夠兼顧多項(xiàng)性能指標(biāo)，因此得到特別的重視，為現(xiàn)代控制理論中發(fā)展較為成熟的一部分。

1 系統(tǒng)建模

在設(shè)計(jì)LQR控制算法之前，要先對(duì)整個(gè)系統(tǒng)進(jìn)行建模，并推導(dǎo)出狀態(tài)方程，才能夠進(jìn)行LQR控制器設(shè)計(jì)的工作。因此，這一部分要對(duì)機(jī)器人自行車(chē)進(jìn)行建模[3]。下面先看看整個(gè)機(jī)器人自行車(chē)的模型，如圖1所示。

圖1用了三個(gè)坐標(biāo)系對(duì)車(chē)子進(jìn)行描述，其中坐標(biāo)系00是大地基準(zhǔn)坐標(biāo)系，O1為描述車(chē)身狀態(tài)的坐標(biāo)系，O2為描述前輪狀態(tài)的坐標(biāo)系。為了能夠表達(dá)機(jī)器人自行車(chē)所處的狀態(tài)，這里選取向量q用于描述車(chē)身的狀態(tài)，q=（q1，q2，q3），其中q表示車(chē)身傾斜角，即坐標(biāo)系00和01兩者的z軸之間的夾角;q2表示車(chē)頭轉(zhuǎn)向角度，也就坐標(biāo)系O0和O2兩者的x軸之間的夾角。q3表示前輪所轉(zhuǎn)過(guò)的角度，系統(tǒng)開(kāi)機(jī)時(shí)所處的初始狀態(tài)為0度。

在動(dòng)力學(xué)中，限制物體運(yùn)動(dòng)的條件稱謂約束。約束又稱謂非完整約束和完整約束。完整約束是指幾何約束和可積分的微分約束的統(tǒng)稱。非完整約束是指系統(tǒng)具有不可積分的動(dòng)力學(xué)方程。對(duì)于完整約束，利用拉格朗日第二方程可以建立它的動(dòng)力學(xué)方程。而對(duì)于非完整約束，主要有拉格朗日方程，羅斯方程，阿佩爾方程這三種建模方法。

我們這里選擇用阿佩爾方程對(duì)機(jī)器人自行車(chē)進(jìn)行建模。下面公式（1）就是阿佩爾方程。

其中G是系統(tǒng)的總加速度能，V是系統(tǒng)的總勢(shì)能。q;為向量q在維度j的角加速度，q;為向量q在維度j的角度，rj為系統(tǒng)在j維度對(duì)用的力矩輸出。我們把機(jī)器人自行車(chē)系統(tǒng)的G和V求出之后帶進(jìn)以上的阿佩爾方程就可以得出以下公式（2）的動(dòng)力學(xué)方程。

其中M，C，G，K都是與向量q有關(guān)維度值相關(guān)的參數(shù)矩陣21。向量q在前面已經(jīng)介紹過(guò)，這里不再重復(fù)寫(xiě)出來(lái)。下面把M，C，G，K展開(kāi)來(lái)看看。

接下來(lái)我們要對(duì)該動(dòng)力學(xué)方程進(jìn)行變換，因?yàn)槲覀儗?shí)現(xiàn)靜態(tài)平衡時(shí)候，車(chē)頭的轉(zhuǎn)彎角度是固定在90度的，所以可以根據(jù)我們的實(shí)際情況對(duì)該方程進(jìn)行一些簡(jiǎn)化。最后得到以下公式（6）為系統(tǒng)的狀態(tài)方程[1]。

其中x=（q1，q3，q1，q3），因?yàn)槲覀円獙?shí)現(xiàn)靜態(tài)平衡，92作為車(chē)頭轉(zhuǎn)向角已經(jīng)固定在90度了，所以我們只需要觀測(cè)q1和q3相關(guān)值的變化就可以反映出當(dāng)前系統(tǒng)的狀態(tài)，所以向量x里面只有這兩個(gè)參數(shù)有關(guān)的變量。對(duì)于A，B矩則是由M，C，G，K推導(dǎo)出來(lái)的。u是τ向量中的第三維度變量，也就是前輪轉(zhuǎn)向力矩。因?yàn)槠渌麅蓚€(gè)維度沒(méi)有力矩輸出，所以這里只有這個(gè)力矩引入到了狀態(tài)方程。有了狀態(tài)方程，就可以進(jìn)人到LQR控制器設(shè)計(jì)階段，下面將進(jìn)人LQR控制器設(shè)計(jì)部分。

2 LQR控制器設(shè)計(jì)

前面已經(jīng)得到了機(jī)器人自行車(chē)的狀態(tài)方程，現(xiàn)在可以開(kāi)始設(shè)計(jì)LQR控制器了。根據(jù)LQR調(diào)節(jié)器的原理，針對(duì)狀態(tài)方程i=Ax+Bu，通過(guò)確定最佳控制量u（k）=-Kx（t）中的矩陣K。使得控制性能指標(biāo)L=∫「（xTQx+uTRu）dt得到極小值。對(duì)Q，R這兩個(gè)x和u的加權(quán)矩陣的選取，我們需要令q1和q3得到最小。選取好Q，R矩陣之后，接下來(lái)就要求解RICCATI方程PA+APT-PBR-1BP+Q=0，求得矩陣P之后，就可以通過(guò)K=R-1BTP計(jì)算出反饋增益矩陣K。得到最優(yōu)控制器u（k）=-Kx（t）。以下是LQR簡(jiǎn)要框圖。

3 系統(tǒng)的實(shí)現(xiàn)

我們前面已經(jīng)對(duì)機(jī)器人自行車(chē)完成了建模和控制器設(shè)計(jì)，我們這里就要進(jìn)行系統(tǒng)的實(shí)現(xiàn)，我們選擇STM32F405作為主控芯片，MPU6050陀螺儀作為檢測(cè)車(chē)身狀態(tài)的傳感器，SMP8048電機(jī)作為力矩輸出。下面圖3是整個(gè)系統(tǒng)的組成框圖。

從上圖3可以看出系統(tǒng)的組成結(jié)構(gòu)和工作流程。其中MPU6050陀螺儀的作用是檢測(cè)機(jī)器人自行車(chē)的傾斜角和傾斜角速度等狀態(tài)量，然后把這些值送到STM32主控芯片當(dāng)中;而另一邊的帶編碼器的電機(jī)，把編碼信號(hào)經(jīng)過(guò)電機(jī)驅(qū)動(dòng)機(jī)轉(zhuǎn)換后傳回給主控芯片，主控芯片即獲得電機(jī)轉(zhuǎn)速。主控芯片根據(jù)這些回傳數(shù)據(jù)進(jìn)行判斷當(dāng)前狀態(tài)是否平衡，然后經(jīng)過(guò)LQR控制算法輸出PWM信號(hào)和方向信號(hào)給電機(jī)驅(qū)動(dòng)器，電機(jī)驅(qū)動(dòng)器把這些信號(hào)轉(zhuǎn)換成三相電等信號(hào)給電機(jī)。這就是整個(gè)系統(tǒng)的工作過(guò)程。圖4是具體的軟件組成框圖。

完成硬件軟件等工作之后，我們進(jìn)行了一系列測(cè)試實(shí)驗(yàn)，經(jīng)過(guò)我們的實(shí)驗(yàn)檢測(cè)，，實(shí)現(xiàn)了靜態(tài)平衡的目標(biāo)，下圖5為實(shí)物作品。

當(dāng)機(jī)器人自行車(chē)往左邊傾斜時(shí)，前輪立馬向左邊加速運(yùn).動(dòng)，防止它往倒下。當(dāng)它往右邊傾斜時(shí)，前輪往右邊加速，防止它往右邊倒下，這就實(shí)現(xiàn)了靜態(tài)平衡。

4 總結(jié)

機(jī)器人自行車(chē)要實(shí)現(xiàn)靜態(tài)平衡，必須有外力支撐它，讓它平衡不倒下，而我們這里把前輪擺到90度，給系統(tǒng)提供了支撐外力。我們的系統(tǒng)是一種典型的欠驅(qū)動(dòng)系統(tǒng)，也就是說(shuō)我們想要抵消車(chē)身不平衡時(shí)候造成的傾斜角偏移，是不能通過(guò)某個(gè)控制量直接抵消，而是借助其他控制量來(lái)抵消，這里我們是靠前輪力矩來(lái)實(shí)現(xiàn)這個(gè)目的。要如何控制前輪力矩抵消這一偏移，我們就設(shè)計(jì)了LQR控制器進(jìn)行完成這些控制過(guò)程。因此，我們可以得出LQR控制器可以實(shí)現(xiàn)機(jī)器人自行車(chē)靜態(tài)平衡的結(jié)論。參考文獻(xiàn)：

[1]C.Xiong，Z.Huang，W.Gu，W.X.Wang，etc.Static Balancing of Robotic Bicycle through Nonlinear Modeling and Control[C]/2018 3rd International Conference on Robotics and AutomationEngineering（ICRAE）.GuangZhou：China，USBISBN：978-1-5386-9593-7.

[2]Trung-Kien Dao，Chih-Keng Chen.Sliding-mode control for the roll-angle tracking of an unmanned bicycle[J].Taylor&Francis.ISSN：0042-3114.

[3]郭磊，廖啟貞，魏世民.自行車(chē)機(jī)器人動(dòng)力學(xué)建模與MIMO反饋線性化[J].北京郵電大學(xué)學(xué)報(bào)，2007，30（1）：80-84

[4]劉璟，梁昔明.LQR控制與PID控制在單級(jí)倒立擺中的對(duì)比研究[J].控制理論與應(yīng)用，2007（1）：13.

[5]張永立，李洪興，苗志宏，程會(huì)鋒.基于變?cè)鲆鍸QR控制方法的二級(jí)倒立擺自動(dòng)擺起[J]系統(tǒng)工程理論與實(shí)踐，2011，31（7）：1341-1355.

[6]金鑫，鐘祥，謝雙義，等.大型風(fēng)力發(fā)電機(jī)轉(zhuǎn)矩LQR控制及載荷優(yōu)化[J].電氣系統(tǒng)保護(hù)與控制，2013（6）：93.

[7]Everett.X.Wang，JunchengZou，Gengping.Xue，YiJun.Liu，etc..Development of Ef fi cient Nonlinear Benchmark BicycleDynamics for Control Applications [J].IEEE，2236-2246.

[通聯(lián)編輯：梁書(shū)]