基于動量梯度下降法的姿態數據采集系統

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

（江蘇科技大學 電子信息學院，江蘇 鎮江 212003）

0 引言

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

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

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

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

1 水下機器人組成及姿態采集系統設計

1.1 水下機器人組成

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

1.2 姿態采集系統設計

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

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

Fig.2 Overall design of the system hardware圖2 系統硬件整體設計

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

Table 1 MPU9250 parameters table表1 MPU9250 參數表

2 姿態解算算法

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

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

2.1 四元數法原理

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

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

四元數轉換為歐拉角的表達式如下：

2.2 梯度下降算法

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

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

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

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

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

3 實驗結果與分析

3.1 實驗平臺

本文實驗參考的實際姿態采用某公司生產的AH100B慣性導航模塊，該模塊具有全姿態右移的穩定性和實時性，廣泛應用于各種運動姿態監測領域。

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

Fig.4 ROV attitude data acquisition board圖4 水下機器人姿態數據采集板

3.2 靜態實驗與分析

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

Fig.5 Static attitude solution data圖5 靜態時姿態解算數據

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

Table 2 Data comparison of the two algorithms in static state表2 靜態時兩種算法數據比較

3.3 動態實驗與分析

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

Fig.6 Dynamic attitude solution data圖6 動態時姿態解算數據

Table 3 Data comparison of the two algorithms in dynamic state表3 動態時兩種算法數據比較

4 結語

本文設計了基于動量梯度下降算法的姿態數據采集系統，實驗結果表明互補濾波法融合精度低且收斂速度較慢。動態梯度下降法得到的姿態信息更為精準、波動范圍小、數據保持性更好、實時性高，且巧妙避免磁力計補償需要知道當地磁場角度的問題，在水下機器人姿態控制中具有很好的應用前景。但由于硬件系統沒有采取屏蔽措施，當附近存在磁鐵或大電流時，姿態數據會產生較大誤差。下一步將在設計中對整個系統增加屏蔽罩，提高系統抗干擾性能。