一种基于窗口滑动的AUV地形辅助导航方法

一种基于窗口滑动的auv地形辅助导航方法
技术领域
1.本发明属于下海底地形辅助导航与定位领域，具体涉及一种基于窗口滑动的auv地形辅助导航方法。


背景技术：

2.随着auv技术的快速发展，auv具有自主作业的能力，也逐渐开始承担海底环境探测、科考等任务，这些任务要求auv具有水下隐蔽航行的能力，对auv的导航系统提出较高的要求。常规的水下导航方法如惯性导航和声学导航由于随时间的累计误差及需要事先安装水下信标，不适合长时间水下隐蔽作业。
3.地形辅助导航方法采用的地形测量传感器多为多波束声呐，多波束声呐一次可以测量得到大量的海底地形数据，测量精度高，稳定性好，能够得到较为精确的导航结果。但auv自身能源供应有限，无法支持多波束声呐长时间运行，导致auv水下续航能力受限，无法进行水下远距离航行。为维持水下长时间导航定位，将多波束声呐更换为单波束声呐，利用单波束声呐能够大幅延长auv的水下续航能力，但单波束每次测量得到的地形信息有限，对于地形特征数量及自身测量精度有较高的要求，在进行auv远距离地形辅助导航中，在地形特征贫乏区域很难得到较好的导航结果。
4.惯性导航系统虽然存在随时间增加的累计误差，无法用于远距离auv水下导航中，但较短时间内的惯性导航的航向和距离测量极为精确，能够在短时间内提供极为精准的航向和航行距离信息。。


技术实现要素：

