[发明专利]一种基于通信延时补偿的多水面无人艇协同定位方法

权利要求书

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时刻系统矩阵。