CN113155124A - 一种多源辅助导航方法及装置 - Google Patents

一种多源辅助导航方法及装置 Download PDF

Info

Publication number
CN113155124A
CN113155124A CN202110461932.6A CN202110461932A CN113155124A CN 113155124 A CN113155124 A CN 113155124A CN 202110461932 A CN202110461932 A CN 202110461932A CN 113155124 A CN113155124 A CN 113155124A
Authority
CN
China
Prior art keywords
inertial navigation
satellite
positioning
formula
visual
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.)
Granted
Application number
CN202110461932.6A
Other languages
English (en)
Other versions
CN113155124B (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.)
Hanwo Zhihang Technology Yuxi Co ltd
Original Assignee
Hanwo Zhihang Technology Yuxi 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 Hanwo Zhihang Technology Yuxi Co ltd filed Critical Hanwo Zhihang Technology Yuxi Co ltd
Priority to CN202110461932.6A priority Critical patent/CN113155124B/zh
Publication of CN113155124A publication Critical patent/CN113155124A/zh
Application granted granted Critical
Publication of CN113155124B publication Critical patent/CN113155124B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/165—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 combined with non-inertial navigation instruments
    • 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
    • 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
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement

Landscapes

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

Abstract

本发明提供了一种多源辅助导航方法，通过至少一个图像获取装置、至少一个惯性传感器采集的数据，计算视觉惯导定位坐标
Figure DDA0003042689700000011
视觉惯导定位权重Wv；通过至少一个卫星信号接收装置、至少一个惯性传感器采集的数据，计算卫星惯导定位坐标
Figure DDA0003042689700000012
卫星惯导定位权重WG；通过所述视觉惯导定位坐标
Figure DDA0003042689700000013
视觉惯导定位权重Wv、卫星惯导定位坐标
Figure DDA0003042689700000014
卫星惯导定位权重WG，计算融合定位坐标
Figure DDA0003042689700000015
通过地图数据、所述融合定位坐标
Figure DDA0003042689700000016
进行行进中导航。本发明具有动态调整视觉惯导定位坐标、卫星惯导定位坐标在融合定位坐标中的占比，进而保证融合定位坐标在室内室外导航环境切换过程中始终保持较高的精确度。本发明还提供了一种用于实现多源辅助导航方法的导航装置。

Description

