基于四元數的PID改進型互補濾波算法

朱科風 周慶華 王廣鵬 王樂 林思宇

文章編號： 2095-2163（2018）03-0127-05中圖分類號： 文獻標志碼： A

摘要： 關鍵詞： （School of Physics & Electronic Science， Changsha University of Science & Technology， Changsha 410114， China）

Abstract： The traditional complementary filter algorithm which realizes the data fusion of multi sensors and compensates the integration error of the gyroscope has the advantage of easier calculation. But it also has some defects， such as large data fluctuation and calculating error. In this paper， the differentiation element is introduced to solve the problem of data fluctuation. An improved algorithm is proposed with PID controller to weaken data fluctuation. The experimental results show that the algorithm in this paper has the advantage of less computation and small data fluctuation. It can achieve stable state very quickly and track quadcopter effectively.

Key words：

基金項目：

作者簡介：

通訊作者：

收稿日期： 引言

科技的不斷發(fā)展使得中小型四軸飛行器（以下簡稱四軸）的續(xù)航能力越來越強，四軸的垂直起落、空中懸停、靈活轉向等特點也使得其在民用和軍事領域都得到了廣泛的應用[1]。而四軸的姿態(tài)獲取是能穩(wěn)定控制四軸飛行的關鍵。

四軸的姿態(tài)獲取指的是獲取四軸相對于面的角位置，獲取方式主要依賴于慣性測量傳感器，其中包括陀螺儀、加速度計以及磁力計。但是能搭載在四軸上的傳感器往往精度不高，主要表現在陀螺儀的動態(tài)特性會因積分運算而產生累積誤差，且誤差隨時間增大；加速度計與磁力計的靜態(tài)特性容易受到噪聲影響[2]。在硬件條件受限的情況下，對姿態(tài)解算算法的研究成為了一個熱點，例如：由Madgwick提出的梯度下降法解決了數據融合，避免了地磁補償前對當地地磁場角度的測量，但是在高速運動情況下偏航角響應存在延遲，會導致姿態(tài)解算出錯[3]。Mahony互補濾波算法通過加速度計與磁力計測量的姿態(tài)數據來補償陀螺儀的積分誤差[4]，運算量小，但精度不高。本文采用經PID控制器改進的互補濾波算法，使姿態(tài)解算響應更快，穩(wěn)定性更強，更有利于四軸的控制。

1姿態(tài)描述

為了對四軸的姿態(tài)進行描述，需要建立相應的2個不同的坐標系。飛行器的姿態(tài)是參考地面來描述的，需要以當地地理位置為參照，以東、北、天方位為X、Y、Z軸建立導航坐標系N[5]。陀螺儀、加速度計等傳感器是搭載機體上的，所測得的數據是以機體為參考，以前、右、上方位為x、y、z軸的載體坐標系b來描述。2個坐標系間的變換關系則如圖1所示。

圖1導航坐標系N與載體坐標系b的變換關系

Fig. 1Transformation relation between coordinate system N and coordinate system b

姿態(tài)的數學表示方式通常采用的是歐拉角和四元數[6]。載體相對于導航坐標系的角位置關系可以通過導航坐標系繞坐標軸連續(xù)旋轉3次來確定：繞導航坐標系Z軸旋轉φ角，再繞新坐標系的Y軸旋轉θ角，再繞新坐標系的X軸旋轉角，實現變換。φ、θ、分別為載體的偏航角、俯仰角、翻滾角，用歐拉角表示的由N系變換到b系的變換矩陣為CbN，數學表述將分別如式（1）、式（2）所示。

CbN=cos φcos θsin φcos θ-sin θ

sin sin θcos φ-sin φcos sin sin θsin φ+cos φcos sin cos θ

sin sin φ+sin θcos φcos sin θsin φcos -sin cos φcos θcos （1）X

Y

Z=CbNx

y

z（2）以歐拉角來表示變換矩陣直觀易懂，但是歐拉角在求取姿態(tài)時存在奇點，不能用于全姿態(tài)解算[7]。為了解決求取姿態(tài)時的奇點問題，本文采用四元數法計算求取姿態(tài)。四元數法具有計算量小，解算無奇點等優(yōu)點，能夠滿足四軸全姿態(tài)解算的需求。

四元數由4個元構成，定義q為一個常規(guī)四元數向量，數學公式具體如下：q=q0+q1i+q2j+q3k（3）其中，q0、q1、q2、q3為實數，i、j、k為兩兩正交的向量。當僅是關注N系與b系的角位置關系時，兩坐標系之間的變換可以等效為無中間過程的一次等效旋轉，這個旋轉可以由四元數向量給出表示[8]。矢量v=q1i+q2j+q3k包含了旋轉軸和旋轉方向的信息，標量u=q0包含了旋轉角度的信息。根據四元數的約束條件，只有單位四元數才能表示旋轉，將四元數進行單位化，研究推得變換公式如下：q=qq20+q21+q22+q23（4）由N系變換到的b系的變換矩陣用四元數表示為RbN，其計算公式可見如下：

