CN111121778B - 一种导航***初始化方法 - Google Patents

Abstract

本发明公开一种导航***初始化方法，包括如下过程：计算推算导航起始位置的置信区间；对起始位置的置信区间进行网格化处理获得规划起点的似然函数；得到起始点的似然函数的前向运动至下一位置的似然函数网格；对后向位置的网格节点似然函数值进行平滑处理，得到初始位置的平滑网格；计算其正态分布网格及初始位置的正态分布网格和平滑网格之间的残差，若其小于预设检验阈值则进行下一步，否则转向根据推算导航的误差得到起始点的似然函数的前向运动至下一位置的似然函数网格的步骤；进行粒子滤波初始化处理。本发明能够提高地形匹配导航初始点的粒子滤波器初始化精度，从而提高其滤波收敛速度和跟踪导航精度。

Description

一种导航***初始化方法

技术领域

本发明属于水下地形测绘技术领域，具体涉及一种导航***初始化方法。

背景技术

卫星定位信号无法穿透水体到达深海，声学定位***的有效作用范围有限，而惯性导航***又存在不可避免的时间累积误差。因此，深海、极地和超长航程任务的水下机器不同通过传统的导航方式获得长时间、高精度的定位信息。水下地形匹配导航技术采用地形信息为定位信息源，具有有界误差，可为水下航行器提供长时间无累积误差的定位信息。目前，地形匹配导航***多采用粒子滤波器进行位置更新。

粒子滤波器被认为是地形匹配导航***最理想的滤波器，但如果参考导航***在地形匹配导航的起点存在较大的定位误差，并且初始点的地形适配性较低时，初始化粒子存在较大的覆盖范围，而且地形的自相似性也将导致粒子滤波器收敛缓慢。为此需要利用初始对准技术和粒子初始化技术获得高精度的初始定位信息和粒子初始化信息，以此提高地形匹配导航初始阶段的滤波收敛速度，并保证在跟踪阶段具有较高的精度。

发明内容

本发明的目的在于克服现有技术中的不足，提供一种导航***初始化方法，提高地形匹配导航初始点的粒子滤波器初始化精度，从而提高其滤波收敛速度和跟踪导航精度。

为解决现有技术问题，本发明公开了一种导航***初始化方法，包括如下过程：

计算推算导航起始位置的置信区间；

对起始位置的置信区间进行网格化处理获得规划起点的似然函数；

根据推算导航的误差得到起始点的似然函数的前向运动至下一位置的似然函数网格；

根据当前位置的网格节点的似然函数值对后向位置的网格节点似然函数值进行平滑处理，得到初始位置的平滑网格；

根据初始位置的平滑网格计算其正态分布网格；

计算初始位置的正态分布网格和平滑网格之间的残差，若其小于预设检验阈值则进行下一步，否则转向根据推算导航的误差得到起始点的似然函数的前向运动至下一位置的似然函数网格的步骤；

根据初始位置的平滑网格进行粒子滤波初始化处理。

进一步地，

对于平面坐标系x0y内的规划起点，其推算导航位置的表达式为：

规划起点的定位误差的表达式为：

置信区间的表达式为：

式中：γ表示置信区间的方放大系数；α表示推算导航的误差椭圆长轴与x轴的夹角；

为规划起点对应的粒子序列(x_i,y_i),i＝1,2,3...n中x轴坐标的均值，/>

为规划起点对应的粒子序列(x_i,y_i),i＝1,2,3...n中y轴坐标的均值；K的表达式为：/>

进一步地，

对起始位置的置信区间进行网格化处理获得规划起点的似然函数的具体过程为：

将置信区间进行网格化处理得到网格节点作为定位点的概率值，其表达式为：

其中l_m×l_n为网格数量；

计算每个网格节点的似然值，其表达式为：

其中/>

表示置信区间内索引号为(i,j)的搜索点；Z表示测量地形的高程序列；/>

表示测量地形的高程序列Z在搜索点(i,j)的地形插值序列；C表示似然函数的归一化参数；

计算规划起点的似然函数，其表达式为：

进一步地，

根据推算导航的误差得到起始点似然函数的前向运动至下一位置的似然函数网格的过程为：

计算从第一个规划起点X¹点运动到第二个规划起点X²点后网格节点的似然值

