一种基于点线特征融合的多传感器室内定位方法技术

一种基于点线特征融合的多传感器室内定位方法，它属于机器人定位技术领域。由于缺乏足够的视觉信息和尺度信息导致的定位精度不高问题，以及以恒定加速度沿直线或圆弧进行运动的轮式机器人进行定位时，单目视觉惯性里程计出现额外不可观方向的问题，本发明专利技术提出了一种基于点线特征融合的多传感器室内定位方法，在前端增加线特征的提取与跟踪模块，充分利用图像信息，并且在预积分阶段融合轮式编码器测量信息，以便实现四自由度的非线性优化，获得更准确的环境尺度信息。将轮式编码器的测量信息、惯性测量单元和视觉信息进行联合初始化，为位姿估计提供更准确鲁棒的初始值。后端采用了基于滑动窗口的非线性优化方法，融合轮式编码器、惯性测量单元约束和视觉约束进行联合优化，实现在室内弱纹理或黑暗环境中的精确鲁棒的定位方案。的定位方案。的定位方案。

【技术实现步骤摘要】
一种基于点线特征融合的多传感器室内定位方法


[0001]本专利技术属于机器人定位
，尤其涉及一种基于视觉惯性里程计的 室内机器人定位方法。

技术介绍

[0002]为了达到在未知环境中的自主运动的目的，地面移动机器人通过自身携 带的传感器对环境进行探索，并估计自身在环境中的位姿，这个任务要求对应的 就是定位问题，也是地面机器人能够自主执行指定任务的基本条件。同时定位与 地图构建(Simultaneous Localization and Mapping,SLAM)技术就很好的解决了 机器人在未知环境下的定位与建图问题。在室外环境下，机器人可以利用全球定 位系统进行定位，而在室内、水下、隧道、森林等GPS不可用的环境下，移动 机器人必须使用其他传感器来进行定位。SLAM可以通过多种传感器来实现，如 激光雷达、相机、超声波、超宽带等，相比之下体积小、价格低且安装方便的相 机成为了SLAM中最受欢迎和最有潜力的传感器。近年来，许多学者在视觉 SLAM的领域做出了许多出色的工作，进一步将IMU与相机融合可以大幅提高 SLAM的准确性和鲁棒性，因此视觉惯性里程计(visual inertial odometry,VIO) 得到了广泛关注。轮式机器人配备的轮式编码器能够提供更精确的相对运动信 息，在位姿估计中轮式编码器和相机的属性是互补的，同时通过融合轮式编码器 信息可以解决轮式机器人在以恒定加速度沿着直线或圆弧进行运动时，单目视觉 惯性里程计出现的额外不可观方向的问题。现有的融合轮式编码器信息的定位方 法一类是将单目信息与轮式编码器相融合，但存在缺少精确旋转测量值的问题。 另一种将单目信息与轮式编码器和惯性测量单元相融合的方案，但惯性测量单元 在地面轮式机器人的运动方式下无法提供精确的加速度测量值，也不是最优选 择。

技术实现思路

