CN111595334A - 基于视觉点线特征与imu紧耦合的室内自主定位方法 - Google Patents
基于视觉点线特征与imu紧耦合的室内自主定位方法 Download PDFInfo
- Publication number
- CN111595334A CN111595334A CN202010362870.9A CN202010362870A CN111595334A CN 111595334 A CN111595334 A CN 111595334A CN 202010362870 A CN202010362870 A CN 202010362870A CN 111595334 A CN111595334 A CN 111595334A
- Authority
- CN
- China
- Prior art keywords
- imu
- line
- feature
- point
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 51
- 230000000007 visual effect Effects 0.000 title claims abstract description 40
- 230000008878 coupling Effects 0.000 title claims abstract description 22
- 238000010168 coupling process Methods 0.000 title claims abstract description 22
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 22
- 238000005259 measurement Methods 0.000 title claims abstract description 14
- 238000005457 optimization Methods 0.000 claims abstract description 20
- 230000007246 mechanism Effects 0.000 claims abstract description 14
- 238000001914 filtration Methods 0.000 claims abstract description 13
- 230000008569 process Effects 0.000 claims abstract description 11
- 238000000605 extraction Methods 0.000 claims abstract description 10
- 230000008859 change Effects 0.000 claims abstract description 9
- 230000010354 integration Effects 0.000 claims description 11
- 238000001514 detection method Methods 0.000 claims description 8
- 230000005484 gravity Effects 0.000 claims description 7
- 238000006073 displacement reaction Methods 0.000 claims description 4
- 239000011159 matrix material Substances 0.000 claims description 4
- 230000003287 optical effect Effects 0.000 claims description 4
- 238000012937 correction Methods 0.000 claims description 3
- 230000004927 fusion Effects 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 230000015572 biosynthetic process Effects 0.000 claims description 2
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 230000009466 transformation Effects 0.000 claims description 2
- 230000004807 localization Effects 0.000 claims 3
- 238000012216 screening Methods 0.000 claims 1
- 238000004364 calculation method Methods 0.000 abstract description 6
- 238000005516 engineering process Methods 0.000 description 13
- 230000000694 effects Effects 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 239000000284 extract Substances 0.000 description 4
- 238000012795 verification Methods 0.000 description 4
- 238000002474 experimental method Methods 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 1
- QBWCMBCROVPCKQ-UHFFFAOYSA-N chlorous acid Chemical compound OCl=O QBWCMBCROVPCKQ-UHFFFAOYSA-N 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
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
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- 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)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于视觉点线特征与IMU紧耦合的室内自主定位方法。所述方法通过增加结构化建筑环境中丰富的线特征作为视觉约束能克服仅依赖点特征约束在弱纹理、光线变化剧烈的走廊等区域室内自主定位失效问题；通过增加了点线过滤提取策略和线特征的4参数正交表示进行图优化，能够克服线特征的误匹配问题和不必要的计算量以及过参数线特征表示导致的优化求解过程的不稳定性问题；且引入IMU数据与视觉点线特征紧耦合的机制，能克服纯视觉在单兵/行人执行任务快速运动、旋转等情况自主定位失效问题。本发明总体上提升了单兵/行人室内自主定位系统的精度和鲁棒性。
Description
技术领域
本发明涉及室内定位导航技术领域，尤其涉及一种基于视觉点线特征与IMU紧耦合的单兵/行人室内自主定位方法。
背景技术
随着现代化大型复杂建筑群日益涌现以及人们在室内活动时间长、活动空间覆盖范围广，人们急需室内定位导航技术提供定位服务、目的地路径规划引导服务、自主空间探索定位导航等服务。例如消费人群在大型购物商圈寻找店铺位置，快递人员在结构化大型建筑群进行配送时定位与路径规划，司机在大型地下停车场寻找停车位置，单兵或者消防人员在紧急情况下搜寻复杂建筑群执行任务时都需要进行自主定位导航。可见行人室内自主定位方法在生活以及公共安全等领域具有很重要的意义。
目前室内自主定位方法有超宽带(Ultra Wide Band，UWB)技术、地磁定位技术、视觉同时定位与建图(Simultaneous Localization and Mapping，SLAM)技术、基于惯性测量单元(Inertial Measurement Unit，IMU)航迹推算技术等。然而，由于UWB技术传播过程容易受物体遮挡、反射等影响且需要事先布置设备，地磁定位技术容易受外界通信干扰影响，均不适合室内自主定位需求。IMU航迹推算具有不受外界干扰且自身响应频率高的优点，然而低精度的IMU器件存在零偏，单纯依靠积分进行航迹推算在长时间情况下很容易发散。视觉定位技术因单目相机成本低廉，通过特征点匹配的多视图几何约束测量旋转和平移，长时间测量下不会产生累积误差，然而视觉受场景条件约束大，如室内光照、纹理丰富度等条件，相机在快速运动下点特征容易跟丢导致匹配失效。
现有的以点特征为主的视觉定位技术主虽然在纹理丰富的特定场景能够执行定位需求，但是大型建筑结构化环境下，通常存在弱纹理区域，如走廊通道里大面积的白墙。且小区域范围的通道内可能光线变化剧烈。这些真实存在的场景条件都将导致基于点特征的视觉定位技术无法正常工作。然而在结构化建筑环境下，线特征丰富且更好地表征了室内环境，通过引入线特征与点特征结合的视觉定位技术，将能够很好地克服点特征在弱纹理走廊、光线剧烈变化等场景失效的问题。然而现有的线特征提取策略只能机械地提取所有线特征，结构化建筑环境下大量不必要的线特征被提取不仅增加了计算量且在线特征及其丰富的区域容易导致误匹配。传统的线特征利用普吕克坐标表示法需要6个参数，然而三维空间的直线是4个自由度，这在优化过程中额外的自由度将导致计算量的增加和数值求解的不稳定性。且室内行人具有运动状况复杂的特征，特别是单兵在执行消防任务等紧急任务时，需要快速运动，单纯的视觉点特征匹配可能暂时失效。如何实现有效的点线特征结合同时又能弥补视觉定位技术失效问题，从而提供一种快速、准确的室内自主定位技术是需要解决的问题。
发明内容
发明目的：针对现有技术的问题，本发明提出了一种基于视觉点线特征与IMU紧耦合的单兵/行人室内自主定位方法，能够提高单兵/行人室内自主定位的精确性和鲁棒性，满足在弱纹理、光线变化剧烈的走廊等各种大型结构化建筑室内场景定位的需求，以及适应单兵在执行消防任务等紧急任务时需要快速运动等需求。
技术方案：本发明提供一种基于视觉点线特征与IMU紧耦合的室内自主定位方法，包括以下步骤：
S1，对获取的IMU数据进行预积分处理；
S2，对获取的单目图像数据，构建Mask过滤机制获取点特征；
S3，对获取的单目图像数据，构建梯度密度过滤机制获取线特征；
S4，对步骤S2提取的点特征和步骤S3提取的线特征进行KLT光流跟踪匹配；
S5，先利用SFM进行单目视觉估计滑窗内所有帧的位姿以及路标点的逆深度，再通过视觉与IMU预积分融合对齐，紧耦合初始化求解初始化参数；
S6，在利用6参数普吕克坐标系表示线的初始化、三角化的基础上，提出利用在4参数正交坐标系中进行线特征的图优化；
S7，利用滑窗内的视觉点线特征约束、IMU约束、先验约束，一起构建残差目标函数进行非线性优化，求解滑窗中所有帧的状态量；
S8，根据关键帧的点线特征进行回环检测，与构建的点线词袋库匹配，并对滑窗状态进行反馈矫正；
S9，经过全局优化输出室内单兵/行人位姿估计。
有益效果：本发明通过增加结构化建筑环境中丰富的线特征作为视觉约束能克服仅依赖点特征约束在弱纹理、光线变化剧烈的走廊等区域室内自主定位失效问题；通过增加了点线过滤提取策略和线特征的4参数正交表示进行图优化，能够克服线特征的误匹配问题和不必要的计算量以及过参数线特征表示导致的优化求解过程的不稳定性问题；且引入IMU数据与视觉点线特征紧耦合的机制，能克服纯视觉在单兵执行任务快速运动、旋转等情况自主定位失效问题。总体上提升了单兵/行人室内自主定位系统的精度和鲁棒性。
附图说明
图1为本发明方法实施例的流程图；
图2为本发明所构建梯度密度过滤机制剔除线特征密集区域后的线特征提取效果图；
图3为本发明线特征初始化示意图；
图4位本发明方法在弱纹理走廊提取点线特征效果图；
图5为传统方法在楼宇内真实环境弱纹理通道里进行上下楼实验的定位结果图；
图6为本发明方法在楼宇内真实环境弱纹理通道里进行上下楼实验的定位结果图。
具体实施方式
下面结合附图对本发明的技术方案作进一步说明。
一种基于视觉点线特征与IMU紧耦合的单兵/行人室内自主定位方法，整体流程框图如图1所示，主要包括以下步骤：
步骤1，对获取的IMU数据进行预积分处理。
在本发明提出的视觉点线特征与IMU紧耦合的方法中，IMU需要提供两个关键帧的相对测量，从而构建误差函数对关键帧姿态的迭代优化。然而IMU的频率远高于相机的采样频率，所以要将两个关键帧之间的相对变化量求解出来，转化为跟初始时刻无关的变量，防止在优化更新中，只要初始量变化了所有的IMU的积分过程都得重复计算。
步骤2，对获取的单目图像数据，构建Mask过滤机制获取点特征。
本发明利用Harris角点检测提取点特征，在传统方法下，对整个图像提取Harris角后自动按照点质量从高低排列，选取排在前面的一定数目(比如300个)作为特征点。但是很有可能这些点都集中在图像中某一个很小的区域，比如40×40的区域内，而整个图像大部分的其他区域都没有，这样得到的点特征就集中于局部区域，将导致后面的特征匹配时失误。本发明对此作出了改进，先划分了网格，然后针对每个网格进行提取，在每个网格区域提取质量最好的前N个，这样可以获取均匀分布的高质量点特征。具体地，包括以下步骤：
步骤2.1，首先设置每帧图像要提取的特征点数目、尺度、金字塔层数、Harris初始阈值、Harris最小阈值等参数；
步骤2.2，然后将640×480像素的单目图像划分成40×40像素的网格区域，先根据Harris初始阈值依次对每块区域提取Harris角点；
步骤2.3，若提取不到特征点，再根据Harris最小阈值对每块区域提取Harris角点；
步骤2.4，如果再次提取不到特征点，则将该区域的编号标记并存入队列中，后续首先对该区域进行线特征检测；
步骤2.5，对于提取到的特征点，根据特征点的质量高低选取最优质的4个特征点。
应当理解，上述过程中设置的网格区域的大小以及最终选取特征点个数仅是示例的作用，并不是对本发明的限制，具体应用时可以根据实际情况设置。
步骤3，对获取的单目图像数据，构建梯度密度过滤机制获取稳健可靠的线特征。
在一个实施例中，获取线特征包括以下步骤：
步骤3.1，对步骤2中提取不到特征点所标记的区域，有可能是白墙交接处等弱纹理区域，直接通过线特征LSD(Line Segment Detector)算法进行线特征检测，以补充该块缺乏点特征的区域里的特征信息，同时可降低整体运算量。
步骤3.2，对步骤2尚未标记的其他区域，因为它是点特征丰富的区域，即纹理很丰富的区域，很可能也是线特征很丰富的区域，构建梯度密度过滤机制对线特征密集区域进行处理，只保留线特征密集区域的最边缘方框的线特征，然后再进行LSD提取，获取稳健可靠的线特征。
具体地，需要计算每个像素梯度Iij并与给定的阈值比较，针对高于给定阈值的像素存入待处理队列1中；Iij中下标i/j分别表示像素在图像中的几行第几列；
对该队列1里像素的下标再次进行筛选，另建一个队列2保存当且仅当行列下标值之差均大于10的像素以便减少后续的计算量。
然后对队列2里的所有像素为中心，划定5×5像素的区域，统计每个区域中队列1中的像素量占5×5像素量的百分比τ；
如果τ大于给定的阈值τthread＝0.75，将该5×5像素区为中心的9个5×5像素区，都判定为线特征密集区域；
最后只保留该15×15像素区域最边缘方框的线特征，利用LSD线特征提取加LBD描述子获取稳健可靠的线特征。本发明所构建梯度密度过滤机制剔除线特征密集区域后的线特征提取效果如图2所示。应当理解，上述过程中划定区域的5×5像素大小以及预设阈值0.75的设置仅是示例的作用，并不是对本发明的限制，具体应用时可以根据实际情况设置。
步骤4，对步骤2提取的点特征和步骤3提取的线特征进行KLT光流跟踪匹配；这是一种经典的对象跟踪算法，此处不加以赘述。
步骤5，利用视觉几何的约束关系，在步骤4对特征点进行提取、光流跟踪匹配，以及RANSAC剔除异常点的基础上，已知图像帧之间的对极几何约束信息，可以计算相机帧间不带尺度的位姿关系，且知道特征点二维坐标，通过三角化可以得到路标点的位置。此过程便是利用SFM(Structure from Motion)进行单目视觉估计滑窗内所有帧的位姿以及路标点的逆深度。然后需要将单目视觉与IMU预积分紧耦合，进行视觉IMU的对齐。
具体操作子步骤如下：
步骤5.1，首先求解相机坐标系和IMU坐标系的旋转外参数qbc；
已知相邻两时刻k,k+1之间有：IMU旋转积分
视觉测量
通过相机和IMU之间的变换关系，求得k+1时刻的相机位姿相对于k时刻的IMU位姿的相对坐标关系：
进一步写成：
[...]L,[...]R分别代表四元素左乘矩阵和右乘矩阵。
是对
的表示。
将多个时刻的上述方程累计起来，得到如下方程：
对该方程采用SVD分解，即可求得相机坐标系和IMU坐标系的旋转外参数qbc。
步骤5.2，通过步骤5.1的旋转外参数qbc估计陀螺仪偏差：利用IMU在k,k+1时刻间的约束，和相机在k,k+1之间的约束，构建如下最小二乘优化方程：
其中B表示所有的图像关键帧集合，通过求解该最小二乘最优解即可得到陀螺仪偏差bg。符号δ表示增量的意思，是微积分的表达形式。
表示四元素对陀螺仪零偏的雅克比。
步骤5.3，利用平移约束pbc估计重力方向、速度、尺度等信息。
首先将这些待估计的量记为：
其中，
表示k时刻载体坐标系的速度在载体坐标系下的表示。
为重力向量在第0帧相机坐标系下的表示。s表示尺度因子，将视觉轨迹拉伸到米制单位。
然后根据预积分公式，将世界坐标系w换成相机初始时刻坐标系c0有：
其中
是两时刻k,k+1之间的位置变化量，
是两时刻k,k+1之间的速度变化量。
是0时刻相机相对于k时刻IMU的旋转矩阵，s是尺度因子，
是k+1时刻IMU和0时刻相机之间的位移，
是k时刻IMU和0时刻相机之间的位移，
为重力向量在第0帧相机坐标系下的表示，
为k时刻IMU和0时刻相机的旋转矩阵，
是k+1时刻IMU的速度，
是k时刻IMU的速度。
进一步整理将待估计变量放到方程右边，有：
其中，
表示整理后的噪声量，
最后求解成线性最小二乘问题得到重力方向、速度、尺度状态量。
步骤6，在利用6参数普吕克坐标系表示线的初始化、三角化的基础上，提出将线特征在普吕克坐标系的6参数表达转化为正交坐标系的4参数表达，以便后面步骤7中进行线特征的优化。
6参数到4参数的转换过程具体描述如下：
首先，利用6参数普吕克坐标可以唯一确定空间上的一条直线，其中n表示原点与直线
构成的平面的法向量，v表示直线
的方向向量。
通过直线
在两个相机平面的投影为a1b1,a2b2，知道两条投影的端点以及各自相机的中心C1,C2，三点确定一个平面，即有过a1C1b1的平面π1和过点a2C2b2的平面π2。
这两个反投影平面的交线即为直线
实现了线的三角化，具体初始化示意图如图3所示。
但是三维空间的直线是4个自由度，而普吕克坐标有6个自由度，在图优化里，额外的自由度将导致计算代价的增加和系统的不稳定。
因此，本发明提出利用在4参数正交坐标系中进行线特征的图优化；其中正交坐标系的参数为(U,W)∈SO(3)×SO(2)。
步骤6.1，从线的普吕克坐标中线的参数化可以进行如下转换：
其中，
步骤6.2，从而，可用
和
来表示直线的待优化的参数pT＝(θT,θ)，且以左乘的形式优化更新正交坐标U←Exp(θ)U,W←R(θ)W。
步骤7，利用滑窗内的视觉点线特征约束、IMU约束、先验约束，一起构建残差目标函数进行非线性优化。
具体操作子步骤如下：
步骤7.1，采用滑动窗口机制，滑窗中第i时刻的状态量为：
其中xi包括IMU的位置
速度
姿态
加速度零偏
陀螺仪零偏
λ表示滑窗内相机所看到的点特征路标的逆深度，
表示滑窗内相机所看到的线特征路标的正交参数表示。
步骤7.2，针对这些视觉点线特征约束、IMU约束以及先验约束，一起构建残差目标函数进行非线性优化，通过Levenberg–Marquard求解算法即可优化出所需要的状态量。
其中第一项为残差约束，rp表示先验残差，Jp表示先验项的雅克比。
第二项为IMU约束，B表示滑窗内所有的IMU帧集合，rb( )表示IMU的残差项，
表示IMU预积分的测量项。
第三项为点特征约束，F表示滑窗内所有帧的特征点集合，rf( )表示点特征残差项，
表示点特征的观测量。
第四项为线特征约束。L表示滑窗内所有帧的线特征集合，rl表示线特征残差项，
表示线特征的观测量。
各项的∑分别表示对应的协方差项；各项的ρ( )表示Huber鲁棒核函数；
Huber鲁棒核函数的定义式为：
步骤8，根据关键帧的点线特征进行回环检测，与构建的点线词袋库匹配，并对滑窗状态进行反馈矫正；点线词袋库即步骤2、3中提取的点特征和线特征构成的特征库。
步骤9，经过全局优化输出单兵/行人位姿估计及绘制单兵实时运动轨迹。
本发明采用数据集验证和真实环境实验，证明了一种基于视觉点线特征与IMU紧耦合的单兵/行人室内自主定位方法的有效性。
EuRoC数据集是苏黎世联邦理工学院公开的一套视觉和IMU数据集。该数据集记录了微型飞行器(MAV)在工厂环境中的5套数据。微型飞行器搭载的传感器为IMU测量单元为ADIS16448，采样频率为200Hz。相机型号为MT9V034，采样频率为20Hz。该数据集还提供有真值轨迹，该真值轨迹由动作捕捉系统(VICON和Leica MS50)记录。
将本发明基于视觉点线特征与IMU紧耦合的单兵/行人室内自主定位方法(IMPL-VINS)与开源的点特征VINS-mono在数据集上进行验证对比，表1统计了两种方法分别与EuRoC数据集真值之间的均方根误差(RMSE)。
表1本发明方法与VINS-mono在EuRoC数据集上验证结果
为了验证本发明在弱纹理、光线变化剧烈的走廊等各种大型结构化建筑室内场景定位的需求，以及适应单兵在执行消防任务等紧急任务时需要快速运动等需求，在宿舍楼道环境里进行了实验验证，证明了一种基于视觉点线特征与IMU紧耦合的单兵/行人室内自主定位方法的有效性。在弱纹理走廊提起点线特征效果图如图4所示。进一步验证大范围的运动时本发明方法的有效性，模拟单兵/行人执行复杂任务时的运动轨迹，包括单兵从固定房间出发，穿过弱纹理、光线变化剧烈的走廊，经过楼梯上楼后，在走廊尽头掉头原路返回，下楼梯回到原来出发的地点。将本发明方法(IMPL-VINS)与目前开源的仅基于点特征的VINS-mono进行轨迹对比，VINS-mono方案的效果如图5所示，可以看出仅基于点特征的未改进方法估计的单兵运动轨迹不能回到原出发点。而本发明方法的效果如图6所示，可以看出本发明方法估计的单兵运动轨迹能回到原出发点，符合真实采集数据。
Claims (10)
1.一种基于视觉点线特征与IMU紧耦合的室内自主定位方法，其特征在于，所述方法包括以下步骤：
S1，对获取的IMU数据进行预积分处理；
S2，对获取的单目图像数据，构建Mask过滤机制获取点特征；
S3，对获取的单目图像数据，构建梯度密度过滤机制获取线特征；
S4，对步骤S2提取的点特征和步骤S3提取的线特征进行KLT光流跟踪匹配；
S5，先利用SFM进行单目视觉估计滑窗内所有帧的位姿以及路标点的逆深度，再通过视觉与IMU预积分融合对齐，紧耦合初始化求解初始化参数；
S6，在利用6参数普吕克坐标系表示线的初始化、三角化的基础上，提出利用在4参数正交坐标系中进行线特征的图优化；
S7，利用滑窗内的视觉点线特征约束、IMU约束、先验约束，一起构建残差目标函数进行非线性优化，求解滑窗中所有帧的状态量；
S8，根据关键帧的点线特征进行回环检测，与构建的点线词袋库匹配，并对滑窗状态进行反馈矫正；
S9，经过全局优化输出室内单兵/行人位姿估计。
2.根据权利要求1所述的基于视觉点线特征与IMU紧耦合的室内自主定位方法，其特征在于，所述步骤S2利用Harris角点检测提取点特征，首先将单目图像划分为若干网格区域，然后针对每个网格进行Harris角点提取，在每个网格区域提取质量最好的前N个点作为特征点。
3.根据权利要求2所述的基于视觉点线特征与IMU紧耦合的室内自主定位方法，其特征在于，所述步骤S2包括：
S2.1、设置每帧图像要提取的特征点数目、尺度、金字塔层数、Harris初始阈值、Harris最小阈值参数；
S2.2、将单目图像划分成第一像素大小T1×T1的网格区域，先根据Harris初始阈值依次对每块区域提取Harris角点；
S2.3、若提取不到特征点，再根据Harris最小阈值对每块区域提取Harris角点；
S2.4、如果再次提取不到特征点，则将该区域的编号标记并存入队列中，后续首先对该区域进行线特征检测；
S2.5、对于提取到的特征点，根据特征点的质量高低选取最优质的前N个特征点。
4.根据权利要求3所述的基于视觉点线特征与IMU紧耦合的室内自主定位方法，其特征在于，所述步骤S3包括：
S3.1、对于步骤S2中提取不到特征点所标记的区域，通过线特征LSD算法进行线特征检测；
S3.2，对步骤S2尚未标记的其他区域，构建梯度密度过滤机制对线特征密集区域进行处理，只保留线特征密集区域的最边缘方框的线特征，再进行LSD提取，得到线特征。
5.根据权利要求4所述的基于视觉点线特征与IMU紧耦合的室内自主定位方法，其特征在于，所述步骤S3.2中梯度密度过滤算法包括以下步骤：
S3.2.1、计算每个像素梯度Iij并与给定的第一阈值比较，针对高于第一阈值的像素存入待处理队列1中；Iij中下标i/j分别表示像素在图像中的第几行第几列；
S3.2.2、对该队列1里像素的下标再次进行筛选，另建一个队列2保存当且仅当行列下标值之差均大于第二阈值的像素；
S3.2.3、对队列2里的所有像素为中心，划定第二像素大小T2×T2的区域，统计每个区域中队列1中的像素量占T2×T2像素量的百分比τ；
S3.2.4、如果τ大于给定的阈值τthread，将该T2×T2像素区为中心的9个T2×T2像素区，都判定为线特征密集区域；
S3.2.5、只保留该3T2×3T2像素区域最边缘方框的线特征。
6.根据权利要求1所述的基于视觉点线特征与IMU紧耦合的室内自主定位方法，其特征在于，所述步骤S5中视觉与IMU预积分融合对齐包括：
S5.1、求解相机坐标系和IMU坐标系的旋转外参数qbc；
S5.2，通过旋转外参数qbc估计陀螺仪偏差bg；
S5.3、利用平移约束pbc估计重力方向、速度、尺度信息；
首先将这些待估计的量记为
其中，
表示k时刻载体坐标系的速度在载体坐标系下的表示，
为重力向量在第0帧相机坐标系下的表示，s表示尺度因子，
然后根据预积分公式，将世界坐标系w换成相机初始时刻坐标系c0有：
其中
是两时刻k,k+1之间的位置变化量，
是两时刻k,k+1之间的速度变化量；
是0时刻相机相对于k时刻IMU的旋转矩阵，
是k+1时刻IMU和0时刻相机之间的位移，
是k时刻IMU和0时刻相机之间的位移，
为k时刻IMU和0时刻相机的旋转矩阵，
是k+1时刻IMU的速度，
是k时刻IMU的速度，
进一步整理将待估计变量放到方程右边，有：
其中，
表示整理后的噪声量，
最后求解成线性最小二乘问题得到重力方向、速度、尺度状态量。
7.根据权利要求6所述的基于视觉点线特征与IMU紧耦合的室内自主定位方法，其特征在于，所述步骤S5.1包括：
已知相邻两时刻k,k+1之间有：IMU旋转积分
视觉测量
通过相机和IMU之间的变换关系，求得k+1时刻的相机位姿相对于k时刻的IMU位姿的相对坐标关系：
进一步写成：
[...]L,[...]R分别代表四元素左乘矩阵和右乘矩阵，
是对
的表示；
将多个时刻的上述方程累计起来，得到如下方程：
对该方程采用SVD分解，求得相机坐标系和IMU坐标系的旋转外参数qbc。
8.根据权利要求7所述的基于视觉点线特征与IMU紧耦合的室内自主定位方法，其特征在于，所述步骤5.2包括：利用IMU在k,k+1时刻间的约束，和相机在k,k+1之间的约束，构建如下最小二乘优化方程：
其中B表示所有的图像关键帧集合，通过求解该最小二乘最优解即可得到陀螺仪偏差bg，其中符号δ表示增量，
表示四元素对陀螺仪零偏的雅克比。
9.根据权利要求1所述的基于视觉点线特征与IMU紧耦合的室内自主定位方法，其特征在于，所述步骤S6中正交坐标系的参数为(U,W)∈SO(3)×SO(2)，从6参数到4参数的转换过程如下：
S6.1，从线的普吕克坐标中线的参数化进行如下转换：
其中，U∈SO(3),
n表示原点与直线
构成的平面的法向量，v表示直线
的方向向量；
S6.2、用
和
表示直线的待优化的参数pT＝(θT,θ)，
且以左乘的形式优化更新正交坐标U←Exp(θ)U,W←R(θ)W。
10.根据权利要求1所述的基于视觉点线特征与IMU紧耦合的室内自主定位方法，其特征在于，所述步骤S7包括：
S7.1、采用滑动窗口机制，滑窗中第i时刻的状态量为：
其中xi包括IMU的位置
速度
姿态
加速度零偏
陀螺仪零偏
λ表示滑窗内相机所看到的点特征路标的逆深度，
表示滑窗内相机所看到的线特征路标的正交参数表示；
S7.2、针对这些视觉点线特征约束、IMU约束以及先验约束，一起构建残差目标函数进行非线性优化，通过Levenberg–Marquard求解算法即可优化出所需要的状态量：
其中第一项为残差约束，rp表示先验残差，Jp表示先验项的雅克比；
第二项为IMU约束，B表示滑窗内所有的IMU帧集合，rb()表示IMU的残差项，
表示IMU预积分的测量项；
第三项为点特征约束，F表示滑窗内所有帧的特征点集合，rf()表示点特征残差项，
表示点特征的观测量；
第四项为线特征约束，L表示滑窗内所有帧的线特征集合，rl表示线特征残差项，
表示线特征的观测量；
各项的∑分别表示对应的协方差项；各项的ρ()表示Huber鲁棒核函数。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010362870.9A CN111595334B (zh) | 2020-04-30 | 2020-04-30 | 基于视觉点线特征与imu紧耦合的室内自主定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010362870.9A CN111595334B (zh) | 2020-04-30 | 2020-04-30 | 基于视觉点线特征与imu紧耦合的室内自主定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111595334A true CN111595334A (zh) | 2020-08-28 |
| CN111595334B CN111595334B (zh) | 2022-01-28 |
Family
ID=72186889
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010362870.9A Active CN111595334B (zh) | 2020-04-30 | 2020-04-30 | 基于视觉点线特征与imu紧耦合的室内自主定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111595334B (zh) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112562052A (zh) * | 2020-12-03 | 2021-03-26 | 广东工业大学 | 一种面向近岸水域的实时定位与建图方法 |
| CN112945233A (zh) * | 2021-01-15 | 2021-06-11 | 北京理工大学 | 一种全局无漂移的自主机器人同时定位与地图构建方法 |
| CN113790726A (zh) * | 2021-09-07 | 2021-12-14 | 中国科学院合肥物质科学研究院 | 一种融合相机、轮速计和单uwb信息的机器人室内定位方法 |
| WO2022073172A1 (zh) * | 2020-10-09 | 2022-04-14 | 浙江大学 | 一种基于点线特征的机器人全局最优视觉定位方法及装置 |
| CN114429500A (zh) * | 2021-12-14 | 2022-05-03 | 中国科学院深圳先进技术研究院 | 一种基于点线特征融合的视觉惯性定位方法 |
| CN114782447A (zh) * | 2022-06-22 | 2022-07-22 | 小米汽车科技有限公司 | 路面检测方法、装置、车辆、存储介质及芯片 |
| CN116442248A (zh) * | 2023-06-19 | 2023-07-18 | 山东工程职业技术大学 | 一种基于目标检测的机器人视觉定位模块及方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170205238A1 (en) * | 2016-01-16 | 2017-07-20 | Hon Hai Precision Industry Co., Ltd. | Indoor positioning method and indoor positioning system |
| CN107255476A (zh) * | 2017-07-06 | 2017-10-17 | 青岛海通胜行智能科技有限公司 | 一种基于惯性数据和视觉特征的室内定位方法和装置 |
| CN108090921A (zh) * | 2016-11-23 | 2018-05-29 | 中国科学院沈阳自动化研究所 | 单目视觉和imu融合的自适应室内定位方法 |
| CN109166149A (zh) * | 2018-08-13 | 2019-01-08 | 武汉大学 | 一种融合双目相机与imu的定位与三维线框结构重建方法与系统 |
| CN110044354A (zh) * | 2019-03-28 | 2019-07-23 | 东南大学 | 一种双目视觉室内定位与建图方法及装置 |
-
2020
- 2020-04-30 CN CN202010362870.9A patent/CN111595334B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170205238A1 (en) * | 2016-01-16 | 2017-07-20 | Hon Hai Precision Industry Co., Ltd. | Indoor positioning method and indoor positioning system |
| CN108090921A (zh) * | 2016-11-23 | 2018-05-29 | 中国科学院沈阳自动化研究所 | 单目视觉和imu融合的自适应室内定位方法 |
| CN107255476A (zh) * | 2017-07-06 | 2017-10-17 | 青岛海通胜行智能科技有限公司 | 一种基于惯性数据和视觉特征的室内定位方法和装置 |
| CN109166149A (zh) * | 2018-08-13 | 2019-01-08 | 武汉大学 | 一种融合双目相机与imu的定位与三维线框结构重建方法与系统 |
| CN110044354A (zh) * | 2019-03-28 | 2019-07-23 | 东南大学 | 一种双目视觉室内定位与建图方法及装置 |
Non-Patent Citations (2)
| Title |
|---|
| YANQING LIU ET AL.: "Stereo Visual-Inertial SLAM With Points and Lines", 《IEEE ACCESS》 * |
| 刘星等: "手机视觉与惯性融合的松耦合滤波算法", 《测绘通报》 * |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2022073172A1 (zh) * | 2020-10-09 | 2022-04-14 | 浙江大学 | 一种基于点线特征的机器人全局最优视觉定位方法及装置 |
| US11964401B2 (en) | 2020-10-09 | 2024-04-23 | Zhejiang University | Robot globally optimal visual positioning method and device based on point-line features |
| CN112562052A (zh) * | 2020-12-03 | 2021-03-26 | 广东工业大学 | 一种面向近岸水域的实时定位与建图方法 |
| CN112945233A (zh) * | 2021-01-15 | 2021-06-11 | 北京理工大学 | 一种全局无漂移的自主机器人同时定位与地图构建方法 |
| CN112945233B (zh) * | 2021-01-15 | 2024-02-20 | 北京理工大学 | 一种全局无漂移的自主机器人同时定位与地图构建方法 |
| CN113790726A (zh) * | 2021-09-07 | 2021-12-14 | 中国科学院合肥物质科学研究院 | 一种融合相机、轮速计和单uwb信息的机器人室内定位方法 |
| CN113790726B (zh) * | 2021-09-07 | 2024-03-29 | 中国科学院合肥物质科学研究院 | 一种融合相机、轮速计和单uwb信息的机器人室内定位方法 |
| CN114429500A (zh) * | 2021-12-14 | 2022-05-03 | 中国科学院深圳先进技术研究院 | 一种基于点线特征融合的视觉惯性定位方法 |
| CN114782447A (zh) * | 2022-06-22 | 2022-07-22 | 小米汽车科技有限公司 | 路面检测方法、装置、车辆、存储介质及芯片 |
| CN114782447B (zh) * | 2022-06-22 | 2022-09-09 | 小米汽车科技有限公司 | 路面检测方法、装置、车辆、存储介质及芯片 |
| CN116442248A (zh) * | 2023-06-19 | 2023-07-18 | 山东工程职业技术大学 | 一种基于目标检测的机器人视觉定位模块及方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111595334B (zh) | 2022-01-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111595334B (zh) | 基于视觉点线特征与imu紧耦合的室内自主定位方法 | |
| Zou et al. | StructVIO: Visual-inertial odometry with structural regularity of man-made environments | |
| CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
| CN109211241B (zh) | 基于视觉slam的无人机自主定位方法 | |
| CN106840148B (zh) | 室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法 | |
| CN107709928B (zh) | 用于实时建图与定位的方法和装置 | |
| CN104748751B (zh) | 姿态矩阵的计算方法及基于姿态矩阵的定位导航方法 | |
| US8711221B2 (en) | Visually tracking an object in real world using 2D appearance and multicue depth estimations | |
| CN109084732A (zh) | 定位与导航方法、装置及处理设备 | |
| CN109461208A (zh) | 三维地图处理方法、装置、介质和计算设备 | |
| CN110260866A (zh) | 一种基于视觉传感器的机器人定位与避障方法 | |
| CN112652001B (zh) | 基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统 | |
| CN110533719A (zh) | 基于环境视觉特征点识别技术的增强现实定位方法及装置 | |
| CN112556719A (zh) | 一种基于cnn-ekf的视觉惯性里程计实现方法 | |
| CN115574816A (zh) | 仿生视觉多源信息智能感知无人平台 | |
| Rahman et al. | Contour based reconstruction of underwater structures using sonar, visual, inertial, and depth sensor | |
| CN111812978B (zh) | 一种多无人机协作slam方法与系统 | |
| CN116772844A (zh) | 一种基于动态环境下的视觉惯性室内机器人的导航方法 | |
| CN112945233B (zh) | 一种全局无漂移的自主机器人同时定位与地图构建方法 | |
| CN113589848A (zh) | 基于机器视觉的多无人机探测定位跟踪系统及方法 | |
| CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
| CN115235454B (zh) | 行人运动约束的视觉惯性融合定位与建图方法和装置 | |
| Irmisch et al. | Robust visual-inertial odometry in dynamic environments using semantic segmentation for feature selection | |
| CN117629241B (zh) | 一种基于连续时间模型的多相机视觉惯性里程计优化方法 | |
| CN114993293B (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 |