1.一種用單臺(tái)聲信標(biāo)修正水下機(jī)器人位置誤差的方法，其特征在于，包括以下步驟：

水下機(jī)器人的測(cè)距器按周期采集運(yùn)動(dòng)中的水下機(jī)器人和聲信標(biāo)的距離；

水下機(jī)器人內(nèi)的計(jì)算機(jī)根據(jù)每?jī)蓚€(gè)時(shí)刻采集到的上述距離及該時(shí)間段內(nèi)水下機(jī)器人的航行距離，計(jì)算水下機(jī)器人的轉(zhuǎn)向角，控制水下機(jī)器人向聲信標(biāo)航行；

當(dāng)水下機(jī)器人和聲信標(biāo)的距離減小到設(shè)定值R_l時(shí)，水下機(jī)器人繞聲信標(biāo)做圓周運(yùn)動(dòng)，使聲信標(biāo)位置在水下機(jī)器人圓周運(yùn)動(dòng)軌跡所包圍的范圍以?xún)?nèi)；

根據(jù)水下機(jī)器人的航向、航速以及采集到的水下機(jī)器人和聲信標(biāo)的距離，利用改進(jìn)的擴(kuò)展卡爾曼濾波器算法對(duì)水下機(jī)器人的位置進(jìn)行在線(xiàn)估計(jì)；

水下機(jī)器人圓周運(yùn)動(dòng)結(jié)束時(shí)，停止濾波，得到水下機(jī)器人的估計(jì)位置，用此估計(jì)位置更新水下機(jī)器人當(dāng)前位置。

2.根據(jù)權(quán)利要求1所述的一種用單臺(tái)聲信標(biāo)修正水下機(jī)器人位置誤差的方法，其特征在于，所述水下機(jī)器人繞聲信標(biāo)做運(yùn)動(dòng)的圈數(shù)不小于0.5圈，其半徑不小于上述設(shè)定值R_l。

3.根據(jù)權(quán)利要求1所述的一種用單臺(tái)聲信標(biāo)修正水下機(jī)器人位置誤差的方法，其特征在于，所述計(jì)算水下機(jī)器人的轉(zhuǎn)向角，具體為：

i)第一個(gè)測(cè)距周期結(jié)束時(shí)，計(jì)算轉(zhuǎn)向角

其中，r_i-1為上一時(shí)刻測(cè)量到的水下機(jī)器人和聲信標(biāo)的距離，r_i為當(dāng)前時(shí)刻測(cè)量到的水下機(jī)器人和聲信標(biāo)的距離，d水下機(jī)器人在上一時(shí)刻和當(dāng)前時(shí)刻期間前進(jìn)的距離，由航速乘以時(shí)間得到；使水下機(jī)器人艏向轉(zhuǎn)動(dòng)GX＝-g度；同時(shí)，得到第一個(gè)測(cè)距周期的距離差dev＝r_i-1-r_i，轉(zhuǎn)到下一個(gè)測(cè)距周期；

ii)當(dāng)?shù)诙€(gè)測(cè)距周期的dev小于第一個(gè)測(cè)距周期的dev時(shí)，GX＝g，反之GX＝-g，轉(zhuǎn)到下一個(gè)測(cè)距周期；

iii)當(dāng)前測(cè)距周期結(jié)束時(shí)，利用余弦公式計(jì)算上一周期測(cè)量到的距離與d之間的夾角為如果該夾角大于90°，則GX＝g，返回到i)，反之，GX＝0；如果上一周期測(cè)量到的距離乘以該夾角的正弦，即所得的結(jié)果大于設(shè)定的接近半徑R_l，則返回到i)。

4.根據(jù)權(quán)利要求1所述的一種用單臺(tái)聲信標(biāo)修正水下機(jī)器人位置誤差的方法，其特征在于，所述改進(jìn)的擴(kuò)展卡爾曼濾波器的觀測(cè)方程為：

z_k＝h_k(s_k)+v_k

其中，

(1)s_k＝[x_k?y_k?ψ_k]′，x_k，y_k和ψ_k分別表示當(dāng)前時(shí)刻k的東向坐標(biāo)、北向坐標(biāo)和航向角；

(2)zk=rk-2-r~k-2rk-1-r~k-1rk-r~kψk-ψ~k′,]]>四個(gè)分量從左到右依次為上上次距離測(cè)量值與當(dāng)時(shí)距離預(yù)測(cè)值之差、上一次距離測(cè)量值與當(dāng)時(shí)距離預(yù)測(cè)值之差、當(dāng)前時(shí)刻距離測(cè)量值與當(dāng)前時(shí)刻距離預(yù)測(cè)值之差、當(dāng)前時(shí)刻航向角測(cè)量值與當(dāng)前時(shí)刻航向角預(yù)測(cè)值之差；

(3)w_k是一個(gè)三維向量，為過(guò)程驅(qū)動(dòng)噪聲，w_k(1)和w_k(2)為正值且不大于10，w_k(3)為正值且不大于0.0873；v_k是一個(gè)四維向量，為觀測(cè)噪聲，v_k(1)、v_k(2)和v_k(3)為正值且不大于50，v_k(4)為正值且不大于0.0873；

(4)hk(sk)=(xk-Δxk-2,k-xb)2+(yk-Δyk-2,k-yb)2-k~k-2(xk-Δxk-1,k-xb)2+(yk-Δyk-1,k-yb)2-r~k-1(xk-xb)2+(yk-yb)2-r~kψk-ψ~k]]>

對(duì)觀測(cè)方程的h_k(s_k)進(jìn)行線(xiàn)性化可得

Hk=-xdk-2-ydk-20-xdk-1-ydk-10-xdk-ydk0001]]>

其中

xdk-2=xb-(x~k-Δxk-2,k)]]>

ydk-2=yb-(y~k-Δyk-2,k)]]>

xdk-1=xb-(x~k-Δxk-1,k)]]>

ydk-1=yb-(y~k-Δyk-1,k)]]>

xdk=xb-x~k]]>

ydk=yb-y~k]]>

上式中的和表示k時(shí)刻?hào)|向坐標(biāo)和北向坐標(biāo)的預(yù)測(cè)值，Δx_k-i，k，Δy_k-i，k(i＝1，2)分別表示k-i到k時(shí)刻之間的東向和北向坐標(biāo)增量；

單信標(biāo)的坐標(biāo)為(x_b，y_b)，水下機(jī)器人航速為v，采樣周期為T(mén)，采樣周期內(nèi)的角增量為Δψ。