CN110933597B - 一种基于蓝牙的多无人车协同容错导航定位方法及系统 - Google Patents

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

Info

Publication number
CN110933597B
CN110933597B CN201911240466.8A CN201911240466A CN110933597B CN 110933597 B CN110933597 B CN 110933597B CN 201911240466 A CN201911240466 A CN 201911240466A CN 110933597 B CN110933597 B CN 110933597B
Authority
CN
China
Prior art keywords
filter
sub
unmanned vehicle
information
moment
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201911240466.8A
Other languages
English (en)
Other versions
CN110933597A (zh
Inventor
梁源
徐兵
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
HUNAN ORDNANCE XINCHENG MACHINE Co.,Ltd.
Hunan Weidao Technology Co., Ltd
Original Assignee
Beijing Yiqing Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Yiqing Technology Co ltd filed Critical Beijing Yiqing Technology Co ltd
Priority to CN201911240466.8A priority Critical patent/CN110933597B/zh
Publication of CN110933597A publication Critical patent/CN110933597A/zh
Application granted granted Critical
Publication of CN110933597B publication Critical patent/CN110933597B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/024—Guidance services
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/40—Services specially adapted for particular environments, situations or purposes for vehicles, e.g. vehicle-to-pedestrians [V2P]
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/80—Services using short range communication, e.g. near-field communication [NFC], radio-frequency identification [RFID] or low energy communication
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

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

Description

一种基于蓝牙的多无人车协同容错导航定位方法及系统
技术领域
本发明涉及导航定位技术领域，特别是涉及一种基于蓝牙的多无人车协同容错导航定位方法及系统。
背景技术
协同导航指的是多个导航系统之间存在可获取的相对观测信息或者间接相对观测信息，通过一定的信息交换融合手段，实现多个系统之间导航资源共享，最终使导航系统性能更优，精度更高。最初的协同导航是为了在没有路标的环境下，修正机器人长距离移动或是遇到不平坦路面导致轮子转动与路程不匹配导致的机器人内部的定位误差，其基本操作流程为：每个机器人重复移动和停止等待过程，在等待时作为另一个机器人的路标，在移动时以等待的机器人作为路标。协同导航在陆地机器人、空中无人机、无线传感网络、车辆定位、卫星导航以及自主式水下潜器(Autonomous Underwater Vehicle，AUV)集群作业方面有着广泛的应用。例如，在空中无人机方面，当GPS信号丢失时，无人机通过对其他相邻无人机的相对位置观察同时获得加速度计等传感器的信息，通过基于信息传播的协同定位方法获得自身的位置信息，从而调整无人机队形，使得其按照预定计划飞行。
可见，协同导航的应用关键在于如下两点：1.能否获得多个导航系统之间的相对观测信息；2.能否实现多个导航之间的信息传输与共享；所以协同导航的关键在于构建一套既能实现相对测距，又能实现信息共享的数据通信系统。其中，测距系统的准确性与可靠性将直接影响到协同导航的精度，而目前现有的测距手段，无论是基于视觉的，还是基于无线电的，都不可避免的会受到各种干扰带来的影响，导致测距精度下降甚至出现距离测量错误，例如，基于无线电的测距系统，在室内这种电磁信号相对复杂、电磁干扰较多的情况的下，极容易出现电磁波之间互相干扰而导致测距精度显著下降的情况。因此，在测距信息可靠性不足的情况下，提升多无人车的导航定位精度具有重要意义。
发明内容
本发明的目的是提供一种基于蓝牙的多无人车协同容错导航定位方法及系统，具有能够提升导航定位精度的优点。
为实现上述目的，本发明提供了如下方案：
一种基于蓝牙的多无人车协同容错导航定位方法，包括：
获取参与协同导航无人车的位置坐标和系统噪声；各无人车通过蓝牙实现导航信息共享，所述导航信息包括无人车的位置坐标；
根据所述位置坐标和所述系统噪声建立与协同导航无人车对应的子滤波器模型；
获取上一时刻主滤波器导航定位信息的滤波信息以及上一时刻子滤波器的权重值；所述主滤波器导航定位信息的滤波信息包括主滤波器滤波结果和主滤波器滤波估计误差的协方差矩阵；
根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器的滤波信息；所述当前时刻子滤波器的滤波信息包括子滤波器滤波结果和子滤波器滤波估计误差的协方差矩阵；
根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息。
可选的，所述根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息，之后还包括：
判断是否收到停止指令；
若收到停止指令，则输出当前时刻主滤波器滤波结果；
若未收到停止指令，则根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型更新上一时刻子滤波器的权重值，返回步骤“获取参与协同导航无人车的位置坐标和系统噪声”。
可选的，其特征在于，所述根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器的滤波信息，具体包括：
根据所述上一时刻主滤波器导航定位信息的滤波信息和所述上一时刻子滤波器的权重值确定子滤波器初始滤波信息；所述子滤波器初始滤波信息包括子滤波器初始滤波结果和子滤波器初始滤波误差的协方差矩阵；
根据所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器过程噪声协方差矩阵；
根据所述子滤波器初始滤波信息、所述当前时刻子滤波器过程噪声协方差矩阵和所述子滤波器模型确定当前时刻子滤波器的滤波信息。
可选的，所述根据所述位置坐标和所述系统噪声建立与协同导航无人车对应的子滤波器模型，具体包括：
根据如下公式建立与协同导航无人车对应的子滤波器模型：
X(k)＝ΦX(k-1)+U(k)+W(k)
Zj(k)＝hj(X(k))+vj(k)
其中，
Figure BDA0002306070550000031
Figure BDA0002306070550000032
Figure BDA0002306070550000033
Figure BDA0002306070550000034
Figure BDA0002306070550000035
上式中，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时刻系统纵坐标过程噪声，
Figure BDA0002306070550000041
表示k时刻利用无人车自身i所接受到无人车j的蓝牙信号强度估计得到的无人车自身i和无人车j之间的相对距离，hj(X(k))表示k时刻无人车自身i与无人车j之间的相对距离真实值，
Figure BDA0002306070550000042
表示k时刻无人车j的位置横坐标，
Figure BDA0002306070550000043
表示k时刻无人车j的位置纵坐标，vj(k)表示k时刻无人车j的系统量测噪声，j＝1,...,m-1，m表示参与协同导航无人车的总个数；所述系统噪声包括系统过程噪声和系统量测噪声。
可选的，
所述根据所述上一时刻主滤波器导航定位信息的滤波信息和所述上一时刻子滤波器的权重值确定子滤波器初始滤波信息，具体包括：
根据如下公式确定子滤波器初始滤波信息：
Figure BDA0002306070550000044
Figure BDA0002306070550000045
式中，Pj,k-1表示k-1时刻无人车j对应的子滤波器初始滤波误差的协方差矩阵，
Figure BDA0002306070550000046
表示k-1时刻无人车j对应的子滤波器的权重值，Pg,k-1表示k-1时刻主滤波器滤波估计误差的协方差矩阵，
Figure BDA0002306070550000047
表示k-1时刻无人车j对应的子滤波器初始滤波结果，
Figure BDA0002306070550000048
表示k-1时刻主滤波器导航定位信息的滤波结果；所述上一时刻为k-1时刻；
所述根据所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器过程噪声协方差矩阵，具体包括：
根据如下公式确定当前时刻子滤波器过程噪声协方差矩阵：
Figure BDA0002306070550000049
式中，Qj,k表示k时刻无人车j对应的子滤波器过程噪声协方差矩阵，Q表示W(k)的协方差矩阵；所述当前时刻为k时刻；
所述根据所述子滤波器初始滤波信息、所述当前时刻子滤波器过程噪声协方差矩阵和所述子滤波器模型确定当前时刻子滤波器的滤波信息，具体包括：
根据如下公式确定当前时刻子滤波器的滤波信息：
Figure BDA00023060705500000410
Figure BDA00023060705500000411
其中，
Figure BDA0002306070550000051
Pj,k/k-1＝ΦPj,k-1ΦT+Qj,k
Figure BDA0002306070550000052
式中，
Figure BDA0002306070550000053
表示k时刻无人车j对应的子滤波器滤波结果，
Figure BDA0002306070550000054
表示状态一步预测，
Figure BDA0002306070550000055
表示k时刻无人车j对应的子滤波器滤波增益，
Figure BDA0002306070550000056
表示
Figure BDA0002306070550000057
的映射结果，Pj,k表示k时刻无人车j对应的子滤波器滤波估计误差的协方差矩阵，I代表对应维度的单位矩阵，Hj代表
Figure BDA0002306070550000058
在
Figure BDA0002306070550000059
处的雅可比矩阵，Pj,k/k-1代表状态一步预测误差协方差矩阵，
Figure BDA00023060705500000510
Rj表示vj(k)的协方差矩阵。
可选的，所述根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息，具体包括：
根据如下公式确定当前时刻k主滤波器导航定位信息的滤波信息：
Figure BDA00023060705500000511
式中，
Figure BDA00023060705500000512
表示k时刻主滤波器滤波结果，Pg,k表示k时刻主滤波器滤波估计误差的协方差矩阵。
可选的，所述根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型更新上一时刻子滤波器的权重值，具体包括：
根据如下公式更新上一时刻子滤波器的权重值：
Figure BDA00023060705500000513
其中，
Figure BDA0002306070550000061
Figure BDA0002306070550000062
Figure BDA0002306070550000063
Figure BDA0002306070550000064
式中，
Figure BDA0002306070550000065
表示无人车j对应的子滤波器更新后的权重值，
Figure BDA0002306070550000066
表示k时刻无人车j对应的子滤波器故障诊断函数，q表示数据窗口内的时间，p表示数据窗口长度，
Figure BDA0002306070550000067
表示q时刻无人车j对应的子滤波器残差，
Figure BDA0002306070550000068
表示k时刻无人车j对应的子滤波器残差，
Figure BDA0002306070550000069
表示k时刻利用无人车自身i所接受到无人车j的蓝牙信号强度估计得到的无人车自身i和无人车j之间的相对距离，
Figure BDA00023060705500000610
表示状态一步预测
Figure BDA00023060705500000611
的映射结果，
Figure BDA00023060705500000612
表示q时刻无人车j对应的子滤波器滤波误差协方差矩阵，
Figure BDA00023060705500000613
表示k时刻无人车j对应的子滤波器滤波误差协方差矩阵，Hj代表
Figure BDA00023060705500000614
在
Figure BDA00023060705500000615
处的雅可比矩阵，Pj,k/k-1代表状态一步预测误差协方差矩阵，Rj表示vj(k)的协方差矩阵，vj(k)表示k时刻无人车j的系统量测噪声，j＝1,...,m-1，m表示参与协同导航无人车的总个数。
本发明还提供一种基于蓝牙的多无人车协同容错导航定位系统，包括：
位置坐标和系统噪声获取模块，用于获取参与协同导航无人车的位置坐标和系统噪声；各无人车通过蓝牙实现导航信息共享，所述导航信息包括无人车的位置坐标；
子滤波器模型建立模块，用于根据所述位置坐标和所述系统噪声建立与协同导航无人车对应的子滤波器模型；
主滤波器滤波信息和子滤波器权重值获取模块，用于获取上一时刻主滤波器导航定位信息的滤波信息以及上一时刻子滤波器的权重值；所述主滤波器导航定位信息的滤波信息包括主滤波器滤波结果和主滤波器滤波估计误差的协方差矩阵；
子滤波器的滤波信息确定模块，用于根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器的滤波信息；所述当前时刻子滤波器的滤波信息包括子滤波器滤波结果和子滤波器滤波估计误差的协方差矩阵；
主滤波器滤波信息更新模块，用于根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息。
可选的，所述基于蓝牙的多无人车协同容错导航定位系统，还包括：
判断模块，用于判断是否收到停止指令；若收到停止指令，则将指令发送至滤波结果输出模块；若未收到停止指令，则将指令发送至子滤波器权重值更新模块；
滤波结果输出模块，用于在收到停止指令时输出当前时刻主滤波器滤波结果；
子滤波器权重值更新模块，用于在未收到停止指令时根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型更新上一时刻子滤波器的权重值，然后将指令发送至位置坐标和系统噪声获取模块。
可选的，所述子滤波器的滤波信息确定模块，具体包括：
子滤波器初始滤波信息确定单元，用于根据所述上一时刻主滤波器导航定位信息的滤波信息和所述上一时刻子滤波器的权重值确定子滤波器初始滤波信息；所述子滤波器初始滤波信息包括子滤波器初始滤波结果和子滤波器初始滤波误差的协方差矩阵；
子滤波器过程噪声协方差矩阵确定单元，用于根据所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器过程噪声协方差矩阵；
子滤波器的滤波信息确定单元，用于根据所述子滤波器初始滤波信息、所述当前时刻子滤波器过程噪声协方差矩阵和所述子滤波器模型确定当前时刻子滤波器的滤波信息。
与现有技术相比，本发明的有益效果是：
本发明提出了一种基于蓝牙的多无人车协同容错导航定位方法及系统，通过各无人车通过蓝牙实现导航信息共享，获取参与协同导航无人车的位置坐标和系统噪声，根据位置坐标和系统噪声建立与协同导航无人车对应的子滤波器模型；根据上一时刻主滤波器导航定位信息的滤波信息、上一时刻子滤波器的权重值和子滤波器模型确定当前时刻子滤波器的滤波信息；再根据当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息，有效解决了多无人车协同导航定位的问题，能够在不明显增加硬件成本的基础上，显著的提升了多无人车的导航定位精度。
此外，根据上一时刻主滤波器导航定位信息的滤波信息、上一时刻子滤波器的权重值和子滤波器模型更新上一时刻子滤波器的权重值，实现了子滤波器的权重值动态更新，基于kalman滤波融合架构实现多源导航信息的数据融合，在该融合架构中，采用了基于χ2检验的残差检验方法进行蓝牙信息质量的衡量，可以有效的抑制蓝牙传输内容错误或者RSSI测量精度显著下降所导致的导航精度下降问题，同时保证了正常蓝牙信息可以有效参与导航信息融合，进一步提升了导航精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例中基于蓝牙的多无人车协同容错导航定位方法整体流程框图；
图2为本发明实施例中相对距离与RSSI的变化关系曲线示意图；
图3为本发明实施例中容错融合阶段的计算流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种能够提升导航定位精度的基于蓝牙的多无人车协同容错导航定位方法及系统，以实现多无人车之间的协同容错导航定位，提升多无人车的导航定位精度，同时通过在协同导航算法中引入容错算法模块，有效提升了导航系统的抗干扰/错误的能力。本发明的核心思想简要介绍如下：在全部的无人车上均安装蓝牙模块(包含收发功能)，在每个蓝牙通信周期内，对外发送自身的当前的定位结果，同时接受周围无人车(与自身无人车存在通信关系)的所发送的定位结果，其中，对于无人车自身而言，接受到的蓝牙信息包含两部分可利用信息：1.周围无人车的位置信息；2.当前接收到的蓝牙信号的信号强度(Received Signal Strength Indication，RSSI)，这两部分信息的使用方式如下：首先无人车利用蓝牙信号的RSSI信息进行相对距离估算，进而利用接收到的无人车的位置信息、估算相对距离信息与自身导航定位信息实现导航信息的容错融合，从而实现协同导航。其中导航信息的容错融合算法主要体现为：由于蓝牙本质是一种无线传输系统，在某些场景或者使用条件下会不可避免的出现故障或者受到电磁干扰的影响，从而导致蓝牙传输内容错误或者RSSI测量精度显著下降，而如果在融合计算过程中不加筛选的直接使用错误信息，则会导致当前无人车的导航定位精度显著下降，最终导致全部无人车导航精度出现显著下降；因此本方法中采用基于联邦滤波的卡尔曼滤波(kalman滤波)融合架构，采用多层次的滤波架构实现导航信息融合与故障隔离。在该滤波架构中，每一个接收到的蓝牙信息首先在其对应的子滤波器中进行局部滤波计算，同时在该子滤波器中，利用基于检验的残差检验方法进行蓝牙信息质量的衡量与诊断，如果诊断结果为不包含错误，则该子滤波器结果以正常的权重继续参与后续主滤波器滤波；如果诊断结果为包含错误，则该子滤波器结果以较低权重、甚至是0权重参与后续主滤波器滤波；通过采用动态调整子滤波器结果权重的方式，能够有效的抑制蓝牙传输内容错误或者RSSI测量精度显著下降所导致的导航精度下降问题，隔绝错误信息对导航精度的影响，达到提升导航精度和系统的鲁棒性的目的；同时可以保证正常蓝牙信息能够正常参与导航信息的滤波融合，从而提升导航定位精度。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
实施例
基于蓝牙的多无人车协同容错导航定位方法整体流程框图如图1所示(以其中一个无人车的运算流程为例)。
该方法分为四个阶段，分别为系统准备阶段、自主导航阶段、蓝牙通信阶段和容错融合阶段。
系统准备阶段：首先在全部的、参与协同导航的无人车上均安装具有收发功能的蓝牙传输模块，同时确保全部的无人车均具有自主导航能力(例如，装配有MEMS陀螺仪、MEMS加速度计、磁罗盘、码盘等)。
在本阶段中主要实现系统硬件条件的具备，需确保全部的无人车均具备算法所必须的功能：蓝牙通信能力和自主导航能力，其中蓝牙通信用于无人车之间信息的传输与无人车之间相对距离的估算，自主导航用于无人车自身位姿的自主更新。此外，为了尽可能的抑制通信误差/噪声的影响，蓝牙传输模块应尽可能安置于无人车顶部，使得该无人车与其他无人车进行通信时尽可能少的出现信号遮挡/衍射的情况。
自主导航阶段：全部无人车开始运行，利用自身的导航设备进行自主导航。
本阶段中指的自主导航，指的是无人车在不接受其它无人车信息的前提下，利用自身所装载的导航设备，对自身的导航信息(主要是位置信息)进行自主更新。
蓝牙通信阶段：各个无人车之间利用蓝牙实现导航信息的共享，同时利用接收到的蓝牙信息的RSSI完成对相对距离的估算。
为方便介绍，此处定义有四辆无人车a、b、c、d，各无人车对应的位置坐标为
Figure BDA0002306070550000101
(室内情况下，对于无人车而言，一般不需要考虑其高度信息)。其中，无人车a可以通过蓝牙通讯与无人车b、c、d进行数据通信，在无人车a上可以接收到的无人车b，c，d的蓝牙信号强度分别为：RSSIb、RSSIc、RSSId。
在本阶段中，主要利用RSSI值对距离进行估算，估算方法如下：由于蓝牙信号的强弱(RSSI值的大小)与距离之间的近似简化函数关系如下：
d＝d0+n lg(RSSI)
其中，d代表相对距离，d0与n分别为第一参数和第二参数，第一、二参数与蓝牙型号，发射功率等信息相关参数的参数，需要根据不同的设备、不同的使用环境进行调整，lg()代表求取以自然常数为底的对数值。以本发明中所使用的蓝牙为例，相对距离与RSSI的变化关系曲线示意图如图2所示。
利用上述函数关系可以分别估计得到对应的相对距离信息：
Figure BDA0002306070550000111
Figure BDA0002306070550000112
其中，
Figure BDA0002306070550000113
为无人车a和b之间的相对距离估算值，
Figure BDA0002306070550000114
为无人车a和c之间的相对距离估算值，
Figure BDA0002306070550000115
为无人车a和d之间的相对距离估算值。
容错融合阶段：各个无人车利用接收到的无人车的位置信息、估算的相对距离信息与自身导航定位信息实现导航信息的容错融合，输出最终自身的导航定位结果。容错融合阶段的计算流程图如图3所示。
为了介绍方便，以无人车a为例进行说明，首先对本阶段中所采用的基于联邦滤波的kalman滤波融合架构进行介绍，从图3中可以看出，滤波融合可以分为如下两个阶段依序进行：
①局部滤波阶段：
利用无人车a自身的导航定位信息，结合估算相对距离信息与周围无人车的导航定位信息，在子滤波器中进行滤波，获得子滤波器滤波结果，进而利用基于χ2检验的残差检验方法对滤波结果进行故障诊断，根据故障诊断结果自适应调整对应子滤波器滤波结果的权重；
②全局融合阶段：
接收全部子滤波器的滤波结果，根据其对应的权重信息进行全局融合，得到最终的导航定位结果。
首先对局部滤波阶段进行介绍，介绍滤波模型如下：
无人车b对应的子滤波器模型：
Figure BDA0002306070550000116
Figure BDA0002306070550000117
无人车c对应的子滤波器模型：
Figure BDA0002306070550000118
Figure BDA0002306070550000121
无人车d对应的子滤波器模型：
Figure BDA0002306070550000122
Figure BDA0002306070550000123
其中，k代表系统时刻，
Figure BDA0002306070550000124
为k时刻无人车a的位置，
Figure BDA0002306070550000125
代表k时刻由无人车a装配的自主导航系统所提供的位置增量，
Figure BDA0002306070550000126
代表k时刻的系统过程噪声。
Figure BDA0002306070550000127
代表k时刻利用无人车a所接受到的蓝牙信号强度RSSIb估计得到的无人车a和无人车b之间的相对距离；
Figure BDA0002306070550000128
为k时刻无人车b的位置，由蓝牙系统传输得到；vb(k)代表k时刻的对应无人车b的系统量测噪声。
Figure BDA0002306070550000129
代表k时刻利用无人车a所接受到的蓝牙信号强度RSSIc估计得到的无人车a和无人车c之间的相对距离；
Figure BDA00023060705500001210
为k时刻无人车c的位置，由蓝牙系统传输得到；vc(k)代表k时刻的对应无人车c的系统量测噪声。
Figure BDA00023060705500001211
代表k时刻利用无人车a所接受到的蓝牙信号强度RSSId估计得到的无人车a和无人车d之间的相对距离；
Figure BDA00023060705500001212
为k时刻无人车d的位置，由蓝牙系统传输得到；vd(k)代表k时刻的对应无人车d的系统量测噪声。其中，W(k)，vb(k)，vc(k)和vd(k)均为互不相关的高斯白噪声，且满足：W(k)的协方差矩阵为Q，vb(k)，vc(k)和vd(k)的协方差矩阵为Rb,Rc,Rd。
为方便下文进一步说明，将上述模型转换为如下形式：
无人车b对应的子滤波器模型：
X(k)＝ΦX(k-1)+U(k)+W(k)
Zb(k)＝hb(X(k))+vb(k)
无人车c对应的子滤波器模型：
X(k)＝ΦX(k-1)+U(k)+W(k)
Zc(k)＝hc(X(k))+vc(k)
无人车d对应的子滤波器模型：
X(k)＝ΦX(k-1)+U(k)+W(k)
Zd(k)＝hd(X(k))+vd(k)
在上述模型的基础上，进行多源导航信息的融合，首先做如下变量定义：设在k-1时刻的全局状态估计结果为
Figure BDA0002306070550000131
其估计误差的协方差矩阵为Pg,k-1，在k-1时刻子滤波器b,c,d对应的权重值为
Figure BDA0002306070550000132
在上述定义的基础上，开始进行滤波，流程如下：
1.滤波初始值分配：
根据k-1时刻的滤波结果，生成k时刻的滤波初值，分配原则如下：
Figure BDA0002306070550000133
Figure BDA0002306070550000134
Figure BDA0002306070550000135
Figure BDA0002306070550000136
Figure BDA0002306070550000137
Figure BDA0002306070550000138
其中，
Figure BDA0002306070550000139
分别代表子滤波器b,c,d在k时刻的滤波初始值，而Pb,k-1,Pc,k-1,Pd,k-1分别代表子滤波器b,c,d在k时刻的滤波初始值误差协方差矩阵。
2.过程噪声分配：
根据k-1时刻的滤波结果，生成k时刻的滤波参数值，分配原则如下：
Figure BDA0002306070550000141
Figure BDA0002306070550000142
Figure BDA0002306070550000143
其中，Qb,k,Qc,k,Qd,k分别代表子滤波器b,c,d在k时刻的过程噪声协方差矩阵分配结果。
3.每个子滤波器独立进行滤波计算
每个子滤波器利用自身观测信息进行滤波更新，所使用算法为扩展Kalman滤波(Extended Kalman Filter，EKF)算法，具体计算流程如下：
子滤波器b的计算过程：
Figure BDA0002306070550000144
Pb,k/k-1＝ΦPb,k-1ΦT+Qb,k
Figure BDA0002306070550000145
Figure BDA0002306070550000146
Figure BDA0002306070550000147
其中，
Figure BDA0002306070550000148
代表滤波增益，
Figure BDA0002306070550000149
代表状态一步预测，Pb,k/k-1代表状态一步预测误差协方差矩阵，
Figure BDA00023060705500001410
代表子滤波器b的滤波估计结果，Pb,k代表
Figure BDA00023060705500001411
估计误差的协方差矩阵；I代表对应维度的单位矩阵，Hb代表hb在
Figure BDA00023060705500001412
处的雅可比矩阵，雅可比矩阵是数学中常用的一种函数的一阶偏导数以一定方式排列成的矩阵。
子滤波器c的计算过程：
Figure BDA00023060705500001420
Pc,k/k-1＝ΦPc,k-1ΦT+Qc,k
Figure BDA00023060705500001413
Figure BDA00023060705500001414
Figure BDA00023060705500001415
其中，
Figure BDA00023060705500001416
代表滤波增益，
Figure BDA00023060705500001417
代表状态一步预测，Pc,k/k-1代表状态一步预测误差协方差矩阵，
Figure BDA00023060705500001418
代表子滤波器c的滤波估计结果，Pc,k代表
Figure BDA00023060705500001419
估计误差的协方差矩阵；I代表对应维度的单位矩阵，Hc代表hc在
Figure BDA0002306070550000151
处的雅可比矩阵。
子滤波器d的计算过程：
Figure BDA0002306070550000152
Pd,k/k-1＝ΦPd,k-1ΦT+Qd,k
Figure BDA0002306070550000153
Figure BDA0002306070550000154
Figure BDA0002306070550000155
其中，
Figure BDA0002306070550000156
代表滤波增益，
Figure BDA0002306070550000157
代表状态一步预测，Pd,k/k-1代表状态一步预测误差协方差矩阵，
Figure BDA0002306070550000158
代表子滤波器d的滤波估计结果，Pdk代表
Figure BDA0002306070550000159
估计误差的协方差矩阵；I代表对应维度的单位矩阵，Hd代表hd在
Figure BDA00023060705500001510
处的雅可比矩阵。
4.子滤波器故障诊断
利用各子滤波器的滤波结果，进行基于χ2检验的残差检验计算，计算方法如下，首先构建各子滤波器的残差向量与残差协方差矩阵：
子滤波器b：
Figure BDA00023060705500001511
Figure BDA00023060705500001512
子滤波器c：
Figure BDA00023060705500001513
Figure BDA00023060705500001514
子滤波器d：
Figure BDA00023060705500001515
Figure BDA00023060705500001516
基于上述残差向量与残差协方差矩阵构建相应的故障诊断函数：
Figure BDA00023060705500001517
Figure BDA00023060705500001518
Figure BDA0002306070550000161
其中，p为数据窗口长度，可是视无人车动态性能的强弱、滤波频率的快慢而进行调整，本发明中设计窗口长度p＝10。
5.子滤波器权重更新
根据上述结果进行权重更新计算，计算方法如下：
Figure BDA0002306070550000162
Figure BDA0002306070550000163
Figure BDA0002306070550000164
进一步进行权重归一化处理：
Figure BDA0002306070550000165
Figure BDA0002306070550000166
Figure BDA0002306070550000167
上述故障诊断原理简要介绍如下，以子滤波器b出现故障为例：子滤波器b出现故障，则
Figure BDA0002306070550000168
随之变大，则
Figure BDA0002306070550000169
随之减小，使得子滤波器b的状态信息在融合时所占的比例会自动减小，并会相应增加其它子滤波器的状态信息所占的比例。从而达到了故障诊断与隔离的作用。
至此完成了全部的子滤波器的计算，结束局部滤波阶段。
在局部滤波阶段结果的基础上，执行全局融合，即进行主滤波器信息融合，以得到全局最优估计，具体运算流程如下：
Figure BDA0002306070550000171
将局部滤波结果带入上式，即可获得第k时刻的滤波结果
Figure BDA0002306070550000172
其中，Pg,k代表最终主滤波器在k时刻的滤波误差的协方差矩阵，
Figure BDA0002306070550000173
代表最终主滤波器在k时刻的滤波结果。而由前文中定义的滤波模型可知，得到
Figure BDA0002306070550000174
后，便可以得到在k时刻，对于无人车a位置的在线估计结果，将该位置的在线估计结果作为最终导航定位结果进行输出。
本发明还提供一种基于蓝牙的多无人车协同容错导航定位系统，包括：
位置坐标和系统噪声获取模块，用于获取参与协同导航无人车的位置坐标和系统噪声；各无人车通过蓝牙实现导航信息共享，导航信息包括无人车的位置坐标。
子滤波器模型建立模块，用于根据位置坐标和系统噪声建立与协同导航无人车对应的子滤波器模型。
主滤波器滤波信息和子滤波器权重值获取模块，用于获取上一时刻主滤波器导航定位信息的滤波信息以及上一时刻子滤波器的权重值；主滤波器导航定位信息的滤波信息包括主滤波器滤波结果和主滤波器滤波估计误差的协方差矩阵。
子滤波器的滤波信息确定模块，用于根据上一时刻主滤波器导航定位信息的滤波信息、上一时刻子滤波器的权重值和子滤波器模型确定当前时刻子滤波器的滤波信息；当前时刻子滤波器的滤波信息包括子滤波器滤波结果和子滤波器滤波估计误差的协方差矩阵。
子滤波器的滤波信息确定模块，具体包括：
子滤波器初始滤波信息确定单元，用于根据上一时刻主滤波器导航定位信息的滤波信息和上一时刻子滤波器的权重值确定子滤波器初始滤波信息；子滤波器初始滤波信息包括子滤波器初始滤波结果和子滤波器初始滤波误差的协方差矩阵。
子滤波器过程噪声协方差矩阵确定单元，用于根据上一时刻子滤波器的权重值和子滤波器模型确定当前时刻子滤波器过程噪声协方差矩阵。
子滤波器的滤波信息确定单元，用于根据子滤波器初始滤波信息、当前时刻子滤波器过程噪声协方差矩阵和子滤波器模型确定当前时刻子滤波器的滤波信息。
主滤波器滤波信息更新模块，用于根据当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息。
判断模块，用于判断是否收到停止指令；若收到停止指令，则将指令发送至滤波结果输出模块；若未收到停止指令，则将指令发送至子滤波器权重值更新模块。
滤波结果输出模块，用于在收到停止指令时输出当前时刻主滤波器滤波结果。
子滤波器权重值更新模块，用于在未收到停止指令时根据上一时刻主滤波器导航定位信息的滤波信息、上一时刻子滤波器的权重值和子滤波器模型更新上一时刻子滤波器的权重值，然后将指令发送至位置坐标和系统噪声获取模块。
对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种基于蓝牙的多无人车协同容错导航定位方法，其特征在于，包括：
获取参与协同导航无人车的位置坐标和系统噪声；各无人车通过蓝牙实现导航信息共享，所述导航信息包括无人车的位置坐标；
根据所述位置坐标和所述系统噪声建立与协同导航无人车对应的子滤波器模型；
获取上一时刻主滤波器导航定位信息的滤波信息以及上一时刻子滤波器的权重值；所述主滤波器导航定位信息的滤波信息包括主滤波器滤波结果和主滤波器滤波估计误差的协方差矩阵；
根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器的滤波信息；所述当前时刻子滤波器的滤波信息包括子滤波器滤波结果和子滤波器滤波估计误差的协方差矩阵；
所述根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器的滤波信息，具体包括：
根据所述上一时刻主滤波器导航定位信息的滤波信息和所述上一时刻子滤波器的权重值确定子滤波器初始滤波信息；所述子滤波器初始滤波信息包括子滤波器初始滤波结果和子滤波器初始滤波误差的协方差矩阵；
根据所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器过程噪声协方差矩阵；
根据所述子滤波器初始滤波信息、所述当前时刻子滤波器过程噪声协方差矩阵和所述子滤波器模型确定当前时刻子滤波器的滤波信息；
根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息。
2.根据权利要求1所述的基于蓝牙的多无人车协同容错导航定位方法，其特征在于，所述根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息，之后还包括：
判断是否收到停止指令；
若收到停止指令，则输出当前时刻主滤波器滤波结果；
若未收到停止指令，则根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型更新上一时刻子滤波器的权重值，返回步骤“获取参与协同导航无人车的位置坐标和系统噪声”。
3.根据权利要求2所述的基于蓝牙的多无人车协同容错导航定位方法，其特征在于，所述根据所述位置坐标和所述系统噪声建立与协同导航无人车对应的子滤波器模型，具体包括：
根据如下公式建立与协同导航无人车对应的子滤波器模型：
X(k)＝ΦX(k-1)+U(k)+W(k)
Zj(k)＝hj(X(k))+vj(k)
其中，
Figure FDA0002715534370000021
Figure FDA0002715534370000022
Figure FDA0002715534370000023
Figure FDA0002715534370000024
Figure FDA0002715534370000025
上式中，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时刻系统纵坐标过程噪声，
Figure FDA0002715534370000026
表示k时刻利用无人车自身i所接受到无人车j的蓝牙信号强度估计得到的无人车自身i和无人车j之间的相对距离，hj(X(k))表示k时刻无人车自身i与无人车j之间的相对距离真实值，
Figure FDA0002715534370000027
表示k时刻无人车j的位置横坐标，
Figure FDA0002715534370000028
表示k时刻无人车j的位置纵坐标，vj(k)表示k时刻无人车j的系统量测噪声，j＝1,...,m-1，m表示参与协同导航无人车的总个数；所述系统噪声包括系统过程噪声和系统量测噪声。
4.根据权利要求3所述的基于蓝牙的多无人车协同容错导航定位方法，其特征在于，
所述根据所述上一时刻主滤波器导航定位信息的滤波信息和所述上一时刻子滤波器的权重值确定子滤波器初始滤波信息，具体包括：
根据如下公式确定子滤波器初始滤波信息：
Figure FDA0002715534370000031
Figure FDA0002715534370000032
式中，Pj,k-1表示k-1时刻无人车j对应的子滤波器初始滤波误差的协方差矩阵，
Figure FDA0002715534370000033
表示k-1时刻无人车j对应的子滤波器的权重值，Pg,k-1表示k-1时刻主滤波器滤波估计误差的协方差矩阵，
Figure FDA0002715534370000034
表示k-1时刻无人车j对应的子滤波器初始滤波结果，
Figure FDA0002715534370000035
表示k-1时刻主滤波器导航定位信息的滤波结果；所述上一时刻为k-1时刻；
所述根据所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器过程噪声协方差矩阵，具体包括：
根据如下公式确定当前时刻子滤波器过程噪声协方差矩阵：
Figure FDA0002715534370000036
式中，Qj,k表示k时刻无人车j对应的子滤波器过程噪声协方差矩阵，Q表示W(k)的协方差矩阵；所述当前时刻为k时刻；
所述根据所述子滤波器初始滤波信息、所述当前时刻子滤波器过程噪声协方差矩阵和所述子滤波器模型确定当前时刻子滤波器的滤波信息，具体包括：
根据如下公式确定当前时刻子滤波器的滤波信息：
Figure FDA0002715534370000037
Figure FDA0002715534370000038
其中，
Figure FDA0002715534370000039
Pj,k/k-1＝ΦPj,k-1ΦT+Qj,k
Figure FDA00027155343700000310
式中，
Figure FDA00027155343700000311
表示k时刻无人车j对应的子滤波器滤波结果，
Figure FDA00027155343700000312
表示状态一步预测，
Figure FDA00027155343700000313
表示k时刻无人车j对应的子滤波器滤波增益，
Figure FDA00027155343700000314
表示
Figure FDA00027155343700000315
的映射结果，Pj,k表示k时刻无人车j对应的子滤波器滤波估计误差的协方差矩阵，I代表对应维度的单位矩阵，Hj代表
Figure FDA0002715534370000041
在
Figure FDA0002715534370000042
处的雅可比矩阵，Pj,k/k-1代表状态一步预测误差协方差矩阵，
Figure FDA0002715534370000043
Rj表示vj(k)的协方差矩阵。
5.根据权利要求4所述的基于蓝牙的多无人车协同容错导航定位方法，其特征在于，所述根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息，具体包括：
根据如下公式确定当前时刻k主滤波器导航定位信息的滤波信息：
Figure FDA0002715534370000044
式中，
Figure FDA0002715534370000045
表示k时刻主滤波器滤波结果，Pg,k表示k时刻主滤波器滤波估计误差的协方差矩阵。
6.根据权利要求2所述的基于蓝牙的多无人车协同容错导航定位方法，其特征在于，所述根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型更新上一时刻子滤波器的权重值，具体包括：
根据如下公式更新上一时刻子滤波器的权重值：
Figure FDA0002715534370000046
其中，
Figure FDA0002715534370000047
Figure FDA0002715534370000048
Figure FDA0002715534370000049
Figure FDA00027155343700000410
式中，
Figure FDA00027155343700000411
表示无人车j对应的子滤波器更新后的权重值，
Figure FDA00027155343700000412
表示k时刻无人车j对应的子滤波器故障诊断函数，q表示数据窗口内的时间，p表示数据窗口长度，
Figure FDA0002715534370000051
表示q时刻无人车j对应的子滤波器残差，
Figure FDA0002715534370000052
表示k时刻无人车j对应的子滤波器残差，
Figure FDA0002715534370000053
表示k时刻利用无人车自身i所接受到无人车j的蓝牙信号强度估计得到的无人车自身i和无人车j之间的相对距离，
Figure FDA0002715534370000054
表示状态一步预测
Figure FDA0002715534370000055
的映射结果，
Figure FDA0002715534370000056
表示q时刻无人车j对应的子滤波器滤波误差协方差矩阵，
Figure FDA0002715534370000057
表示k时刻无人车j对应的子滤波器滤波误差协方差矩阵，Hj代表
Figure FDA0002715534370000058
在
Figure FDA0002715534370000059
处的雅可比矩阵，Pj,k/k-1代表状态一步预测误差协方差矩阵，Rj表示vj(k)的协方差矩阵，vj(k)表示k时刻无人车j的系统量测噪声，j＝1,...,m-1，m表示参与协同导航无人车的总个数。
7.一种基于蓝牙的多无人车协同容错导航定位系统，其特征在于，包括：
位置坐标和系统噪声获取模块，用于获取参与协同导航无人车的位置坐标和系统噪声；各无人车通过蓝牙实现导航信息共享，所述导航信息包括无人车的位置坐标；
子滤波器模型建立模块，用于根据所述位置坐标和所述系统噪声建立与协同导航无人车对应的子滤波器模型；
主滤波器滤波信息和子滤波器权重值获取模块，用于获取上一时刻主滤波器导航定位信息的滤波信息以及上一时刻子滤波器的权重值；所述主滤波器导航定位信息的滤波信息包括主滤波器滤波结果和主滤波器滤波估计误差的协方差矩阵；
子滤波器的滤波信息确定模块，用于根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器的滤波信息；所述当前时刻子滤波器的滤波信息包括子滤波器滤波结果和子滤波器滤波估计误差的协方差矩阵；
所述子滤波器的滤波信息确定模块，具体包括：
子滤波器初始滤波信息确定单元，用于根据所述上一时刻主滤波器导航定位信息的滤波信息和所述上一时刻子滤波器的权重值确定子滤波器初始滤波信息；所述子滤波器初始滤波信息包括子滤波器初始滤波结果和子滤波器初始滤波误差的协方差矩阵；
子滤波器过程噪声协方差矩阵确定单元，用于根据所述上一时刻子滤波器的权重值和所述子滤波器模型确定当前时刻子滤波器过程噪声协方差矩阵；
子滤波器的滤波信息确定单元，用于根据所述子滤波器初始滤波信息、所述当前时刻子滤波器过程噪声协方差矩阵和所述子滤波器模型确定当前时刻子滤波器的滤波信息；
主滤波器滤波信息更新模块，用于根据所述当前时刻子滤波器的滤波信息确定当前时刻主滤波器导航定位信息的滤波信息。
8.根据权利要求7所述的基于蓝牙的多无人车协同容错导航定位系统，其特征在于，还包括：
判断模块，用于判断是否收到停止指令；若收到停止指令，则将指令发送至滤波结果输出模块；若未收到停止指令，则将指令发送至子滤波器权重值更新模块；
滤波结果输出模块，用于在收到停止指令时输出当前时刻主滤波器滤波结果；
子滤波器权重值更新模块，用于在未收到停止指令时根据所述上一时刻主滤波器导航定位信息的滤波信息、所述上一时刻子滤波器的权重值和所述子滤波器模型更新上一时刻子滤波器的权重值，然后将指令发送至位置坐标和系统噪声获取模块。
CN201911240466.8A 2019-12-06 2019-12-06 一种基于蓝牙的多无人车协同容错导航定位方法及系统 Active CN110933597B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911240466.8A CN110933597B (zh) 2019-12-06 2019-12-06 一种基于蓝牙的多无人车协同容错导航定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911240466.8A CN110933597B (zh) 2019-12-06 2019-12-06 一种基于蓝牙的多无人车协同容错导航定位方法及系统

Publications (2)

Publication Number Publication Date
CN110933597A CN110933597A (zh) 2020-03-27
CN110933597B true CN110933597B (zh) 2021-02-19

Family

ID=69858108

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911240466.8A Active CN110933597B (zh) 2019-12-06 2019-12-06 一种基于蓝牙的多无人车协同容错导航定位方法及系统

Country Status (1)

Country Link
CN (1) CN110933597B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113108797B (zh) * 2021-04-19 2024-04-12 上海商汤临港智能科技有限公司 车辆定位方法、装置、电子设备以及计算机可读存储介质
CN113514052A (zh) * 2021-06-10 2021-10-19 西安因诺航空科技有限公司 一种多机协同高精度有源目标定位方法及系统
CN114205787A (zh) * 2021-11-17 2022-03-18 北京牛电科技有限责任公司 电动车的无线定位方法、装置、电子设备和存储介质
CN114895241B (zh) * 2022-05-09 2024-05-03 知微空间智能科技(苏州)有限公司 一种基于数据和模型联合驱动的弹性融合定位方法和装置
CN116704388B (zh) * 2023-08-09 2023-11-03 南京航空航天大学 一种基于视觉的多无人机协同目标定位方法

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102052924A (zh) * 2010-11-25 2011-05-11 哈尔滨工程大学 一种小型水下机器人组合导航定位方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN100575877C (zh) * 2007-12-12 2009-12-30 南京航空航天大学 基于多信息融合的航天器组合导航方法
CN101598556B (zh) * 2009-07-15 2011-05-04 北京航空航天大学 一种未知环境下无人机视觉/惯性组合导航方法
EP2337242B1 (en) * 2009-12-18 2014-05-14 Alcatel Lucent A method for transmission of data signals using an analogue radio frequency power amplifier, a transmitting device and a receiving device therefor
CN104866669B (zh) * 2015-05-25 2017-11-24 北京理工大学 一种分析惯性导航系统可观测性的方法
CN105588566B (zh) * 2016-01-08 2019-09-13 重庆邮电大学 一种基于蓝牙与mems融合的室内定位系统及方法
CN109084760B (zh) * 2018-07-11 2020-06-26 北京壹氢科技有限公司 一种楼宇间导航系统
CN109901108A (zh) * 2019-03-19 2019-06-18 南京航空航天大学 一种基于后验线性化置信传播的编队无人机协同定位方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102052924A (zh) * 2010-11-25 2011-05-11 哈尔滨工程大学 一种小型水下机器人组合导航定位方法

Also Published As

Publication number Publication date
CN110933597A (zh) 2020-03-27

Similar Documents

Publication Publication Date Title
CN110933597B (zh) 一种基于蓝牙的多无人车协同容错导航定位方法及系统
CN108731667B (zh) 用于确定无人驾驶车辆的速度和位姿的方法和装置
US10102694B2 (en) Systems and methods for monitoring and reporting road quality
JP4853490B2 (ja) 移動体間干渉測位システム、装置及び方法
CN104729506B (zh) 一种视觉信息辅助的无人机自主导航定位方法
US10175043B2 (en) Toroidal shape recognition for automatic compass calibration systems and methods
CN104296776A (zh) 用于磁力计校准和补偿的系统和方法
CN112284384B (zh) 考虑量测异常的集群式多深海潜航器的协同定位方法
CN110954132B (zh) Grnn辅助自适应卡尔曼滤波进行导航故障识别的方法
CN113063425B (zh) 车辆定位方法、装置、电子设备及存储介质
Chen et al. A novel fusion methodology to bridge GPS outages for land vehicle positioning
WO2018225067A1 (en) Fusion and calibration of sensor signals in a moving vehicle
CN101122637A (zh) 一种sar运动补偿用sins/gps组合导航自适应降维滤波方法
CN111750854A (zh) 车辆定位方法、装置、系统及存储介质
CN114623823A (zh) 一种融合里程计的uwb多模态定位系统、方法及装置
KR20130036145A (ko) 이동 정보 결정 장치, 수신기 및 그에 의한 방법
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN110873813B (zh) 一种水流速度估算方法、组合导航方法及装置
CN117320148A (zh) 多源数据融合定位方法、系统、电子设备及存储介质
JP2022537361A (ja) ドリフト補正を用いるモーションセンサを使用した相対位置追跡
CN111829511A (zh) 一种基于m估计的auv组合导航方法及系统
CN108240807B (zh) 估计空间占据的方法
CN104792336B (zh) 一种飞行状态测量方法及装置
CN109916401B (zh) 采用ls-svm辅助ekf滤波方法的分布式无缝紧组合导航方法及系统
CN109752006B (zh) 一种非完备外测数据在实时滤波中的使用方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right

Effective date of registration: 20220119

Address after: 412000 3rd floor, complex building, entrepreneurship and Innovation Park, No. 88, Changtang Road, Yunlong demonstration zone, Zhuzhou City, Hunan Province

Patentee after: HUNAN ORDNANCE XINCHENG MACHINE Co.,Ltd.

Patentee after: Hunan Weidao Technology Co., Ltd

Address before: Room 315, 3 / F, building 10, yard 1, JinFang Road, Chaoyang District, Beijing 100020

Patentee before: BEIJING YIQING TECHNOLOGY Co.,Ltd.

TR01 Transfer of patent right