以及X²位置对应的网格节点的地形匹配定位似然函数/>

其表达式为：

K，L表示网格的行数和列数；ΔX¹²表示定位点X¹与推算导航位置X²点之间的距离；P¹²表示从X¹到X²点的推算导航误差，C表示归一化参数；

计算经过观测修正后的地形匹配导航似然函数网格节点的似然值

其表达式为：

重复上述过程得到X^N位置的地形匹配定位似然函数L^N或者地形匹配导航误差T^N。

进一步地，

根据当前位置的网格节点的似然函数值对后向位置的网格节点似然函数值进行平滑处理，得到初始位置的平滑网格的具体过程为：

利用S^N平滑地形匹配导航点X^N-1位置的地形匹配导航定位似然函数网格T^N-1，其表达式为：

式中：/>

表示地形匹配导航点位置X^N-1位置的地形匹配定位似然函数网格中索引点(i,j)的似然值；它可以替换为/>

该变量表示地形匹配导航点位置X^N-1位置的地形匹配导航似然函数网格中索引点(i,j)的似然值；ΔX^N-1,N表示从X^N-1点到X^N点的推算导航距离；

重复上述过程依次得到X^N-2，X^N-3，……，X¹位置的平滑结果S^N-2，S^N-3，……，S¹。

进一步地，

根据初始位置的平滑网格计算其正态分布网格的具体过程为：

对初始位置的S¹计算定位点

其表达式为：

表示S¹网格中索引号为(i,j)网格点的坐标；

进行高斯拟合优度检验，设置检验阈值ε，计算S¹的网格节点构成的样本的方差

其表达式为：

计算服从方差为

均值为/>

的正态分布的网格的取值G_ij，其表达式为：

进一步地，

残差的表达式为：

进一步地，

根据初始位置的平滑网格进行粒子滤波初始化处理的具体过程为：

对网格进行加密处理，获得加密后的似然函数；

将加密后的似然函数转化为灰度图；

对灰度图进行四叉树分解；

对分解后的网格中心布置粒子，并将该中心的似然值赋值为粒子的初始权重。

进一步地，

对灰度图进行四叉树分解的具体过程为：

判断某一子块的面积和其中心对应的似然值的乘机是否大于四叉树分解的阈值，若是则继续分解该子块直至所有子块均满足其面积和其中心对应的似然值的乘机大于四叉树分解的阈值。

本发明具有的有益效果：

1.本发明采用本发明提出的地形匹配导航粒子滤波***初始化方法可以有效的降低粒子滤波器的初始化误差。

2.本发明利用四叉树分解方法进行似然函数的离散化，可以控制粒子滤波器初始化采样误差。

3.本发明在地形适配性较低的区域进行初始对准时，可以通过本方法自主的判断初始定位点的定位精度，并为执行初始化步骤提供决策参考。

附图说明

图1为本发明的方法流程图；

图2为本发明中地形匹配定位的搜索区间和网格划分示意图；

图3为本发明中根据推算导航信息进行网格空间运动的示意图；

图4为本发明中四叉树分解的流程图；

图5为本发明中由灰度图到四叉树分解图的过程示意图；

图6为本发明中由四叉树分解图到初始化粒子分布的过程示意图。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

如图1所示，一种导航***初始化方法，包括如下过程：

S1、根据规划起点的推算导航位置X_I和其定位误差

计算起始位置的置信区间；具体地，对于平面坐标系x0y内的规划起点，其推算导航位置的表达式为：

规划起点的定位误差的表达式为：

置信区间的表达式为：

式中：γ表示置信区间的方放大系数；α表示推算导航的误差椭圆长轴与x轴的夹角；

为规划起点对应的粒子序列(x_i,y_i),i＝1,2,3...n中x轴坐标的均值，/>

为规划起点对应的粒子序列(x_i,y_i),i＝1,2,3...n中y轴坐标的均值；K的表达式为：/>

S2、对起始位置的置信区间1进行网格化处理获得规划起点的似然函数；具体过程为：

如图2所示，将置信区间进行网格化处理得到网格节点2作为定位点的概率值，其表达式为：

其中l_m×l_n为网格数量；

计算每个网格节点2的似然值，其表达式为：

其中/>

表示置信区间内索引号为(i,j)的搜索点；Z表示测量地形的高程序列；/>