一种多源辅助导航方法及装置
技术领域
本发明涉及导航定位领域，具体涉及一种多源辅助导航方法及装置。
背景技术
当前的导航***已不再局限于对车辆等工具的路径规划、方向指引，针对行人的步行导航也逐步推广，行人在不熟悉的场景或环境中徒步行走时，步行导航能够极大地避免迷路的发生，减少路线错误。此外，步行导航在盲人的行走辅助当中也扮演了越来越重要的角色。
步行导航的场景不仅局限于露天场景，室内场景同样占有十分重要的比例。但是，由于当前的导航方式主要依靠卫星信号进行定位，在室内场景下的定位效果较差，极大地影响了室内的步行导航的效果。
为了解决上述问题，文献CN111024072A公开了一种基于深度学习的卫星地图辅助导航定位方法，包括：步骤一，根据惯性导航设备的漂移误差，在飞行区域内选取图像地标点。步骤二，根据地标点处的卫星遥感图像建立目标检测数据集，并用改进的YOLOv3进行训练。步骤三，飞行过程中，根据训练好的模型及模型参数，检测位置信息已知的地标点，经坐标转换关系得到无人机的位置。步骤四，采用卡尔曼滤波将其与惯导***输出的位置参数进行融合，实现组合导航。该方法应用于无人机上，目的在于解决全球导航卫星***失锁时的导航定位问题，对无人机全球导航卫星***失锁的情况，利用目标检测实现辅助导航定位，有效地解决了单惯导***的累积漂移误差问题。
但是，上述技术方案需要事先获取地标点的位置，并使用地标点的遥感信息训练地标点的识别模型，从而识别地标点并以此进行坐标变换，从而获得无人机的位置，在行人的导航中，由于行人所处场景元素十分丰富，且要求在局部位置具有较高的定位精度，使用大量的局部地标点训练识别模型存在较大困难，难以应用于行人的导航当中。
视觉SLAM(simultaneous localization and mapping,同步定位与建图)是使用图像传感器获取的图像数据确定当前位置的方法，能够有效应对无卫星信号的定位场景。文献CN109946732B公开了一种基于多传感器数据融合的无人车定位方法，该方法充分利用厘米级高精度地图、视觉和激光雷达传感器数据，在卫星定位信号被遮挡区域实现高精度定位。通过实时监测卫星定位数据有效性，不满足无人车定位要求时，利用厘米级高精度地图、激光雷达、里程计、视觉多传感器数据融合对卫星定位数据进行辅助，同时，对惯性导航累积误差进行修正，提高定位***的可靠性、连续性和精度，且不需要增加传感器硬件成本。
但是，上述技术方案存在以下问题：需要通过厘米级高精度地图来对无人车的位置、航向、姿态等状态进行校正，在应用于行人导航时需要巨量的地图数据，严重影响导航的灵敏度，从而导致导航无法正常使用；上述技术方案在判断卫星信号弱时，直接放弃将卫星信号参与至导航当中，当卫星信号强弱变动时，需要在有卫星信号参与的导航模式与无卫星信号参与的导航模式之间进行切换，容易因两种导航方式的位置差异，影响导航路线的规划和导航精度。
可见，现有技术方案难以在室内室外切换过程中保持导航信号稳定，容易对导航效果产生不利影响。
发明内容
针对现有技术存在的无法在卫星信号强弱变动过程中保持稳定导航的问题，本发明提供了一种多源辅助导航方法。
为了实现上述目的，本发明所采用的技术方案如下：一种多源辅助导航方法，通过至少一个图像获取装置、至少一个惯性传感器采集的数据，计算视觉惯导定位坐标
Figure BDA0003042689680000021
视觉惯导定位权重Wv；
通过至少一个卫星信号接收装置、至少一个惯性传感器采集的数据，计算卫星惯导定位坐标
Figure BDA0003042689680000022
卫星惯导定位权重WG；
通过所述视觉惯导定位坐标
Figure BDA0003042689680000023
视觉惯导定位权重Wv、卫星惯导定位坐标
Figure BDA0003042689680000024
卫星惯导定位权重WG，计算融合定位坐标
Figure BDA0003042689680000025
通过地图数据、所述融合定位坐标
Figure BDA0003042689680000026
进行路径规划、导航。
优选地，所述视觉惯导定位坐标
Figure BDA0003042689680000027
通过如下方式计算：
通过式1计算k时间点的所述视觉惯导定位坐标
Figure BDA0003042689680000028
的状态估计量
Figure BDA0003042689680000029
Figure BDA00030426896800000210
式1中，
Figure BDA00030426896800000211
为k-1时间点的视觉惯导定位坐标，uk-1为k-1时间点的所述惯性传感器采集的信号向量，A(v)为预设的视觉惯导状态更新矩阵，B(v)为预设的惯导输入信号变换矩阵；
通过式2、式3计算k时间点的视觉惯导卡尔曼增益K(v)k：
Figure BDA00030426896800000212
Figure BDA00030426896800000213
式2中，
Figure BDA0003042689680000031
为k时间点的所述
Figure BDA0003042689680000032
的协方差矩阵P(v)k的估计量，P(v)k-1为k-1时间点的所述
Figure BDA0003042689680000033
的协方差矩阵，Q(v)为预设的视觉惯导噪声矩阵；式3中，H(v)为预设的视觉惯导观测坐标变换矩阵；R(v)为视觉量测噪声矩阵；
通过式4计算k时间点的所述视觉惯导定位坐标
Figure BDA0003042689680000034
Figure BDA0003042689680000035
其中，Z(v)k为k时间点的视觉定位坐标；
通过式5计算k时间点的所述视觉惯导定位坐标
Figure BDA0003042689680000036
的协方差矩阵P(v)k，所述P(v)k用于k+1时间点的视觉惯导定位坐标
Figure BDA0003042689680000037
的计算：
Figure BDA0003042689680000038
优选地，所述卫星惯导定位坐标
Figure BDA0003042689680000039
通过如下方式计算：
通过式6计算k时间点的所述卫星惯导定位坐标
Figure BDA00030426896800000310
的状态估计量
Figure BDA00030426896800000311
Figure BDA00030426896800000312
式6中，
Figure BDA00030426896800000313
为k-1时间点的卫星惯导定位坐标，uk-1为k-1时间点的所述惯性传感器采集的信号向量，A(G)为预设的卫星惯导状态更新矩阵，B(G)为预设的惯导输入信号变换矩阵；
通过式7、式8计算k时间点的卫星惯导卡尔曼增益K(G)k：
Figure BDA00030426896800000314
Figure BDA00030426896800000315
式7中，
Figure BDA00030426896800000316
为k时间点的所述
Figure BDA00030426896800000317
的协方差矩阵P(G)k的估计量，P(G)k-1为k-1时间点的所述
Figure BDA00030426896800000318
的协方差矩阵，Q(G)为预设的卫星惯导噪声矩阵；式8中，H(G)为预设的卫星惯导观测坐标变换矩阵；R(G)为卫星量测噪声矩阵；
通过式9计算k时间点的所述卫星惯导定位坐标
Figure BDA00030426896800000319
Figure BDA00030426896800000320
其中，Z(G)k为k时间点的卫星定位坐标；
通过式10计算k时间点的所述卫星惯导定位坐标
Figure BDA00030426896800000321
的协方差矩阵P(G)k，所述P(G)k用于k+1时间点的卫星惯导定位坐标
Figure BDA00030426896800000322
的计算：
Figure BDA00030426896800000323
更进一步地，所述Wv通过式11计算：
Wv＝1-tr(P(v)k)/(tr(P(v)k)+tr(P(G)k))式11
更进一步地，所述WG通过式12计算：
WG＝1-tr(P(G)k)/(tr(P(v)k)+tr(P(G)k))式12
优选地，所述视觉惯导定位权重Wv、卫星惯导定位权重WG满足如下关系：Wv+WG＝1。
进一步地，所述融合定位坐标
Figure BDA0003042689680000041
通过式13计算：
Figure BDA0003042689680000042
优选地，所述辅助导航方法还包括：通过至少一个输出装置输出路线信息；所述输出装置为声音发生装置、振动发生装置、发热装置、有线通信连接端口、无线通信发射器的至少之一。
优选地，通过所述
Figure BDA0003042689680000043
地图数据、至少一个图像获取装置采集的数据，判断是否需要躲避；当需要躲避时，通过如下方式输出所述路线信息：
当所述输出装置为声音发生装置时，通过所述声音发生装置发出提示音；
当所述输出装置为振动发生装置时，通过所述振动发生装置发出振动；
当所述输出装置为发热装置时，通过所述发热装置发出热量；
当所述输出装置为有线通信连接端口时，通过有线通信连接端口向外部设备传输路线信息；
当所述输出装置为无线通信发射器时，通过无线通信发射器向外部设备传输路线信息。
更进一步地，所述提示音为语音、有规律且短暂的间隔的声音的其中之一；所述振动发生装置为多个，所述振动为靠近需躲避位置的所述振动发生装置产生；所述发热装置为多个，发出热量的所述发热装置为靠近需躲避位置的所述发热装置。
在上述更进一步方案的基础上，再进一步地，所述发热装置发出的热量引发的温度最高不超过45摄氏度。
优选地，所述图像获取装置为双目摄像头，所述双目摄像头采集到的数据被用于构建三维图像信息，所述三维图像信息用于计算所述路线信息。
所述地图数据包括局部地图、全局地图。
本发明还提供了一种多源辅助导航装置，包括至少一个存储器、至少一个处理器、至少一个图像获取装置、至少一个惯性传感器、至少一个卫星信号接收装置、至少一个输出装置，所述处理器通过运行所述存储器中存储的可执行程序，使用至少一个所述图像获取装置、至少一个所述惯性传感器、至少一个所述卫星信号接收装置获取的数据，实现前述的多源辅助导航方法。
本发明具有以下有益效果：通过将视觉SLAM、惯性导航、卫星导航技术相融合，通过图像获取装置和惯性传感器计算视觉惯导定位坐标
Figure BDA0003042689680000051
通过卫星信号接收装置和惯性传感器计算卫星惯导定位坐标
Figure BDA0003042689680000052
进而将视觉惯导定位坐标
Figure BDA0003042689680000053
卫星惯导定位坐标
Figure BDA0003042689680000054
进行融合以计算融合定位坐标
Figure BDA0003042689680000055
通过视觉惯导定位权重Wv、卫星惯导定位权重WG调整视觉惯导定位坐标
Figure BDA0003042689680000056
卫星惯导定位坐标
Figure BDA0003042689680000057
在融合定位坐标
Figure BDA0003042689680000058
中的权重和作用，从而在视觉惯导定位坐标
Figure BDA0003042689680000059
卫星惯导定位坐标
Figure BDA00030426896800000510
的定位精度发生相对变化时，动态调整二者的占比，进而保证室内室外导航环境切换过程中，融合定位坐标
Figure BDA00030426896800000511
精确度始终保持较高水平。
此外，本发明的优选方案还具有以下有益效果：通过声音发生装置、振动发生装置、发热装置的多种方式对行人进行提醒，输出路线信息，从而从多种感官提示路线信息，方便本发明对行人，尤其是盲人的导航效果。
附图说明
图1为本发明的多源辅助导航方法的效果示意图。
具体实施方式
下面，结合附图具体实施方式，对本发明作进一步描述。
需要说明的是，在不相冲突的前提下，以下所描述的各个实施例之间或各个技术特征之间可以任意组合形成新的实施例。
实施例一：
一种多源辅助导航方法，通过至少一个图像获取装置、至少一个惯性传感器采集的数据，计算视觉惯导定位坐标
Figure BDA00030426896800000512
视觉惯导定位权重Wv；
通过至少一个卫星信号接收装置、至少一个惯性传感器采集的数据，计算卫星惯导定位坐标
Figure BDA00030426896800000513
卫星惯导定位权重WG；
通过所述视觉惯导定位坐标
Figure BDA00030426896800000514
视觉惯导定位权重Wv、卫星惯导定位坐标
Figure BDA00030426896800000515
卫星惯导定位权重WG，计算融合定位坐标
Figure BDA00030426896800000516
通过地图数据、所述融合定位坐标
Figure BDA00030426896800000517
进行路径规划、导航。
通过使用视觉惯导定位坐标
Figure BDA00030426896800000518
视觉惯导定位权重Wv、卫星惯导定位坐标
Figure BDA00030426896800000519
卫星惯导定位权重WG，计算融合定位坐标
Figure BDA00030426896800000520
当待定位装置在室外和室内之间进行移动时，通过计算视觉惯导定位权重Wv与卫星惯导定位权重WG，来量化视觉惯导定位信号与卫星惯导定位信号的准确度，通过视觉惯导定位权重Wv与卫星惯导定位权重WG来调整视觉惯导定位坐标
Figure BDA00030426896800000521
与卫星惯导定位坐标
Figure BDA00030426896800000522
在融合定位坐标
Figure BDA00030426896800000523
计算中的占比，从而使融合定位坐标
Figure BDA0003042689680000061
的计算更加趋向于准确度较高的定位信号的定位结果，从而使融合定位坐标
Figure BDA0003042689680000062
的准确度能够式中保持在较高的水平，实现室内室外定位环境切换过程中的较高精度的定位。
VO(Visual Odometry，视觉里程计)能够基于相机获得的图像数据，通过在像素平面捕捉目标、确定相差从而计算出目标物体的三维坐标，在低速运动的场景下，能够较为准确地定位。但是，在高速运动或运动状态剧烈改变的场景下，VO难以获得较为准确的定位结果；同时，在低速运动的场景下，当光照或背景存在干扰时，同样会影响到VO的定位精度。由于IMU(Inertial Measurement Unit，惯性测量单元)能够高速、实时地测量物体的运动状态或运动状态的改变，对于快速运动或运动状态剧烈变化等情况具有良好的适应性。因此，把VO与IMU进行了融合，从而得到了VIO(Visual Inertial Odometry，视觉惯性里程计)。如图1中的方格线显示仅使用VIO进行局部优化的轨迹。可见，通过VIO能够一定程度上实现在室内或者无卫星信号的场所里的导航与定位。
在本实施例中，通过图像获取装置、惯性传感器组成VIO，作为视觉惯导***。在此基础上，所述视觉惯导定位坐标
Figure BDA0003042689680000063
通过如下方式计算：
通过式1计算k时间点的所述视觉惯导定位坐标
Figure BDA0003042689680000064
的状态估计量
Figure BDA0003042689680000065
Figure BDA0003042689680000066
式1中，
Figure BDA0003042689680000067
为k-1时间点的视觉惯导定位坐标，uk-1为k-1时间点的所述惯性传感器采集的信号向量，A(v)为预设的视觉惯导状态更新矩阵，B(v)为预设的惯导输入信号变换矩阵；
通过式2、式3计算k时间点的视觉惯导卡尔曼增益K(v)k：
Figure BDA0003042689680000068
Figure BDA0003042689680000069
式2中，
Figure BDA00030426896800000610
为k时间点的所述
Figure BDA00030426896800000611
的协方差矩阵P(v)k的估计量，P(v)k-1为k-1时间点的所述
Figure BDA00030426896800000612
的协方差矩阵，Q(v)为预设的视觉惯导噪声矩阵，通过对所述图像获取装置、惯性传感器进行多次测试获得；式3中，H(v)为预设的视觉惯导观测坐标变换矩阵；R(v)为视觉量测噪声矩阵，通过所述图像获取装置获取到的数据的噪声获得；
通过式4计算k时间点的所述视觉惯导定位坐标
Figure BDA00030426896800000613
Figure BDA00030426896800000614
其中，Z(v)k为k时间点的视觉定位坐标，通过所述图像获取装置获得；
通过式5计算k时间点的所述视觉惯导定位坐标
Figure BDA0003042689680000071
的协方差矩阵P(v)k，所述P(v)k用于k+1时间点的视觉惯导定位坐标
Figure BDA0003042689680000072
的计算：
Figure BDA0003042689680000073
通过上述技术方案对视觉惯导定位坐标
Figure BDA0003042689680000074
进行迭代计算，通过计算视觉惯导卡尔曼增益K(v)k，进而通过式4对视觉惯导定位坐标
Figure BDA0003042689680000075
的状态估计量
Figure BDA0003042689680000076
进行修正和调整，改变基于惯性传感器和上一时间点的定位坐标获取到的
Figure BDA0003042689680000077
通过图像获取装置获得的Z(v)k在
Figure BDA0003042689680000078
的计算中的重要度，使
Figure BDA0003042689680000079
的计算更加倾向于具有较高精度、较低噪声的定位坐标，从而使得修正后的视觉惯导定位坐标
Figure BDA00030426896800000710
保持较高的精度，以保证视觉和惯导的融合效果。
通过VIO能很好的实现室内或者无卫星信号的场所里的导航与定位。但是，因为VIO在缺少GNSS(Global Navigation Satellite System，全球导航卫星***)卫星信号绝对地理坐标的精度补偿下，长时间工作会发生位置偏移，因此，需要借助GNSS对VIO进行修正。
在本实施例中，通过卫星信号接收装置、惯性传感器组成卫星惯导***。在此基础上，所述卫星惯导定位坐标
Figure BDA00030426896800000711
通过如下方式计算：
通过式6计算k时间点的所述卫星惯导定位坐标
Figure BDA00030426896800000712
的状态估计量
Figure BDA00030426896800000713
Figure BDA00030426896800000714
式6中，
Figure BDA00030426896800000715
为k-1时间点的卫星惯导定位坐标，uk-1为k-1时间点的所述惯性传感器采集的信号向量，A(G)为预设的卫星惯导状态更新矩阵，B(G)为预设的惯导输入信号变换矩阵；
通过式7、式8计算k时间点的卫星惯导卡尔曼增益K(G)k：
Figure BDA00030426896800000716
Figure BDA00030426896800000717
式7中，
Figure BDA00030426896800000718
为k时间点的所述
Figure BDA00030426896800000719
的协方差矩阵P(G)k的估计量，P(G)k-1为k-1时间点的所述
Figure BDA00030426896800000720
的协方差矩阵，Q(G)为预设的卫星惯导噪声矩阵，通过对所述卫星信号接收装置、惯性传感器进行多次测试获得；式8中，H(G)为预设的卫星惯导观测坐标变换矩阵；R(G)为卫星量测噪声矩阵，通过所述卫星信号接收装置获取到的数据的噪声获得；
通过式9计算k时间点的所述卫星惯导定位坐标
Figure BDA00030426896800000721
Figure BDA00030426896800000722
其中，Z(G)k为k时间点的卫星定位坐标，通过所述卫星信号接收装置获得；
通过式10计算k时间点的所述卫星惯导定位坐标
Figure BDA0003042689680000081
的协方差矩阵P(G)k，所述P(G)k用于k+1时间点的卫星惯导定位坐标
Figure BDA0003042689680000082
的计算：
Figure BDA0003042689680000083
通过上述技术方案对卫星惯导定位坐标
Figure BDA0003042689680000084
进行迭代计算，通过计算卫星惯导卡尔曼增益K(G)k，进而通过式9对卫星惯导定位坐标
Figure BDA0003042689680000085
的状态估计量
Figure BDA0003042689680000086
进行修正和调整，改变基于惯性传感器和上一时间点的定位坐标获取到的
Figure BDA0003042689680000087
通过图像获取装置获得的Z(G)k在
Figure BDA0003042689680000088
的计算中的重要度，使
Figure BDA0003042689680000089
的计算更加倾向于具有较高精度、较低噪声的定位坐标，从而使得修正后的卫星惯导定位坐标
Figure BDA00030426896800000810
保持较高的精度，以保证卫星和惯导的融合效果。
在上述技术方案的基础上，更进一步地，所述Wv通过式11计算：
Wv＝1-tr(P(v)k)/(tr(P(v)k)+tr(P(G)k))式11
更进一步地，所述WG通过式12计算：
WG＝1-tr(P(G)k)/(tr(P(v)k)+tr(P(G)k))式12
优选地，所述视觉惯导定位权重Wv、卫星惯导定位权重WG满足如下关系：Wv+WG＝1。
基于本发明的构思，所述视觉惯导定位权重Wv、卫星惯导定位权重WG之和可以为其他的固定值。
在上述技术方案的基础上，将VIO与GNSS进行了融合，得到了GVIO(GNSS VisualInertial Odometry，卫星视觉惯性里程计)，从而使得融合定位坐标
Figure BDA00030426896800000811
的计算能够同时通过VIO与GNSS信号进行计算，保证
Figure BDA00030426896800000812
的精度。
在此构思的基础上，进一步地，所述融合定位坐标
Figure BDA00030426896800000813
通过式13计算：
Figure BDA00030426896800000814
如图1中的实线显示通过本实施例进行定位得到的轨迹。可以看出，与图1中的圆圈线所示的实际路线相比，通过本实施例的方法得到的轨迹与实际轨迹更加接近，说明本发明提出的多源辅助导航方法具有较好的导航效果。
实施例二：
在实施例一的基础上，优选地，所述辅助导航方法还包括：通过至少一个输出装置输出路线信息；所述输出装置为声音发生装置、振动发生装置、发热装置、有线通信连接端口、无线通信发射器的至少之一。通过多种方式输出导航信号，从而满足不同的导航需求。
优选地，通过所述
Figure BDA00030426896800000815
地图数据、至少一个图像获取装置采集的数据，判断是否需要躲避。作为一种可选方案，通过被导航人或物的外形特征构建包围盒，通过使用碰撞检测与最优路径规划算法计算是否需要躲避。
当需要躲避时，通过如下方式输出所述路线信息：
当所述输出装置为声音发生装置时，通过所述声音发生装置发出提示音；
当所述输出装置为振动发生装置时，通过所述振动发生装置发出振动；
当所述输出装置为发热装置时，通过所述发热装置发出热量；
当所述输出装置为有线通信连接端口时，通过有线通信连接端口向外部设备传输路线信息；
当所述输出装置为无线通信发射器时，通过无线通信发射器向外部设备传输路线信息。
更进一步地，所述提示音为语音、有规律且短暂的间隔的声音的其中之一；所述振动发生装置为多个，所述振动为靠近需躲避位置的所述振动发生装置产生；所述发热装置为多个，发出热量的所述发热装置为靠近需躲避位置的所述发热装置。
在上述更进一步方案的基础上，再进一步地，所述发热装置发出的热量引发的温度最高不超过45摄氏度，防止烫伤被导航人员。
优选地，所述图像获取装置为双目摄像头，所述双目摄像头采集到的数据被用于构建三维图像信息，所述三维图像信息用于计算所述路线信息。
导航过程中使用的地图数据包括局部地图、全局地图。
实施例三：
一种多源辅助导航装置，包括至少一个存储器、至少一个处理器、至少一个图像获取装置、至少一个惯性传感器、至少一个卫星信号接收装置、至少一个输出装置，所述处理器通过运行所述存储器中存储的可执行程序，使用至少一个所述图像获取装置、至少一个所述惯性传感器、至少一个所述卫星信号接收装置获取的数据，实现如实施例一中所述的多源辅助导航方法。
最后应当说明的是，以上内容仅用以说明本发明的技术方案，而非对本发明保护范围的限制，本领域的普通技术人员对本发明的技术方案进行的简单修改或者等同替换，均不脱离本发明技术方案的实质和范围。

