基于LQR控制的機器人自行車靜態平衡研究

古文生 王峰 周全 熊超偉

摘要：針對本團隊目前研究的機器人自行車靜態平衡控制，使機器人自行車達到自動靜止的目的。本文在對該系統進行動力學建模的基礎上，設計了一種基于LQR控制的算法，完成機器人自行車靜態平衡系統。該機器人自行車是在一輛自行車的基礎上改裝的，要使它能夠靜止不倒下，我們先讓前輪擺到90度。利用陀螺儀等傳感器進行實時檢測系統的狀態，然后STM32程序通過LQR算法輸出控制力矩給前輪電機，改變當前機器人自行車的狀態，使系統達到靜止平衡的狀態。

關鍵詞：機器人自行車;靜態平衡;動力學建模;LQR控制算法;STM32;陀螺儀

中圖分類號：TP2

文獻標識碼：A

文章編號：1009-3044（2020）04-0192-03

收稿日期：2019-11-21

作者簡介：古文生（1993—），男，廣東韶關人，碩士研究生，研究方向為機器人和智能車輛動力學建?？刂?王峰（1961—），男，美籍華人，特聘教授，研究生導師，主要研究方向為機器人和智能車輛建?？刂?周全（1995—），男，湖北黃岡人，碩士研究生，研究方向為機器人和智能車輛動力學建?？刂?熊超偉（1995—），男，江西豐城人，碩士研究生，研究方向為機器人和智能車輛動力學建?？刂?。

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.

機器人自行車是一種典型的欠驅動系統，要實現靜態平衡，必須要通過某一個控制量改變當前自行車的狀態，以抵消某個自由度產生的偏移，從而達到平衡。之前流行著一種實現機器人自行車靜態平衡的方法，就是在自行車上安裝一個高速運轉的飛輪，通過這個飛輪達到平衡的目的。但是這種飛輪需要轉很快才能達到平衡，需要消耗巨大能量。而本文介紹的方法是，機器人自行車切換到靜態時候，直接把前輪轉到90度，使前輪和車主軸呈現垂直的狀態。通過控制前輪的電機，改變車的狀態，達到靜態平衡。這種方法解決了飛輪消耗能量的缺點[1]。

線性二次型[4]最優控制設計是基于狀態空間技術設計一個優化的動態控制器，線性二次型（LQR）算法應用對象是現代控制理論中以狀態空間形式給出的線性系統，而目標函數為對象狀態和控制輸入的二次型函數。LQR最優設計是指設計出的狀態反饋控制矩陣K要使得二次型目標函數輸出最小值，而K矩陣由Q，R唯一確定。所以在選取這兩個矩陣的時候要特別注意。LQR可得到狀態線性反饋的最優控制規律，易于構成閉環最優控制。

針對線性系統的控制算法設計問題[5-7]，如果該系統的性能指標是狀態變量或者控制變量的二次型函數積分，那么這種動態系統的最優化問題稱為線性系統二次型性能指標的最優控制問題，也就是線性二次問題。線性二次型求優問題可以寫成統一的解析表達式和實現求解過程的規范化，并可簡單地采用狀態線性反饋控制律構成閉環最優控制系統，能夠兼顧多項性能指標，因此得到特別的重視，為現代控制理論中發展較為成熟的一部分。

1 系統建模

在設計LQR控制算法之前，要先對整個系統進行建模，并推導出狀態方程，才能夠進行LQR控制器設計的工作。因此，這一部分要對機器人自行車進行建模[3]。下面先看看整個機器人自行車的模型，如圖1所示。

圖1用了三個坐標系對車子進行描述，其中坐標系00是大地基準坐標系，O1為描述車身狀態的坐標系，O2為描述前輪狀態的坐標系。為了能夠表達機器人自行車所處的狀態，這里選取向量q用于描述車身的狀態，q=（q1，q2，q3），其中q表示車身傾斜角，即坐標系00和01兩者的z軸之間的夾角;q2表示車頭轉向角度，也就坐標系O0和O2兩者的x軸之間的夾角。q3表示前輪所轉過的角度，系統開機時所處的初始狀態為0度。

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

