基于動量梯度下降法的姿態(tài)數(shù)據(jù)采集系統(tǒng)

朱延栓，戴曉強，蘭 武，吳 偉，楊淦華

（江蘇科技大學 電子信息學院，江蘇 鎮(zhèn)江 212003）

0 引言

面對世界人口飛增、土地資源緊缺和生態(tài)環(huán)境惡化，人類將目光逐漸轉(zhuǎn)向了海洋。由于海洋中復雜的水下情況，水下機器人（Remotely Operated Vehicle，ROV）成為目前各國進行海洋資源探索與開發(fā)的有效工具。由于工作條件惡劣、周圍環(huán)境復雜多變，水下機器人受到機械手以及水流等干擾因素的影響，且機械手的姿態(tài)、持重、載荷、水流等情況很難定量得出，故姿態(tài)解算的準確性和快速性直接決定著水下機器人執(zhí)行任務(wù)過程中的穩(wěn)定性和可靠性［1］。

由于機械陀螺儀及光纖陀螺儀等高精度慣性導航傳感器價格昂貴、體積較大，因此在應(yīng)用中大多利用微機電系統(tǒng)（MEMS）技術(shù)制造的加速度計、陀螺儀和磁力計等傳感器進行姿態(tài)檢測。九軸原始數(shù)據(jù)由三軸陀螺儀、三軸加速度計和三軸磁力計提供，陀螺儀用來檢測ROV 運動時的角速度，對其進行積分處理再經(jīng)過坐標轉(zhuǎn)換后可得到參考坐標系下的姿態(tài)角度，短時間內(nèi)數(shù)據(jù)精確度高，但隨著積分的存在，誤差也隨時間的增加而增大；加速度計檢測ROV 運動的加速度信息，其靜態(tài)性能良好，長時間工作不存在漂移現(xiàn)象，但是由于電機運動產(chǎn)生的噪聲會使數(shù)據(jù)出現(xiàn)異常；磁力計檢測ROV 周圍的磁場信息，精確度高，但對周圍磁場強度敏感，易受干擾［2-4］。通過多傳感器數(shù)據(jù)融合后的解算信息，能夠有效利用不同MEMS 器件之間的互補特性，穩(wěn)定地、精確地反應(yīng)參考坐標系下的ROV 姿態(tài)角，其結(jié)果勝于單個MEMS 器件的處理效果［5-9］。

文獻［10］在互補濾波器中引入PI 控制思想，通過自適應(yīng)調(diào)整截止頻率實現(xiàn)了陀螺儀、加速度計和磁力計數(shù)據(jù)的有效融合，但是姿態(tài)收斂速度較慢；文獻［11］研究了互補濾波的階數(shù)對慣性傳感器數(shù)據(jù)融合的影響，實驗結(jié)果證明融合后的數(shù)據(jù)精度隨互補濾波器階數(shù)的增加而提高，但是高階濾波器計算量太大；文獻［12］采用卡爾曼濾波器，通過加速度計和磁力計對陀螺儀偏差進行矯正，實驗結(jié)果證明了在磁場異常環(huán)境下穩(wěn)定效果好且精度較高，但是由于卡爾曼濾波計算量大，對于頻率低的單片機而言數(shù)據(jù)實時性不如互補濾波和梯度下降法；文獻［13］在對慣性傳感器姿態(tài)解算過程中，通過引入梯度下降法對四元數(shù)進行尋優(yōu)估計，實驗結(jié)果表明該過程收斂速度塊、測得的俯仰角誤差較小，但是該算法使用定步長解算姿態(tài)角，解算精度不高、收斂速度較慢。

本文基于梯度下降法設(shè)計了以STM32F429 系列單片機為主控制器的姿態(tài)檢測系統(tǒng)，MPU9250 九軸傳感器為姿態(tài)檢測模塊的姿態(tài)采集系統(tǒng)，由陀螺儀測量物體運動的角速度，進而解算成角度。利用加速度計和磁力計分別測量加速度和地磁場，作為機器人的觀測矢量校準陀螺儀的測量值。在數(shù)據(jù)融合方面，改進的梯度下降法巧妙地避免了磁力計補償需要知道當?shù)卮艌鼋嵌鹊膯栴}，當補償較小時3 個姿態(tài)靜態(tài)值非常穩(wěn)定，慢速運動下可以達到較高精度的要求；步長較大時，可以快速跟進高速運動下的姿態(tài)變化。

1 水下機器人組成及姿態(tài)采集系統(tǒng)設(shè)計

1.1 水下機器人組成

本文所述水下機器人是江蘇科技大學自主設(shè)計并研發(fā)的一款應(yīng)用于船體檢測及作業(yè)的開架式ROV，本體如圖1 所示。機器人本體攜帶攝像頭及水下燈等多種設(shè)備完成水下作業(yè)任務(wù)，水下機器人通過臍帶纜與水面控制臺相連。臍帶纜內(nèi)置四根網(wǎng)線和兩根電源線，實現(xiàn)對水下系統(tǒng)的供電以及水陸信息交互。水面控制臺負責顯示各設(shè)備工況信息并實現(xiàn)控制指令下達。

