王 鐸，袁 亮，侯愛萍，姜 宏

(新疆大學(xué) 機(jī)械工程學(xué)院，烏魯木齊 830047)

0 引言

隨著國內(nèi)外無人機(jī)(Unmanned Aerial Vehicle, 簡稱UAV)研究領(lǐng)域的迅猛發(fā)展，人們對無人機(jī)擴(kuò)展功能的需求愈發(fā)強(qiáng)烈。無人機(jī)視覺技術(shù)作為其中較為突出的分支，在測繪、監(jiān)控和安防等方面具有廣闊的應(yīng)用前景，而高度自穩(wěn)定的機(jī)載云臺則是無人機(jī)視覺技術(shù)獲取穩(wěn)定圖像數(shù)據(jù)的基礎(chǔ)。微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System, 簡稱MEMS) 作為云臺主要傳感器，承擔(dān)著實時反饋云臺姿態(tài)，維持機(jī)載相機(jī)穩(wěn)定的重任。

通常使用的MEMS中，陀螺儀存在嚴(yán)重的偏移積分累計誤差[1]，需多傳感器的融合處理，同時考慮機(jī)載云臺載重量和有限的計算性能，選擇合適的姿態(tài)解算尤為重要。擴(kuò)展卡爾曼濾波(Extended Kalman Filter, 簡稱EKF)雖然具有較高的精準(zhǔn)度[2-3]，但較大計算量和復(fù)雜性一直是EKF的難題。R Mahony等[4-5]提出的顯式互補(bǔ)濾波算法(Explicit Complementary Filter,簡稱ECF)在較少的計算量下?lián)碛休^高的性能[6]，但面對復(fù)雜環(huán)境需要調(diào)整參數(shù)，適應(yīng)性較差。S O H Madgwick等[7-8]提出的基于梯度下降法的姿態(tài)解算已在微控制器中應(yīng)用[9]，其核心思想是通過梯度下降法縮小加速度計與地磁計表征的姿態(tài)四元數(shù)誤差[10]，并與由陀螺儀計算得到的姿態(tài)四元數(shù)線性融合得到最優(yōu)的姿態(tài)。該算法即能保證較高精度的同時又能滿足云臺計算量小的要求，無需調(diào)整參數(shù)適應(yīng)性較強(qiáng)，但在較大運(yùn)動加速度或姿態(tài)變化劇烈時，加速度計較差的動態(tài)性能對整體算法影響較大，解算出的姿態(tài)信息仍有所欠缺。

本文作者將卡爾曼濾波與梯度下降法相結(jié)合，提出一種改進(jìn)梯度下降法的云臺姿態(tài)解算。利用卡爾曼濾波器對加速度計進(jìn)行預(yù)處理，減小劇烈振動引起的加速度噪聲對姿態(tài)解算的影響，再通過梯度下降法消除姿態(tài)四元數(shù)誤差，從而得到矯正后較為精準(zhǔn)的姿態(tài)信息，并與同樣是較低計算量的顯式互補(bǔ)濾波作對比，印證了改進(jìn)算法的優(yōu)劣性和可行性。

1 姿態(tài)參考系

1.1 四元數(shù)及姿態(tài)解算

(1)

(2)

根據(jù)z-y-x的旋轉(zhuǎn)順序，可由四元數(shù)求解出最終姿態(tài)角：

(3)

1.2 角速度表征的姿態(tài)更新

由四元數(shù)描述E系相對S系的變化率，則有陀螺儀在t時刻表征的姿態(tài)四元數(shù)微分方程與更新方程：

(4)

(5)

2 梯度下降法

雖然陀螺儀的動態(tài)穩(wěn)定性良好，但易累計誤差產(chǎn)生漂移，而加速度計的靜態(tài)穩(wěn)定性好，動態(tài)性能較差，二者融合后yaw角誤差較大，地磁計能夠有效縮小yaw角誤差，因此需要多傳感器融合。

由梯度下降法通用迭代公式應(yīng)用在本文中有：

(6)

(7)

(8)

(9)

將梯度(9)代入迭代公式(6)，得到t時刻的加速度計和地磁計表征的姿態(tài)四元數(shù)更新方程：

(10)

為融合多傳感器表征的姿態(tài)四元數(shù)，由文獻(xiàn)[9]可得融合的梯度下降法姿態(tài)解算四元數(shù)更新方程：

(11)

3 改進(jìn)梯度下降法的加速度計預(yù)處理

為保證系統(tǒng)最終輸出可靠、穩(wěn)定的姿態(tài)角，本文在進(jìn)行基于梯度下降法的姿態(tài)解算前，先對加速度計輸出的數(shù)據(jù)進(jìn)行卡爾曼濾波預(yù)處理。

在常規(guī)卡爾曼濾波[11]中，考慮如下線性離散系統(tǒng)有狀態(tài)方程[12]：

xk+1=Axk+ωk

(17)

zk=Cxk+νk

(18)

系統(tǒng)下一點狀態(tài)預(yù)測：

(19)

狀態(tài)預(yù)測方差：

Pk|k-1=APk-1AT+Qk-1

