一种基于蓝牙的多无人车协同容错导航定位方法及系统技术方案

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

A Bluetooth based collaborative fault tolerant navigation and positioning method and system for multiple unmanned vehicles

【技术实现步骤摘要】
一种基于蓝牙的多无人车协同容错导航定位方法及系统
本专利技术涉及导航定位
，特别是涉及一种基于蓝牙的多无人车协同容错导航定位方法及系统。
技术介绍
协同导航指的是多个导航系统之间存在可获取的相对观测信息或者间接相对观测信息，通过一定的信息交换融合手段，实现多个系统之间导航资源共享，最终使导航系统性能更优，精度更高。最初的协同导航是为了在没有路标的环境下，修正机器人长距离移动或是遇到不平坦路面导致轮子转动与路程不匹配导致的机器人内部的定位误差，其基本操作流程为：每个机器人重复移动和停止等待过程，在等待时作为另一个机器人的路标，在移动时以等待的机器人作为路标。协同导航在陆地机器人、空中无人机、无线传感网络、车辆定位、卫星导航以及自主式水下潜器(AutonomousUnderwaterVehicle，AUV)集群作业方面有着广泛的应用。例如，在空中无人机方面，当GPS信号丢失时，无人机通过对其他相邻无人机的相对位置观察同时获得加速度计等传感器的信息，通过基于信息传播的协同定位方法获得自身的位置信息，从而调整无人机队形，使得其按照预定计划飞行。可见，协同导航的应用关键在于如下两点：1.能否获得多个导航系统之间的相对观测信息；2.能否实现多个导航之间的信息传输与共享；所以协同导航的关键在于构建一套既能实现相对测距，又能实现信息共享的数据通信系统。其中，测距系统的准确性与可靠性将直接影响到协同导航的精度，而目前现有的测距手段，无论是基于视觉的，还是基于无线电的，都不可避免的会受到各种干扰带来的影响，导致测距精度下降甚至出现距离测量错误，例如，基于无线电的测距系统，在室内这种电磁信号相对复杂、电磁干扰较多的情况的下，极容易出现电磁波之间互相干扰而导致测距精度显著下降的情况。因此，在测距信息可靠性不足的情况下，提升多无人车的导航定位精度具有重要意义。
技术实现思路
本专利技术的目的是提供一种基于蓝牙的多无人车协同容错导航定位方法及系统，具有能够提升导航定位精度的优点。为实现上述目的，本专利技术提供了如下方案：一种基于蓝牙的多无人车协同容错导航定位方法，包括：获取参与协同导航无人车的位置坐标和系统噪声；各无人车通过蓝牙实现导航信息共享，所述导航信息包括无人车的位置坐标；根据所述位置坐标和所述系统噪声建立与协同导航无人车对应的子滤波器模型；获取上一时刻主滤波器导航定位信息的滤波信息以及上一时刻子滤波器的权重值；所述主滤波器导航定位信息的滤波信息包括主滤波器滤波结果和主滤波器滤波估计误差的协方差矩阵；根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器的滤波信息；所述当前时刻子滤波器的滤波信息包括子滤波器滤波结果和子滤波器滤波估计误差的协方差矩阵；根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息。可选的，所述根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息，之后还包括：判断是否收到停止指令；若收到停止指令，则输出当前时刻主滤波器滤波结果；若未收到停止指令，则根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型更新上一时刻子滤波器的权重值，返回步骤“获取参与协同导航无人车的位置坐标和系统噪声”。可选的，其特征在于，所述根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器的滤波信息，具体包括：根据所述上一时刻主滤波器导航定位信息的滤波信息和所述上一时刻子滤波器的权重值确定子滤波器初始滤波信息；所述子滤波器初始滤波信息包括子滤波器初始滤波结果和子滤波器初始滤波误差的协方差矩阵；根据所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器过程噪声协方差矩阵；根据所述子滤波器初始滤波信息、所述当前时刻子滤波器过程噪声协方差矩阵和所述子滤波器模型确定当前时刻子滤波器的滤波信息。可选的，所述根据所述位置坐标和所述系统噪声建立与协同导航无人车对应的子滤波器模型，具体包括：根据如下公式建立与协同导航无人车对应的子滤波器模型：X(k)＝ΦX(k-1)+U(k)+W(k)Zj(k)＝hj(X(k))+vj(k)其中，上式中，X(k)表示k时刻无人车自身位置，px(k)表示k时刻无人车自身位置横坐标，py(k)表示k时刻无人车自身位置纵坐标，X(k-1)表示k-1时刻无人车自身位置，Φ表示k时刻无人车位置状态转移矩阵，px(k-1)表示k-1时刻无人车自身位置横坐标，py(k-1)表示k-1时刻无人车自身位置纵坐标，U(k)表示k时刻由无人车自身装配的自主导航系统所提供的位置增量，Δpx(k)表示k时刻由无人车自身装配的自主导航系统所提供的位置横坐标增量，Δpy(k)表示k时刻由无人车自身装配的自主导航系统所提供的位置纵坐标增量，W(k)表示k时刻系统过程噪声，wx(k)表示k时刻系统横坐标过程噪声，wy(k)表示k时刻系统纵坐标过程噪声，表示k时刻利用无人车自身i所接受到无人车j的蓝牙信号强度估计得到的无人车自身i和无人车j之间的相对距离，hj(X(k))表示k时刻无人车自身i与无人车j之间的相对距离真实值，表示k时刻无人车j的位置横坐标，表示k时刻无人车j的位置纵坐标，vj(k)表示k时刻无人车j的系统量测噪声，j＝1,...,m-1，m表示参与协同导航无人车的总个数；所述系统噪声包括系统过程噪声和系统量测噪声。可选的，所述根据所述上一时刻主滤波器导航定位信息的滤波信息和所述上一时刻子滤波器的权重值确定子滤波器初始滤波信息，具体包括：根据如下公式确定子滤波器初始滤波信息：式中，Pj,k-1表示k-1时刻无人车j对应的子滤波器初始滤波误差的协方差矩阵，表示k-1时刻无人车j对应的子滤波器的权重值，Pg,k-1表示k-1时刻主滤波器滤波估计误差的协方差矩阵，表示k-1时刻无人车j对应的子滤波器初始滤波结果，表示k-1时刻主滤波器导航定位信息的滤波结果；所述上一时刻为k-1时刻；所述根据所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器过程噪声协方差矩阵，具体包括：根据如下公式确定当前时刻子滤波器过程噪声协方差矩阵：式中，Qj,k表示k时刻无人车j对应的子滤波器过程噪声协方差矩阵，Q表示W(k)的协方差矩阵；所述当前时刻为k时刻；所述根据所述子滤波器初始滤波信息、所述当前时刻子滤波器过程噪声协方差矩阵和所述子滤波器模型确定当前时刻子滤波器的滤波信息，具体包括：根据如下公式确定当前时刻子滤波器的滤波信息：其中，Pj,k/k-1＝ΦPj,k-1ΦT+Qj,k本文档来自技高网...