1.2 姿態(tài)采集系統(tǒng)設(shè)計

設(shè)計的姿態(tài)采集系統(tǒng)以STM32F429 單片機為核心，通過MPU9250 九軸傳感器對運動姿態(tài)進行檢測，對獲取的九軸傳感器信息進行濾波去噪，用梯度下降算法對水下機器人的姿態(tài)信息進行解算，融合磁力計信息修正航向角。處理后的姿態(tài)角度信息通過串口發(fā)送到上位機，并在上位機軟件上完成數(shù)據(jù)的曲線繪制。系統(tǒng)設(shè)計框圖如圖2 所示。

Fig.1 Underwater robot body圖1 水下機器人本體

Fig.2 Overall design of the system hardware圖2 系統(tǒng)硬件整體設(shè)計

MPU9250 內(nèi)部集成了3 軸加速度計、3 軸陀螺儀以及3軸磁力計，具有3 個16 位加速度AD 輸出，3 個16 位陀螺儀AD 輸出，3 個16 位磁力計AD 輸出，采用IIC 通訊方式，可以直接輸出9 軸的全部數(shù)據(jù)。MPU9250 九軸傳感器參數(shù)如表1 所示。

Table 1 MPU9250 parameters table表1 MPU9250 參數(shù)表

2 姿態(tài)解算算法

水下機器人的位姿信息需要通過地理坐標系n和載體坐標系b的坐標轉(zhuǎn)換求得，地理坐標系采用東北天作為固定參考坐標系，選擇水下機器人的質(zhì)心作為載體坐標系原點。水下機器人在執(zhí)行任務(wù)時，隨著載體坐標系的不停變化，用其相對于地理坐標系的變化角描述水下機器人位姿變化，水下機器人載體坐標系繞地理坐標系X 軸旋轉(zhuǎn)稱為橫滾角γ、繞Y 軸旋轉(zhuǎn)稱為俯仰角θ、繞Z 軸旋轉(zhuǎn)稱為偏航角ψ［14-15］。轉(zhuǎn)換公式為：

其中，xb、yb、zb為水下機器人機體坐標軸，xn、yn、zn為地理坐標軸，為地理坐標系與機體坐標系的轉(zhuǎn)換矩陣，為：

2.1 四元數(shù)法原理

四元數(shù)q 的微分方程˙=Ωq，寫成矩陣形式為：

式中的ωx、ωy、ωz是在陀螺儀中采集的三軸角速度。使用一階龍格庫塔法更新四元數(shù)，結(jié)合上述微分方程進而可得到當前時刻的水下機器人姿態(tài)信息：

四元數(shù)轉(zhuǎn)換為歐拉角的表達式如下：

2.2 梯度下降算法

梯度下降法是一種通過沿著與目標函數(shù)梯度相反的方向逐次迭代，使目標函數(shù)不斷逼近局部最優(yōu)解的數(shù)值方法［16-17］。在姿態(tài)數(shù)據(jù)融合系統(tǒng)中，通過不同傳感器解算的姿態(tài)數(shù)據(jù)確定目標函數(shù)，并通過不斷迭代的過程實現(xiàn)對最優(yōu)姿態(tài)數(shù)值的逼近。本文基于動量梯度下降法估計姿態(tài)四元數(shù)的流程如圖3 所示。

Fig.3 Flow chart of momentum gradient descent method圖3 動量梯度下降法處理流程

本文通過梯度下降法估計最優(yōu)姿態(tài)四元數(shù)，將姿態(tài)四元數(shù)的估計問題轉(zhuǎn)換為一個最優(yōu)值求取的過程。首先對參考坐標系下的重力加速度值與上一時刻最優(yōu)姿態(tài)四元數(shù)做叉乘處理，將得到的載體坐標系下的重力加速度估計值與加速度計的實際測量值再做誤差處理得到誤差向量，磁力計處理過程相似，得到另一個誤差向量；其次對誤差向量構(gòu)造目標函數(shù)，利用梯度下降法求解目標函數(shù)的梯度，并進行歸一化處理［18］；然后將歸一化后的梯度值與收斂步長β 相結(jié)合，并利用此結(jié)果修正由陀螺儀數(shù)據(jù)求解姿態(tài)四元數(shù)過程產(chǎn)生的累積誤差；最后通過一階龍格庫塔法更新出當前時刻的最優(yōu)姿態(tài)值［8-9］。梯度下降法的核心是構(gòu)建目標函數(shù)，姿態(tài)四元數(shù)最優(yōu)估計方法通過對加速度計和磁力計的誤差矢量構(gòu)造誤差目標函數(shù)，重力加速度誤差函數(shù)和地磁誤差函數(shù)為：

在誤差目標函數(shù)下降最快的方向，計算其梯度值并作歸一化處理，并利用此梯度修正經(jīng)陀螺儀解算的單位姿態(tài)四元數(shù)向量。采用梯度下降法，沿著負梯度方向不斷迭代修正四元數(shù)向量，修正后的四元數(shù)更新方程為：