RbN=

1-2q22+q232q1q2+q0q32q1q3-q0q2

2q1q2-q0q31-2q21+q232q2q3+q0q1

2q1q3+q0q22q2q3-q0q11-2q21+q22（5）

由RbN=CbN得出歐拉角與四元數關系，即有如下數學公式：θ=sin-1q0q2-q1q3

=tan-12q2q3+q0q11-2q21+q22

φ=tan-12q1q2+q0q31-2q22+q23（6）2改進型互補濾波算法

2.1傳統(tǒng)的互補濾波算法原理

四軸通過陀螺儀、加速度計以及磁力計獲得原始的姿態(tài)數據。陀螺儀的動態(tài)特性好，短時間內測量的數據精度較高，但是由于零漂、溫漂的影響隨著時間的積累會形成較大的誤差；而加速度計與磁力計靜態(tài)特性好，但動態(tài)特性差，容易受高頻噪聲影響。即在頻域上陀螺儀高頻特性好，低頻特性差；加速度計與磁力計低頻特性好，高頻特性差。通過互補的高通、低通濾波器融合3個傳感器的數據，可以實現較為準確的姿態(tài)角解算。

根據傳感器的頻域特性，將陀螺儀的監(jiān)測數據通過高通濾波器的設計處理，同時將加速度計與磁力計的數據通過低通濾波器的綜合處理，2部分數據相加即得到最終姿態(tài)?；パa濾波原理可如圖2所示。

假設陀螺儀測量運算后的數據為θg=θ+μL，加速度計與磁力計的測量數據為θma=θ+μH。其中，θ為無誤差姿態(tài)數據，μL為低頻噪聲，μH為高頻噪聲。選取一組傳遞函數H1s=ss+Cs、H2s=Css+Cs滿足H1s+H2s=1。并且，H1s為高通濾波器傳遞函數，H2s為低通濾波器傳遞函數，Cs的選取滿足θs對互補濾波器全通[9]。濾波器的估計量頻域響應為：

θ^˙s=θgsH1s+θmasH2s=

θs+H1sμLs+H2sμHs（7）

在實現數據互補的同時為了消除靜態(tài)誤差，選取Cs=Kp+Kis，算法通過PI控制來重點調整濾波器的截止頻率與消除靜態(tài)誤差的時間。通過將加速度計與磁力計測得的姿態(tài)角θma與濾波器預估的姿態(tài)角求差，將差值經過PI調節(jié)后補償陀螺儀的數據得到最終的姿態(tài)角，構成一個如圖2所示的動態(tài)的閉環(huán)系統(tǒng)，互補濾波器的估計量即如式（8）所示：

θ^˙=θg+Kpθma-θ^+Ki∫θma-θ^dt（8）

其中，θ^˙為互補濾波器估計的輸出量；θ^為反饋量；dt為采樣時間。

2.2PID改進型互補濾波算法

互補濾波算法可以達到傳感器數據的充分融合，通過運用PI控制器，有效地解決了互補濾波器的低通阻帶衰減較慢，受噪聲影響大的問題[10- 11]。PI控制器可通過選取合適的Kp、Ki來調整濾波器的截止頻率和消除靜態(tài)誤差的時間[12]，但是周期性的誤差調整使得姿態(tài)數據不斷地振動，導致解算的姿態(tài)角并不穩(wěn)定。四軸應對突變的恢復能力較弱，不能迅捷恢復平衡。

為此，針對姿態(tài)數據的振動問題，本文設計引入微分環(huán)節(jié)D，構成PID控制器。PID控制器可以通過微分環(huán)節(jié)反映誤差信號的變化趨勢，再據此展開超前調節(jié)，抑制數據的振動，并且增強系統(tǒng)的快速性。因此使用PID控制器可以抵消周期調整誤差所產生的振動，還可以加快互補濾波算法的響應速度。改進算法的流程如圖3所示。

由圖3可知，ab、mb分別為加速度計與磁力計的測量值，ab=axayazT、mb=mxmymzT，歸一化后得ab^、mb^。vb^表示b系重力加速度，wb^表示b系的地磁強度，分別由N系的重力加速度001T與地磁強度bx0bzT經過旋轉矩陣變換得到，旋轉變換矩陣則為式（5）的RbN。改進算法通過計算加速度計、磁力計的測量數據與重力加速度、地磁強度的誤差e來補償陀螺儀的數據，補償后的陀螺儀數據通過一階龍哥庫塔方程對四元數進行更新。各個參數的關系推導可表示如下：e=ab^×vb^+mb^×wb^（9）