【技术保护点】
1.一种基于蓝牙的多无人车协同容错导航定位方法，其特征在于，包括：/n获取参与协同导航无人车的位置坐标和系统噪声；各无人车通过蓝牙实现导航信息共享，所述导航信息包括无人车的位置坐标；/n根据所述位置坐标和所述系统噪声建立与协同导航无人车对应的子滤波器模型；/n获取上一时刻主滤波器导航定位信息的滤波信息以及上一时刻子滤波器的权重值；所述主滤波器导航定位信息的滤波信息包括主滤波器滤波结果和主滤波器滤波估计误差的协方差矩阵；/n根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器的滤波信息；所述当前时刻子滤波器的滤波信息包括子滤波器滤波结果和子滤波器滤波估计误差的协方差矩阵；/n根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息。/n

【技术特征摘要】
1.一种基于蓝牙的多无人车协同容错导航定位方法，其特征在于，包括：
获取参与协同导航无人车的位置坐标和系统噪声；各无人车通过蓝牙实现导航信息共享，所述导航信息包括无人车的位置坐标；
根据所述位置坐标和所述系统噪声建立与协同导航无人车对应的子滤波器模型；
获取上一时刻主滤波器导航定位信息的滤波信息以及上一时刻子滤波器的权重值；所述主滤波器导航定位信息的滤波信息包括主滤波器滤波结果和主滤波器滤波估计误差的协方差矩阵；
根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器的滤波信息；所述当前时刻子滤波器的滤波信息包括子滤波器滤波结果和子滤波器滤波估计误差的协方差矩阵；
根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息。


