基于粒子濾波的水庫(kù)清淤機(jī)器人初始姿態(tài)對(duì)準(zhǔn)解算方法?

沈 圓 王 勇 倪福生

（1.河海大學(xué)機(jī)電工程學(xué)院 常州 213022）（2.江蘇省水利機(jī)械制造有限公司 揚(yáng)州 225000）（3.疏浚技術(shù)教育部工程研究中心 常州 213022）

1 引言

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

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

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

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

圖1 初始姿態(tài)對(duì)準(zhǔn)流程

2 初始姿態(tài)對(duì)準(zhǔn)難題

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

陀螺儀和加速度計(jì)提供3個(gè)相互垂直方向上的三軸角速率信息和三軸比力信息。一般來(lái)說(shuō)，陀螺儀和加速度計(jì)的所在的三軸都是對(duì)齊的。對(duì)準(zhǔn)過(guò)程主要是由相對(duì)于給定參考坐標(biāo)系的加速度計(jì)軸定義的笛卡爾坐標(biāo)系的三個(gè)角度來(lái)確定。

由慣導(dǎo)基本公式：

從而

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

從而

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

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

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

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

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

3.1 狀態(tài)方程

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

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

在沒(méi)有進(jìn)行初始姿態(tài)對(duì)準(zhǔn)時(shí)，實(shí)際上導(dǎo)航坐標(biāo)系n系是由計(jì)算導(dǎo)航坐標(biāo)系n?系來(lái)代替的。所以要想求得真實(shí)的姿態(tài)矩陣，必須要乘以一個(gè)誤差矩陣，對(duì)其進(jìn)行補(bǔ)償。

其中?n?是加速度計(jì)零偏在計(jì)算導(dǎo)航坐標(biāo)系上的投影。在靜基座初始對(duì)準(zhǔn)中，當(dāng)?shù)匚恢貌蛔儯浴ⅰn和 δgn皆為0，整理上式得到：

3.2 觀測(cè)方程

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

可以求取當(dāng)前時(shí)刻的速度，因?yàn)榍逵贆C(jī)器人靜止不動(dòng)，所以真實(shí)速度為0，所以速度差即為當(dāng)前時(shí)刻的速度，所以有觀測(cè)方程為

η =[ηδVE，ηδVN]即為觀測(cè)噪聲。

4 算法設(shè)計(jì)及實(shí)現(xiàn)

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

5 仿真計(jì)算及分析

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

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

由于對(duì)準(zhǔn)時(shí)間較短，慣性傳感器的慢變漂移［13～16］相對(duì)于啟動(dòng)漂移以及快變漂移非常小，所以在仿真過(guò)程中僅僅考慮啟動(dòng)漂移以及快變漂移。

狀態(tài)噪聲：

觀測(cè)噪聲：

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

圖2 橫滾角

圖3 俯仰角

圖4 航向角

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

6 結(jié)語(yǔ)

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