沈 圓 王 勇 倪福生

（1.河海大學機電工程學院 常州 213022）（2.江蘇省水利機械制造有限公司 揚州 225000）（3.疏浚技術教育部工程研究中心 常州 213022）

1 引言

水庫清淤方法有泄洪沖刷、吸力挖泥船、抓斗挖泥船等方法。挖泥船由于漂浮在水面，使用范圍受到水深的制約，而清淤機器人能夠不受水深影響，在水庫底部行走、清淤及向地面排沙，因此能夠保證水庫庫容、避免泥沙對水量調(diào)節(jié)和水電站的運轉(zhuǎn)帶來不利影響。在水下進行精確的定位［1］是清淤機器人運動和操控的必要條件，在深海環(huán)境下，電磁波衰減很快，傳統(tǒng)的陸上定位方式［2］不再適用。相比而言，慣性導航能夠不依賴于外界信息，進行自主定位，是清淤機器人理想的定位方式。

慣性導航分為平臺式以及捷聯(lián)式，由于捷聯(lián)慣導具有可靠性高、成本低以及使用方便的特點，本文選取捷聯(lián)慣導作為清淤機器人的定位方式。捷聯(lián)慣導解算前，需要先求得姿態(tài)的初始值，即為載體坐標系與導航坐標系的相對關系，包括橫滾角γ、俯仰角θ和航向角ψ。載體坐標系O-XbYbZb和導航坐標系O-XnYnZn之間的變換關系如下：

將上面的轉(zhuǎn)換關系表示成矩陣形式即為

即為姿態(tài)矩陣，初始姿態(tài)對準［3］即為了求得高精度的初始姿態(tài)矩陣，也就是初始姿態(tài)角，橫滾角γ0、俯仰角θ0和航向角ψ0，利用粒子濾波法改進初始對準結(jié)果，將其與濾波前的結(jié)果進行對比研究，流程圖如圖1所示。

圖1 初始姿態(tài)對準流程

2 初始姿態(tài)對準難題

初始姿態(tài)對準是捷聯(lián)慣導中一項難題，初始對準精度直接影響最終的定位精度［4］。濾波技術在初始對準中具有重要作用。

陀螺儀和加速度計提供3個相互垂直方向上的三軸角速率信息和三軸比力信息。一般來說，陀螺儀和加速度計的所在的三軸都是對齊的。對準過程主要是由相對于給定參考坐標系的加速度計軸定義的笛卡爾坐標系的三個角度來確定。

由慣導基本公式：

從而

地球自轉(zhuǎn)角速度ωnie與陀螺儀輸出值之間的關系：

從而

根據(jù)加速度計和陀螺儀的輸出值就可以按照式（4）和（6）求出姿態(tài)角的初始值，但是由于加速度計和陀螺儀噪聲的存在，得到的值始終在一個范圍內(nèi)波動，無法得到精確值，因此，陀螺儀和加速度計的誤差會導致對準誤差；而且對準時載體受到的干擾也會產(chǎn)生對準誤差。

針對初始姿態(tài)對準問題，前人做了大量的研究。L Zhao［5］等將高斯逼近法和蒙特卡洛方法應用到初始姿態(tài)對準中，比較了兩者的優(yōu)缺點，高斯法速度更快，蒙特卡洛方法精度更高。李辰淑［6］詳細分析了初始姿態(tài)對準全過程，以及誤差的成因。Li J［7］等提出了一種基于固定積分區(qū)間滑動的優(yōu)化對準方法，減少了對準時間。李宏成［8］將卡爾曼濾波方法應用到初始姿態(tài)對準中，并將其優(yōu)化，達到了對準精度要求。

慣性傳感器是慣性導航的核心技術，在積分運算中，慣性導航系統(tǒng)和對慣性傳感器誤差極為敏感，而慣性傳感器存在誤差，所以計算出的姿態(tài)角也會偏離真實值，當其偏差過大時，定位結(jié)果將會完全失真，而這個誤差將不會被消除，持續(xù)累積，慣導系統(tǒng)失去定位能力。因此必須對誤差進行補償，得到一個理想的初始姿態(tài)角，濾波技術對于捷聯(lián)慣導尤其重要。

