[发明专利]一种基于距离与到达角复合测量的多潜器协同定位方法

权利要求书

1.一种基于距离与到达角复合测量的多潜器协同定位方法，其特征在于，包括以下步骤：

步骤1、各潜器在传感器校准和自身位置信息初始化后，开始水下航行；

步骤2、在航行时刻t＜T_thr时，各潜器仅使用惯性定位，其中，T_thr的取值取决于惯性测量设备的误差累积速度；

步骤3、在航行时刻t＝T_thr时，各潜器间建立通信链接，并开始执行潜器间的协同定位；

步骤4、各潜器基于自身在时刻(t-1)的位置信息的粒子表征对自身在时刻(t)的位置信息进行预测，各潜器接收从邻居潜器j∈S_i(S_i代表邻居潜器的集合)发出的声信号，并使用接收阵列计算到达角测量值各潜器基于自身在时刻(t)的位置预测值和邻居潜器j的位置预测值计算从邻居潜器j到自身的方位估计值并建立与航向角的关系各潜器使用作为自身在时刻(t)的航向角估计值；在各潜器位置信息预测中，使用上一时刻的航向角估计以及累积不确定性的估计获取用于预测的航向粒子并通过粒子和预测自身在时刻(t)的位置信息的粒子表征；

步骤5、各潜器将在时刻(t)的位置预测值与相应的不确定性矩阵进行广播，用于邻居潜器生成相对位置表征；

步骤6、各潜器在时刻(t)基于接收到的邻居潜器广播的位置信息，计算用于自身定位的相对位置表征；

步骤7、各潜器使用所述步骤4中预测的位置信息和所述步骤6中基于邻居潜器的相对位置信息，计算得到自身在时刻(t)的位置估计。

2.如权利要求1所述的基于距离与到达角复合测量的多潜器协同定位方法，其特征在于，所述步骤4到所述步骤7应在潜器协同定位的每一时刻均执行一次，即在时刻(t)，各潜器分别顺序执行所述步骤4到所述步骤7，以获取时刻(t)的位置估计；在时刻(t+1)，各潜器应重新执行所述步骤4到所述步骤7，以获取时刻(t+1)的位置估计，以此类推直至潜器航行结束。

3.如权利要求1所述的基于距离与到达角复合测量的多潜器协同定位方法，其特征在于，所述步骤4包括以下步骤：

步骤4.1、获取潜器间的到达角测量值；

潜器i的邻居潜器j发射声信号，声信号到达潜器i处被潜器i搭载的声阵列接收，并通过声信号到达不同阵元的到达时间差，计算得到到达角测量值假设潜器i搭载的声阵列阵元间距为l，水下声速为s，声波到达相邻阵元的时间差为Δt，则潜器i与其它邻居潜器的到达角测量值使用相同方法获得；

步骤4.2、计算潜器i的航行姿态角；

潜器i在时刻(t-1)的位置估计值为使用从惯性测量单元中本地获取的惯性测量值得到潜器i在时刻(t)的位置预测值

其中，和分别是潜器i在对应时刻的速率、航向角、俯仰角和深度的测量值，τ代表相邻时刻间的时间长度；潜器j在时刻(t)的位置预测值可使用相同方法得到；

使用潜器i和潜器j的位置预测值，计算从潜器j到潜器i的方位估计值

从潜器j的角度，计算潜器i在时刻(t)的航向角估计值从潜器i的其它邻居潜器角度，潜器i在时刻(t)的航向角估计值可使用相同方法获得；

潜器i在时刻(t)的航向角估计值为其中，S_i代表潜器i的邻居潜器的集合，|S_i|代表潜器i的邻居潜器个数；

步骤4.3、预测潜器i的位置信息；

假设在时刻(t-1)潜器i位置信息的粒子表征为其中K代表粒子总数，为粒子的权重，则在时刻(t)潜器i预测位置的粒子表征可计算如下：

其中，和分别是速率、俯仰角、深度的测量方差，表示航向角测量和估计过程中的累积不确定性；

计算粒子对应的权重为

4.如权利要求1所述的基于距离与到达角复合测量的多潜器协同定位方法，其特征在于，所述步骤5为：

潜器i的位置预测值不确定性矩阵中和表示在水平方向上的不确定性；

在时刻(t-1)潜器i定位得到的位置信息的粒子表征为则其中，