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