CN107588773B - 一种基于航位推算与测距信息的多节点协同导航方法 - Google Patents

Abstract

为了解决现有技术中航位推算方法的导航误差随时间或里程积累，造成设备导航精度呈现下降趋势的技术问题，本发明提出了一种基于航位推算与测距信息的多节点协同导航方法，利用定位精度较高的设备对定位精度较低的设备进行协同导航与误差校正，从而提升整全部设备的整体定位精度。

Description

一种基于航位推算与测距信息的多节点协同导航方法

技术领域

本发明涉及一种多节点协同导航技术，具体涉及一种基于航位推算与测距信息的多节点协同导航方法。

背景技术

在军、民领域中的多种设备上，均采用航位推算方法作为一种基础的导航方式，该方法通过利用设备当前的航向和里程增量，在导航坐标系中对设备的坐标进行推算，从而得到设备当前位置。该方法的缺点是导航误差会随时间或里程积累，造成设备导航精度呈现下降趋势。

现有技术中还可利用其它手段如电子标签、无线信号、卫星导航等对航位推算设备的坐标进行修正，此类技术措施的缺点是修正信号具有不确定性，无法保证所有导航设备都能够被成功修正。

以上因素会导致同一工作场所中的不同设备的定位精度也呈现不同的水平，因此提出一种利用高精度设备的定位信息对低精度设备进行协同导航的方法具有重要的意义。

发明内容

为了解决上述技术问题，本发明提出了一种基于航位推算与测距信息的多节点协同导航方法，利用定位精度较高的设备对定位精度较低的设备进行协同导航与误差校正，从而提升整全部设备的整体定位精度。

本发明请求保护一种基于航位推算与测距信息的协同导航方法，其中具有位置精度较差或异常的第一设备和具有正常导航能力的第二设备，该方法包括如下步骤：

步骤101，在(k-1)时刻：

测得第二设备的位置坐标，为p₂(k-1)＝[x₂(k-1),y₂(k-1)]^T；此时第一设备的实际坐标为 此时是未知量；

步骤102，利用测距传感器测量得到第二设备与第一设备间的直线距离d_1,2(k-1),考虑到测距误差，可以确定处于以p₂(k-1)为中心，d_1,2(k-1)±δd为半径的环形区域内，其中δd为测距误差参数；

在此环形区域内生成N个距离为σ，均匀分布的坐标点本专利称之为粒子，则第一设备的真实坐标与这些粒子的最小距离小于σ；

真实粒子的生成算法如下：

dδθ＝σ

其中，σ为相邻两个粒子间的距离；

按照此距离，将圆环的一周分为J个扇形区域，每个扇形的角为δθ；在圆环的直径方向上将其分为K等份，相邻等份间的距离为σ₁；

最终得到共j*k个粒子的坐标

步骤103，由上一步骤可得到一个粒子集合，该集合中的粒子均为第一设备在(k-1)时刻可能的坐标；

进一步，将集合中的每个粒子通过粒子传播模型变换到k时刻；

基于航位推算的粒子传播模型表示如下：

其中，表示粒子i在k时刻的预测状态；为粒子i在k-1时刻的状态；δL_k为k-1～k时刻的位移增量；θ_k为k时刻的航向角；

步骤104，在k时刻，可以通过测距设备测量得到第一设备与第二设备的距离测量值d_1,2(k)；同时，还可获得与p₂(k)之间的距离其中由步骤103获得，p₂(k)通过第一设备与第二设备的信息共享获得；

以与d_1,2(k)的偏差作为观测量，建立粒子权重更新方程：

其中wⁱ表示第i个粒子的权重，δd表示测距设备误差参数；

步骤105，当k时刻第一设备能够接收到M(M>0)个辅助设备时，将这M个设备重新编号，设备集合记为{p_m(k)|m＝2,3...M+1}，此时粒子权重更新方程可表示为：

其中a_j表示对第j个辅助设备的置信程度，取值范围为(0,1],与该设备的导航精度成正比；

步骤106，完成粒子权重更新后，可以按照权重的大小对粒子进行筛选；设定权重阈值T，权重小于T的粒子被删除，其权重直接被置0；

对粒子权重做归一化处理：

其中wⁱ表示第i个粒子的权值，J表示当前的粒子个数；

步骤107，当k时刻，剩余的粒子个数为K个，则当前第一设备的协同导航位置为：

当K小于某个设定的数量时，则认为协同导航算法完成收敛，用协同导航位置替换第一设备的当前位置，协同导航结束。