2.根据权利要求1所述的基于蓝牙的多无人车协同容错导航定位方法，其特征在于，所述根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息，之后还包括：
判断是否收到停止指令；
若收到停止指令，则输出当前时刻主滤波器滤波结果；
若未收到停止指令，则根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型更新上一时刻子滤波器的权重值，返回步骤“获取参与协同导航无人车的位置坐标和系统噪声”。


3.根据权利要求1或2所述的基于蓝牙的多无人车协同容错导航定位方法，其特征在于，所述根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器的滤波信息，具体包括：
根据所述上一时刻主滤波器导航定位信息的滤波信息和所述上一时刻子滤波器的权重值确定子滤波器初始滤波信息；所述子滤波器初始滤波信息包括子滤波器初始滤波结果和子滤波器初始滤波误差的协方差矩阵；
根据所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器过程噪声协方差矩阵；
根据所述子滤波器初始滤波信息、所述当前时刻子滤波器过程噪声协方差矩阵和所述子滤波器模型确定当前时刻子滤波器的滤波信息。


4.根据权利要求3所述的基于蓝牙的多无人车协同容错导航定位方法，其特征在于，所述根据所述位置坐标和所述系统噪声建立与协同导航无人车对应的子滤波器模型，具体包括：
根据如下公式建立与协同导航无人车对应的子滤波器模型：
X(k)＝ΦX(k-1)+U(k)+W(k)
Zj(k)＝hj(X(k))+vj(k)
其中，















上式中，X(k)表示k时刻无人车自身位置，px(k)表示k时刻无人车自身位置横坐标，py(k)表示k时刻无人车自身位置纵坐标，X(k-1)表示k-1时刻无人车自身位置，Φ表示k时刻无人车位置状态转移矩阵，px(k-1)表示k-1时刻无人车自身位置横坐标，py(k-1)表示k-1时刻无人车自身位置纵坐标，U(k)表示k时刻由无人车自身装配的自主导航系统所提供的位置增量，Δpx(k)表示k时刻由无人车自身装配的自主导航系统所提供的位置横坐标增量，Δpy(k)表示k时刻由无人车自身装配的自主导航系统所提供的位置纵坐标增量，W(k)表示k时刻系统过程噪声，wx(k)表示k时刻系统横坐标过程噪声，wy(k)表示k时刻系统纵坐标过程噪声，表示k时刻利用无人车自身i所接受到无人车j的蓝牙信号强度估计得到的无人车自身i和无人车j之间的相对距离，hj(X(k))表示k时刻无人车自身i与无人车j之间的相对距离真实值，表示k时刻无人车j的位置横坐标，表示k时刻无人车j的位置纵坐标，vj(k)表示k时刻无人车j的系统量测噪声，j＝1,...,m-1，m表示参与协同导航无人车的总个数；所述系统噪声包括系统过程噪声和系统量测噪声。


5.根据权利要求4所述的基于蓝牙的多无人车协同容错导航定位方法，其特征在于，
所述根据所述上一时刻主滤波器导航定位信息的滤波信息和所述上一时刻子滤波器的权重值确定子滤波器初始滤波信息，具体包括：
根据如下公式确定子滤波器初始滤波信息：






式中，Pj,k-1表示k-1时刻无人车j对应的子滤波器初始滤波误差的协方差矩阵，表示k-1时刻无人车j对应的子滤波器的权重值，Pg,k-1表示k-1时刻主滤波器滤波估计误差的协方差矩阵，表示k-1时刻无人车j对应的子滤波器初始滤波结果，表示k-1时刻主滤波器导航定位信息的滤波结果；所述上一时刻为k-1时刻；
所述根据所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器过程噪声协方差矩阵，具体包括：
根据如下公式确定当前时刻子滤波器过程噪声协方差矩阵：



式中，Qj,k表示k时刻无人车j对应的子滤波器过程噪声协方差矩阵，Q表示W(k)的协方差矩阵；...

【专利技术属性】
技术研发人员：梁源，徐兵，
申请(专利权)人：北京壹氢科技有限公司，
类型：发明
国别省市：北京;11