[0003]本专利技术的目的是为解决在室内弱纹理和黑暗环境下，基于视觉惯性里程 计定位精度低鲁棒性差的问题，而提出了一种基于点线特征融合的多传感器定位 方法。
[0004]本专利技术为解决上述技术问题所采取的技术方案是：一种基于点线特征融合的多传感器定位方法，所述方法具体包括以下步骤：步骤1、以单目相机作为视觉传感器读取图像，标定相机内参并对图像进行预处 理，惯性测量单元和轮式编码器数据以各自频率读入到系统中；步骤2、对预处理后的图像提取Harris角点，用改进的LSD方法提取线特征， 基于光流法对提取线段的端点进行跟踪，通过将跟踪端点连接起来获得新的线特 征建立对应关系，利用线描述符剔除异常值，同时对惯性测量单元和轮式编码器 的测量数据进行预积分处理；步骤3、进行传感器松耦合初始化，对于每个进入滑动窗口的图像，使用五点法 法恢复相对旋转矩阵，进行视觉惯性坐标系对齐，利用惯性测量单元和轮式编码 器的预积分
值，对当前帧所有特征点的偏差求和并求取最小值以获得最优位姿结果，推导出特征点的 重投影误差为：其中，为特征点p
i
在C
j
帧估计的位姿进行投影得到的位置坐标，和为该点 的像素坐标。对于提取到的线特征，利用估计线段和提取线段的两个端点之间的几何距离的最小值 作为该线段的重投影误差，如图3所示。(x
s
,y
s
)、(x
e
,y
e
)为线段的两个端点坐标，L'1为 估计线段，为线特征L1在C
j
帧下的重投影误差：其中：构建的优化目标函数为：目标函数由四个部分组成，||r
p
‑
J
p
X||2为滑动窗口的边缘化先验信息项， 分别为惯性测量单元和轮式编码器的预积分残差项，为点 特征的重投影残差，为线特征的重投影残差，其中和P
l
分别为惯性测 量单元约束、轮式编码器约束和相机约束，l表示特征点的索引，i表示线特征的索引，C 表示特征出现的图像组。
附图说明
[0009]图1为本专利技术的一种基于点线特征融合的多传感器室内定位方法的整体 框架示意图；图2为计算线特征重投影误差示意图；图3为本专利技术实例中在室内环境下提取线特征和光流法跟踪图；
具体实施方式
[0010]具体实施方式一、结合图1说明本实施方式。本实施方式所述的一种基 于点线特征融合的多传感器室内定位方法，所述方法具体包括以下步骤：
步骤1、以单目相机作为视觉传感器读取图像，标定相机内参并对图像进行预处 理，惯性测量单元和轮式编码器数据以各自频率读入到系统中，并完成时间同步； 步骤2、对预处理后的图像提取Harris角点，用改进的LSD方法提取线特征， 并采用光流法进行帧间的跟踪与匹配，同时对惯性测量单元和轮式编码器的测量 数据进行预积分处理；步骤3、进行传感器松耦合初始化，对于每个进入滑动窗口的图像，使用五点法 恢复相对旋转矩阵，进行视觉惯性坐标系对齐，利用惯性测量单元和轮式编码器 的预积分结果来优化重力并进行速度初始化；步骤4、计算点线特征的重投影误差以及惯性测量单元和轮式编码器的预积分残 差项，加入到滑动窗口队列中进行后端联合优化实现多传感器的紧耦合优化，估 计出当前时刻对应的位姿和速度。
[0011]本专利技术是一种基于点线特征融合的多传感器室内定位方法，该方法采用 相机、惯性测量单元和轮式编码器紧耦合的方式融合；利用室内环境内相对丰富 的线特征实现更精准的定位，为了保证方法的实时性采用改进的LSD算法提取 线特征，并通过光流法进行帧间匹配，最后基于滑动窗口方法进行非线性优化获 得优化后的位姿估计。
[0012]具体实施方式二：将单目相机安装在地面移动机器人本体的正前方，在 机器人运行的过程中，将采集到的图像传输到机器人上搭载的计算机中，同时轮 式编码器和惯性测量单元将当前时刻的角速度与线速度输入到计算机中，计算机 通过ROS进行数据传输和发送，以此获得三个传感器的数据输入。
[0013]具体实施方式三：本实施方式与具体实施方式一或二不同的是，所述步 骤2的具体实施过程为：步骤21、点特征的提取与匹配方法。首先使用Shi
‑
Tomasi算法检测当前输入图 像的Harris角点，角点对应于图像中物体的拐点，可以是两个边缘的交点，也可 以是像素点邻域中具有两个主方向的点，并且保证图像中的点特征数量不少于阈 值。使用KLT金字塔光流追踪法，从当前帧的点跟踪到上一帧的角点，同时删 除跟踪失败的点；步骤22、线特征的提取与跟踪方法。使用LSD线段检测器提取图像中的线段， LSD算法的目标在于检测图像中局部的直的轮廓，会检测到许多难以进行匹配 跟踪的短线，因此选择使用改进的LSD算法。优化后的LSD算法主要体现在， 对采样图像缩放参数进行调整，以及对短线段进行过滤，利用线段长度最小阈值 来过滤不达标的线段，其中阈值取0.1。提取到线特征之后进行线特征的匹配。 使用LK光流法进行跟踪，线特征的光流跟踪算法流程如下：首先对图像建立n级高斯金字塔，针对当前帧检测到的线段L1取其端点为步骤23、惯性测量单元和轮式编本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于点线特征融合的多传感器室内定位方法，其特征在于，所述方法具体包括以下步骤：步骤1、以单目相机作为视觉传感器读取图像，标定相机内参并对图像进行预处理，惯性测量单元和轮式编码器数据以各自频率读入到系统中；步骤2、对预处理后的图像提取Harris角点基于光流法对提取线段的端点进行跟踪，通过将跟踪端点连接起来获得新的线特征建立对应关系，利用线描述符剔除异常值，同时对惯性测量单元和轮式编码器的测量数据进行预积分处理。步骤3、进行传感器松耦合初始化，对于每个进入滑动窗口的图像，使用五点法纯视觉的方法恢复相对旋转矩阵，进行视觉惯性坐标系对齐，利用惯性测量单元和轮式编码器的预积分结果来优化重力并进行速度初始化；步骤4、计算点线特征的重投影误差以及惯性测量单元和轮式编码器的预积分残差项，加入到滑动窗口队列中进行后端联合优化实现多传感器的紧耦合优化，估计出当前时刻对应的位姿和速度。2.根据权利要求1所述的一种基于点线特征融合的室内多传感器定位方法，其特征在于，将采集到的图像传输到机器人上搭载的计算机中，同时轮式编码器和惯性测量单元将当前时刻的角速度与线速度输入到计算机中，计算机通过ROS进行数据传输和发送，以此获得三个传感器的数据输入。3.根据权利要求2所述的一种基于点线特征融合的室内多传感器定位方法，其特征在于，使用LSD线段检测器提取图像中的线段，LSD算法的目标在于检测图像中局部的直的轮廓，会检测到许多难以进行匹配跟踪的短线，因此选择使用改进的LSD算法。优化后的LSD算法主要体现在，对采样图像缩放参数进行调整，以及对短线段进行过滤，利用线段长度最小阈值来过滤不达标的线段，其中阈值取0.1。提取到线特征之后进行线特征的匹配。使用LK光流法进行跟踪，线特征的光流跟踪算法流程如下：首先对图像建立n级高斯金字塔，针对当前帧检测到的线段L1取其端点为L1(x
s
,y
s
)和L1(x
e
,y
e
)，估计两个端点的运动向量分别为r1和r2，从而获得在下一帧图像中跟踪到的线段的两个端点L2(x
s
,y
s
)和L2(x
e
,y
e
)，由端点获得下一帧跟踪到的线段L2，最后对于可能出现的异常点，利用基于几何约束的线性细化方法进行筛除。4.根据权利要求3所述的一种基于点线特征融合的室内多传感器定位方法，其特征在于，所述步骤3中将相机坐标系对齐到惯性测量单元坐标系进行转换...

【专利技术属性】
技术研发人员：曹宇，郝文敏，赵新洋，李新年，唐悦，
申请(专利权)人：哈尔滨理工大学，
类型：发明
国别省市：