(20)

卡爾曼增益計算：

Kk=Pk|k-1CT[CPk|k-1CT+Rk]-1

(23)

系統(tǒng)狀態(tài)更新：

(21)

協(xié)方差更新：

Pk=[I-KkC]Pk|k-1

(22)

此處^為預(yù)測值，利用上述卡爾曼濾波器來減小加速度計動態(tài)誤差對梯度下降法的影響，提高云臺姿態(tài)反饋的真實性和穩(wěn)定性。本文提出的改進(jìn)梯度下降法敘述完畢，算法原理如圖1所示。

圖1 改進(jìn)梯度下降法的云臺伺服系統(tǒng)

4 實驗結(jié)果與分析

4.1 實驗平臺

本文實驗平臺是以ARM-CortexM3為內(nèi)核的STM32F103微處理器搭建的三軸無刷云臺。慣性測量單元(Inertial measurement unit，簡稱 IMU)選用MPU6000，該測量單元為集三軸陀螺儀與三軸加速度計的MEMS傳感器，采樣頻率25Hz，地磁計選用HMC5883L電子指南針羅盤。

實驗參考的真實姿態(tài)由無人機(jī)飛行控制器pixhack px4提供，pixhack擁有雙IMU、雙地磁計和GPS等多種傳感器來獲得穩(wěn)定、真實的姿態(tài)角。實驗平臺實物如圖2所示。

圖2 機(jī)載云臺姿態(tài)解算系統(tǒng)實物

4.2 靜態(tài)實驗

為驗證改進(jìn)算法的綜合性能，本文作者進(jìn)行了兩組對比實驗：第一組為靜態(tài)實驗，以測試算法在平臺空轉(zhuǎn)情況下的靜態(tài)性能；第二組為動態(tài)實驗，以測試算法在復(fù)雜飛行環(huán)境中的動態(tài)性能。

靜態(tài)實驗中，在無人機(jī)電機(jī)未安裝螺旋槳空轉(zhuǎn)情況下，測試改進(jìn)梯度下降法的靜態(tài)性能，并與真實姿態(tài)和顯式互補(bǔ)濾波姿態(tài)解算作對比，靜態(tài)實驗局部放大結(jié)果如圖3所示。

圖3 靜態(tài)實驗下各姿態(tài)算法

表1給出了各算法姿態(tài)角與真實姿態(tài)角的均方根誤差，由此可知改進(jìn)算法整體精確度高于梯度下降法算法，整體姿態(tài)角精確度提高15.25%，但相較于顯式互補(bǔ)濾波姿態(tài)解算，梯度下降法姿態(tài)解算及其改進(jìn)算法靜態(tài)性能明顯較差，其根本原因在于梯度下降法的動態(tài)步長在平穩(wěn)環(huán)境下步長過大，無法快速得到最優(yōu)解，在靜態(tài)環(huán)境中性能較差。

表1 靜態(tài)實驗姿態(tài)角均方根誤差

4.3 動態(tài)實驗

在動態(tài)實驗中，無人機(jī)分別完成起飛-偏航前進(jìn)-定點懸停-爬升-返航降落的動作，實驗天氣環(huán)境：雨后風(fēng)力3級，以此復(fù)雜環(huán)境來測試改進(jìn)梯度下降法的動態(tài)性能。動態(tài)實驗姿態(tài)角對比結(jié)果由圖4所示，局部放大圖如圖5所示。

圖4 動態(tài)實驗姿態(tài)角對比

表2給出了各算法解算出的姿態(tài)角與真是姿態(tài)的均方根誤差，由表中可知，改進(jìn)梯度下降法在動態(tài)性能上要明顯優(yōu)于顯式互補(bǔ)濾波姿態(tài)算法，改進(jìn)算法在梯度下降法的基礎(chǔ)上，有效減小了加速度計的動態(tài)誤差影響，精確度提升了27.53%，已基本接近由飛行控制器解算的真實姿態(tài)。圖6為動態(tài)實驗機(jī)載相機(jī)航拍圖像。

表2 動態(tài)實驗姿態(tài)角均方根誤差

圖5 動態(tài)實驗局部放大圖

圖6 動態(tài)實驗相機(jī)航拍圖像

5 結(jié)論

本文針對低成本的機(jī)載云臺系統(tǒng)，提出了一種改進(jìn)梯度下降法的姿態(tài)解算，利用卡爾曼濾波對加速度計數(shù)據(jù)預(yù)處理，減小加速度計動態(tài)誤差對姿態(tài)信息的影響，同時與顯式互補(bǔ)濾波作比對，印證了改進(jìn)梯度下降法姿態(tài)解算在靜態(tài)與動態(tài)性能中的優(yōu)劣勢。在靜態(tài)性能實驗中，顯式互補(bǔ)濾波要明顯優(yōu)于改進(jìn)梯度下降法，證明了合適的動態(tài)步長對于梯度下降法的必要性。在動態(tài)性能實驗中，改進(jìn)梯度下降法精度要優(yōu)于顯式互補(bǔ)濾波，證明了其算法在機(jī)載云臺系統(tǒng)中的可行性。