进一步的，所述第一设备和第二设备具备航位推算能力；上述设备能够提供实时的航向信息ψ(t)与里程增量δL(t)信息，并利用该信息推算设备载体的当前坐标。

进一步的，上述设备具备测量与其它设备间直线距离的能力，测量方法可采用激光测距模块、视觉测距、无线电测距等。

进一步的，上述设备之间需要具备定位信息实时共享能力，共享信息包括当前坐标、当前航向、里程增量、设备间距离。

附图说明

图1为本发明第一设备和第二设备的位置示意图。

具体实施方式

下面通过具体实施例对本发明作进一步详述，以下实施例只是描述性的，不是限定性的，不能以此限定本发明的保护范围。

本发明提出了一种基于航位推算系统与测距信息的协同导航方法，利用定位精度较高的设备对定位精度较低的设备进行协同导航与误差校正，从而提升整全部设备的整体定位精度。

本发明所提出的协同导航方法，其中的设备需要3个方面的能力：

1.本发明中的设备需要具备航位推算能力；

设备需要能够提供实时的航向信息ψ(t)与里程增量δL(t)信息，并利用该信息推算设备载体的当前坐标：

x(t)＝x(t-δt)+δL cosψ(t)

y(t)＝y(t-δt)+δL sinψ(t)

将上式进行离散化：

x(k)＝x(k-1)+δL cosψ(k)

y(k)＝y(k-1)+δL sinψ(k)

2.设备需要具备测量与其它设备间直线距离(ψ(k))的能力，测量方法可采用激光测距模块、视觉测距、无线电测距等。

3.设备之间需要具备定位信息实时共享能力，共享信息包括当前坐标(x(k),y(k))、当前航向(ψ(k))、里程增量(δL(k))、设备间距离(d(k))等。

在设备具备以上3种能力之后，即可利用以下的方法实现高精度设备对低精度设备的协同导航。

本发明的方法概述如下：

如图1所示，圆点p₂(k-1)表示(k-1)时刻具备正常导航功能的第二设备的位置；表示(k-1)时刻第一设备的真实位置，该设备由于某种原因定位精度过低，因此是未知量；d_1,2(k-1)为(k-1)时刻第一设备、2间的距离，通过测量得到；

在以p₂(k-1)为圆心，d_1,2(k-1)±δd为半径的范围内生成M个粒子每个粒子都代表第一设备可能的位置，δd的选取与测距传感器精度相关；从(k-1)到(k)时刻，第二设备的位置从p₂(k-1)到了p₂(k)，第一设备的每个粒子状态利用航位推算更新为此时第i个粒子与第二设备的预测距离为而第一设备与第二设备在(k)时刻的实际距离为通过预测距离与实测距离的差值可以对粒子权重进行更新，粒子群将逐步收敛到真实位置，实现第一设备、2的协同导航。

本发明请求保护一种基于航位推算系统与测距信息的协同导航方法，其中具有位置精度较差或异常的第一设备和具有正常导航能力的第二设备，该方法包括如下步骤：

步骤101，在(k-1)时刻：

测得第二设备的位置坐标，为p₂(k-1)＝[x₂(k-1),y₂(k-1)]^T；此时第一设备的实际坐标为 此时是未知量；

步骤102，利用测距传感器测量得到第二设备与第一设备间的直线距离d_1,2(k-1),考虑到测距误差，可以确定处于以p₂(k-1)为中心，d_1,2(k-1)±δd为半径的环形区域内，其中δd为测距误差参数；

在此环形区域内生成N个距离为σ，均匀分布的坐标点本专利称之为粒子，则第一设备的真实坐标与这些粒子的最小距离小于σ；

真实粒子的生成算法如下：

dδθ＝σ

其中，σ为相邻两个粒子间的距离；

按照此距离，将圆环的一周分为J个扇形区域，每个扇形的角为δθ；在圆环的直径方向上将其分为K等份，相邻等份间的距离为σ₁；

最终得到共j*k个粒子的坐标

步骤103，由上一步骤可得到一个粒子集合，该集合中的粒子均为第一设备在(k-1)时刻可能的坐标；

进一步，将集合中的每个粒子通过粒子传播模型变换到k时刻；

基于航位推算的粒子传播模型表示如下：

其中，表示粒子i在k时刻的预测状态；为粒子i在k-1时刻的状态；δL_k为k-1～k时刻的位移增量；θ_k为k时刻的航向角；

步骤104，在k时刻，可以通过测距设备测量得到第一设备与第二设备的距离测量值d_1,2(k)；同时，还可获得与p₂(k)之间的距离其中由步骤103获得，p₂(k)通过第一设备与第二设备的信息共享获得；