我們這里選擇用阿佩爾方程對機器人自行車進行建模。下面公式（1）就是阿佩爾方程。

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

其中M，C，G，K都是與向量q有關維度值相關的參數矩陣21。向量q在前面已經介紹過，這里不再重復寫出來。下面把M，C，G，K展開來看看。

接下來我們要對該動力學方程進行變換，因為我們實現靜態平衡時候，車頭的轉彎角度是固定在90度的，所以可以根據我們的實際情況對該方程進行一些簡化。最后得到以下公式（6）為系統的狀態方程[1]。

其中x=（q1，q3，q1，q3），因為我們要實現靜態平衡，92作為車頭轉向角已經固定在90度了，所以我們只需要觀測q1和q3相關值的變化就可以反映出當前系統的狀態，所以向量x里面只有這兩個參數有關的變量。對于A，B矩則是由M，C，G，K推導出來的。u是τ向量中的第三維度變量，也就是前輪轉向力矩。因為其他兩個維度沒有力矩輸出，所以這里只有這個力矩引入到了狀態方程。有了狀態方程，就可以進人到LQR控制器設計階段，下面將進人LQR控制器設計部分。

2 LQR控制器設計

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

3 系統的實現

我們前面已經對機器人自行車完成了建模和控制器設計，我們這里就要進行系統的實現，我們選擇STM32F405作為主控芯片，MPU6050陀螺儀作為檢測車身狀態的傳感器，SMP8048電機作為力矩輸出。下面圖3是整個系統的組成框圖。

從上圖3可以看出系統的組成結構和工作流程。其中MPU6050陀螺儀的作用是檢測機器人自行車的傾斜角和傾斜角速度等狀態量，然后把這些值送到STM32主控芯片當中;而另一邊的帶編碼器的電機，把編碼信號經過電機驅動機轉換后傳回給主控芯片，主控芯片即獲得電機轉速。主控芯片根據這些回傳數據進行判斷當前狀態是否平衡，然后經過LQR控制算法輸出PWM信號和方向信號給電機驅動器，電機驅動器把這些信號轉換成三相電等信號給電機。這就是整個系統的工作過程。圖4是具體的軟件組成框圖。

完成硬件軟件等工作之后，我們進行了一系列測試實驗，經過我們的實驗檢測，，實現了靜態平衡的目標，下圖5為實物作品。

當機器人自行車往左邊傾斜時，前輪立馬向左邊加速運.動，防止它往倒下。當它往右邊傾斜時，前輪往右邊加速，防止它往右邊倒下，這就實現了靜態平衡。

4 總結

機器人自行車要實現靜態平衡，必須有外力支撐它，讓它平衡不倒下，而我們這里把前輪擺到90度，給系統提供了支撐外力。我們的系統是一種典型的欠驅動系統，也就是說我們想要抵消車身不平衡時候造成的傾斜角偏移，是不能通過某個控制量直接抵消，而是借助其他控制量來抵消，這里我們是靠前輪力矩來實現這個目的。要如何控制前輪力矩抵消這一偏移，我們就設計了LQR控制器進行完成這些控制過程。因此，我們可以得出LQR控制器可以實現機器人自行車靜態平衡的結論。參考文獻：

[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]郭磊，廖啟貞，魏世民.自行車機器人動力學建模與MIMO反饋線性化[J].北京郵電大學學報，2007，30（1）：80-84

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

[5]張永立，李洪興，苗志宏，程會鋒.基于變增益LQR控制方法的二級倒立擺自動擺起[J]系統工程理論與實踐，2011，31（7）：1341-1355.

[6]金鑫，鐘祥，謝雙義，等.大型風力發電機轉矩LQR控制及載荷優化[J].電氣系統保護與控制，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.

[通聯編輯：梁書]