CN112762929B - 一种智能导航方法、装置和设备 - Google Patents

一种智能导航方法、装置和设备 Download PDF

Info

Publication number
CN112762929B
CN112762929B CN202011552550.6A CN202011552550A CN112762929B CN 112762929 B CN112762929 B CN 112762929B CN 202011552550 A CN202011552550 A CN 202011552550A CN 112762929 B CN112762929 B CN 112762929B
Authority
CN
China
Prior art keywords
wireless
measurement
imu
measurements
environment
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
CN202011552550.6A
Other languages
English (en)
Other versions
CN112762929A (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.)
Huazhong University of Science and Technology
Original Assignee
Huazhong University of Science and Technology
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 Huazhong University of Science and Technology filed Critical Huazhong University of Science and Technology
Priority to CN202011552550.6A priority Critical patent/CN112762929B/zh
Publication of CN112762929A publication Critical patent/CN112762929A/zh
Application granted granted Critical
Publication of CN112762929B publication Critical patent/CN112762929B/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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Automation & Control Theory (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种智能导航方法、装置和设备，属于无线定位技术与机器人技术的交叉领域，所述方法包括：S1：采集环境图像并获取环境图像中的原始视觉特征；S2：将无线测量获取的距离角度信息作为参考信息对原始视觉特征进行筛选得到目标视觉特征；S3：基于IMU测量利用目标视觉特征初始化环境中无线信号源的位置以及无线测量的偏置；S4：利用无线测量的测量结果、IMU测量的测量结果和目标视觉特征估计运动状态信息，运动状态信息包括：状态变量、视觉特征点的深度和无线测量的偏置；S5：根据运动状态信息和环境中的视觉特征对应的三维信息进行导航。本发明能够提高弱纹理环境下的视觉信息的可靠性，进而实现弱纹理环境下的准确导航。

Description

一种智能导航方法、装置和设备
技术领域
本发明属于无线定位技术与机器人技术的交叉领域，更具体地，涉及一种智能导航方法、装置和设备。
背景技术
近年来快速发展的机器人等智能导航设备的导航技术能代替人执行各种各样的任务，以降低人类生命危险和提高工作效率。例如：机器人能帮助消防员在危险的建筑中搜寻幸存者，能通过空中扫描提高仓储盘库效率30倍。这些应用的基础需求是智能导航设备能够在各种各样复杂的环境中安全有效的通过精准的控制来执行任务。复杂环境精准控制的能力由鲁棒的状态估计赋予。状态估计的目标是实时估计设备的运动状态，至少包括位置、速度和姿态。鲁棒性是当前状态估计***的关键命题。
鲁棒状态估计的关键挑战之一是弱纹理环境。这类环境十分普遍，例如：四周纯白色墙壁的房间、纯色地板、墙上的镜子、大落地窗等等。微小、轻巧、低成本的单目视觉是目前准确状态估计***的主流方案。但是，单目视觉需要光照纹理状况良好的环境才能捕捉到足够多的视觉特征，进而通过投影几何实现状态估计，如：光流为了更好的特征匹配，需要明显的视觉纹理。一种解决方法是人为的在弱纹理物体的表面增加视觉标记(二维码等)，但这具有侵入性且费时费力。另一种方法是通过评估当前环境的纹理程度来决定是否绕开弱纹理区域。更多的传统方法使用回环检测作为一个补救措施来修正弱纹理区域的累积误差。尽管这可能足以应付短暂的弱纹理飞行，但无法支持设备长期在弱纹理区域的运行。此外，实际应用中，设备的运动轨迹可能没有回环用意修正累积误差。
近来，许多工作将无线信号，如LoRa、UWB以及WiFi，作为状态估计的另一种传感方式。它们不受视觉条件的限制，并且日益广泛的存在于日常生活中，例如：UWB已经集成到iPhone 11中。但是，由于无线信号较大的波长和环境干扰，它们的状态估计精度是分米级，比视觉条件良好情况下的单目视觉方案差了一个数量级。因此，当前基于无线信号的状态估计***无法支持设备的高精度导航。
发明内容
针对现有技术的以上缺陷或改进需求，本发明提供了一种智能导航方法、装置和设备，其目的在于解决在弱纹理环境下无法支持高精度导航问题。
为实现上述目的，按照本发明的一个方面，提供了一种智能导航方法，包括：
S1：采集环境图像并获取所述环境图像中的原始视觉特征；
S2：将无线测量获取的距离角度信息作为参考信息对所述原始视觉特征进行筛选得到目标视觉特征；
S3：基于IMU测量利用所述目标视觉特征初始化环境中无线信号源的位置以及无线测量的偏置；
S4：利用无线测量的测量结果、IMU测量的测量结果和所述目标视觉特征估计运动状态信息，所述运动状态信息包括：状态变量、视觉特征点的深度和无线测量的偏置，所述状态变量包括：三维位置、三维速度和三维姿态；
S5：根据所述运动状态信息和环境中的视觉特征对应的三维信息进行导航。
在其中一个实施例中，所述步骤S1包括：
利用单目相机、双目光学相机、孔径相机或者鱼眼相机采集环境图像并获取所述环境图像中的原始视觉特征。
在其中一个实施例中，所述步骤S2包括：
S201：测量与信号源之间的距离和角度；
S202：获取搭载的IMU测量的加速度和角速度；
S203：利用IMU预积分技术将IMU测量数据与无线测量对齐；
S204：将无线测量的距离通过余弦定理进行降噪处理，以抵消无线测量的偏置：
S205：采用卡尔曼滤波将无线测量的距离和角度与IMU的短时积分进行融合，以获得相邻无线测量信息之间的相对位移与旋转；
S206：基于相邻无线测量信息之间的相对位移与旋转，将第k帧图像中的视觉特征通过对极几何重投影到第k+1帧，以形成一组无线匹配的特征点，每个特征点都与光学匹配的特征点相关联；最后，根据无线匹配和光学匹配特征点之间的距离进行排序，排序最高的特征点对应最小距离；
S207：将原始视觉特征点按特征距离的升序排列，每个原始视觉特征点映射一个归一化距离对应的分数，定义阈值∈并选择分数小于∈的原始视觉特征点作为目标视觉特征点参与后续的状态估计。
在其中一个实施例中，所述步骤S203包括：利用IMU预积分技术将IMU测量数据与无线测量对齐；
IMU预积分技术为：
Figure BDA0002858418030000031
其中，
Figure BDA0002858418030000032
表示智能导航设备在t时刻的加速度测量值，
Figure BDA0002858418030000033
表示智能导航设备在t时刻的角速度测量值，
Figure BDA0002858418030000034
表示四元数乘法操作符，(·)i表示IMU参考系下的测量，
Figure BDA0002858418030000035
是当接收到第(k+1)个无线测量时相对于收到第k个无线测量时的IMU参考系，
Figure BDA0002858418030000036
表示设备直接测量的量或者可以由测量的量直接估计出的量。
在其中一个实施例中，所述步骤S3包括：
S301：初始化运动状态并通过无线测距确定无线信号源的位置；初始化操作仅在执行导航任务前执行一次；初始化向量定义为：
Figure BDA0002858418030000041
其中，
Figure BDA0002858418030000042
Figure BDA0002858418030000043
和
Figure BDA0002858418030000044
为第j个关键帧时的位置和速度，
Figure BDA0002858418030000045
是初始时刻相机参考系的重力，s是单目视觉测量的尺度信息，n是一组数据中关键帧的数量，n为大于等于4的正整数；
S302：初始化后将利用更新的传感器测量信息估计和更新MAV的状态，运行过程中无线信号源对应的无线信道会随着环境变化，导致无线测量的距离偏置发生变化，以此来修正距离测量。
在其中一个实施例中，所述步骤S301包括：
单目视觉运动恢复结构算法SfM提供像平面坐标的无尺度位置
Figure BDA0002858418030000046
以及相机旋转
Figure BDA0002858418030000047
其中，将初始相机参考系
Figure BDA0002858418030000048
设为SfM的参考系；
Figure BDA0002858418030000049
表示第j个关键帧时相对于初始相机参考系的相机位姿；
通过手动测量IMU和相机之间的外部参数
Figure BDA00028584180300000410
以将位姿在相机和IMU参考系之间进行转换；二者关系为：
Figure BDA00028584180300000411
且
Figure BDA00028584180300000412
Figure BDA00028584180300000413
Figure BDA00028584180300000414
Figure BDA00028584180300000415
的旋转矩阵；
将相对位移
Figure BDA00028584180300000416
和旋转
Figure BDA00028584180300000417
替换为基于单目视觉的无尺度位姿，根据运动获取测量模型如下：
Figure BDA00028584180300000418
Figure BDA00028584180300000419
是加性高斯噪声；求解下列最小二乘问题得到X0，
Figure BDA00028584180300000420
F为参与计算的数据组中所有的帧图像；
获取一组位置数据
Figure BDA0002858418030000051
并通过解下列二次规划问题(QP)问题来计算无线节点的位置：
Figure BDA0002858418030000052
Figure BDA0002858418030000053
Figure BDA0002858418030000054
表示第j帧图像时设备与第l个无线信号源的距离；
Figure BDA0002858418030000055
是无线测量参考系到IMU参考系之间的相对位移。
在其中一个实施例中，所述步骤S4包括：
基于图优化的滑动窗口估计器建立三种传感器的测量模型，通过最小化三种传感器的测量残差；通过初始化设备状态以及无线信号源位置驱动优化问题的求解，实时计算得出所述运动状态信息，所述运动状态信息还包括位置、速度、姿态、无线测量的偏置以及视觉特征点的深度。
按照本发明的另一方面，提供了一种智能导航装置，包括：
图像获取模块，用于采集环境图像并获取所述环境图像中的原始视觉特征；
无线测量模块，用于将无线测量获取的距离角度信息作为参考信息对所述原始视觉特征进行筛选得到目标视觉特征；
IMU测量模块，用于基于IMU测量利用所述目标视觉特征初始化环境中无线信号源的位置以及无线测量的偏置；
融合模块，用于利用无线测量的测量结果、IMU测量的测量结果和所述目标视觉特征估计运动状态信息，所述运动状态信息包括：状态变量、视觉特征点的深度和无线测量的偏置，所述状态变量包括：三维位置、三维速度和三维姿态；
导航模块，用于根据所述运动状态信息和环境中的视觉特征对应的三维信息进行导航。
按照本发明的另一方面，提供了一种智能导航设备，包括：无线测量节点、图像获取单元、惯性测量单元、存储器及处理器，所述存储器中储存有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行所述的智能导航方法的步骤。
总体而言，通过本发明所构思的以上技术方案与现有技术相比，具有以下有益效果：
(1)本发明利用无线信号测量智能导航设备相对于无线信号源的方位角与距离，同时结合智能导航设备上的相机拍摄的视觉图像以及惯性测量数据，对设备的运动状态(至少包括位置、速度、姿态)进行估计，同时也估计视觉特征的深度，绘制稀疏地图。采用的无线射频信号不受视觉传感的限制，发明的方法能够提高弱纹理环境下的视觉信息的可靠性，进而实现弱纹理环境下的准确导航。
(2)本发明所提供的基于无线测量的视觉特征筛选方法，可以有效选择弱纹理环境下更加优质的视觉特征，进而反过来优化无线测量的可靠性，***无需事先知道环境中无线信号源节点的位置，为智能导航设备提供了一种准确且能立即部署的导航方案。
(3)本发明利用因子图优化模型融合多种异质传感器，取长补短，可以在包括弱纹理环境在内的各种复杂环境下保持稳定的性能，为智能导航设备提供了一种更加鲁棒的导航方案。
(4)本发明所提供的无线-视觉-惯导结合的初始化方案使得智能导航设备可以在任意姿态下开始导航，并且具有导航能力自主恢复机能。
附图说明
图1a为本发明实施例提供的无线-视觉-惯导融合的智能导航设备框架图；
图1b为本发明实施例提供的以无人机为例的智能导航设备示意图；
图2为本发明实施例提供的智能导航方法的流程图；
图3为本发明实施例提供的基于无线测量的视觉特征筛选的示意图；
图4为本发明实施例提供的无线偏置和近似方法的示意图；
图5为本发明实施例基于因子图的状态估计与地图绘制的优化模型示意图；
图6(a)为智能导航设备运动轨迹与真实轨迹以及传统视觉导航轨迹的对比图；
图6(b)为智能导航设备X、Y、Z方向上的定位误差；
图6(c)为智能导航设备在roll、pitch、yaw方向上的姿态估计误差。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
如图1a所示，本发明实施例提供了一种智能导航放，如图2所示，所述方法包括如下步骤：
S1：采集环境图像并获取所述环境图像中的原始视觉特征；
S2：将无线测量获取的距离角度信息作为参考信息对所述原始视觉特征进行筛选得到目标视觉特征；
S3：基于IMU测量利用所述目标视觉特征初始化环境中无线信号源的位置以及无线测量的偏置；
S4：利用无线测量的测量结果、IMU测量的测量结果和所述目标视觉特征估计运动状态信息，所述运动状态信息包括：状态变量、视觉特征点的深度和无线测量的偏置，所述状态变量包括：三维位置、三维速度和三维姿态；
S5：根据所述运动状态信息和环境中的视觉特征对应的三维信息进行导航。
具体实现过程如下：
(1)智能导航设备需搭载有无线测量节点(如UWB、WiFi节点)、相机以及惯性测量单元(IMU)，工作环境中具有无线信号源(如UWB基站、WiFi路由器)。具体地，如图1b所示，设备需要搭载无线测量节点(如UWB标签)、IMU及光学相机，环境中存在无线信号源(如UWB基站，内置UWB芯片的iPhone 11等)。无线测量节点测量与无线信号源之间的距离和角度，IMU测量设备的三维加速度与三维角速度，相机获取图像信息以供设备计算视觉特征点。
(2)所述设备利用无线信号测量的距离和角度作为参考信息对视觉特征进行筛选；具体地，设备需要将无线测量的距离和角度结合IMU预测相对位移与姿态变化，具体包括：
(2.1)设备需要通过IMU预积分技术将IMU测量数据与无线测量对齐，预积分技术为：
Figure BDA0002858418030000081
其中，
Figure BDA0002858418030000082
表示智能导航设备在t时刻的加速度测量值，
Figure BDA0002858418030000083
表示智能导航设备在t时刻的角速度测量值，
Figure BDA0002858418030000084
表示四元数乘法操作符，(·)i表示IMU参考系下的测量，
Figure BDA0002858418030000085
是当接收到第(k+1)个无线测量时相对于收到第k个无线测量时的IMU参考系，
Figure BDA0002858418030000086
表示设备直接测量的量或者可以由测量的量直接估计出的量。
(2.2)如图4所示，将无线测量的距离信息通过余弦定理进行如下操作，可近似抵消无线测量的偏置：
Figure BDA0002858418030000087
(2.3)采用卡尔曼滤波将无线测量的距离和角度，与IMU的短时积分融合，以对抗无线测量的高斯噪声，最终获得相邻无线测量信息之间的相对位移与旋转。
(2.4)如图3所示，根据获得的基于无线测量的相对位姿，将第k帧图像中的视觉特征通过对极几何重投影到第k+1帧。这个操作形成了一组无线匹配的特征点，每个特征点都与一个光学匹配的特征点相关联。最后，根据无线匹配和光学匹配特征点之间的距离进行排序。排序最高的特征点应该对应着最小的距离。如果相机与其他传感器不同步，两个无线测量之间可能会有多帧图像。它们的视差太小而无法成为关键帧，可以忽略这类中间帧，只取最后的一帧图像处理。
(2.5)将视觉特征点按特征距离的升序排列，每个特征都有一个分数，即其光学匹配与无线匹配结果之间的归一化距离。可根据经验定义阈值∈，并允许分数小于∈的特征点参与状态估计。
(3)所述设备利用筛选后的视觉特征以及IMU的测量初始化环境中无线信号源(如UWB基站)的位置以及无线测量的偏置；
具体地，首先初始化智能导航设备的运动状态，然后通过无线测距解一个多边定位问题确定无线信号源的位置。初始化操作仅需在执行导航任务前执行一次，手持设备原地小范围进行变速运动即可。具体包括：
(3.1)初始化向量可定义为：
Figure BDA0002858418030000091
其中
Figure BDA0002858418030000092
Figure BDA0002858418030000093
和
Figure BDA0002858418030000094
是设备在获得第j个关键帧时的位置和速度。
Figure BDA0002858418030000095
是初始时刻相机参考系的重力，s是给予单目视觉测量的尺度信息，n是一组数据中关键帧的数量。n必须≥4才能保证问题可解，但是n也不能太大，否则会影响***的实时性。
(3.1.1)单目视觉运动恢复结构算法SfM提供像平面坐标的无尺度位置
Figure BDA0002858418030000096
以及相机旋转
Figure BDA0002858418030000097
。本文将初始相机参考系
Figure BDA0002858418030000098
设为SfM的参考系。
Figure BDA0002858418030000099
表示第j个关键帧时相对于初始相机参考系的相机位姿。为了将尺度信息带入单目视觉里程计，需求助于IMU的短时积分。通过手动测量IMU和相机之间的外部参数
Figure BDA0002858418030000101
使***能够将位姿在相机和IMU参考系之间进行转换。具体来说，
Figure BDA00028584180300001014
并且
Figure BDA0002858418030000102
其中
Figure BDA0002858418030000103
是
Figure BDA0002858418030000104
的旋转矩阵。
(3.1.2)将相对位移
Figure BDA0002858418030000105
和旋转
Figure BDA0002858418030000106
替换为基于单目视觉的无尺度位姿，根据运动学可得测量模型如下：
Figure BDA0002858418030000107
其中
Figure BDA0002858418030000108
是加性高斯噪声。再求解下列最小二乘问题即可得到X0，
Figure BDA0002858418030000109
其中，F表示参与计算的数据组中所有的帧图像。
(3.1.3)***已经知道了设备的一组位置
Figure BDA00028584180300001015
然后，通过解下列二次规划问题(QP)问题来计算无线节点的位置：
Figure BDA00028584180300001010
其中，
Figure BDA00028584180300001011
Figure BDA00028584180300001012
表示第j帧图像时设备与第l个无线信号源的距离。
Figure BDA00028584180300001013
是无线测量参考系到IMU参考系之间的相对位移，它能够在无线接收器与IMU都安装好之后手动校准得到。
(3.2)初始化之后，***将随着新的传感器测量信息，不断的估计和更新MAV的状态。运行过程中，无线信号源和设备之间的无线信道会随着环境变化，导致无线测量的距离偏置发生变化。给定无线节点的位置，跟踪偏置的变化，以此来修正距离测量。
具体地，无线测量偏置在信道相干时间内不会改变，这与最大多普勒扩展成反比。由于运动学的约束，相干时间不会剧烈变化。于是，使用对第l个无线节点的一组在相干时间内的状态，通过解下列QP问题估计距离的偏置bl：
Figure BDA0002858418030000111
(4)所述设备融合无线测量、IMU测量、以及筛选后的视觉特征点估计设备最多六个自由度的状态变量(包括三维位置、三维速度、三维姿态)、环境中视觉特征点的深度以及无线测量的偏置。
具体地，***全状态向量组可以定义为：
Figure BDA0002858418030000112
其中，xk∈R10表示组内当获得第k个关键帧时的状态，包括位置
Figure BDA0002858418030000113
速度
Figure BDA0002858418030000114
和姿态
Figure BDA0002858418030000115
bl是对于第l个无线信号源的测距偏置，m表示连接智能导航设备的无线信号源的数量，λη是第η个视觉特征的首次被观测到的深度。在全状态向量组中共有o个特征被追踪到。偏置在相干时间内不变，并且它不会剧烈变化。在当前的模型中，假设无线测量偏置在全状态向量组内是同一的，它会在新的测量到来时以滑动窗口的形式更新。本方法固定全状态向量组的规模，在保持足够的多视角约束的同时，约束计算复杂度，保障实时性。
具体地，本方法采用基于因子图的优化模型，如图5所示。通过最小化所有测量残差的马氏范数来获得一个最大后验估计，求解全状态向量组：
Figure BDA0002858418030000116
其中，u表示无线测量信息集合，C是观测到的视觉特征集合，I是IMU的测量信息集合。
Figure BDA0002858418030000117
Figure BDA0002858418030000118
和
Figure BDA0002858418030000119
分别是无线测量，相机和IMU的测量残差函数。为求解该优化问题，需要退到各个传感器的测量模型，步骤如下：
(4.1)针对无线测量残差，将相对于第l个无线信号源的水平角
Figure BDA0002858418030000121
结合IMU提供的roll和pitch角转换成UWB坐标系下的姿态
Figure BDA0002858418030000122
无线测量残差
Figure BDA0002858418030000123
(简记为
Figure BDA0002858418030000124
)可表达为：
Figure BDA0002858418030000125
其中，
Figure BDA0002858418030000126
是第l个无线信号源的位置，前述初始化步骤已经获得。
Figure BDA0002858418030000127
是机载无线接收器与IMU的相对位移,qxyz提取四元数中的向量部分。
(4.2)针对相机测量残差，给定第j帧图像观测到的第η个筛选的视觉特征
Figure BDA0002858418030000128
假设这个特征在第$h$帧图像首次被观测到，它在第j帧的残差
Figure BDA0002858418030000129
(简记为
Figure BDA00028584180300001210
)是它的重投影误差。它对应的3D点
Figure BDA00028584180300001211
是
Figure BDA00028584180300001212
其中，
Figure BDA00028584180300001213
是该特征点的齐次坐标。它的重投影点是
Figure BDA00028584180300001214
于是，视觉残差可以定义为:
Figure BDA00028584180300001215
(4.3)针对IMU测量残差，给定IMU预积分
Figure BDA00028584180300001216
Figure BDA00028584180300001217
和
Figure BDA00028584180300001218
，IMU的测量残差
Figure BDA00028584180300001219
(简记为
Figure BDA00028584180300001220
)可以基于运动学定义为:
Figure BDA00028584180300001221
(5)根据运动状态变量估计值调整无人机运动状态，实现无人机的自主导航。
为了验证本发明方法的有效性，在视觉受限昏暗环境下预设一条运动轨迹，在智能导航设备的运动下，基于本发明提供的一种无线-视觉-惯导融合的智能导航方法，设备运动轨迹与真实轨迹以及最新传统视觉方法的差异如图6(a)所示，直观上反映了本发明方法的导航功能；任意时刻设备实际的三维位置在X、Y、Z方向上的定位与预设位置估计的差值，即定位误差如图6(b)所示；任意时刻，设备在roll、pitch、yaw方向上的姿态估计误差如图6(c)所示。可以看出，本发明方法能够在视觉受限的弱纹理环境下较准确实施智能导航设备的状态估计，从而实现导航，无需事先标定环境中的无线信号源节点位置，解决了现有的导航***由于依赖视觉传感手段进行定位而带来的受弱纹理条件影响而降低导航精度的问题。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种智能导航方法，其特征在于，所述方法包括：
S1：采集环境图像并获取所述环境图像中的原始视觉特征；
S2：将无线测量获取的距离角度信息作为参考信息对所述原始视觉特征进行筛选得到目标视觉特征；
S3：基于IMU测量利用所述目标视觉特征初始化环境中无线信号源的位置以及无线测量的偏置；
S4：利用无线测量的测量结果、IMU测量的测量结果和所述目标视觉特征估计运动状态信息，所述运动状态信息包括：状态变量、视觉特征点的深度和无线测量的偏置，所述状态变量包括：三维位置、三维速度和三维姿态；
S5：根据所述运动状态信息和环境中的视觉特征对应的三维信息进行导航；
所述S2包括：
S201：测量与信号源之间的距离和角度；
S202：获取搭载的IMU测量的加速度和角速度；
S203：利用IMU预积分技术将IMU测量数据与无线测量对齐；
S204：将无线测量的距离通过余弦定理进行降噪处理，以抵消无线测量的偏置：
S205：采用卡尔曼滤波将无线测量的距离和角度与IMU的短时积分进行融合，以获得相邻无线测量信息之间的相对位移与旋转；
S206：基于相邻无线测量信息之间的相对位移与旋转，将第k帧图像中的视觉特征通过对极几何重投影到第k+1帧，以形成一组无线匹配的特征点，每个特征点都与光学匹配的特征点相关联；最后，根据无线匹配和光学匹配特征点之间的距离进行排序，排序最高的特征点对应最小距离；
S207：将原始视觉特征点按特征距离的升序排列，每个原始视觉特征点映射一个归一化距离对应的分数，定义阈值∈并选择分数小于∈的原始视觉特征点作为目标视觉特征点参与后续的状态估计；
所述S3包括：
S301：初始化运动状态并通过无线测距确定无线信号源的位置；初始化操作仅在执行导航任务前执行一次；初始化向量定义为：
Figure FDA0003653042420000021
其中，
Figure FDA0003653042420000022
Figure FDA0003653042420000023
和
Figure FDA0003653042420000024
为第j个关键帧时的位置和速度，
Figure FDA0003653042420000025
是初始时刻相机参考系的重力，s是单目视觉测量的尺度信息，n是一组数据中关键帧的数量，n为大于等于4的正整数；
S302：初始化后将利用更新的传感器测量信息估计和更新MAV的状态，运行过程中无线信号源对应的无线信道会随着环境变化，导致无线测量的距离偏置发生变化，以此来修正距离测量。
2.如权利要求1所述的智能导航方法，其特征在于，所述步骤S203包括：利用IMU预积分技术将IMU测量数据与无线测量对齐；
IMU预积分技术为：
Figure FDA0003653042420000026
其中，
Figure FDA0003653042420000027
表示智能导航设备在t时刻的加速度测量值，
Figure FDA0003653042420000028
表示智能导航设备在t时刻的角速度测量值，
Figure FDA0003653042420000029
表示四元数乘法操作符，(·)i表示IMU参考系下的测量，
Figure FDA00036530424200000210
是当接收到第(k+1)个无线测量时相对于收到第k个无线测量时的IMU参考系，
Figure FDA00036530424200000211
表示设备直接测量的量或者可以由测量的量直接估计出的量。
3.如权利要求1所述的智能导航方法，其特征在于，所述步骤S301包括：
单目视觉运动恢复结构算法SfM提供像平面坐标的无尺度位置
Figure FDA0003653042420000031
以及相机旋转
Figure FDA0003653042420000032
其中，将初始相机参考系
Figure FDA0003653042420000033
设为SfM的参考系；
Figure FDA0003653042420000034
表示第j个关键帧时相对于初始相机参考系的相机位姿；
通过手动测量IMU和相机之间的外部参数
Figure FDA0003653042420000035
以将位姿在相机和IMU参考系之间进行转换；二者关系为：
Figure FDA0003653042420000036
且
Figure FDA0003653042420000037
Figure FDA0003653042420000038
是
Figure FDA0003653042420000039
的旋转矩阵；
将相对位移
Figure FDA00036530424200000310
和旋转
Figure FDA00036530424200000311
替换为基于单目视觉的无尺度位姿，根据运动获取测量模型如下：
Figure FDA00036530424200000312
Figure FDA00036530424200000313
是加性高斯噪声；求解下列最小二乘问题得到X0：
Figure FDA00036530424200000314
F为参与计算的数据组中所有的帧图像；
获取一组位置数据
Figure FDA00036530424200000315
并通过解下列二次规划问题(QP)问题来计算无线节点的位置：
Figure FDA00036530424200000316
Figure FDA00036530424200000317
Figure FDA00036530424200000318
表示第j帧图像时设备与第l个无线信号源的距离；
Figure FDA00036530424200000319
是无线测量参考系到IMU参考系之间的相对位移。
4.如权利要求3所述的智能导航方法，其特征在于，所述步骤S4包括：
基于图优化的滑动窗口估计器建立三种传感器的测量模型，通过最小化三种传感器的测量残差；通过初始化设备状态以及无线信号源位置驱动优化问题的求解，实时计算得出所述运动状态信息，所述运动状态信息还包括位置、速度、姿态、无线测量的偏置以及视觉特征点的深度。
5.如权利要求1-4任一项所述的智能导航方法，其特征在于，所述步骤S1包括：利用单目相机、双目光学相机、孔径相机或者鱼眼相机采集环境图像并获取所述环境图像中的原始视觉特征。
6.一种智能导航装置，其特征在于，用于执行权利要求1-5任一项所述的智能导航方法，包括：
图像获取模块，用于采集环境图像并获取所述环境图像中的原始视觉特征；
无线测量模块，用于将无线测量获取的距离角度信息作为参考信息对所述原始视觉特征进行筛选得到目标视觉特征；
IMU测量模块，用于基于IMU测量利用所述目标视觉特征初始化环境中无线信号源的位置以及无线测量的偏置；
融合模块，用于利用无线测量的测量结果、IMU测量的测量结果和所述目标视觉特征估计运动状态信息，所述运动状态信息包括：状态变量、视觉特征点的深度和无线测量的偏置，所述状态变量包括：三维位置、三维速度和三维姿态；
导航模块，用于根据所述运动状态信息和环境中的视觉特征对应的三维信息进行导航。
7.一种智能导航设备，其特征在于，包括：无线测量节点、图像获取单元、惯性测量单元、存储器及处理器，所述存储器中储存有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如权利要求1至5中任一项所述的智能导航方法的步骤。
CN202011552550.6A 2020-12-24 2020-12-24 一种智能导航方法、装置和设备 Active CN112762929B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011552550.6A CN112762929B (zh) 2020-12-24 2020-12-24 一种智能导航方法、装置和设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011552550.6A CN112762929B (zh) 2020-12-24 2020-12-24 一种智能导航方法、装置和设备

Publications (2)

Publication Number Publication Date
CN112762929A CN112762929A (zh) 2021-05-07
CN112762929B true CN112762929B (zh) 2022-08-02

Family

ID=75694124

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011552550.6A Active CN112762929B (zh) 2020-12-24 2020-12-24 一种智能导航方法、装置和设备

Country Status (1)

Country Link
CN (1) CN112762929B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113124856B (zh) * 2021-05-21 2023-03-14 天津大学 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN114942026A (zh) * 2022-06-01 2022-08-26 四川大学 基于智能数据的多模态三维影像导航***

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9031809B1 (en) * 2010-07-14 2015-05-12 Sri International Method and apparatus for generating three-dimensional pose using multi-modal sensor fusion
CN109341679A (zh) * 2018-09-30 2019-02-15 华中科技大学 一种智能设备导航方法及导航***
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及***
CN111649737A (zh) * 2020-05-08 2020-09-11 中国航空工业集团公司西安航空计算技术研究所 一种面向飞机精密进近着陆的视觉-惯性组合导航方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9031809B1 (en) * 2010-07-14 2015-05-12 Sri International Method and apparatus for generating three-dimensional pose using multi-modal sensor fusion
CN109341679A (zh) * 2018-09-30 2019-02-15 华中科技大学 一种智能设备导航方法及导航***
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及***
CN111649737A (zh) * 2020-05-08 2020-09-11 中国航空工业集团公司西安航空计算技术研究所 一种面向飞机精密进近着陆的视觉-惯性组合导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Demo Abstract_ WINS_ WiFi-Inertial Indoor State Estimation for MAVs;Shengkai Zhang等;《2018 Association for Computing Machinery》;20181107;第309-310页 *
利用惯导测量单元确定关键帧的实时SLAM算法;卫文乐等;《计算机应用》;20200410;第40卷(第04期);第1157-1163页 *
室内环境无人机复合定位方法研究;李康;《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅱ辑》;20190315(第3期);第46-47页 *

Also Published As

Publication number Publication date
CN112762929A (zh) 2021-05-07

Similar Documents

Publication Publication Date Title
Qin et al. A general optimization-based framework for global pose estimation with multiple sensors
CN108827315B (zh) 基于流形预积分的视觉惯性里程计位姿估计方法及装置
CN106840148B (zh) 室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法
Achtelik et al. Stereo vision and laser odometry for autonomous helicopters in GPS-denied indoor environments
CN111288989B (zh) 一种小型无人机视觉定位方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
Chen et al. Indoor localization algorithms for a human-operated backpack system
CN112197770A (zh) 一种机器人的定位方法及其定位装置
CN108332752B (zh) 机器人室内定位的方法及装置
CN111932674A (zh) 一种线激光视觉惯性***的优化方法
CN112254729A (zh) 一种基于多传感器融合的移动机器人定位方法
CN112762929B (zh) 一种智能导航方法、装置和设备
Andert et al. Lidar-aided camera feature tracking and visual slam for spacecraft low-orbit navigation and planetary landing
JP2017524932A (ja) ビデオ支援着艦誘導システム及び方法
CN115355904A (zh) 一种针对地面移动机器人的Lidar-IMU融合的slam方法
Li et al. Aerial-triangulation aided boresight calibration for a low-cost UAV-LiDAR system
Wang et al. UAV navigation in large-scale GPS-denied bridge environments using fiducial marker-corrected stereo visual-inertial localisation
CN114459467A (zh) 一种未知救援环境中基于vi-slam的目标定位方法
CN112747749A (zh) 一种基于双目视觉和激光融合定位导航***
CN116380079A (zh) 一种融合前视声呐与orb-slam3的水下slam方法
Alliez et al. Indoor localization and mapping: Towards tracking resilience through a multi-slam approach
Liu et al. A multi-sensor fusion with automatic vision-LiDAR calibration based on Factor graph joint optimization for SLAM
KR102406240B1 (ko) 강인한 스테레오 비주얼 관성 항법 장치 및 방법
Ross et al. Uncertainty estimation for stereo visual odometry
CN115344033A (zh) 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法

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