一种水下机器人多源声学信息融合精确导航方法

1.本发明属于水下智能机器人(autonomous underwater vehicle,auv)水下导航领域，具体涉及的是一种水下机器人多源声学信息融合精确导航方法。


背景技术：

2.水下智能机器人(autonomous underwater vehicle,auv)已经成为高分辨率深海地形测绘的重要工具，但是auv绘制的水深图的精度取决于auv导航系统误差。惯性导航系统、长基线声学定位系统、超短基线声学定位系统和地球物理导航系统都以为auv深海作业提供导航信息。然而，即使有多普勒速度计程仪和姿态航向参考系统等昂贵设备的补充，惯性导航系统的导航误差也会随着时间的推移而无限增长。长基线声学定位系统产生的定位结果可以达到几厘米的精度，但是在深海中为长基线声学定位系统布设耗时耗力。超短基线声学定位系统的定位精度取决于水深，其定位误差大约为母船与auv之间的0.5％，因此不适合深海导航。地球物理导航系统，如地形辅助导航系统和地磁导航系统，可以提供有界的位置误差，但精确、高分辨率、大范围的先验水深或地磁图获取极为困难。
3.近年来，海底地形同步定位与建图(simultaneous localization and mapping,slam)和信标slam都已成为一种auv深海精确导航的有效手段。然而，地形匹配过程检测到的数据关联仍然是海底slam导航误差约束的唯一依据。只有当auv重新回溯之前访问过的位置时，才能生成准确的数据关联，但在深海作业任务中，auv执行地形回溯往往需要耗费大量的时间和精力。因此，地形回溯通常发生在任务执行很长一段时间后，这意味着auv将在测绘任务开始后的很长一段时间内使用惯性导航系统确定自身位姿，在此期间auv实时位置估计是极为不准确的。对于信标slam，其在于信标保持通讯的短时间内误差约束结果较为准确，但其作业范围受信标限制严重，无法为auv长期、大范围作业提供精确导航信息。


技术实现要素：