經過更新的四元數通過反饋環(huán)節(jié)實現對重力加速度與地磁強度的變換，再與傳感器數據經過叉乘運算求得誤差e，因此誤差e具有上一周期的陀螺儀數據信息。實驗中，四軸姿態(tài)信號采樣周期為0.01 s，陀螺儀數據在短時間內變化較小，上一周期的陀螺儀數據對下一個周期的陀螺儀數據具有參考意義。因此經過上一周期四元數變換后求得的誤差e可以作為下一周期陀螺儀數據的預調量，構成PID控制器的微分部分D。PID控制器的參數Kp、Ki、Kd需要經過反復實驗，選取合適的值以滿足較為穩(wěn)定、準確的姿態(tài)解算。

3實驗驗證及結果分析

本文是基于Arduino101開發(fā)板構建的四軸硬件平臺，Arduino101開發(fā)板搭載有BMI160六軸慣性測量單元，包括三軸加速度計與三軸陀螺儀，磁力計采用的是MAG3110。測試數據通過串口傳輸給PC機，在PC機上完成數據分析，采樣頻率為100 Hz。

陀螺儀與加速度計的原始數據，則如圖4所示。圖4是在小幅連續(xù)振動的情況下在陀螺儀與加速度計上測得的原始數據。由圖4可見，在前3 s連續(xù)振動的情況下，陀螺儀數據能實時反映四軸平臺的運動狀態(tài)，動態(tài)數據較為敏感，可以實時跟蹤四軸平臺的姿態(tài)變化。而加速度計數據不能切實反映四軸平臺的運動狀態(tài)，動態(tài)反應失真。四軸平臺回穩(wěn)后，陀螺儀數據仍有波動，靜態(tài)條件下不穩(wěn)定，有較為明顯的誤差。而加速度計數據靜態(tài)數據非常準確，變化較小。

將本文的四軸測試平臺經過多次實驗測試后可知，改進算法的PID參數設為Kp=2.0，Ki=0.005，Kd=0.05時，能獲得較為理想的姿態(tài)解算結果。將該算法與傳統(tǒng)的互補濾波算法、梯度下降算法對四軸的俯仰角（Pitch）與翻滾角（Roll）分別給出觀測的效果對比。

運算得到3種算法的解算的姿態(tài)角結果，即如圖5所示。

圖5是將測試平臺的俯仰角與翻滾角各自在約10°、3°的位置保持幾秒后恢復到水平靜止狀態(tài)所測得的數據。分析后發(fā)現：梯度下降法數據波動較大，所需回穩(wěn)時間較長；互補濾波算法與改進型算法波動幅度較小，回穩(wěn)速度快，能夠更快地趨于穩(wěn)定。

PID改進型算法是在互補濾波算法的基礎上改進得來。為了驗證改進算法在抑制振動上的優(yōu)勢，分別在俯仰方向與翻滾方向作連續(xù)振動測試，并對2種算法的運算結果進行數據采集，在PC上位機調取相應數據分析，可得結果如圖6所示。

圖6是分別截取2種姿態(tài)角連續(xù)10 s的數據?？梢钥闯觯倪M型互補濾波算法運行解算的姿態(tài)數據波動更小，所需趨于平穩(wěn)的時間更短，而且在回穩(wěn)后數據較為穩(wěn)定。相比于互補濾波算法，改進型算法的穩(wěn)定特性使得四軸更容易控制。

為了測試3種算法的靜態(tài)狀況下的解算準確性，對3種算法在靜止水平（俯仰角、翻滾角均為零度）狀態(tài)下的姿態(tài)角各取50組求出平均值，最終得到測量的平均誤差可見表1。從表1可以得出改進型互補濾波算法在俯仰角上平均誤差相對于梯度下降法和互補濾波算法分別降低了45.4%、52.4%；在翻滾角上，平均誤差分別下降了26.3%、62.2%。

結束語

針對互補濾波算法存在的數據波動大等問題，本文提出了四元數PID改進型互補濾波算法。該算法通過四元數描述四軸姿態(tài)，避免了歐拉角的奇點問題，實現了全姿態(tài)解算，在此基礎上引入了PID控制器調節(jié)姿態(tài)誤差，使得解算的姿態(tài)角更加穩(wěn)定。后續(xù)又通過仿真實驗對比展示了互補濾波算法、梯度下降算法與改進型算法的效果及性能。實驗結果表明，運動狀態(tài)下改進型算法解算出的俯仰角與翻滾角波動較小，且所需穩(wěn)定時間更短；在水平靜止的狀態(tài)下，改進型算法的解算誤差最小。由此可以得知，改進型算法有利于四軸的控制，能使四軸的飛行更加平穩(wěn)。