以与d_1,2(k)的偏差作为观测量，建立粒子权重更新方程：

其中wⁱ表示第i个粒子的权重，δd表示测距设备误差参数；

步骤105，当k时刻第一设备能够接收到M(M>0)个辅助设备时，将这M个设备重新编号，设备集合记为{p_m(k)|m＝2,3...M+1}，此时粒子权重更新方程可表示为：

其中a_j表示对第j个辅助设备的置信程度，取值范围为(0,1],与该设备的导航精度成正比；

步骤106，完成粒子权重更新后，可以按照权重的大小对粒子进行筛选；设定权重阈值T，权重小于T的粒子被删除，其权重直接被置0；

对粒子权重做归一化处理：

其中wⁱ表示第i个粒子的权值，J表示当前的粒子个数；

步骤107，当k时刻，剩余的粒子个数为K个，则当前第一设备的协同导航位置为：

当K小于某个设定的数量时，则认为协同导航算法完成收敛，用协同导航位置替换第一设备的当前位置，协同导航结束。

本发明中应用具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想，对于本领域的一般技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明的保护的范围。

Claims (4)

1.一种基于航位推算与测距信息的协同导航方法，其中具有位置精度较差或异常的第一设备和具有正常导航能力的第二设备，其特征在于，该方法包括如下步骤：

步骤101，在(k-1)时刻：

测得第二设备的位置坐标，为p₂(k-1)＝[x₂(k-1),y₂(k-1)]^T；此时第一设备的实际坐标为 此时是未知量；

步骤102，利用测距传感器测量得到第二设备与第一设备间的直线距离d_1,2(k-1),考虑到测距误差，可以确定处于以p₂(k-1)为中心，d_1,2(k-1)±δd为半径的环形区域内，其中δd为测距误差参数；

在此环形区域内生成N个距离为σ，均匀分布的坐标点本专利称之为粒子，则第一设备的真实坐标与这些粒子的最小距离小于σ；

真实粒子的生成算法如下：

dδθ＝σ

其中，σ为相邻两个粒子间的距离；

按照此距离，将圆环的一周分为J个扇形区域，每个扇形的角为δθ；在圆环的直径方向上将其分为K等份，相邻等份间的距离为σ₁；

最终得到共j*k个粒子的坐标

步骤103，由上一步骤可得到一个粒子集合，该集合中的粒子均为第一设备在(k-1)时刻可能的坐标；

进一步，将集合中的每个粒子通过粒子传播模型变换到k时刻；

基于航位推算的粒子传播模型表示如下：

其中，表示粒子i在k时刻的预测状态；为粒子i在k-1时刻的状态；δL_k为k-1～k时刻的位移增量；θ_k为k时刻的航向角；

步骤104，在k时刻，可以通过测距设备测量得到第一设备与第二设备的距离测量值d_1,2(k)；同时，还可获得与p₂(k)之间的距离其中由步骤103获得，p₂(k)通过第一设备与第二设备的信息共享获得；

以与d_1,2(k)的偏差作为观测量，建立粒子权重更新方程：

其中wⁱ表示第i个粒子的权重，δd表示测距设备误差参数；

步骤105，当k时刻第一设备能够接收到M(M>0)个辅助设备时，将这M个设备重新编号，设备集合记为{p_m(k)|m＝2,3...M+1}，此时粒子权重更新方程可表示为：

其中a_j表示对第j个辅助设备的置信程度，取值范围为(0,1],与该设备的导航精度成正比；

步骤106，完成粒子权重更新后，可以按照权重的大小对粒子进行筛选；设定权重阈值T，权重小于T的粒子被删除，其权重直接被置0；

对粒子权重做归一化处理：

其中wⁱ表示第i个粒子的权值，J表示当前的粒子个数；

步骤107，当k时刻，剩余的粒子个数为K个，则当前第一设备的协同导航位置为：

当K小于某个设定的数量时，则认为协同导航算法完成收敛，用协同导航位置替换第一设备的当前位置，协同导航结束。

2.如权利要求1所述的方法，其特征在于，所述第一设备和第二设备具备航位推算能力；上述设备能够提供实时的航向信息ψ(t)与里程增量δL(t)信息，并利用该信息推算设备载体的当前坐标。

3.如权利要求1所述的方法，其特征在于，上述设备具备测量与其它设备间直线距离的能力，测量方法可采用激光测距模块、视觉测距、无线电测距等。

4.如权利要求1所述的方法，其特征在于，上述设备之间需要具备定位信息实时共享能力，共享信息包括当前坐标、当前航向、里程增量、设备间距离。