4.本发明的目的在于通过构造多源声学信息融合图优化slam算法框架，为auv大范围深海作业提供实时、精确导航信息的一种水下机器人多源声学信息融合精确导航方法。
5.本发明的目的是这样实现的：
6.一种水下机器人多源声学信息融合精确导航方法，包括以下步骤：
7.步骤1：auv在航行过程中通过多波束声纳连续获取海底地形信息，并同其通讯范围内信标通过水声学手段进行距离观测，信标初始位置未知，auv在i时刻与第k个信标之间的距离观测结果表示为执行步骤2。
8.步骤2：auv在i时刻获得1个与第k个信标的距离观测结果后，通过能观性方程判断该距离观测值是否有效，具体而言，若auv运动满足υ
i
y
i
cosθ
i
‑
ω
i
x
i
sinθ
i
≠0(其中x
i
、y
i
分别为auv在i时刻的东向、北向位置，υ
i
和ω
i
分别表示auv在i时刻的航速和转艏角速度，θ
i
为auv在i时刻的航速)，则该距离观测值有效并执行步骤3，否则，该距离观测值无效并执行步骤5；若auv在i时刻未获得与信标距离的观测，执行步骤5。
9.步骤3：如果auv获得关于同一个信标的50次未完成离群点判别的距离观测，则通过概率图划分方法去除观测值中的离群点并执行步骤4，否则执行步骤5。
10.步骤4：针对任一信标，若存在4个以上对其与auv之间距离的有效观测且该信标初始位置未确定，对其位置进行初始化并执行步骤5，否则直接执行步骤5。
11.步骤5：通过海底地形匹配手段，在当前时刻和所有历史时刻地形测绘结果间构建数据关联，假设检测到当前i时刻与历史时刻中的j时刻海底地形相似，构建数据关联l
n
：
[0012][0013]
其中，v
b
(n)为第n个数据关联的地形匹配定位噪声，满足高斯分布n(0,q
b
(n))。如果成功获取新的数据关联或者获得了auv与信标间新的有效距离测量，执行步骤6，否则执行步骤1。
[0014]
步骤6：根据与信标间的距离观测结果和地形匹配获得的数据关联结果，进行auv导航误差修正并准确估计各信标位置。
[0015]
本发明还包括这样一些特征：
[0016]
1.步骤3具体为：
[0017]
以第k个信标为例，步骤3具体步骤包括：
[0018]
步骤3.1：对50个观测结果两两计算一致性系数，以观测和为例，一致性系数a
i,j
表示为：
[0019][0020]
其中表示观测和一致，表示观测和一致的概率，式中可以表示为
[0021][0022]
其中表示以(x
i
,y
i
)为圆心、以为半径的圆和以(x
j
,y
j
)为圆心、以为半径的圆的交点的集合，x
l
代表l时刻auv的位置，q
r
表示auv与信标之间距离观测误差矩阵的协方差。
[0023]
步骤3.2：构造判别矩阵v，v为50行1列矩阵，且
[0024][0025]
通过求解获得最优判别矩阵v
*
，v
*
中对应值为0的距离观测结果被判定为无效，本次处理的所有距离观测值标记为“已完成离群点检测”。
[0026]
2.步骤4中信标位置初始化方法具体为：
[0027]
假设第k个信标初始位置未确定，针对第k个信标存在有效距离观测值和且对应时刻auv坐标为x1＝(x1,y1)、x2＝(x2,y2)、x3＝(x3,y3)、x4＝(x4,
y4)，其初始位置x
b,k
可以通过下式求解：
[0028][0029]
3.步骤6中auv导航误差修正和各信标位置估计方法具体为：
[0030]
通过解算下式求解包含auv轨迹和所有观测过信标坐标估计值的集合x
*
。
[0031][0032]
其中x
*
为包含auv轨迹和所有观测过的信标的坐标估计值，f(x
i
‑1,u
i
)为auv在i时刻的运动方程，u
i
＝(υ
i
,ω
i
)为auv在i时刻运动控制输入，q为auv在i时刻的状态转移噪声协方差，为t(k)时刻auv位置x
t(k)
与第b(k)个信标(其坐标为x
b,b(k)
)间的距离，而l
n
、q
b
(n)、q
r
已在前文公式中说明。
[0033]
与现有技术相比，本发明的有益效果是：
[0034]
本发明提供能够一种水下机器人多源声学信息融合精确导航方法，利用该方法，auv通过多波束声纳和与信标通讯获取海底地形测绘信息和其与信标间距离观测信息，并通过能观性检验和概率图划分算法对弱通讯环境导致的距离观测离群点进行判别和剔除，最终以auv海底地形测绘信息、auv与信标间距离观测信息、auv自身导航系统信息为输入，构建多源声学信息融合slam位姿图模型，通过位姿图优化算法，同时估计auv当前位置、auv历史轨迹和信标定位信息，以解决auv深海长期、隐蔽、全天候、精确实时导航问题。本发明通过构造多源声学信息融合图优化slam算法框架，为auv大范围深海作业提供实时、精确导航信息。
附图说明
[0035]
图1是本发明的算法流程图；
[0036]
图2是本发明概率图划分算法中概率图构造示意图；
[0037]
图3是本发明仿真试验结果图。
具体实施方式
[0038]
下面结合附图对本发明具体实施方式做进一步说明。
[0039]
结合图1，本发明的具体步骤包括：
[0040]
步骤1、auv在航行过程中通过多波束声纳连续获取海底地形信息，并同其通讯范围内信标通过水声学手段进行距离观测，信标初始位置未知，auv在i时刻与第k个信标之间的距离观测结果表示为执行步骤2。
[0041]
步骤2、auv在i时刻获得1个与第k个信标的距离观测结果后，通过能观性方程判断该距离观测值是否有效，具体而言，若auv运动满足υ
i
y
i
cosθ
i
‑
ω
i
x
i
sinθ
i
≠0(其中x
i
、y
i
分别为auv在i时刻的东向、北向位置，υ
i
和ω
i
分别表示auv在i时刻的航速和转艏角速度，θ
i
为
auv在i时刻的航速)，则该距离观测值有效并执行步骤3，否则，该距离观测值无效并执行步骤5。若auv在i时刻未获得与信标距离的观测，执行步骤5。
[0042]
步骤3、如果auv获得关于同一个信标的50次未完成离群点判别的距离观测，则通过概率图划分方法去除观测值中的离群点并执行步骤4，否则执行步骤5。以第k个信标为例，具体步骤包括：
[0043]
步骤3.1、结合图2，对50个观测结果两两计算一致性系数，以观测和为例，一致性系数a
i,j
表示为：
[0044][0045]
其中表示观测和一致，表示观测和一致的概率，式中可以表示为
[0046][0047]
其中表示以(x
i
,y
i
)为圆心、以为半径的圆和以(x
j
,y
j
)为圆心、以为半径的圆的交点的集合，x
l
代表l时刻auv的位置，q
r
表示auv与信标之间距离观测误差矩阵的协方差。
[0048]
步骤3.2、构造判别矩阵v，v为50行1列矩阵，且
[0049][0050]
通过求解获得最优判别矩阵v
*
，v
*
中对应值为0的距离观测结果被判定为无效，本次处理的所有距离观测值标记为“已完成离群点检测”。
[0051]
步骤4、针对任一信标，若存在4个以上对其与auv之间距离的有效观测且该信标初始位置未确定，对其位置进行初始化并执行步骤5，否则直接执行步骤5。具体来说，假设第k个信标初始位置未确定，针对第k个信标存在有效距离观测值和且对应时刻auv坐标为x1＝(x1,y1)、x2＝(x2,y2)、x3＝(x3,y3)、x4＝(x4,y4)，其初始位置x
b,k
可以通过下式求解：
[0052][0053]
步骤5、通过海底地形匹配手段，在当前时刻和所有历史时刻地形测绘结果间构建数据关联，假设检测到当前i时刻与历史时刻中的j时刻海底地形相似，构建数据关联l
n
：
[0054][0055]
其中，v
b
(n)为第n个数据关联的地形匹配定位噪声，满足高斯分布n(0,q
b
(n))。如果成功获取新的数据关联或者获得了auv与信标间新的有效距离测量，执行步骤6，否则执行步骤1。
[0056]
步骤6、根据与信标间的距离观测结果和地形匹配获得的数据关联结果，通过下式进行auv导航误差修正并准确估计各信标位置：
[0057][0058]
其中x
*
为包含auv轨迹和所有观测过的信标的坐标估计值，f(x
i
‑1,u
i
)为auv在i时刻的运动方程，u
i
＝(υ
i
,ω
i
)为auv在i时刻运动控制输入，q为auv在i时刻的状态转移噪声协方差，为t(k)时刻auv位置x
t(k)
与第b(k)个信标(其坐标为x
b,b(k)
)间的距离，而l
n
、q
b
(n)、q
r
已在前文公式中说明。
[0059]
结合图3可以看到，相比于海底地形slam和信标slam，本发明具备更高的实时导航精度。