3 基于粒子濾波法的初始姿態(tài)對準模型

捷聯(lián)慣導的誤差主要是由于慣性傳感器引起的，而錯誤的誤差消除方法將會放大缺點，因此，建立合適的初始姿態(tài)對準誤差糾正模型是非常有必要的，本小節(jié)主要分析了失準角以及速度誤差，建立了相應的狀態(tài)方程以及觀測方程［9］。

3.1 狀態(tài)方程

在本文中采用地理坐標系n（即東北天坐標系）為導航坐標系［10］，計算導航坐標系為 n?，慣性坐標系為i。n系繞zn軸轉(zhuǎn)動得到n1系，n1系繞yn軸轉(zhuǎn)動得到n2系，n2系繞xn轉(zhuǎn)動得到 n?系，寫成矩陣形式即為

在真實的導航坐標系中，根據(jù)SINS原理可知捷聯(lián)矩陣的微分方程為

在沒有進行初始姿態(tài)對準時，實際上導航坐標系n系是由計算導航坐標系n?系來代替的。所以要想求得真實的姿態(tài)矩陣，必須要乘以一個誤差矩陣，對其進行補償。

其中?n?是加速度計零偏在計算導航坐標系上的投影。在靜基座初始對準中，當?shù)匚恢貌蛔儯?、、Vn和 δgn皆為0，整理上式得到：

3.2 觀測方程

本文以東北向的速度差作為系統(tǒng)的觀測量建立觀測方程［11］，Z=[δVE，δVN]為觀測量。根據(jù)捷聯(lián)慣導比力方程：

可以求取當前時刻的速度，因為清淤機器人靜止不動，所以真實速度為0，所以速度差即為當前時刻的速度，所以有觀測方程為

η =[ηδVE，ηδVN]即為觀測噪聲。

4 算法設計及實現(xiàn)

本文選用粒子濾波法進行求解，可以直接應用非線性的狀態(tài)方程，不需要對其進行線性處理［12］。

5 仿真計算及分析

仿真模擬中，采樣間隔為0.02s，粒子數(shù)為1000，輸出次數(shù)為1500次。

加速度的啟動漂移為5×10-5m/s2，快變漂移為10-4m/s2；陀螺儀的啟動漂移為0.02°/h，快變漂移為0.01°/h。

由于對準時間較短，慣性傳感器的慢變漂移［13～16］相對于啟動漂移以及快變漂移非常小，所以在仿真過程中僅僅考慮啟動漂移以及快變漂移。

狀態(tài)噪聲：

觀測噪聲：

理想的三個姿態(tài)角皆為0度，仿真結(jié)果如下：

圖2 橫滾角

圖3 俯仰角

圖4 航向角

圖2～4可以看出，橫滾角、俯仰角以及航向角逐漸向理想值逼近，最終結(jié)果為0.29''、3.57''和-0.39'。濾波剛開始時，角度波動幅度較大，特別是航向角。這是因為在根據(jù)概率分布進行真實值的估計時，盡量選擇更多的可能性，使粒子分布更廣，在經(jīng)過一段時間以后，選擇出最接近真實值的粒子。粒子濾波對于非線性問題的處理比較方便［17］，不需要進行線性化處理，而且降低了狀態(tài)量的個數(shù)。

6 結(jié)語

本文從捷聯(lián)慣導初始姿態(tài)對準的基本原理出發(fā)，在慣性導航系統(tǒng)初始階段，就姿態(tài)對準流程，待處理的疑難點進行了相關的分析，在靜基座條件下，分析了捷聯(lián)慣導的失準角和速度差，建立了狀態(tài)以及觀測方程。簡單依據(jù)地球自轉(zhuǎn)角速率和重力加速度進行初始姿態(tài)角計算，精度差之甚遠。在初始姿態(tài)對準過程中，濾波后的結(jié)果大為改善，與理想值十分接近，證明了基于粒子濾波法的初始對準誤差校正模型的有效性。