Claims (10)

1.一种多源辅助导航方法，其特征在于：通过至少一个图像获取装置、至少一个惯性传感器采集的数据，计算视觉惯导定位坐标
Figure FDA0003042689670000011
视觉惯导定位权重Wv；
通过至少一个卫星信号接收装置、至少一个惯性传感器采集的数据，计算卫星惯导定位坐标
Figure FDA0003042689670000012
卫星惯导定位权重WG；
通过所述视觉惯导定位坐标
Figure FDA0003042689670000013
视觉惯导定位权重Wv、卫星惯导定位坐标
Figure FDA0003042689670000014
卫星惯导定位权重WG，计算融合定位坐标
Figure FDA0003042689670000015
通过地图数据、所述融合定位坐标
Figure FDA0003042689670000016
进行路径规划、导航。
2.如权利要求1所述的多源辅助导航方法，特征在于：所述视觉惯导定位坐标
Figure FDA0003042689670000017
通过如下方式计算：
通过式1计算k时间点的所述视觉惯导定位坐标
Figure FDA0003042689670000018
的状态估计量
Figure FDA0003042689670000019
Figure FDA00030426896700000110
式1中，
Figure FDA00030426896700000111
为k-1时间点的视觉惯导定位坐标，uk-1为k-1时间点的所述惯性传感器采集的信号向量，A(v)为预设的视觉惯导状态更新矩阵，B(v)为预设的惯导输入信号变换矩阵；
通过式2、式3计算k时间点的视觉惯导卡尔曼增益K(v)k：
Figure FDA00030426896700000112
Figure FDA00030426896700000113
式2中，
Figure FDA00030426896700000114
为k时间点的所述
Figure FDA00030426896700000115
的协方差矩阵P(v)k的估计量，P(v)k-1为k-1时间点的所述
Figure FDA00030426896700000116
的协方差矩阵，Q(v)为预设的视觉惯导噪声矩阵；式3中，H(v)为预设的视觉惯导观测坐标变换矩阵；R(v)为视觉量测噪声矩阵；
通过式4计算k时间点的所述视觉惯导定位坐标
Figure FDA00030426896700000117
Figure FDA00030426896700000118
其中，Z(v)k为k时间点的视觉定位坐标；
通过式5计算k时间点的所述视觉惯导定位坐标
Figure FDA00030426896700000119
的协方差矩阵P(v)k，所述P(v)k用于k+1时间点的视觉惯导定位坐标
Figure FDA00030426896700000120
的计算：
Figure FDA00030426896700000121
3.如权利要求1所述的多源辅助导航方法，特征在于：所述卫星惯导定位坐标
Figure FDA00030426896700000122
通过如下方式计算：
通过式6计算k时间点的所述卫星惯导定位坐标
Figure FDA00030426896700000123
的状态估计量
Figure FDA00030426896700000124
Figure FDA0003042689670000021
式6中，
Figure FDA0003042689670000022
为k-1时间点的卫星惯导定位坐标，uk-1为k-1时间点的所述惯性传感器采集的信号向量，A(G)为预设的卫星惯导状态更新矩阵，B(G)为预设的惯导输入信号变换矩阵；
通过式7、式8计算k时间点的卫星惯导卡尔曼增益K(G)k：
Figure FDA0003042689670000023
Figure FDA0003042689670000024
式7中，
Figure FDA0003042689670000025
为k时间点的所述
Figure FDA0003042689670000026
的协方差矩阵P(G)k的估计量，P(G)k-1为k-1时间点的所述
Figure FDA0003042689670000027
的协方差矩阵，Q(G)为预设的卫星惯导噪声矩阵；式8中，H(G)为预设的卫星惯导观测坐标变换矩阵；R(G)为卫星量测噪声矩阵；
通过式9计算k时间点的所述卫星惯导定位坐标
Figure FDA0003042689670000028
Figure FDA0003042689670000029
其中，Z(G)k为k时间点的卫星定位坐标；
通过式10计算k时间点的所述卫星惯导定位坐标
Figure FDA00030426896700000210
的协方差矩阵P(G)k，所述P(G)k用于k+1时间点的卫星惯导定位坐标
Figure FDA00030426896700000211
的计算：
Figure FDA00030426896700000212
4.如权利要求2或3任一项所述的多源辅助导航方法，特征在于：所述Wv通过式11计算：
Wv＝1-tr(P(v)k)/(tr(P(v)k)+tr(P(G)k)) 式11。
5.如权利要求2或3任一项所述的多源辅助导航方法，特征在于：所述WG通过式12计算：
WG＝1-tr(P(G)k)/(tr(P(v)k)+tr(P(G)k)) 式12。
6.如权利要求1所述的多源辅助导航方法，特征在于：所述视觉惯导定位权重Wv、卫星惯导定位权重WG满足如下关系：Wv+WG＝1。
7.如权利要求6所述的多源辅助导航方法，特征在于：所述融合定位坐标
Figure FDA00030426896700000213
通过式13计算：
Figure FDA00030426896700000214
8.如权利要求1所述的多源辅助导航方法，特征在于：所述辅助导航方法还包括：通过至少一个输出装置输出路线信息；所述输出装置为声音发生装置、振动发生装置、发热装置、有线通信连接端口、无线通信发射器的至少之一。
9.如权利要求8所述的多源辅助导航方法，特征在于：
通过所述
Figure FDA0003042689670000031
地图数据、至少一个图像获取装置采集的数据，判断是否需要躲避；当需要躲避时，通过如下方式输出所述路线信息：
当所述输出装置为声音发生装置时，通过所述声音发生装置发出提示音；
当所述输出装置为振动发生装置时，通过所述振动发生装置发出振动；
当所述输出装置为发热装置时，通过所述发热装置发出热量；
当所述输出装置为有线通信连接端口时，通过有线通信连接端口向外部设备传输路线信息；
当所述输出装置为无线通信发射器时，通过无线通信发射器向外部设备传输路线信息。
10.一种多源辅助导航装置，特征在于：包括至少一个存储器、至少一个处理器、至少一个图像获取装置、至少一个惯性传感器、至少一个卫星信号接收装置、至少一个输出装置，所述处理器通过运行所述存储器中存储的可执行程序，使用至少一个所述图像获取装置、至少一个所述惯性传感器、至少一个所述卫星信号接收装置获取的数据，实现如权利要求1所述的多源辅助导航方法。
CN202110461932.6A 2021-04-27 2021-04-27 一种多源辅助导航方法及装置 Active CN113155124B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110461932.6A CN113155124B (zh) 2021-04-27 2021-04-27 一种多源辅助导航方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110461932.6A CN113155124B (zh) 2021-04-27 2021-04-27 一种多源辅助导航方法及装置