表示测量地形的高程序列Z在搜索点(i,j)的地形插值序列；C表示似然函数的归一化参数；

计算规划起点的似然函数，其表达式为：

S3、根据推算导航的误差得到起始点的似然函数的前向运动至下一位置的似然函数网格；具体过程为：

如图3所示，计算从第一个规划起点X¹点运动到第二个规划起点X²点后网格节点的似然值

以及X²位置对应的网格节点的地形匹配定位似然函数/>

其表达式为：

K，L表示网格的行数和列数；ΔX¹²表示定位点X¹与推算导航位置X²点之间的距离；P¹²表示从X¹到X²点的推算导航误差，C表示归一化参数。

计算经过观测修正后的地形匹配导航似然函数网格节点的似然值

其表达式为：

以同样的方法可以得到X^N位置的地形匹配定位似然函数网格L^N或者地形匹配导航网格T^N，其中N表示地形匹配导航点的索引号。假设当前位置为第N个地形匹配导航点，则此时已经获得了X¹～X^N点的地形匹配定位似然函数网格T¹～T^N和地形匹配导航似然函数网格L¹～L^N。

S4、根据当前位置的网格节点的似然函数值对后向位置的网格节点似然函数值进行平滑处理，得到初始位置的平滑网格。具体过程为：

假设地形匹配导航的似然函数网格T^N为地形匹配导航点X^N的定位似然函数网格平滑结果。

利用S^N平滑地形匹配导航点X^N-1位置的地形匹配导航定位似然函数网格T^N-1，其表达式为：

式中：

表示地形匹配导航点位置X^N-1位置的地形匹配定位似然函数网格中索引点(i,j)的似然值；它可以替换为/>

该变量表示地形匹配导航点位置X^N-1位置的地形匹配导航似然函数网格中索引点(i,j)的似然值；ΔX^N-1,N表示从X^N-1点到X^N点的推算导航距离。

基于同样的方法，可以计算X^N-2，X^N-3，……，X¹位置的平滑结果，只需要按照上式做少量的修改，并带入相应的值，最终可以得到X¹位置的平滑网格S¹。

S5、根据初始位置的平滑网格计算其正态分布网格。具体过程为：

对初始位置的S¹计算定位点

其表达式为：

表示S¹网格中索引号为(i,j)网格点的坐标。

进行高斯拟合优度检验，设置检验阈值ε，计算S¹的网格节点构成的样本的方差

其表达式为：

计算服从方差为

均值为/>

的正态分布的网格的取值G_ij，其表达式为：

S6、计算初始位置的正态分布网格和平滑网格之间的残差，残差的表达式为：

判断Δ是否满足Δ＜ε。

如果不满足，则重复S3，继续获取X^N+1位置的地形匹配导航似然函数网格T^N+1和地形匹配定位似然函数网格L^N+1。然后，利用S2～S6步中获得的推算导航位置X¹～X^N+1点的地形匹配定位似然函数网格T¹～T^N+1和地形匹配导航似然函数网格L¹～L^N+1，对位置X¹处的地形匹配导航似然函数的网格似然值进行平滑，直至满足Δ＜ε为止。如果满足，则继续进行下面的步骤。

S7、如图4所示，根据初始位置的平滑网格进行粒子滤波初始化处理。具体过程为：

采用差值法对网格进行加密处理，获得加密后的似然函数

如图5所示，将加密后的似然函数

转化为灰度图。

对灰度图进行四叉树分解；具体过程为：

判断某一子块s_i的面积Δ_i和其中心对应的似然值l_i的乘机是否满足Δ_il_i＞ε，ε表示四叉树分解的阈值，若是则继续分解该子块直至所有子块均满足Δ_il_i＞ε。

如图6所示，对分解后的网格中心布置粒子，并将该中心的似然值赋值为粒子的初始权重。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (6)

1.一种导航***初始化方法，其特征在于：包括如下过程：

计算推算导航起始位置的置信区间；

对起始位置的置信区间进行网格化处理获得规划起点的似然函数；

根据推算导航的误差得到起始点的似然函数的前向运动至下一位置的似然函数网格；

根据当前位置的网格节点的似然函数值对后向位置的网格节点似然函数值进行平滑处理，得到初始位置的平滑网格；

