基于姿態傳感器的自平衡系統設計

高亮

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

關鍵詞：自平衡控制；四元數和歐拉角；模糊PID

0 引言

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

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

1 自平衡控制平臺的動態平衡原理

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

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

2 四元數與歐拉角

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

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

其中A表示姿態矩陣。

在單位時間Δt內，假定剛體的角速度是，根據歐拉位移定理那么該轉動軸的轉動方向、環繞該軸轉動的角度分別是：

四元數表達式為：

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

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

可以把四元數轉變為姿態矩陣：

將姿態矩陣表達式帶入表達式（1）可得：

3 參數自定的模糊PID控制原理

由于本系統結構復雜，很難得到被控對象的精確數學模型，所以采用模糊PID控制控制相應舵機進行補償運動。

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

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

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

4 實驗測試

4.1 姿態傳感器數據修正

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

x'=0.9*x-0.6

y'=0.9*y+3.0 （2）

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

4.2 模糊PID控制算法MatLab仿真

在MatLab中使用模糊PID算法進行仿真實驗。經過調試，設定PID控制參數初值為kp=0.9，ki=0，kd=1時，系統能夠迅速進入穩態。階躍響應的變化如下圖4所示：

實驗仿真中，模糊PID控制器在經過0.1秒左右的調整時間，系統進入穩態，響應速度快，控制結果穩定，是比較理想的控制器。

5 結語

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

參考文獻：

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

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

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

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

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