其中，β為收斂步長，β值的設(shè)定需依據(jù)水下機器人角速度ω和系統(tǒng)采樣的實際周期T。β過小會導致四元數(shù)更新過程收斂速度慢，而β過大會引起系統(tǒng)震蕩，靜態(tài)效果差［19-20］。傳統(tǒng)的梯度下降法采用固定步長解算姿態(tài)角，當水下機器人運動速度變化較快時，容易導致解算的姿態(tài)角收斂緩慢、精度低，甚至發(fā)生錯誤。因此，本文對傳統(tǒng)梯度下降法進行改進，引入動態(tài)步長和動量：

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

3.1 實驗平臺

本文實驗參考的實際姿態(tài)采用某公司生產(chǎn)的AH100B慣性導航模塊，該模塊具有全姿態(tài)右移的穩(wěn)定性和實時性，廣泛應(yīng)用于各種運動姿態(tài)監(jiān)測領(lǐng)域。

將互補濾波算法（CF）和動量梯度下降算法（SGDM）解算ROV 姿態(tài)的過程用C 語言表示成代碼形式，并下載到慣性數(shù)據(jù)處理板中進行實驗。通過姿態(tài)數(shù)據(jù)處理板將解算的姿態(tài)數(shù)據(jù)和AH100B 參考姿態(tài)數(shù)據(jù)通過串口發(fā)送到上位機，并將該數(shù)據(jù)保存至txt 文件中，并通過MATLAB 繪制曲線，便于對姿態(tài)角度變化進行分析。水下機器人姿態(tài)數(shù)據(jù)采集板實物如圖4 所示（彩圖掃OSID 碼可見，下同）。

Fig.4 ROV attitude data acquisition board圖4 水下機器人姿態(tài)數(shù)據(jù)采集板

3.2 靜態(tài)實驗與分析

將姿態(tài)數(shù)據(jù)處理板固定在靜止的水平平面上，采用本文提出的互補濾波算法和動量梯度下降算法相比較，輸出的三軸姿態(tài)信息如圖5 所示。

Fig.5 Static attitude solution data圖5 靜態(tài)時姿態(tài)解算數(shù)據(jù)

姿態(tài)數(shù)據(jù)采集靜態(tài)實驗中，翻滾角、俯仰角、偏航角數(shù)據(jù)的理想值為0°。通過對比表1 中的三軸姿態(tài)數(shù)據(jù)誤差，動量梯度下降算法解算的橫滾角、俯仰角、偏航角的標準差分別為0.144 8、0.124 6、0.151 1，相比互補濾波算法解算的橫滾角、俯仰角、偏航角的標準差分別降低1.22%、47.47%、70.31%。結(jié)果表明，靜態(tài)時動量梯度下降算法解算的姿態(tài)角精度更高，效果明顯優(yōu)于互補濾波算法。

Table 2 Data comparison of the two algorithms in static state表2 靜態(tài)時兩種算法數(shù)據(jù)比較

3.3 動態(tài)實驗與分析

將姿態(tài)數(shù)據(jù)處理板和AH100B 一起固定在轉(zhuǎn)軸上，繞著三軸分別旋轉(zhuǎn)，觀察姿態(tài)數(shù)據(jù)處理的動態(tài)性能。從圖6和表3 可以得出，在動態(tài)環(huán)境下互補濾波算法解算的姿態(tài)角誤差大且震蕩嚴重；而本文提出的動量梯度下降算法解算的姿態(tài)角標準差比互補濾波算法解算的姿態(tài)角分別降低31.51%、20.62%、37.01%，與參考姿態(tài)角吻合度高、誤差小、收斂速度快。綜上所述，本文所提出的動量梯度下降算法明顯優(yōu)于互補濾波算法。

Fig.6 Dynamic attitude solution data圖6 動態(tài)時姿態(tài)解算數(shù)據(jù)

Table 3 Data comparison of the two algorithms in dynamic state表3 動態(tài)時兩種算法數(shù)據(jù)比較

4 結(jié)語

本文設(shè)計了基于動量梯度下降算法的姿態(tài)數(shù)據(jù)采集系統(tǒng)，實驗結(jié)果表明互補濾波法融合精度低且收斂速度較慢。動態(tài)梯度下降法得到的姿態(tài)信息更為精準、波動范圍小、數(shù)據(jù)保持性更好、實時性高，且巧妙避免磁力計補償需要知道當?shù)卮艌鼋嵌鹊膯栴}，在水下機器人姿態(tài)控制中具有很好的應(yīng)用前景。但由于硬件系統(tǒng)沒有采取屏蔽措施，當附近存在磁鐵或大電流時，姿態(tài)數(shù)據(jù)會產(chǎn)生較大誤差。下一步將在設(shè)計中對整個系統(tǒng)增加屏蔽罩，提高系統(tǒng)抗干擾性能。