Publications (2)

Publication Number Publication Date
CN113155124A true CN113155124A (zh) 2021-07-23
CN113155124B CN113155124B (zh) 2022-03-11

Family

ID=76871694

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110461932.6A Active CN113155124B (zh) 2021-04-27 2021-04-27 一种多源辅助导航方法及装置

Country Status (1)

Country Link
CN (1) CN113155124B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113534849A (zh) * 2021-09-16 2021-10-22 中国商用飞机有限责任公司 集成机器视觉的飞行组合导引***、方法和介质
CN113777635A (zh) * 2021-08-06 2021-12-10 香港理工大学深圳研究院 一种全球导航卫星数据校准方法、装置、终端及存储介质
CN114124290A (zh) * 2022-01-27 2022-03-01 成都市克莱微波科技有限公司 一种修正无线电信号的方法和***
CN115793001A (zh) * 2023-02-07 2023-03-14 立得空间信息技术股份有限公司 一种基于惯导复用的视觉、惯导、卫导融合定位方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106412826A (zh) * 2016-09-07 2017-02-15 清华大学 基于多源信息融合的室内定位方法及定位装置
CN107402012A (zh) * 2016-05-20 2017-11-28 北京自动化控制设备研究所 一种车辆的组合导航方法
CN109900265A (zh) * 2019-03-15 2019-06-18 武汉大学 一种camera/mems辅助北斗的机器人定位算法
CN110779521A (zh) * 2019-11-12 2020-02-11 成都中科微信息技术研究院有限公司 一种多源融合的高精度定位方法与装置
US20200219267A1 (en) * 2017-09-04 2020-07-09 Universität Zürich Visual-inertial odometry with an event camera
CN111721290A (zh) * 2020-07-13 2020-09-29 南京理工大学 一种多源传感器信息融合定位切换方法
US20200309529A1 (en) * 2019-03-29 2020-10-01 Trimble Inc. Slam assisted ins
CN111780755A (zh) * 2020-06-30 2020-10-16 南京理工大学 一种基于因子图和可观测度分析的多源融合导航方法
CN112577496A (zh) * 2020-11-25 2021-03-30 哈尔滨工程大学 一种基于自适应选权的多源融合定位方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107402012A (zh) * 2016-05-20 2017-11-28 北京自动化控制设备研究所 一种车辆的组合导航方法
CN106412826A (zh) * 2016-09-07 2017-02-15 清华大学 基于多源信息融合的室内定位方法及定位装置
US20200219267A1 (en) * 2017-09-04 2020-07-09 Universität Zürich Visual-inertial odometry with an event camera
CN109900265A (zh) * 2019-03-15 2019-06-18 武汉大学 一种camera/mems辅助北斗的机器人定位算法
US20200309529A1 (en) * 2019-03-29 2020-10-01 Trimble Inc. Slam assisted ins
CN110779521A (zh) * 2019-11-12 2020-02-11 成都中科微信息技术研究院有限公司 一种多源融合的高精度定位方法与装置
CN111780755A (zh) * 2020-06-30 2020-10-16 南京理工大学 一种基于因子图和可观测度分析的多源融合导航方法
CN111721290A (zh) * 2020-07-13 2020-09-29 南京理工大学 一种多源传感器信息融合定位切换方法
CN112577496A (zh) * 2020-11-25 2021-03-30 哈尔滨工程大学 一种基于自适应选权的多源融合定位方法

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113777635A (zh) * 2021-08-06 2021-12-10 香港理工大学深圳研究院 一种全球导航卫星数据校准方法、装置、终端及存储介质
CN113777635B (zh) * 2021-08-06 2023-11-03 香港理工大学深圳研究院 一种全球导航卫星数据校准方法、装置、终端及存储介质
CN113534849A (zh) * 2021-09-16 2021-10-22 中国商用飞机有限责任公司 集成机器视觉的飞行组合导引***、方法和介质
CN114124290A (zh) * 2022-01-27 2022-03-01 成都市克莱微波科技有限公司 一种修正无线电信号的方法和***
CN115793001A (zh) * 2023-02-07 2023-03-14 立得空间信息技术股份有限公司 一种基于惯导复用的视觉、惯导、卫导融合定位方法

