|
卡爾曼濾波算法在自主式水下機(jī)器人超短基線定位中的應(yīng)用0 引言 目前,自主式水下機(jī)器人成為人類獲取海洋信息的重要平臺,具有廣闊的發(fā)展前景。自主式水下機(jī)器人利用自身攜帶的設(shè)備如聲能換器、地磁傳感器、慣性測量器、地形匹配裝置等實(shí)現(xiàn)導(dǎo)航。由于海洋環(huán)境自身的復(fù)雜性,以聲波探測為基礎(chǔ)的水聲定位技術(shù)是傳遞和獲取水下信息的有效手段之一,被廣泛應(yīng)用于水下機(jī)器人中。 水聲定位系統(tǒng)根據(jù)接收基陣尺寸可以分成超短基線( usbl)、短基線( sbl)和長基線( lbl) 3 種定位系統(tǒng);诔袒的定位系統(tǒng)利用測量信號的相位差原理進(jìn)行水下定位,其中,接收基陣的基元間距離通常是小于或等于波長的一半,使得設(shè)備更加簡單、易于安裝,同時(shí)具有很強(qiáng)的準(zhǔn)確性、適應(yīng)性和靈活性。文深入研究基于超短基線的水聲定位系統(tǒng)的組成及其定位原理,在此基礎(chǔ)上,對聲波傳播過程中存在的能量損失和噪聲干擾進(jìn)行分析,提出基于卡爾曼濾波算法的測量和運(yùn)動(dòng)方程,最后進(jìn)行仿真實(shí)驗(yàn)。 1 水聲定位系統(tǒng) 1.1 超短基線水聲定位系統(tǒng) 在水聲定位系統(tǒng)中,根據(jù)基線的長短可以將水聲定位分為超短基線、短基線和長基線定位。其中,超短基線定位利用基元間接收信號的相位差實(shí)現(xiàn)對水下機(jī)器人的定位,該技術(shù)中基元間距離不大于 1/2 個(gè)波長,基陣長度通常在厘米量級或幾十厘米量級。在基于超短基線的水下機(jī)器人定位系統(tǒng)中,通常將由至少 3 個(gè)換能器組成的聲基陣安裝在水面船體上,并且在安裝時(shí)對船和聲基陣間的坐標(biāo)關(guān)系進(jìn)行精確測定,換能器之間的距離一般為幾厘米。在定位過程中,換能器向裝在自主式水下機(jī)器人上的應(yīng)答裝置發(fā)送聲波信號,然后,對換能器的接收信號進(jìn)行測量,通過計(jì)算信號間的相位差來確定應(yīng)答器位置,進(jìn)而確定水下機(jī)器人的置。通過計(jì)算聲波船舶時(shí)間,再利用聲速剖面波束線進(jìn)行修正,從而確定換能器和水下機(jī)器人之間的距離。 將基于超短基線的水下機(jī)器人定位系統(tǒng)和船載GPS 系統(tǒng)相結(jié)合,能夠有效提升水下機(jī)器人定位的準(zhǔn)確性。將 GPS 接收裝置放置在水面船舶上,利用 GPS系統(tǒng)對船舶位置進(jìn)行實(shí)時(shí)測定,同時(shí),對測量船舶和水下機(jī)器人間的相對位置和方位進(jìn)行同步測定,從而計(jì)算出水下機(jī)器人的準(zhǔn)確位置。基于超短基線的水下機(jī)器人定位系統(tǒng)具有性價(jià)比高、易操作、安裝簡便、測量精度高等優(yōu)點(diǎn)。 基于超短基線的定位系統(tǒng)的工作原理如圖1 所示,其中,接收基陣裝于水面船舶的底部,應(yīng)答器裝于水下機(jī)器人的上部。 圖 1 定位系統(tǒng)工作示意圖 1.2 定位原理 基于超短基線的定位系統(tǒng)由接收基陣、應(yīng)答器和發(fā)射換能器構(gòu)成。接收基陣和發(fā)射換能器安裝在水面船舶上,應(yīng)答器安裝于水下機(jī)器人上。發(fā)射換能器向應(yīng)答器發(fā)射聲脈沖,當(dāng)應(yīng)答器接收到聲脈沖后將發(fā)送應(yīng)答聲脈沖,當(dāng)接收基陣接收到應(yīng)答聲脈沖后,對X、 Y 方向上的相位差進(jìn)行測量,然后根據(jù)聲波傳播的時(shí)間求出水下機(jī)器人與接收基陣間的距離,進(jìn)而求得水下機(jī)器人在平面坐標(biāo)系上的位置及其水下深度。 基于相位差的定位原理是:假設(shè)在平面中,等間距安裝 5 個(gè)接收換能器,特性一致,且位置滿足垂直正交,每個(gè)接收換能器作為一個(gè)基元。在坐標(biāo)原點(diǎn) O處放置 1 個(gè)接收換能器,其他 4 個(gè)接收換能器 Xa,Xb, Ya, Yb 放置在距離原點(diǎn) O 為 d/2 的位置,放置在水下機(jī)器人上的應(yīng)答器為 P, P 到坐標(biāo)原點(diǎn) O 距為 S,到其他接收換能器的距離為 SXa, SXb, SYa, SYb。換能器發(fā)射脈沖信號,當(dāng)應(yīng)答器接收到脈沖信號后發(fā)射應(yīng)答信號,在各個(gè)基元分別接收到應(yīng)答信號后,可以根據(jù) Xa, Xb 信號的相位差求得 OP 和 X 軸間的夾角 θX,根據(jù) Ya, Yb 信號的相位差求得 OP 和 Y 軸間的夾角θY。數(shù)學(xué)模型為 式中: ∆ϕ 為基元間的信號相位差; f 為信號頻率。 2 卡爾曼濾波算法 2.1 算法原理 卡爾曼濾波算法是最小線性方差估計(jì)的一種,具有如下特點(diǎn): 1)由于卡爾曼濾波算法在時(shí)域內(nèi)采用狀態(tài)空間方法設(shè)計(jì)濾波器,并且遞推,所以該算法適用于對多維隨機(jī)系統(tǒng)進(jìn)行估計(jì)。 2)卡爾曼濾波算法利用狀態(tài)方程和動(dòng)力學(xué)方程對待估計(jì)量的變化規(guī)律進(jìn)行描述,對于待估計(jì)量信息通常采用動(dòng)力學(xué)方程和激勵(lì)白噪聲統(tǒng)計(jì)方法來確定。由于動(dòng)力方程已知,激勵(lì)白噪聲平穩(wěn),所以待估計(jì)量可以表現(xiàn)出平穩(wěn)狀態(tài),也可以表現(xiàn)出非平穩(wěn)狀態(tài),也就是說卡爾曼濾波算法對非平穩(wěn)過程同樣適用。 3)卡爾曼濾波算法具有離散型和連續(xù)型 2 種算法,其中,離散型算法對非平穩(wěn)過程也適用。 對于離散控制系統(tǒng),采用隨機(jī)線性微分方程進(jìn)行描述,則卡爾曼濾波算法如下: 1)首先利用系統(tǒng)模型對系統(tǒng)的下一狀態(tài)進(jìn)行預(yù)測,如下式: 2)采集系統(tǒng)狀態(tài)測量值,根據(jù)預(yù)測值求得系統(tǒng)狀態(tài)的最優(yōu)估算值,如下式: 其中 Kg 為卡爾曼增益。 為了使算法能夠自回歸運(yùn)算,還需要對系統(tǒng) k 狀態(tài)下對應(yīng)的 covariance 進(jìn)行更新,如下式: 2.2 水下機(jī)器人定位方程 根據(jù)基陣測量得到的混有噪聲的數(shù)據(jù),利用卡爾曼濾波算法對水下機(jī)器人定位進(jìn)行最優(yōu)估計(jì),構(gòu)建水下機(jī)器人的運(yùn)動(dòng)方程,其向量形式為: 3 仿真實(shí)驗(yàn) 本文在 Matlab 平臺對基于卡爾曼濾波的水下機(jī)器人定位算法進(jìn)行了仿真實(shí)驗(yàn),在實(shí)驗(yàn)過程中,假設(shè)測量船舶發(fā)生橫搖或者縱搖,設(shè)置橫搖角為 10°,縱搖角也為 10°,實(shí)驗(yàn)結(jié)果如圖2所示。 圖 2 中,第 1 個(gè)點(diǎn)是測量誤差在 [0,1)范圍內(nèi)點(diǎn)的數(shù)量,第 n 個(gè)點(diǎn)是測量誤差在 [n – 1,n)范圍內(nèi)點(diǎn)的數(shù)量,從實(shí)驗(yàn)結(jié)果可知,誤差在15m 內(nèi)的占總數(shù)的75%,所以,本文提出的算法在降低誤差方面具有明顯效果。 4 結(jié)語 在超短基水聲定位技術(shù)被廣泛應(yīng)用于自主式水下機(jī)器人定位系統(tǒng)的背景下,如何利用卡爾曼濾波算法提高定位的準(zhǔn)確率是本文研究的重點(diǎn)。本文對卡爾曼濾波算法應(yīng)用于基于超短基的水下機(jī)器人定位系統(tǒng)的可行性進(jìn)行了研究,建立了相應(yīng)的運(yùn)動(dòng)和測量模型,最后,進(jìn)行了仿真實(shí)驗(yàn),實(shí)驗(yàn)結(jié)果達(dá)到預(yù)期。 參考文獻(xiàn): [1] 李曄, 蘇玉民, 萬磊, 等. 自適應(yīng)卡爾曼濾波技術(shù)在水下機(jī)器人運(yùn)動(dòng)控制中的應(yīng)用 [J]. 中國造船, 2006, 47(4): 83–88. [2] 李曄, 常文田, 萬磊, 等. 水下機(jī)器人自適應(yīng)卡爾曼濾波技術(shù)研究[J]. 智能系統(tǒng)學(xué)報(bào), 2006, 1(2): 44–47. |