[发明专利]一种基于蓝牙的多无人车协同容错导航定位方法及系统

说明书

本发明公开了一种基于蓝牙的多无人车协同容错导航定位方法及系统。该方法包括：获取参与协同导航无人车的位置坐标和系统噪声；根据位置坐标和系统噪声建立与协同导航无人车对应的子滤波器模型；获取上一时刻主滤波器导航定位信息的滤波信息以及上一时刻子滤波器的权重值；根据上一时刻主滤波器导航定位信息的滤波信息、上一时刻子滤波器的权重值和子滤波器模型确定当前时刻子滤波器的滤波信息；根据当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息。采用本发明的方法及系统，具有能够提升导航定位精度的优点。

技术领域

本发明涉及导航定位技术领域，特别是涉及一种基于蓝牙的多无人车协同容错导航定位方法及系统。

背景技术

协同导航指的是多个导航系统之间存在可获取的相对观测信息或者间接相对观测信息，通过一定的信息交换融合手段，实现多个系统之间导航资源共享，最终使导航系统性能更优，精度更高。最初的协同导航是为了在没有路标的环境下，修正机器人长距离移动或是遇到不平坦路面导致轮子转动与路程不匹配导致的机器人内部的定位误差，其基本操作流程为：每个机器人重复移动和停止等待过程，在等待时作为另一个机器人的路标，在移动时以等待的机器人作为路标。协同导航在陆地机器人、空中无人机、无线传感网络、车辆定位、卫星导航以及自主式水下潜器(Autonomous Underwater Vehicle，AUV)集群作业方面有着广泛的应用。例如，在空中无人机方面，当GPS信号丢失时，无人机通过对其他相邻无人机的相对位置观察同时获得加速度计等传感器的信息，通过基于信息传播的协同定位方法获得自身的位置信息，从而调整无人机队形，使得其按照预定计划飞行。

可见，协同导航的应用关键在于如下两点：1.能否获得多个导航系统之间的相对观测信息；2.能否实现多个导航之间的信息传输与共享；所以协同导航的关键在于构建一套既能实现相对测距，又能实现信息共享的数据通信系统。其中，测距系统的准确性与可靠性将直接影响到协同导航的精度，而目前现有的测距手段，无论是基于视觉的，还是基于无线电的，都不可避免的会受到各种干扰带来的影响，导致测距精度下降甚至出现距离测量错误，例如，基于无线电的测距系统，在室内这种电磁信号相对复杂、电磁干扰较多的情况的下，极容易出现电磁波之间互相干扰而导致测距精度显著下降的情况。因此，在测距信息可靠性不足的情况下，提升多无人车的导航定位精度具有重要意义。

发明内容

本发明的目的是提供一种基于蓝牙的多无人车协同容错导航定位方法及系统，具有能够提升导航定位精度的优点。

为实现上述目的，本发明提供了如下方案：

一种基于蓝牙的多无人车协同容错导航定位方法，包括：

获取参与协同导航无人车的位置坐标和系统噪声；各无人车通过蓝牙实现导航信息共享，所述导航信息包括无人车的位置坐标；

根据所述位置坐标和所述系统噪声建立与协同导航无人车对应的子滤波器模型；

获取上一时刻主滤波器导航定位信息的滤波信息以及上一时刻子滤波器的权重值；所述主滤波器导航定位信息的滤波信息包括主滤波器滤波结果和主滤波器滤波估计误差的协方差矩阵；

根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器的滤波信息；所述当前时刻子滤波器的滤波信息包括子滤波器滤波结果和子滤波器滤波估计误差的协方差矩阵；

根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息。

可选的，所述根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息，之后还包括：

判断是否收到停止指令；

若收到停止指令，则输出当前时刻主滤波器滤波结果；