朱延栓，戴曉強(qiáng)，蘭 武，吳 偉，楊淦華

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

0 引言

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

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

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

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

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

1.1 水下機(jī)器人組成

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

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

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

Fig.1 Underwater robot body圖1 水下機(jī)器人本體

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

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

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

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

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

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

2.1 四元數(shù)法原理

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

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

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

2.2 梯度下降算法

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

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

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

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

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

3 實(shí)驗(yàn)結(jié)果與分析

3.1 實(shí)驗(yàn)平臺(tái)

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

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

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

3.2 靜態(tài)實(shí)驗(yàn)與分析

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

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

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

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

3.3 動(dòng)態(tài)實(shí)驗(yàn)與分析

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

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

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

4 結(jié)語

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