[發明專利]一種基于通信延時補償的多水面無人艇協同定位方法

有效

• 申請號: 201310045731.3
• 申請日: 2013-02-05
• 公開（公告）號: CN103090864A
• 公開（公告）日: 2013-05-08
• 發明（設計）人: 高偉;楊建;于強;徐博;劉菊;史宏洋;陳春;夏秀瑋
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/12
• 分類號: G01C21/12;G01C21/20
• 代理公司: 哈爾濱市松花江專利商標事務所 23109
• 代理人: 牟永林

1.一種基于通信延時補償的多水面無人艇協同定位方法，其特征是：它由以下步驟實現：

步驟一、接收艇采集發送艇發送的相對定位信息，所述相對定位信息包括：接收艇與發送艇的距離信息r和發送艇的位置信息，即：發送艇相對于起始點的相對位置信息(x_a、y_a)；

接收艇采集自身的航推定位信息：所述航推定位信息包括航速V_b和航向角速度w_b；

步驟二、通過接收艇自身攜帶的世界標準時間系統以及步驟一中采集到的相對定位信息中加載的時間戳，接收艇計算本次接收的信息延時時間，并與濾波周期比較，獲得本次接收的相對定位信息延遲步長N，N為實數；

步驟三、利用步驟一采集的相對定位信息，建立基于延遲步長N的協同定位系統狀態方程；

系統狀態方程為：

X(k+1)＝Φ(k)X(k)；

其中：X(k+1)為T_k-1時刻系統狀態量，X(k)為T_k時刻系統狀態量，Φ(k)為T_k時刻系統矩陣；

其中：Φ(k)=I+δt10000010000010-δt·Vbksinφbk0001δt·Vbkcosφbk00001;]]>

式中：I為單位陣，δt為系統采樣時間，V_bk為T_k時刻接收艇航速，φ_bk為T_k時刻接收艇航向角；

G(k)=1000010000δt·cosφk000δt·sinφk0000δt;]]>

系統輸入量為u(k)＝(x_as，y_as，v_bk，φ_bk)^T；

具中：x_as、y_as為T_s時刻發送艇相對于起始點的相對位置信息；T_s時刻為發送艇上當前次信息采集時刻，T_k時刻為接收艇上當前次信息到達時刻，所述T_s時刻與T_k時刻之間存在著N個濾波步長；

系統的狀態量X(k)為：

X(k)＝(x_as，y_as，x_bk，y_bk，φ_bk)^T＝[X_a(s)^TX_b(k)^T]^T；

其中：x_bk、y_bk為T_k時刻接收艇相對于起始點的相對位置信息；X_a(s)^T為T_s時刻發送艇定位系統狀態量的轉置，上標T代表矩陣轉置；X_b(k)^T為T_k時刻接收艇定位系統狀態量的轉置；

步驟四、利用系統的狀態量X(k)通過公式：

Φb(i)=I+δt10-δt·Vbisinφbi01δt·Vbicosφbi001≈10-δt2·Vbisinφbi01-δt2·Vbicosφbi001]]>

計算接收艇狀態轉移矩陣Φ_b(i)；

其中：s＝＜i＜＝k；s、i、k分別代表T_s、T_i、T_k時刻；

V_bi為T_i時刻接收艇航速，φ_bi為T_i時刻接收艇航向角；

并根據公式：

Φb(k,s)-1=Πi=skΦb(i)-1=I2-Σi=skL(i)O1×21]]>

其中：k的取值為k＞＝3；I₂為2階單位陣；O_1×2為零矩陣；L(i)為等價方塊陣；

獲得T_s到T_k時刻的接收艇狀態轉移矩陣Φ_b(k，s)^-1；

步驟五、利用步驟四獲得的T_s到T_k時刻的接收艇狀態轉移矩陣Φ_b(k，s)^-1，建立基于N個延遲步長的協同定位系統量測方程：

Z(k)=ca(s)Xa(s)+cb(s)Xb(s)]]>

=ca(s)Xa(s)+cb(s)Φb(k,s)-1Xb(k)]]>

