在基于超短基線的水下機器人定位系統中，通常將由至少3個換能器組成的聲基陣安裝在水面船體上，并且在安裝時對船和聲基陣間的坐標關系進行精確測定，換能器之間的距離一般為幾厘米。在定位過程中，換能器向裝在自主式水下機器人上的應答裝置發送聲波信號，然后，對換能器的接收信號進行測量，通過計算信號間的相位差來確定應答器位置，進而確定水下機器人的位置。通過計算聲波船舶時間，再利用聲速剖面波束線進行修正，從而確定換能器和水下機器人之間的距離。

將基于超短基線的水下機器人定位系統和船載GPS系統相結合，能夠有效提升水下機器人定位的準確性。將GPS接收裝置放置在水面船舶上，利用GPS系統對船舶位置進行實時測定，同時，對測量船舶和水下機器人間的相對位置和方位進行同步測定，從而計算出水下機器人的準確位置�；诔袒€的水下機器人定位系統具有性價比高、易操作、安裝簡便、測量精度高等優點。

基于超短基線的定位系統的工作原理如圖 1 所示，其中，接收基陣裝于水面船舶的底部，應答器裝于水下機器人的上部。

圖 1 定位系統工作示意圖

1.2 定位原理

基于超短基線的定位系統由接收基陣、應答器和發射換能器構成。接收基陣和發射換能器安裝在水面船舶上，應答器安裝于水下機器人上。發射換能器向應答器發射聲脈沖，當應答器接收到聲脈沖后將發送應答聲脈沖，當接收基陣接收到應答聲脈沖后，對 X、Y 方向上的相位差進行測量，然后根據聲波傳播的時間求出水下機器人與接收基陣間的距離，進而求得水下機器人在平面坐標系上的位置及其水下深度[3]。

基于相位差的定位原理是：假設在平面中，等間距安裝5個接收換能器，特性一致，且位置滿足垂直正交，每個接收換能器作為一個基元。在坐標原點O處放置1個接收換能器，其他4個接收換能器Xa， Xb，Ya，Yb 放置在距離原點O為d/2 的位置，放置在水下機器人上的應答器為 P，P 到坐標原點O距為 S，到其他接收換能器的距離為 SXa，SXb，SYa，SYb。換能器發射脈沖信號，當應答器接收到脈沖信號后發射應答信號，在各個基元分別接收到應答信號后，可以根據 Xa，Xb 信號的相位差求得 OP 和 X 軸間的夾角 θX，根據 Ya，Yb 信號的相位差求得 OP 和 Y 軸間的夾角 θY。數學模型為：

式中：∆ϕ 為基元間的信號相位差；f為信號頻率。

2 卡爾曼濾波算法

2.1 算法原理

卡爾曼濾波算法是最小線性方差估計的一種，具有如下特點：

1）由于卡爾曼濾波算法在時域內采用狀態空間方法設計濾波器，并且遞推，所以該算法適用于對多維隨機系統進行估計。

2）卡爾曼濾波算法利用狀態方程和動力學方程對待估計量的變化規律進行描述，對于待估計量信息通常采用動力學方程和激勵白噪聲統計方法來確定。由于動力方程已知，激勵白噪聲平穩，所以待估計量可以表現出平穩狀態，也可以表現出非平穩狀態，也就是說卡爾曼濾波算法對非平穩過程同樣適用。

3）卡爾曼濾波算法具有離散型和連續型2種算法，其中，離散型算法對非平穩過程也適用。

對于離散控制系統，采用隨機線性微分方程進行描述，則卡爾曼濾波算法[4]如下：

1）首先利用系統模型對系統的下一狀態進行預測，如下式：

式中：A，B 為系統參數；U（k）為當前系統控制量； X（k-1丨k-1）為系統上一狀態的預測結果。

對系統預測狀態對應的 covariance 進行更新，如下式：

其中，P 為 covariance，A′ 為 A 的轉置矩陣，Q 為系統過程的 covariance。

2）采集系統狀態測量值，根據預測值求得系統狀態的最優估算值，如下式：

其中 Kg 為卡爾曼增益。

為了使算法能夠自回歸運算，還需要對系統 k狀態下對應的 covariance 進行更新，如下式：

2.2 水下機器人定位方程

根據基陣測量得到的混有噪聲的數據，利用卡爾曼濾波算法對水下機器人定位進行最優估計，構建水下機器人的運動方程，其向量形式為：

式中 X（ k ）為水下機器人的狀態向量，由XA (k+1)，XA (k+1)，YA (k+1)，YA (k+1) 四部分組成，和為 k + 1 時刻水下機器人的位置在 x 和y方向上的分量，和為k + 1 時刻水下機器人的速度在x和y方向上的分量。

在測量過程中，水下機器人在x和y方向上的投影分別為和，那么，測量方程的向量形式為：

3 仿真實驗

本文在 Matlab 平臺對基于卡爾曼濾波的水下機器人定位算法進行了仿真實驗，在實驗過程中，假設測量船舶發生橫搖或者縱搖，設置橫搖角為 10°，縱搖角也為 10°，實驗結果如圖 2 所示。

圖 2 中，第 1 個點是測量誤差在 [0，1）范圍內點的數量，第n個點是測量誤差在 [n – 1，n）范圍內點的數量，從實驗結果可知，誤差在15m 內的占總數的 75%，所以，本文提出的算法在降低誤差方面具有明顯效果。

4 結語

在超短基水聲定位技術被廣泛應用于自主式水下機器人定位系統的背景下，如何利用卡爾曼濾波算法提高定位的準確率是本文研究的重點。本文對卡爾曼濾波算法應用于基于超短基的水下機器人定位系統的可行性進行了研究，建立了相應的運動和測量模型，最后，進行了仿真實驗，實驗結果達到預期。

參考文獻：

[1] 李曄, 常文田, 萬磊, 等. 水下機器人自適應卡爾曼濾波技術研究[J]. 智能系統學報, 2006, 1(2): 44–47.

[2] KAMGAR-PARSI B, ROSENBLUM I, PIPITONE F, et, al. Toward an automated system for a correctly registered bathymetric chart [J]. IEEE J. Ocean Engineering, 1999(4): 314–325.

[3] VERMA A K . Variability index constant false alarm rate (VICFAR) for sonar target detection [J]. IEEE-International Conference on Signal processing, CNMIT, Anna University Chennai India, 2008,(4–6): 38–141.