Also Published As

Publication number Publication date
CN113155124B (zh) 2022-03-11

Similar Documents

Publication Publication Date Title
CN113155124B (zh) 一种多源辅助导航方法及装置
US11243081B2 (en) Slam assisted INS
Fan et al. Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB
Milanés et al. Autonomous vehicle based in cooperative GPS and inertial systems
CN105509738B (zh) 基于惯导/多普勒雷达组合的车载定位定向方法
CN110702091B (zh) 一种沿地铁轨道移动机器人的高精度定位方法
US8775063B2 (en) System and method of lane path estimation using sensor fusion
CA2833186C (en) Measuring system and method for determining new points
JP2022113746A (ja) 判定装置
CN109682375B (zh) 一种基于容错决策树的uwb辅助惯性定位方法
JP3536044B2 (ja) 地形ナビゲーション装置
CN109282808B (zh) 用于桥梁三维巡航检测的无人机与多传感器融合定位方法
JP6380936B2 (ja) 移動体及びシステム
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN111025366B (zh) 基于ins及gnss的网格slam的导航***及方法
CN110187372B (zh) 一种低速无人车园区内组合导航方法及***
KR102322538B1 (ko) 무인 운전차의 내비게이션 설비
JP2016080460A (ja) 移動体
CN111637888A (zh) 基于惯导和激光雷达单点测距的掘进机定位方法及***
CN111221020A (zh) 一种室内外定位方法、装置及***
KR20210058640A (ko) 골프장 자동 운전차의 내비게이션 전환 설비
CN115930977A (zh) 特征退化场景的定位方法、***、电子设备和可读存介质
CN116242372B (zh) 一种gnss拒止环境下的uwb-激光雷达-惯导融合定位方法
TWI635302B (zh) 載具即時精準定位系統
KR101987413B1 (ko) 측위 시스템 및 방법

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