參考文獻

[1] 萬曉鳳， 康利平， 余運俊， 等. 互補濾波算法在四旋翼飛行器姿態(tài)解算中的應用[J]. 測控技術， 2015，34（2）：8-11.

[2] 陳亮， 楊柳慶， 肖前貴. 基于梯度下降法和互補濾波的航向姿態(tài)參考系統(tǒng)[J]. 電子設計工程， 2016，24（24）：38-41，45.

[3] MADGWICK S. An efficient orientation filter for inertial and inertial/magnetic sensor arrays[R]. UK：University of Bristol，2010.

[4] BALDWIN G， MAHONY R， TRUMPF J， et al. Complementary filter design on the Special Euclidean group SE（3）[C]// Proceedings of the European Control Conference. Kos， Greece： IEEE，2007：3763-3770.

[5] 季元揚， 陳躍東， 陳孟元. 基于PID改進型互補濾波[J]. 控制工程， 2017，24（5）：952-957.

[6] 徐云川. 四軸飛行器姿態(tài)解算算法設計與仿真[J]. 科技視界， 2016（23）：17-18.

[7] 陳孟元， 謝義建， 陳躍東. 基于四元數改進型互補濾波的MEMS姿態(tài)解算[J]. 電子測量與儀器學報， 2015，29（9）：1391-1397.

[8] 秦永元. 慣性導航[M]. 2版. 北京：科學出版社有限責任公司，2014.

[9] MAHONY R， HAMEL T， PFLIMLIN J. Nonlinear complementary filters on the special orthogonal group[J]. IEEE Transactions on Automatic Control， 2008，53（5）：1203-1218.

[10]呂印新， 肖前貴， 胡壽松. 基于四元數互補濾波的無人機姿態(tài)解算[J]. 燕山大學學報， 2014，38（2）：175-180.

[11]杜杉杉， 吳昊， 張繼文， 等. 一種面向AHRS的改進互補濾波融合算法[J]. 國外電子測量技術， 2015，34（3）：13-18.

[12]陳雷， 卞鴻巍. 基于模糊自適應互補濾波的姿態(tài)解算算法[J]. 兵器裝備工程學報， 2017，38（4）：183-187.（上接第126頁）

[4] Yang Xinshe. Flower pollination algorithm for global optimization[C]//LNCS 7445： Proceedings of the 11th Internation Conference on Unconventional Computation and Natural Computation， Orléan， France， Sep 3-7， 2012. Berlin， Heidelberg： Springer， 2012： 240-249.

[5] Cheng S， Zhang Q， Qin Q. Big data analytics with swarm intelligence[J]. Industrial Management & Data Systems， 2016， 116（4）：646-666.

[6] 馮翔，張進文，虞慧群. 仿生蚊子追蹤算法[J]. 計算機學報，2014，37（8）：1794-1808.

[7] 喬瑩瑩，宋威，馬偉. 基于GA優(yōu)化QPSO算法的文本聚類[J]. 計算機應用研究，2014，31（10）：2912-2915.

[8] Yang X S. A New Meta-heuristic Bat-Inspired Algorithm[J]. Computer Knowledge & Technology， 2010， 284：65-74.

[9] Wang G G， Chu H C E， Mirjalili S. Three-dimensional path planning for UCAV using an improved bat algorithm[J]. Aerospace Science & Technology， 2016， 49：231-238.

[10]Karri C， Jena U. Fast vector quantization using a Bat algorithm for image compression[J]. Engineering Science & Technology An International Journal， 2016， 19（2）：769-781.

[11]姚妮，李紅嬋. 基于混合蝙蝠算法的多目標柔性作業(yè)車間調度問題[J]. 微電子學與計算機，2017，34（3）：25-29，34.

[12]肖輝輝，段艷明. 基于DE算法改進的蝙蝠算法的研究及應用[J]. 計算機仿真，2014，31（1）：272-277.

[13]He X S， Ding W J， Yang X S. Bat algorithm based on simulated annealing and Gaussian perturbations[J]. Neural Computing & Applications， 2014， 25（2）：459-468.

[14]裴宇航，劉景森，李煜. 一種動態(tài)調整慣性權重的自適應蝙蝠算法[J]. 計算機科學，2017，44（6）：240-244.

[15]Mirjalili S. SCA： A Sine Cosine Algorithm for solving optimization problems[J]. Knowledge-Based Systems， 2016， 96：120-133.

[16]王文， 王勇， 王曉偉. 采用機動飛行的蝙蝠算法[J]. 計算機應用研究， 2014， 31（10）：2962-2964.