5.本发明的目的是为了解决单波束声呐地形辅助导航鲁棒性较低问题而提出的一种基于窗口滑动的auv地形辅助导航方法，充分考虑惯性导航系统误差特性，提高单波束地形辅助导航方法的鲁棒性。
6.一种基于窗口滑动的auv地形辅助导航方法，包括以下步骤：
7.步骤1：计算地形置信度；
8.步骤2：根据步骤1，进行初始路径搜索；
9.步骤3：根据步骤2，进入匹配定位循环；
10.步骤4：计算与惯性导航提供的距离之差；
11.步骤5：根据步骤4，判断是否进行窗口滑动，如进行窗口滑动则循环执行步骤2至4，直至到达终点，完成匹配定位。
12.本发明还可以包括：
13.1、步骤1具体如下：
14.步骤1.1：根据先验海图生成等深线图，根据网格划分标准，将等深线图划分为n个等深线子地图；
15.步骤1.2：根据每个等深线子地图中的等深线密度进行环境建模，计算每个子地图
区域内的等深线数量nc，通过等深线数量计算每个子地图的地形置信度。
16.2、步骤2具体如下：
17.步骤2.1：进行地形辅助导航算法初始化，根据惯性导航提供的位置信息，设置初始搜索范围半径为r，保证在较大定位误差情况下，auv的真实位置在搜索范围内；
18.步骤2.2：在初始搜索范围内进行两次连续单波束声呐测量得到水深值为z1和z2，得到两条搜索范围内的等深线数据为l1和l2，计算得到两条等深线与搜索范围内栅格线的交点为p1和p2，并以p1为初始点进行初始范围搜索。
19.3、步骤3具体如下：
20.步骤3.1：以p1为起点，计算其沿着惯性导航系统的航行θ1是否与等深线l2存在交点，若存在交点则初始点保留，否则删除，其中判断是否存在交点计算方法为，等深线1上的点向等深线2中连续两点做连线，求出每条连线的斜率；通过判断三条斜率之间的关系，确定是否存在交点，其判断公式为：
[0021][0022]
式中，θ1为惯性导航提供的航向信息，p
1,i
为p1中第i个初始点的坐标，p
2,j
和p
2,j+1
为p2中第j和j+1个点的坐标；
[0023]
步骤3.2：计算等深线p1沿惯性导航航行与p2的交点坐标为计算得到p1距离等深线l2的距离为l
range
，将计算得到的距离与惯性导航提供的距离l
ins
进行比较，并将距离差与设定的误差阈值δ进行比较，小于误差阈值的初始点p1进行保留，其余点则舍弃；
[0024]
步骤3.3：步骤六得到的初始点较多，导致运算速度下降，将初始点在初始搜索范围内进行m等分，并取每个分段内误差最小的初始路径点进行保留，而其余点则进行舍弃；
[0025]
步骤3.4：上述得到的路径点均在初始搜索范围内栅格线上，可能与真实位置存在一定的不同，为此将初始节点在一定范围内进行加密，避免因初始点都在栅格线上导致不必要的误差，并将得到的路径点进行交点判断与距离计算，通过误差阈值进行有效初始点筛选，完成路径初始化；
[0026]
步骤3.5：启动单波束声呐测量得到水深数据为z
*
，并得到此时的位置信息为(x
*
,y
*
)，根据等深线疏密设置此区域的等深线置信度，并根据等深线置信度设置水深测量置信区间，当等深线密集时，设置小的置信区间，当等深线稀疏时，设置较大的置信区间，并得到置信区间的水深数据；
[0027]
根据测量得到的水深数据与置信区间的水深数据，得到对应的等深线数据，并将等深线离散为等深线点，通过步骤3.1和3.2，计算上一个等深线上的路径点沿着惯性导航提供的航向是否与该等深线存在交点，若有交点进行距离计算，将计算得到的距离与惯性导航提供的航行距离进行比较，小于误差阈值的路径进行保留；
[0028]
步骤3.6：随着时间的增加，步骤3.5得到的初始路径点数量逐渐降低，直到所有的等深线之间的距离差都满足误差阈值δ，此时导航方法已经得到收敛，此阶段的地形辅助导航方法结束；
[0029]
步骤3.7：计算此区域的最终路径，若最终存在初始路径点大于1，最终路径数量为w，根据价值函数确定最终路径初始点，确定整条路径信息。路径的价值函数与整条路径信
息有关，其大小为路径长度误差和的倒数，计算公式为：
[0030][0031]
式中为第j条最终路径惯性导航的距离测量值，为第j条最终路径两条等深线之间的路径距离，j为最终保留的路径中第j条路径；
[0032]
步骤3.8：根据每条路径价值函数确定每条路径权重，其权重计算公式如下所示：
[0033][0034]
步骤3.9：权重归一化，公式如下：
[0035][0036]
步骤3.10：根据权重公式得到路径终点的位置信息，计算公式如下所示：
[0037][0038]
式中xj与yj分别为第j条最终路径上终点的水平坐标。
[0039]
4、步骤4随着时间的增加，步骤3.5得到的初始路径点数量逐渐降低，直到所有的等深线之间的距离差都满足误差阈值δ，此时导航方法已经得到收敛，此阶段的地形辅助导航方法结束。
[0040]
本发明的有益效果在于：
[0041]
与现有技术相比，本发明的有益效果是：在利用单波束声呐进行地形辅助定位中，由于获取的信息有限且存在地形特征贫乏区域，导致定位鲁棒性丧失，同时对于惯性导航系统仅考虑其定位特性，并未将惯性导航在短时间的高精度测量信息考虑到匹配方法中，导致无法实现高精度的定位结果。
[0042]
本发明通过充分考虑惯性导航系统的导航误差特性，将短时间内的惯性导航精准航向及航行距离信息考虑到地形辅助导航方法中，提高匹配算法的鲁棒性。在初始搜索阶段，根据惯性导航得到的位置信息设置初始搜索范围，并根据水深测量数据进行路径初始化，根据水深测量数据、惯性导航提供的航向及航行距离信息进行路径点筛选，并对得到的路径与惯性导航航行距离进行比较判断是否进行窗口滑动，实现分段匹配定位。在每个分段中根据价值函数进行最终路径确定，完成整条路径的匹配定位。
附图说明
[0043]
图1是本发明的流程图；
[0044]
图2是本发明匹配方法流程图；
[0045]
图3是本发明的窗口滑动流程图。
具体实施方式
[0046]
下面结合附图对本发明做进一步描述。
[0047]
实施例1：
[0048]
本发明涉及一种基于窗口滑动的auv地形辅助导航方法，包括初始路径搜索、地形置信度计算、匹配定位循环以及窗口滑动定位。根据等深线疏密进行地形置信度计算，在地形匹配中，首先进行初始路径搜索，寻找到合适的初始路径点，根据地形置信度设置置信区间，利用惯性导航提供的测量信息进行路径点筛选，根据距离误差判断是否进行窗口滑动匹配，根据价值函数计算最终路径信息，直至到达终点。
[0049]
本发明的内容是这样实现的：包括以下步骤：
[0050]
1、首先计算地形匹配置信度，根据先验海图数据生成等深线地图，根据网格划分标准，对等深线地图进行网格划分，生成n个等深线子地图；
[0051]
2、进行环境建模，根据等深线匹配特性，等深线匹配结果与等深线疏密相关，通过计算每个子地图中的等深线数量为nc，通过子地图中的等深线数量来计算每个子地图的地形置信度；
[0052]
3、进行地形辅助导航定位，首先进行算法初始化，根据惯性导航提供的初始位置信息，设置初始搜索范围半径为r，其中搜索半径与惯性导航的误差相关，保证在较大定位误差情况下，auv的真实位置在搜索范围内，提高定位的鲁棒性；
[0053]
4、在初始搜索范围内进行初始路径点确定，在搜索范围内进行两次连续单波束声呐测量得到水深值为z1和z2，根据等深线地图得到搜索范围内水深数据对应的等深线数据为l1和l2，通过计算得到两条等深线与搜索范围内的栅格线的交点为p1和p2，并以p1为初始路径点进行初始路径搜索；
[0054]
5、根据惯性导航系统得到在两条等深线之间惯性导航提供的航向θ1及航行距离l
ins
信息，以初始点p1为起点，沿着惯性导航提供的航向，判断其是否与搜索范围内的等深线l2存在交点，若存在交点则初始点保留，否则删除，其中判断是否存在交点计算方法为，从等深线l1上的初始路径点p1向等深线l2上任意两个连续的点做连线，求出每条连线的斜率。根据连线的斜率与惯性导航提供的航向信息，判断三条斜率之间的关系，确定是否存在交点，其判断公式为：
[0055][0056]
式中，θ1为惯性导航提供的航向信息，p
1,i
为p1中第i个初始点的坐标，p
2,j
和p
2,j+1
为p2中第j和j+1个点的坐标。
[0057]
6、通过计算判断初始路径点是否与搜索范围内的等深线l2是否存在交点，并计算得到p1沿着惯性导航的航向与p2中两个连续点连线所在直线的交点坐标为计算得到p1与之间的距离l
range
，将计算得到的距离与惯性导航提供的距离l
ins
进行比较，根据惯性导航误差特性设定误差阈值δ，将计算得到的距离与惯性导航距离的差与误差阈值进行比较，小于误差阈值的初始路径点进行保留，其余初始路径点则删除；
[0058]
7、上述得到的初始路径点较多，可能导致算法运算效率降低，为此需要对路径点进行筛选，首先对搜索范围内的等深线路径点p1进行m等分，并取每段内两条等深线连线与惯性导航提供的距离误差最小值进行保留，在保证定位精度的基础上，降低计算负担，而其余点则进行舍弃；
[0059]
8、上述得到的初始路径点坐标均在栅格线中，而真实的路径点信息可能不在栅格线上，导致定位的鲁棒性丧失，为此需要进行等深线加密，在上述得到的初始路径点附近进行路径加密，加密数量保证任意两个连续的初始路径点之间的距离不大于先验地图分辨率的五分之一，避免因auv的真实位置不在栅格线上，导致定位结果精度下降，并将得到的路径点进行交点判断与距离计算，通过误差阈值进行有效初始点筛选，完成路径初始化；
[0060]
9、下面开始进行路径搜索循环阶段，启动单波束声呐测量得到auv所在位置的水深数据为z
*
，根据惯性导航数据得到此处的位置信息为(x
*
,y
*
)，根据计算得到的地形置信度结合惯性导航给出的位置，得到此位置处的地形置信度，将置信度与测量得到的水深相结合，设置测量置信区间，当地形置信度较低时，需要设置较大的置信区间，提高算法的鲁棒性，当置信度较高时，需要设置较小的置信度区间提高算法的计算效率；
[0061]
根据测量得到的水深数据与置信区间的水深数据，得到对应的等深线数据，将等深线离散为等深线点，通过5和6，计算上一个等深线上的路径点沿着惯性导航的航向是否与该等深线存在交点，若有交点计算等深线之间的距离，并将计算得到的距离与惯性导航提供的航行距离进行比较，并与误差阈值进行比较，确定初始路径点是否保留；
[0062]
10、随着时间的增加，10得到的初始路径点数量逐渐降低，直到所有等深线之间的初始路径点得到的距离差都满足误差阈值δ，说明此时导航方法已经得到收敛，此阶段的地形辅助导航方法结束，并进行窗口滑动匹配定位；
[0063]
11、计算此区域的最终路径，若最终存在初始路径点大于1，最终路径数量为w，根据价值函数确定最终路径初始点，确定整条路径信息。路径的价值函数与整条路径信息有关，其大小为路径长度误差和的倒数，计算公式为：
[0064][0065]
式中为第j条最终路径惯性导航的距离测量值，为第j条最终路径两条等深线之间的路径距离，j为最终保留的路径中第j条路径；
[0066]
12、根据每条路径价值函数确定每条路径权重，其权重计算公式如下所示：
[0067][0068]
13、权重归一化，公式如下：
[0069][0070]
14、根据权重公式得到路径终点的位置信息，计算公式如下所示：
[0071][0072]
式中xj与yj分别为第j条最终路径上终点的水平坐标。
[0073]
15、进行窗口滑动匹配，循环执行3至14，直至到达终点。
[0074]
实施例2：
[0075]
图1是本发明的流程图，具体实施流程如下：
[0076]
(1)计算地形置信度；
[0077]
(2)进行初始路径搜索；
[0078]
(3)进入匹配定位循环；
[0079]
(4)计算与惯性导航提供的距离之差；
[0080]
(5)判断是否进行窗口滑动，完成匹配定位。
[0081]
图2是本发明匹配方法流程图
[0082]
(1)首先根据惯性导航提供的位置信息确定算法的搜索范围；
[0083]
(2)在初始搜索范围内进行初始路径点寻找；
[0084]
(3)根据惯性导航提供的航向及距离信息进行初始点筛选；
[0085]
(4)对搜索范围进行分段划分并选择每个区域最优点进行保留；
[0086]
(5)对保留的路径点进行加密，提高算法鲁棒性；
[0087]
(6)利用单波束声呐测量水深并生成对应的等深线数据；
[0088]
(7)根据地形置信度设置测量置信区间；
[0089]
(8)利用等深线之间距离与惯性导航距离进行比较，进行路径点筛选；
[0090]
(9)完成匹配定位。
[0091]
图3是本发明的窗口滑动流程图
[0092]
(1)首先进行初始路径确定；
[0093]
(2)进入路径搜索循环阶段；
[0094]
(3)进行匹配定位；
[0095]
(4)与惯性导航提供的距离信息进行比较；
[0096]
(5)判断所有的路径信息是否满足误差阈值要求；
[0097]
(6)计算auv的位置信息
[0098]
(7)进行窗口滑动直至到达终点。
[0099]
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。