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

1.本发明属于机器人定位技术领域，尤其涉及一种基于视觉惯性里程计的 室内机器人定位方法。


背景技术：

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


技术实现要素：

3.本发明的目的是为解决在室内弱纹理和黑暗环境下，基于视觉惯性里程 计定位精度低鲁棒性差的问题，而提出了一种基于点线特征融合的多传感器定位 方法。
4.本发明为解决上述技术问题所采取的技术方案是：一种基于点线特征融合的多传感器定位方法，所述方法具体包括以下步骤：步骤1、以单目相机作为视觉传感器读取图像，标定相机内参并对图像进行预处 理，惯性测量单元和轮式编码器数据以各自频率读入到系统中；步骤2、对预处理后的图像提取harris角点，用改进的lsd方法提取线特征， 基于光流法对提取线段的端点进行跟踪，通过将跟踪端点连接起来获得新的线特 征建立对应关系，利用线描述符剔除异常值，同时对惯性测量单元和轮式编码器 的测量数据进行预积分处理；步骤3、进行传感器松耦合初始化，对于每个进入滑动窗口的图像，使用五点法 法恢复相对旋转矩阵，进行视觉惯性坐标系对齐，利用惯性测量单元和轮式编码 器的预积分
值，对当前帧所有特征点的偏差求和并求取最小值以获得最优位姿结果，推导出特征点的 重投影误差为：其中，为特征点pi在cj帧估计的位姿进行投影得到的位置坐标，和为该点 的像素坐标。对于提取到的线特征，利用估计线段和提取线段的两个端点之间的几何距离的最小值 作为该线段的重投影误差，如图3所示。(xs,ys)、(xe,ye)为线段的两个端点坐标，l'1为 估计线段，为线特征l1在cj帧下的重投影误差：其中：构建的优化目标函数为：目标函数由四个部分组成，||r
p-j
p
x||2为滑动窗口的边缘化先验信息项， 分别为惯性测量单元和轮式编码器的预积分残差项，为点 特征的重投影残差，为线特征的重投影残差，其中和p
l
分别为惯性测 量单元约束、轮式编码器约束和相机约束，l表示特征点的索引，i表示线特征的索引，c 表示特征出现的图像组。
附图说明
9.图1为本发明的一种基于点线特征融合的多传感器室内定位方法的整体 框架示意图；图2为计算线特征重投影误差示意图；图3为本发明实例中在室内环境下提取线特征和光流法跟踪图；
具体实施方式
10.具体实施方式一、结合图1说明本实施方式。本实施方式所述的一种基 于点线特征融合的多传感器室内定位方法，所述方法具体包括以下步骤：
步骤1、以单目相机作为视觉传感器读取图像，标定相机内参并对图像进行预处 理，惯性测量单元和轮式编码器数据以各自频率读入到系统中，并完成时间同步； 步骤2、对预处理后的图像提取harris角点，用改进的lsd方法提取线特征， 并采用光流法进行帧间的跟踪与匹配，同时对惯性测量单元和轮式编码器的测量 数据进行预积分处理；步骤3、进行传感器松耦合初始化，对于每个进入滑动窗口的图像，使用五点法 恢复相对旋转矩阵，进行视觉惯性坐标系对齐，利用惯性测量单元和轮式编码器 的预积分结果来优化重力并进行速度初始化；步骤4、计算点线特征的重投影误差以及惯性测量单元和轮式编码器的预积分残 差项，加入到滑动窗口队列中进行后端联合优化实现多传感器的紧耦合优化，估 计出当前时刻对应的位姿和速度。
11.本发明是一种基于点线特征融合的多传感器室内定位方法，该方法采用 相机、惯性测量单元和轮式编码器紧耦合的方式融合；利用室内环境内相对丰富 的线特征实现更精准的定位，为了保证方法的实时性采用改进的lsd算法提取 线特征，并通过光流法进行帧间匹配，最后基于滑动窗口方法进行非线性优化获 得优化后的位姿估计。
12.具体实施方式二：将单目相机安装在地面移动机器人本体的正前方，在 机器人运行的过程中，将采集到的图像传输到机器人上搭载的计算机中，同时轮 式编码器和惯性测量单元将当前时刻的角速度与线速度输入到计算机中，计算机 通过ros进行数据传输和发送，以此获得三个传感器的数据输入。
13.具体实施方式三：本实施方式与具体实施方式一或二不同的是，所述步 骤2的具体实施过程为：步骤21、点特征的提取与匹配方法。首先使用shi-tomasi算法检测当前输入图 像的harris角点，角点对应于图像中物体的拐点，可以是两个边缘的交点，也可 以是像素点邻域中具有两个主方向的点，并且保证图像中的点特征数量不少于阈 值。使用klt金字塔光流追踪法，从当前帧的点跟踪到上一帧的角点，同时删 除跟踪失败的点；步骤22、线特征的提取与跟踪方法。使用lsd线段检测器提取图像中的线段， lsd算法的目标在于检测图像中局部的直的轮廓，会检测到许多难以进行匹配 跟踪的短线，因此选择使用改进的lsd算法。优化后的lsd算法主要体现在， 对采样图像缩放参数进行调整，以及对短线段进行过滤，利用线段长度最小阈值 来过滤不达标的线段，其中阈值取0.1。提取到线特征之后进行线特征的匹配。 使用lk光流法进行跟踪，线特征的光流跟踪算法流程如下：首先对图像建立n级高斯金字塔，针对当前帧检测到的线段l1取其端点为步骤23、惯性测量单元和轮式编码器的测量值为加速度、角速度和线速度，通 过预积分获得两帧之间的旋转和位移变换。同时，在进行非线性优化时，采用预 积分的方式可以避免重新传递测量值，因此对惯性测量单元和轮式编码器的测量 值采用预积分策略进行处理。在两个连续的关键帧之间，采用四元数参数化旋转， 并传播协方差矩阵；其它步骤及参数与具体实施方式一或二相同。
14.具体实施方式四：本实施方式与具体实施方式一至三之一不同的是，所述步骤3的具 体过程为：步骤31、首先通过计算惯性测量单元加速度的标准差判断机器人在初始是否进行
了 充足的旋转变化，为之后进行更准确全面的状态估计做基础。接下来将当前滑动窗口内的 所有特征进行纯视觉sfm估计相机初始位姿，并计算基础矩阵和本征矩阵，通过本征矩 阵计算帧间的运动，求解图像的旋转和平移参数。步骤32、将解算出的状态量对齐到第0帧惯性测量单元的坐标系。首先利用推导出 的旋转约束估计陀螺仪的零偏bgs，利用平移约束估计尺度因子、速度和重力方向。将相机坐标系对齐到惯性测量单元坐标系进行转换时需要引入尺度因子s，由于轮式 编码器的测量数据是基于实际尺度的，利用轮式编码器和惯性测量单元的平移约束建立等 式，经过整理可以得到公式：其中表示从编码器坐标原点到惯性测量单元坐标系的变换矩阵，表示从相机坐 标原点到惯性测量单元坐标系的变换矩阵，是通过预积分结果推算得到的imu坐标 系的从初始时刻坐标系到第k帧坐标系旋转矩阵，接下来利用惯性测量单元和轮式编码器 的约束估计速度和重力方向的初始值，推导公式如下：其中，为imu加速度的预积分值，为imu角速度的预积分值，为imu 线速度的预积分值，为轮式编码器位移的预积分值，δtk代表在第k张图像和第k+1 张图像的时间间隔。初始的重力向量为b由3
×
2的基础矩阵组成， 即重力切线空间的两个正交基，δg用于更新重力向量。将公式转换为最小二乘问题对其 右侧的待优化变量进行求解，对连续图像帧之间的公式迭代直到收敛。最终，通过滑动窗 口中的关键帧传播位置推算位姿，使用这些位姿信息进行三角化计算，初始化后位置和速 度都是在真实比例下计算的。其它步骤及参数与具体实施方式一至三之一相同。
15.具体实施方式五：本实施方式与具体实施方式一至四之一不同的是，所述步骤4的具 体过程为：采用滑动窗口方法进行非线性优化估计地面移动机器人的当前位姿，通过点线重投影 误差和imu以及轮式编码器预积分残差项构建目标函数进行高精度和鲁棒的状态估计。 本发明使用的目标函数c(x)由边缘化项、点线重投影误差以及惯性测量单元和轮式编码 器残差项组成。步骤41、构建点线重投影残差。对于匹配成功的点特征，已知当前帧在真实三维空 间的点坐标以及计算得到的相机位姿，使用光束平差法进行二次投影并取两次投影结果的 差值，对当前帧所有特征点的偏差求和并求取最小值以获得最优位姿结果，推导出特征点 的重投影误差为：
其中，为特征点pi在cj帧估计的位姿进行投影得到的位置坐标，和为该点 的像素坐标。对于提取到的线特征，利用估计线段和提取线段的两个端点之间的几何距离的最小值 作为该线段的重投影误差，如图3所示。(xs,ys)、(xe,ye)为线段的两个端点坐标，l'1为 估计线段，为线特征l1在cj帧下的重投影误差：其中：步骤42、惯性测量单元和轮式编码器残差。分别为利用惯性测 量单元和轮式编码器的预积分结果推导出的残差项。元和轮式编码器的预积分结果推导出的残差项。元和轮式编码器的预积分结果推导出的残差项。和分别代表利用惯性测量单元和轮式编码器计算预积分残差， δb
ak
、δb
ωk
分别表示陀螺仪和加速度计偏置估计的误差项。步骤43、进行目标函数的构建。本发明进行非线性优化的目标函数如下所示：目标函数由四个部分组成，||r
p-j
p
x||2为滑动窗口的边缘化先验信息项， 分别为惯性测量单元和轮式编码器的预积分残差项，为 点特征的重投影残差,为线特征的重投影残差，其中和p
l
分别为惯性 测量单元约束、轮式编码器约束和相机约束，l表示特征点的索引，i表示线特征的索引， c表示特征出现的图像组。非线性优化由ceres solver进行。
其它步骤及参数与具体实施方式一至四之一相同。
16.本实例针对地面移动机器人在室内低纹理或者光照不均匀的情况下定位 精度低的问题，基于传统的视觉惯性里程计定位系统，通过在后端优化部分用紧 耦合的方式融合轮式编码器信息，并在前端增加了线特征的提取与跟踪模块，跟 踪效果如图2所示。本发明通过同时利用图像中的点特征和线特征以提升图像信 息的利用率，将提取出的点线特征使用光流法跟踪提高特征的匹配效率，同时融 合轮式编码器的真实尺度信息和平面约束进一步优化定位信息，在保证定位精度 的同时，提高了移动机器人在室内环境下定位的鲁棒性；本发明的上述算例仅为详细地说明本发明的计算模型和计算流程，而并非是对本 发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础 上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷 举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明 的保护范围之列。