基于姿態(tài)傳感器的自平衡系統(tǒng)設計

高亮

摘 要：針對現(xiàn)有裝置在運動過程由于顛簸、搖晃產(chǎn)生抖動、晃動等問題，設計了自平衡控制系統(tǒng)。該系統(tǒng)利用四元數(shù)和歐拉角轉(zhuǎn)換關(guān)系，從姿態(tài)傳感器輸出數(shù)據(jù)中得到橫滾角和俯仰角，并在常規(guī)PID基礎上加入了模糊控制方法控制相應舵機進行補償運動。實驗結(jié)果表明，該系統(tǒng)姿態(tài)數(shù)據(jù)誤差為0.1度；在階躍沖擊作為輸入時，經(jīng)過0.1秒的調(diào)整進入穩(wěn)態(tài)，有較快的響應速度。

關(guān)鍵詞：自平衡控制；四元數(shù)和歐拉角；模糊PID

0 引言

自平衡控制系統(tǒng)是動力學理論和自動控制理論技術(shù)相結(jié)合的研究課題，可以認為是計算機控制下，通過對系統(tǒng)狀態(tài)參數(shù)的實時分析，使系統(tǒng)在水平方向或垂直方向的位移和角度的偏移量控制在允許的范圍以內(nèi)，從而使系統(tǒng)保持平衡[1]。自平衡控制系統(tǒng)關(guān)鍵是解決如何計算和補償系統(tǒng)在運動過程中自平衡裝置跟隨其載體運動而發(fā)生的傾斜與晃動。

針對以上問題，本文提出利用四元數(shù)和歐拉位移定理求出橫滾角與俯仰角，再通過模糊PID算法輸出兩路PWM驅(qū)動兩個方向的相應舵機進行補償運動達到自平衡效果。

1 自平衡控制平臺的動態(tài)平衡原理

假設系統(tǒng)的前進方向為X，并以此建立右手坐標系，當自平衡發(fā)生傾斜，其傾斜姿態(tài)如圖1所示，其中繞Y軸運動的角度稱之為俯仰角θ，繞前進方向運動X軸的運動角度稱為橫滾角φ。

當自平衡控制系統(tǒng)載體發(fā)生傾斜時，與系統(tǒng)固定在同一支架的姿態(tài)傳感器會跟隨傾斜相同的角度，通過傳感數(shù)據(jù)進行解算可以得到當前系統(tǒng)的俯仰角度和橫滾角度，再經(jīng)過PID等算法輸出兩路PWM驅(qū)動兩個方向的相應舵機進行補償，這就是本自平衡控制系統(tǒng)的基本原理。

2 四元數(shù)與歐拉角

通過四元數(shù)和歐拉角解算可以快速得到當前自平衡系統(tǒng)傾斜狀態(tài)下的橫滾角φ與俯仰角θ[2] 。

使用自平衡控制系統(tǒng)載體坐標系XYZ與世界坐標系依次按照Z-Y-X的順序進行旋轉(zhuǎn)得到俯仰角θ和橫滾角為φ表達式[3]。

其中A表示姿態(tài)矩陣。

在單位時間Δt內(nèi)，假定剛體的角速度是，根據(jù)歐拉位移定理那么該轉(zhuǎn)動軸的轉(zhuǎn)動方向、環(huán)繞該軸轉(zhuǎn)動的角度分別是：

四元數(shù)表達式為：

并滿足約束條件q20+q21+q22+q23=1。

用超復數(shù)形式，可以表達為。利用三角公式：ij=-ji=k

可以把四元數(shù)轉(zhuǎn)變?yōu)樽藨B(tài)矩陣：

將姿態(tài)矩陣表達式帶入表達式（1）可得：

3 參數(shù)自定的模糊PID控制原理

由于本系統(tǒng)結(jié)構(gòu)復雜，很難得到被控對象的精確數(shù)學模型，所以采用模糊PID控制控制相應舵機進行補償運動。