=ca(s)cb(s)Φb(k,s)-1Xa(s)Xb(k)]]>；

=C(k)X(k)]]>

其中：C(k)為T_k時刻系統量測矩陣，c_a(s)為T_s時刻發送艇系統量測矩陣，c_b(s)為T_s時刻接收艇系統量測矩陣；

ca(s)=∂Z(s)∂Xa(s,s-1)T=1001(xas-xbs)rs(yas-ybs)rs;]]>

式中：Z(s)為T_s時刻系統量測量；r_s為T_s時刻兩艇間間距，上標T代表矩陣轉置；

cb(s)=∂Z(s)∂Xb(s,s-1)T=000000(xbs-xas)rs(ybs-yas)rs0;]]>

步驟六、利用步驟一采集的相對定位信息以及步驟五獲得的系統量測方程，根據公式：

M=EX~(s)X~(k,k-1)T]]>

=P(s)Πi=1N-1[Φ(s+i)T(I-K(s+i+1)C(s+i+1))T]]]>

其中：為T_s時刻系統狀態估計誤差；為T_k時刻系統狀態一步預測估計誤差；P(s)為T_s時刻系統狀態估計誤差方差陣；Φ(s+i)^T為系統T_s+1時刻系統矩陣的轉置；K(s+i+1)為系統T_s+i+1時刻濾波增益矩陣；C(s+i+1)為系統T_s+i+1時刻量測矩陣；

獲得延時階段基于不同時刻狀態估計的均方誤差矩陣M；

步驟七、利用步驟六獲得的不同時刻狀態估計均方誤差矩陣M，根據公式：

K(k)＝M^TC(s)^T[C(s)P(s)C(s)^T]^-1

獲得最小方差估計下的濾波增益矩陣K(k)；

其中：C(s)為T_s時刻系統量測矩陣；

步驟八、利用步驟七獲得的最小方差估計下的濾波增益矩陣K(k)，根據公式：

P(k)＝P(k，k-1)-K(k)C(s)M

獲得基于N個延時步長的系統狀態均方誤差矩陣P(k)；

其中：P(k，k-1)＝Φ(k，k-1)P(k)Φ^T(k，k-1)；

Φ(k，k-1)為系統T_k-1到T_k時刻系統矩陣；P(k)為系統T_k-1到T_k時刻系統狀態估計誤差方差陣，上標T代表矩陣轉置；

步驟九、利用步驟五獲得的系統基于N個延遲步長的量測方程，以及步驟七獲得的最小方差估計下的濾波增益矩陣，采用公式：

X^k=(I-K(k)C(k))X^(k,k-1)+K(k)Z(k)]]>

其中：為系統T_k-1到T_k時刻的狀態估計；

對協同定位系統由于通信延時造成的定位誤差進行估計補償，進而實現通信延時補償的接收艇定位；

X^(k,k-1)=X^a(s,s-1)TX^b(k,k-1)TT;]]>

其中：為發送艇T_s-1到T_s時刻的狀態估計；為接收艇T_k-1到T_k時刻的狀態估計；

X^a(s,s-1)=(1+δt)Xas-1yas-1;]]>

其中：x_as-1、y_as-1為發送艇T_s-1時刻相對于起始點的相對位置信息；

X^b(k,k-1)=Φb(k,k-1)X^b(k-1)]]>

其中：Φ_b(k，k-1)為系統T_k-1到T_k時刻系統矩陣；為接受艇T_k-1時刻的狀態估計。

2.根據權利要求1所述的一種基于通信延時補償的多水面無人艇協同定位方法，具特征在于步驟六中獲得的延時階段基于不同時刻狀態估計的均方誤差矩陣M是基于Kalman濾波方程實現的；所述Kalman濾波方程為：

X(k,k-1)=Πi=1N-1[(I-K(k-i)C(k-i))Φ(k-i)]X(s)]]>

其中：K(k-i)為系統T_k-i時刻濾波增益矩陣；C(k-i)為系統T_k-i時刻量測矩陣；Φ(k-i)為系統T_k-i時刻系統矩陣。