根据初始位置的平滑网格计算其正态分布网格；

计算初始位置的正态分布网格和平滑网格之间的残差，若其小于预设检验阈值则进行下一步，否则转向根据推算导航的误差得到起始点的似然函数的前向运动至下一位置的似然函数网格的步骤；

根据初始位置的平滑网格进行粒子滤波初始化处理；

对于平面坐标系x0y内的规划起点，其推算导航位置的表达式为：

所述规划起点的定位误差的表达式为：

所述置信区间的表达式为：

式中：γ表示置信区间的方放大系数；α表示推算导航的误差椭圆长轴与x轴的夹角；

为规划起点对应的粒子序列(xi,yi),i＝1,2,3...n中x轴坐标的均值，/>

为规划起点对应的粒子序列(xi,yi),i＝1,2,3...n中y轴坐标的均值；K的表达式为：/>

所述对起始位置的置信区间进行网格化处理获得规划起点的似然函数的具体过程为：

将置信区间进行网格化处理得到网格节点作为定位点的概率值，其表达式为：

其中l_m×l_n为网格数量；

计算每个网格节点的似然值，其表达式为：

其中/>

表示置信区间内索引号为(i,j)的搜索点；Z表示测量地形的高程序列；/>

表示测量地形的高程序列Z在搜索点(i,j)的地形插值序列；C表示似然函数的归一化参数；

计算规划起点的似然函数，其表达式为：

所述根据推算导航的误差得到起始点似然函数的前向运动至下一位置的似然函数网格的过程为：

计算从第一个规划起点X¹点运动到第二个规划起点X²点后网格节点的似然值

以及X²位置对应的网格节点的地形匹配定位似然函数/>

其表达式为：

L表示网格的行数和列数；ΔX¹²表示定位点X¹与推算导航位置X²点之间的距离；P¹²表示从X¹到X²点的推算导航误差，C表示归一化参数；

计算经过观测修正后的地形匹配导航似然函数网格节点的似然值

其表达式为：

重复上述过程得到X^N位置的地形匹配定位似然函数L^N或者地形匹配导航误差T^N。

2.根据权利要求1所述的一种导航***初始化方法，其特征在于：

所述根据当前位置的网格节点的似然函数值对后向位置的网格节点似然函数值进行平滑处理，得到初始位置的平滑网格的具体过程为：

利用S^N平滑地形匹配导航点X^N-1位置的地形匹配导航定位似然函数网格T^N-1，其表达式为：

式中：/>

表示地形匹配导航点位置X^N-1位置的地形匹配定位似然函数网格中索引点(i,j)的似然值；它可以替换为

该变量表示地形匹配导航点位置X^N-1位置的地形匹配导航似然函数网格中索引点(i,j)的似然值；ΔX^N-1,N表示从X^N-1点到X^N点的推算导航距离；

重复上述过程依次得到X^N-2，X^N-3，……，X¹位置的平滑结果S^N-2，S^N-3，……，S¹。

3.根据权利要求2所述的一种导航***初始化方法，其特征在于：

所述根据初始位置的平滑网格计算其正态分布网格的具体过程为：

对初始位置的S¹计算定位点

其表达式为：

表示S¹网格中索引号为(i,j)网格点的坐标；

进行高斯拟合优度检验，设置检验阈值ε，计算S¹的网格节点构成的样本的方差

其表达式为：

计算服从方差为

均值为/>

的正态分布的网格的取值G_ij，其表达式为：

4.根据权利要求3所述的一种导航***初始化方法，其特征在于：

所述残差的表达式为：

5.根据权利要求4所述的一种导航***初始化方法，其特征在于：

所述根据初始位置的平滑网格进行粒子滤波初始化处理的具体过程为：

对网格进行加密处理，获得加密后的似然函数；

将加密后的似然函数转化为灰度图；

对灰度图进行四叉树分解；

对分解后的网格中心布置粒子，并将该中心的似然值赋值为粒子的初始权重。

6.根据权利要求5所述的一种导航***初始化方法，其特征在于：

所述对灰度图进行四叉树分解的具体过程为：

判断某一子块的面积和其中心对应的似然值的乘机是否大于四叉树分解的阈值，若是则继续分解该子块直至所有子块均满足其面积和其中心对应的似然值的乘机大于四叉树分解的阈值。

Images