模糊PID控制器主要由兩部分組成，分別為常規(guī)PID控制器和模糊推理。其原理圖如圖2[4]所示。

常規(guī)PID控制器比例、積分、微分控制的簡稱，由于其控制算法簡單、魯棒性好和可靠性高，而成為眾多自動控制方法中應用最為普遍的控制方法[5]?？刂圃韴D如圖3所示。

模糊推理是通過模糊條件語句來實現(xiàn)模糊控制器的控制規(guī)則。經(jīng)過量化后的模糊態(tài)變量為{NB，NM，NS，O，PS，PM，PB}，子集中的元素從左到右依次表示當前控制誤差值的狀態(tài)，從負大到正大。精確態(tài)到模糊態(tài)是通過隸屬函數(shù)實現(xiàn)的。當前普遍采用的隸屬函數(shù)有梯形分布、正態(tài)分布、三角形分布等。

4 實驗測試

4.1 姿態(tài)傳感器數(shù)據(jù)修正

在自平衡控制平臺完成組裝后，由于存在安裝誤差，不能保證姿態(tài)傳感器的在安裝后全與自平衡系統(tǒng)姿態(tài)保持絕對一致，所以需要對傳感器進行數(shù)據(jù)修正。使用角位移測量平臺對整個控制平臺進行傾斜測試，同時讀出傳感器的姿態(tài)輸出數(shù)據(jù)，通過數(shù)據(jù)分析可以得到安裝誤差以及系統(tǒng)誤差。把角位移測量平臺的數(shù)據(jù)與傳感器輸出數(shù)據(jù)進行對比分析，得出以下修正公式：

x'=0.9*x-0.6

y'=0.9*y+3.0 （2）

其中，x代表姿態(tài)解算后的橫滾角，x代表修正后的橫滾角，y代表姿態(tài)解算后的俯仰角，y代表修正后的橫滾角。

4.2 模糊PID控制算法MatLab仿真

在MatLab中使用模糊PID算法進行仿真實驗。經(jīng)過調(diào)試，設定PID控制參數(shù)初值為kp=0.9，ki=0，kd=1時，系統(tǒng)能夠迅速進入穩(wěn)態(tài)。階躍響應的變化如下圖4所示：

實驗仿真中，模糊PID控制器在經(jīng)過0.1秒左右的調(diào)整時間，系統(tǒng)進入穩(wěn)態(tài)，響應速度快，控制結(jié)果穩(wěn)定，是比較理想的控制器。

5 結(jié)語

實驗表明安裝誤差橫滾方向為負0.6度，俯仰方向安裝誤差正3度，修正系數(shù)為0.9，經(jīng)過公式（2）修正后，得到實測值與修正值的對比關(guān)系，姿態(tài)誤差約為0.1度，符合設計要求；在MatLab仿真測試中，在階躍沖擊作為輸入，系統(tǒng)經(jīng)過0.1秒的調(diào)整進入穩(wěn)態(tài)，有較快的響應速度。因此，本自平衡控制系統(tǒng)能夠有效的解決現(xiàn)有裝置在顛簸、搖晃中產(chǎn)生抖動、晃動等問題。

參考文獻：

[1]張華賓等.基于DSP的兩輪式倒立擺自平衡控制系統(tǒng)[J].測控技術(shù)，2007，26（11）：23-26.

[2]張帆.一種新的全角度四元數(shù)與歐拉角的轉(zhuǎn)換算法[J].南京理工大學學報（自然科學版），2002，26（04）：376-380.

[3]章仁為.衛(wèi)星軌道姿態(tài)動力學控制[M]. 北京：北京航空航天大學出版社，1998.

[4]黃忠森.控制系統(tǒng)MatLab計算及仿真[M].北京：國防工業(yè)出版社，2001.

[5]劉金坤.先進PID控制及其MatLab仿真[M].北京：電子工業(yè)出版社，2003.endprint