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

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

Info

Publication number
CN115218889A
CN115218889A CN202210776807.9A CN202210776807A CN115218889A CN 115218889 A CN115218889 A CN 115218889A CN 202210776807 A CN202210776807 A CN 202210776807A CN 115218889 A CN115218889 A CN 115218889A
Authority
CN
China
Prior art keywords
line
measurement unit
image
inertial measurement
encoder
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.)
Pending
Application number
CN202210776807.9A
Other languages
English (en)
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.)
Harbin University of Science and Technology
Original Assignee
Harbin 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 Harbin University of Science and Technology filed Critical Harbin University of Science and Technology
Priority to CN202210776807.9A priority Critical patent/CN115218889A/zh
Publication of CN115218889A publication Critical patent/CN115218889A/zh
Pending legal-status Critical Current

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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

Landscapes

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

Abstract

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

Description

一种基于点线特征融合的多传感器室内定位方法
技术领域
本发明属于机器人定位技术领域，尤其涉及一种基于视觉惯性里程计的 室内机器人定位方法。
背景技术
为了达到在未知环境中的自主运动的目的，地面移动机器人通过自身携 带的传感器对环境进行探索，并估计自身在环境中的位姿，这个任务要求对应的 就是定位问题，也是地面机器人能够自主执行指定任务的基本条件。同时定位与 地图构建(SimultaneousLocalization and Mapping,SLAM)技术就很好的解决了 机器人在未知环境下的定位与建图问题。在室外环境下，机器人可以利用全球定 位***进行定位，而在室内、水下、隧道、森林等GPS不可用的环境下，移动 机器人必须使用其他传感器来进行定位。SLAM可以通过多种传感器来实现，如 激光雷达、相机、超声波、超宽带等，相比之下体积小、价格低且安装方便的相 机成为了SLAM中最受欢迎和最有潜力的传感器。近年来，许多学者在视觉 SLAM的领域做出了许多出色的工作，进一步将IMU与相机融合可以大幅提高 SLAM的准确性和鲁棒性，因此视觉惯性里程计(visual inertial odometry,VIO) 得到了广泛关注。轮式机器人配备的轮式编码器能够提供更精确的相对运动信 息，在位姿估计中轮式编码器和相机的属性是互补的，同时通过融合轮式编码器 信息可以解决轮式机器人在以恒定加速度沿着直线或圆弧进行运动时，单目视觉 惯性里程计出现的额外不可观方向的问题。现有的融合轮式编码器信息的定位方 法一类是将单目信息与轮式编码器相融合，但存在缺少精确旋转测量值的问题。 另一种将单目信息与轮式编码器和惯性测量单元相融合的方案，但惯性测量单元 在地面轮式机器人的运动方式下无法提供精确的加速度测量值，也不是最优选择。
发明内容
本发明的目的是为解决在室内弱纹理和黑暗环境下，基于视觉惯性里程 计定位精度低鲁棒性差的问题，而提出了一种基于点线特征融合的多传感器定位 方法。
本发明为解决上述技术问题所采取的技术方案是：
一种基于点线特征融合的多传感器定位方法，所述方法具体包括以下步骤：
步骤1、以单目相机作为视觉传感器读取图像，标定相机内参并对图像进行预处理，惯性测量单元和轮式编码器数据以各自频率读入到***中；
步骤2、对预处理后的图像提取Harris角点，用改进的LSD方法提取线特征， 基于光流法对提取线段的端点进行跟踪，通过将跟踪端点连接起来获得新的线特 征建立对应关系，利用线描述符剔除异常值，同时对惯性测量单元和轮式编码器 的测量数据进行预积分处理；
步骤3、进行传感器松耦合初始化，对于每个进入滑动窗口的图像，使用五点法 法恢复相对旋转矩阵，进行视觉惯性坐标系对齐，利用惯性测量单元和轮式编码 器的预积分结果来优化重力并进行速度初始化；
步骤4、计算点线特征的重投影误差以及惯性测量单元和轮式编码器的预积分残差项，加入到滑动窗口队列中进行后端联合优化实现多传感器的紧耦合优化，估 计出当前时刻对应的位姿和速度。
进一步地，所述步骤1中将采集到的图像传输到机器人上搭载的计算机 中，同时轮式编码器和惯性测量单元将当前时刻的角速度与线速度输入到计算机 中，计算机通过ROS进行数据传输和发送，以此获得三个传感器的数据输入。
进一步地，所述步骤2中，使用LSD线段检测器提取图像中的线段，LSD 算法的目标在于检测图像中局部的直的轮廓，会检测到许多难以进行匹配跟踪的 短线，因此选择使用改进的LSD算法。优化后的LSD算法主要体现在，对采样 图像缩放参数进行调整，以及对短线段进行过滤，利用线段长度最小阈值来过滤 不达标的线段，其中阈值取0.1。提取到线特征之后进行线特征的匹配。使用LK 光流法进行跟踪，线特征的光流跟踪算法流程如下：首先对图像建立n级高斯金 字塔，针对当前帧检测到的线段L1取其端点为L1(xk，yk)和L1(xe，ye)，估计两个端 点的运动向量分别为r1和r2，从而获得在下一帧图像中跟踪到的线段的两个端点 L2(xs，ys)和L2(xe，ye)，由端点获得下一帧跟踪到的线段L2，最后对于可能出现的异常点，利用基于几何约束的线性细化方法进行筛除。
进一步地，所述步骤3中将相机坐标系对齐到惯性测量单元坐标系进行转换时需要引 入尺度因子s，由于轮式编码器的测量数据是基于实际尺度的，利用轮式编码器和惯性测 量单元的平移约束建立等式，经过整理可以得到尺度因子的估计公式为：
Figure RE-GDA0003832952010000021
其中
Figure RE-GDA0003832952010000022
表示从编码器坐标原点到惯性测量单元坐标系的变换矩阵，
Figure RE-GDA0003832952010000023
表示从相机坐 标原点到惯性测量单元坐标系的变换矩阵，
Figure RE-GDA0003832952010000031
是通过预积分结果推算得到的IMU坐标 系的从初始时刻坐标系到第k帧坐标系旋转矩阵，接下来利用惯性测量单元和轮式编码器 的约束估计速度和重力方向的初始值，推导公式如下：
Figure RE-GDA0003832952010000032
其中，
Figure RE-GDA0003832952010000033
为IMU加速度的预积分值，
Figure RE-GDA0003832952010000034
为IMU角速度的预积分值，
Figure RE-GDA0003832952010000035
为IMU 线速度的预积分值，
Figure RE-GDA0003832952010000036
为轮式编码器位移的预积分值，Δtk代表在第k张图像和第k+1 张图像的时间间隔。初始的重力向量为
Figure RE-GDA0003832952010000037
B由3×2的基础矩阵组成， 即重力切线空间的两个正交基，Δg用于更新重力向量。将公式转换为最小二乘问题对其 右侧的待优化变量进行求解，对连续图像帧之间的公式迭代直到收敛。最终，通过滑动窗 口中的关键帧传播位置推算位姿，使用这些位姿信息进行三角化计算，初始化后位置和速 度都是在真实比例下计算的。
进一步地，所述步骤4中，对于匹配成功的点特征，已知当前帧在真实三维空间的点坐标以及计算得到的相机位姿，使用光束平差法进行二次投影并取两次投影结果的差值，对当前帧所有特征点的偏差求和并求取最小值以获得最优位姿结果，推导出特征点的重投影误差为：
Figure RE-GDA0003832952010000038
其中，
Figure RE-GDA0003832952010000039
为特征点pi在Cj帧估计的位姿进行投影得到的位置坐标，
Figure RE-GDA00038329520100000310
和
Figure RE-GDA00038329520100000311
为该点的像素坐标。
对于提取到的线特征，利用估计线段和提取线段的两个端点之间的几何距离的最小值 作为该线段的重投影误差，如图3所示。(xs,ys)、(xe,ye)为线段的两个端点坐标，L'1为估计线段，
Figure RE-GDA00038329520100000312
为线特征L1在Cj帧下的重投影误差：
Figure RE-GDA00038329520100000313
其中：
Figure RE-GDA0003832952010000041
构建的优化目标函数为：
Figure RE-GDA0003832952010000042
目标函数由四个部分组成，||rp-JpX||2为滑动窗口的边缘化先验信息项，
Figure RE-GDA0003832952010000043
分别为惯性测量单元和轮式编码器的预积分残差项，
Figure RE-GDA0003832952010000044
为点特征的重投影残差，
Figure RE-GDA0003832952010000045
为线特征的重投影残差，其中
Figure RE-GDA0003832952010000046
和Pl分别为惯性测量单元约束、轮式编码器约束和相机约束，l表示特征点的索引，i表示线特征的索引，C 表示特征出现的图像组。
附图说明
图1为本发明的一种基于点线特征融合的多传感器室内定位方法的整体 框架示意图；
图2为计算线特征重投影误差示意图；
图3为本发明实例中在室内环境下提取线特征和光流法跟踪图；
具体实施方式
具体实施方式一、结合图1说明本实施方式。本实施方式所述的一种基 于点线特征融合的多传感器室内定位方法，所述方法具体包括以下步骤：
步骤1、以单目相机作为视觉传感器读取图像，标定相机内参并对图像进行预处理，惯性测量单元和轮式编码器数据以各自频率读入到***中，并完成时间同步； 步骤2、对预处理后的图像提取Harris角点，用改进的LSD方法提取线特征， 并采用光流法进行帧间的跟踪与匹配，同时对惯性测量单元和轮式编码器的测量 数据进行预积分处理；
步骤3、进行传感器松耦合初始化，对于每个进入滑动窗口的图像，使用五点法 恢复相对旋转矩阵，进行视觉惯性坐标系对齐，利用惯性测量单元和轮式编码器 的预积分结果来优化重力并进行速度初始化；
步骤4、计算点线特征的重投影误差以及惯性测量单元和轮式编码器的预积分残差项，加入到滑动窗口队列中进行后端联合优化实现多传感器的紧耦合优化，估 计出当前时刻对应的位姿和速度。
本发明是一种基于点线特征融合的多传感器室内定位方法，该方法采用 相机、惯性测量单元和轮式编码器紧耦合的方式融合；利用室内环境内相对丰富 的线特征实现更精准的定位，为了保证方法的实时性采用改进的LSD算法提取 线特征，并通过光流法进行帧间匹配，最后基于滑动窗口方法进行非线性优化获 得优化后的位姿估计。
具体实施方式二：将单目相机安装在地面移动机器人本体的正前方，在 机器人运行的过程中，将采集到的图像传输到机器人上搭载的计算机中，同时轮 式编码器和惯性测量单元将当前时刻的角速度与线速度输入到计算机中，计算机 通过ROS进行数据传输和发送，以此获得三个传感器的数据输入。
具体实施方式三：本实施方式与具体实施方式一或二不同的是，所述步 骤2的具体实施过程为：
步骤21、点特征的提取与匹配方法。首先使用Shi-Tomasi算法检测当前输入图 像的Harris角点，角点对应于图像中物体的拐点，可以是两个边缘的交点，也可 以是像素点邻域中具有两个主方向的点，并且保证图像中的点特征数量不少于阈 值。使用KLT金字塔光流追踪法，从当前帧的点跟踪到上一帧的角点，同时删 除跟踪失败的点；
步骤22、线特征的提取与跟踪方法。使用LSD线段检测器提取图像中的线段， LSD算法的目标在于检测图像中局部的直的轮廓，会检测到许多难以进行匹配 跟踪的短线，因此选择使用改进的LSD算法。优化后的LSD算法主要体现在， 对采样图像缩放参数进行调整，以及对短线段进行过滤，利用线段长度最小阈值 来过滤不达标的线段，其中阈值取0.1。提取到线特征之后进行线特征的匹配。 使用LK光流法进行跟踪，线特征的光流跟踪算法流程如下：
首先对图像建立n级高斯金字塔，针对当前帧检测到的线段L1取其端点为
步骤23、惯性测量单元和轮式编码器的测量值为加速度、角速度和线速度，通 过预积分获得两帧之间的旋转和位移变换。同时，在进行非线性优化时，采用预 积分的方式可以避免重新传递测量值，因此对惯性测量单元和轮式编码器的测量 值采用预积分策略进行处理。在两个连续的关键帧之间，采用四元数参数化旋转， 并传播协方差矩阵；
其它步骤及参数与具体实施方式一或二相同。
具体实施方式四：本实施方式与具体实施方式一至三之一不同的是，所述步骤3的具 体过程为：
步骤31、首先通过计算惯性测量单元加速度的标准差判断机器人在初始是否进行了 充足的旋转变化，为之后进行更准确全面的状态估计做基础。接下来将当前滑动窗口内的 所有特征进行纯视觉SFM估计相机初始位姿，并计算基础矩阵和本征矩阵，通过本征矩阵计算帧间的运动，求解图像的旋转和平移参数。
步骤32、将解算出的状态量对齐到第0帧惯性测量单元的坐标系。首先利用推导出的旋转约束估计陀螺仪的零偏Bgs，利用平移约束估计尺度因子、速度和重力方向。
将相机坐标系对齐到惯性测量单元坐标系进行转换时需要引入尺度因子s，由于轮式 编码器的测量数据是基于实际尺度的，利用轮式编码器和惯性测量单元的平移约束建立等 式，经过整理可以得到公式：
Figure RE-GDA0003832952010000061
其中
Figure RE-GDA0003832952010000062
表示从编码器坐标原点到惯性测量单元坐标系的变换矩阵，
Figure RE-GDA0003832952010000063
表示从相机坐 标原点到惯性测量单元坐标系的变换矩阵，
Figure RE-GDA0003832952010000064
是通过预积分结果推算得到的IMU坐标 系的从初始时刻坐标系到第k帧坐标系旋转矩阵，接下来利用惯性测量单元和轮式编码器 的约束估计速度和重力方向的初始值，推导公式如下：
Figure RE-GDA0003832952010000065
其中，
Figure RE-GDA0003832952010000066
为IMU加速度的预积分值，
Figure RE-GDA0003832952010000067
为IMU角速度的预积分值，
Figure RE-GDA0003832952010000068
为IMU 线速度的预积分值，
Figure RE-GDA0003832952010000069
为轮式编码器位移的预积分值，Δtk代表在第k张图像和第k+1 张图像的时间间隔。初始的重力向量为
Figure RE-GDA00038329520100000610
B由3×2的基础矩阵组成， 即重力切线空间的两个正交基，Δg用于更新重力向量。将公式转换为最小二乘问题对其 右侧的待优化变量进行求解，对连续图像帧之间的公式迭代直到收敛。最终，通过滑动窗 口中的关键帧传播位置推算位姿，使用这些位姿信息进行三角化计算，初始化后位置和速 度都是在真实比例下计算的。
其它步骤及参数与具体实施方式一至三之一相同。
具体实施方式五：本实施方式与具体实施方式一至四之一不同的是，所述步骤4的具 体过程为：
采用滑动窗口方法进行非线性优化估计地面移动机器人的当前位姿，通过点线重投影 误差和IMU以及轮式编码器预积分残差项构建目标函数进行高精度和鲁棒的状态估计。 本发明使用的目标函数c(x)由边缘化项、点线重投影误差以及惯性测量单元和轮式编码 器残差项组成。
步骤41、构建点线重投影残差。对于匹配成功的点特征，已知当前帧在真实三维空间的点坐标以及计算得到的相机位姿，使用光束平差法进行二次投影并取两次投影结果的差值，对当前帧所有特征点的偏差求和并求取最小值以获得最优位姿结果，推导出特征点的重投影误差为：
Figure RE-GDA0003832952010000071
其中，
Figure RE-GDA0003832952010000072
为特征点pi在Cj帧估计的位姿进行投影得到的位置坐标，
Figure RE-GDA0003832952010000073
和
Figure RE-GDA0003832952010000074
为该点的像素坐标。
对于提取到的线特征，利用估计线段和提取线段的两个端点之间的几何距离的最小值 作为该线段的重投影误差，如图3所示。(xs,ys)、(xe,ye)为线段的两个端点坐标，L'1为估计线段，
Figure RE-GDA0003832952010000075
为线特征L1在Cj帧下的重投影误差：
Figure RE-GDA0003832952010000076
其中：
Figure RE-GDA0003832952010000077
步骤42、惯性测量单元和轮式编码器残差。
Figure RE-GDA0003832952010000078
分别为利用惯性测 量单元和轮式编码器的预积分结果推导出的残差项。
Figure RE-GDA0003832952010000079
Figure RE-GDA00038329520100000710
Figure RE-GDA00038329520100000711
和
Figure RE-GDA00038329520100000712
分别代表利用惯性测量单元和轮式编码器计算预积分残差， δbak、δbωk分别表示陀螺仪和加速度计偏置估计的误差项。
步骤43、进行目标函数的构建。本发明进行非线性优化的目标函数如下所示：
Figure RE-GDA0003832952010000081
目标函数由四个部分组成，||rp-JpX||2为滑动窗口的边缘化先验信息项，
Figure RE-GDA0003832952010000082
分别为惯性测量单元和轮式编码器的预积分残差项，
Figure RE-GDA0003832952010000083
为点特征的重投影残差,
Figure RE-GDA0003832952010000084
为线特征的重投影残差，其中
Figure RE-GDA0003832952010000085
和Pl分别为惯性测量单元约束、轮式编码器约束和相机约束，l表示特征点的索引，i表示线特征的索引， C表示特征出现的图像组。非线性优化由CERES SOLVER进行。
其它步骤及参数与具体实施方式一至四之一相同。
本实例针对地面移动机器人在室内低纹理或者光照不均匀的情况下定位 精度低的问题，基于传统的视觉惯性里程计定位***，通过在后端优化部分用紧 耦合的方式融合轮式编码器信息，并在前端增加了线特征的提取与跟踪模块，跟 踪效果如图2所示。本发明通过同时利用图像中的点特征和线特征以提升图像信 息的利用率，将提取出的点线特征使用光流法跟踪提高特征的匹配效率，同时融 合轮式编码器的真实尺度信息和平面约束进一步优化定位信息，在保证定位精度 的同时，提高了移动机器人在室内环境下定位的鲁棒性；
本发明的上述算例仅为详细地说明本发明的计算模型和计算流程，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础 上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷 举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明 的保护范围之列。

Claims (5)

1.一种基于点线特征融合的多传感器室内定位方法，其特征在于，所述方法具体包括以下步骤：
步骤1、以单目相机作为视觉传感器读取图像，标定相机内参并对图像进行预处理，惯性测量单元和轮式编码器数据以各自频率读入到***中；
步骤2、对预处理后的图像提取Harris角点基于光流法对提取线段的端点进行跟踪，通过将跟踪端点连接起来获得新的线特征建立对应关系，利用线描述符剔除异常值，同时对惯性测量单元和轮式编码器的测量数据进行预积分处理。
步骤3、进行传感器松耦合初始化，对于每个进入滑动窗口的图像，使用五点法纯视觉的方法恢复相对旋转矩阵，进行视觉惯性坐标系对齐，利用惯性测量单元和轮式编码器的预积分结果来优化重力并进行速度初始化；
步骤4、计算点线特征的重投影误差以及惯性测量单元和轮式编码器的预积分残差项，加入到滑动窗口队列中进行后端联合优化实现多传感器的紧耦合优化，估计出当前时刻对应的位姿和速度。
2.根据权利要求1所述的一种基于点线特征融合的室内多传感器定位方法，其特征在于，将采集到的图像传输到机器人上搭载的计算机中，同时轮式编码器和惯性测量单元将当前时刻的角速度与线速度输入到计算机中，计算机通过ROS进行数据传输和发送，以此获得三个传感器的数据输入。
3.根据权利要求2所述的一种基于点线特征融合的室内多传感器定位方法，其特征在于，使用LSD线段检测器提取图像中的线段，LSD算法的目标在于检测图像中局部的直的轮廓，会检测到许多难以进行匹配跟踪的短线，因此选择使用改进的LSD算法。优化后的LSD算法主要体现在，对采样图像缩放参数进行调整，以及对短线段进行过滤，利用线段长度最小阈值来过滤不达标的线段，其中阈值取0.1。提取到线特征之后进行线特征的匹配。使用LK光流法进行跟踪，线特征的光流跟踪算法流程如下：
首先对图像建立n级高斯金字塔，针对当前帧检测到的线段L1取其端点为L1(xs,ys)和L1(xe,ye)，估计两个端点的运动向量分别为r1和r2，从而获得在下一帧图像中跟踪到的线段的两个端点L2(xs,ys)和L2(xe,ye)，由端点获得下一帧跟踪到的线段L2，最后对于可能出现的异常点，利用基于几何约束的线性细化方法进行筛除。
4.根据权利要求3所述的一种基于点线特征融合的室内多传感器定位方法，其特征在于，所述步骤3中将相机坐标系对齐到惯性测量单元坐标系进行转换时需要引入尺度因子s，由于轮式编码器的测量数据是基于实际尺度的，利用轮式编码器和惯性测量单元的平移约束建立等式，经过整理可以得到尺度因子的估计公式为：
Figure RE-FDA0003832950000000021
其中
Figure RE-FDA0003832950000000022
表示从编码器坐标原点到惯性测量单元坐标系的变换矩阵，
Figure RE-FDA0003832950000000023
表示从相机坐标原点到惯性测量单元坐标系的变换矩阵，
Figure RE-FDA0003832950000000024
是通过预积分结果推算得到的IMU坐标系的从初始时刻坐标系到第k帧坐标系旋转矩阵，接下来利用惯性测量单元和轮式编码器的约束估计速度和重力方向的初始值，推导公式如下
Figure RE-FDA0003832950000000025
其中，
Figure RE-FDA0003832950000000026
为IMU加速度的预积分值，
Figure RE-FDA0003832950000000027
角速度的预积分值，
Figure RE-FDA0003832950000000028
线速度的预积分值，
Figure RE-FDA0003832950000000029
为轮式编码器位移的预积分值，Δtk代表在第k张图像和第k+1张图像的时间间隔。初始的重力向量为
Figure RE-FDA00038329500000000210
B由3×2的基础矩阵组成，即重力切线空间的两个正交基，Δg用于更新重力向量。将公式转换为最小二乘问题对其右侧的待优化变量进行求解，对连续图像帧之间的公式迭代直到收敛。最终，通过滑动窗口中的关键帧传播位置推算位姿，使用这些位姿信息进行三角化计算，初始化后位置和速度都是在真实比例下计算的。
5.根据权利要求4所述的一种基于点线特征融合的室内多传感器定位方法，其特征在于，所述步骤4的具体过程为，对于匹配成功的点特征，已知当前帧在真实三维空间的点坐标以及计算得到的相机位姿，使用光束平差法进行二次投影并取两次投影结果的差值，对当前帧所有特征点的偏差求和并求取最小值以获得最优位姿结果，推导出特征点的重投影误差为：
Figure RE-FDA00038329500000000211
其中，
Figure RE-FDA00038329500000000212
为特征点pi在Cj帧估计的位姿进行投影得到的位置坐标，
Figure RE-FDA00038329500000000213
和
Figure RE-FDA00038329500000000214
为该点的像素坐标。
对于提取到的线特征，利用估计线段和提取线段的两个端点之间的几何距离的最小值作为该线段的重投影误差，如图3所示。(xs,ys)、(xe,ye)为线段的两个端点坐标，L'1为估计线段，
Figure RE-FDA0003832950000000031
为线特征L1在Cj帧下的重投影误差：
Figure RE-FDA0003832950000000032
其中：
Figure RE-FDA0003832950000000033
构建的优化目标函数为：
Figure RE-FDA0003832950000000034
目标函数由四个部分组成，||rp-JpX||2为滑动窗口的边缘化先验信息项，
Figure RE-FDA0003832950000000035
分别为惯性测量单元和轮式编码器的预积分残差项，
Figure RE-FDA0003832950000000036
为点特征的重投影残差，
Figure RE-FDA0003832950000000037
为线特征的重投影残差，其中
Figure RE-FDA0003832950000000038
和Pl分别为惯性测量单元约束、轮式编码器约束和相机约束，l表示特征点的索引，i表示线特征的索引，C表示特征出现的图像组。
CN202210776807.9A 2022-07-02 2022-07-02 一种基于点线特征融合的多传感器室内定位方法 Pending CN115218889A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210776807.9A CN115218889A (zh) 2022-07-02 2022-07-02 一种基于点线特征融合的多传感器室内定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210776807.9A CN115218889A (zh) 2022-07-02 2022-07-02 一种基于点线特征融合的多传感器室内定位方法

Publications (1)

Publication Number Publication Date
CN115218889A true CN115218889A (zh) 2022-10-21

Family

ID=83609883

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210776807.9A Pending CN115218889A (zh) 2022-07-02 2022-07-02 一种基于点线特征融合的多传感器室内定位方法

Country Status (1)

Country Link
CN (1) CN115218889A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116610129A (zh) * 2023-07-17 2023-08-18 山东优宝特智能机器人有限公司 一种腿足机器人的局部路径规划方法及***
CN117109568A (zh) * 2023-08-24 2023-11-24 北京自动化控制设备研究所 惯性/多维视觉联合定位方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116610129A (zh) * 2023-07-17 2023-08-18 山东优宝特智能机器人有限公司 一种腿足机器人的局部路径规划方法及***
CN116610129B (zh) * 2023-07-17 2023-09-29 山东优宝特智能机器人有限公司 一种腿足机器人的局部路径规划方法及***
CN117109568A (zh) * 2023-08-24 2023-11-24 北京自动化控制设备研究所 惯性/多维视觉联合定位方法

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN109993113B (zh) 一种基于rgb-d和imu信息融合的位姿估计方法
CN110261870B (zh) 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及***
CN111795686A (zh) 一种移动机器人定位与建图的方法
CN115218889A (zh) 一种基于点线特征融合的多传感器室内定位方法
CN113837277B (zh) 一种基于视觉点线特征优化的多源融合slam***
CN104281148A (zh) 基于双目立体视觉的移动机器人自主导航方法
CN111932674A (zh) 一种线激光视觉惯性***的优化方法
CN114234967B (zh) 一种基于多传感器融合的六足机器人定位方法
CN114608554B (zh) 一种手持slam设备以及机器人即时定位与建图方法
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN114754768A (zh) 一种点线融合的视觉惯性导航方法
CN116448100A (zh) 一种多传感器融合的近岸无人船slam方法
CN115435779A (zh) 一种基于gnss/imu/光流信息融合的智能***姿估计方法
CN112945233B (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及***
CN116380079A (zh) 一种融合前视声呐与orb-slam3的水下slam方法
CN116577801A (zh) 一种基于激光雷达和imu的定位与建图方法及***
Wang et al. SW-LIO: A Sliding Window Based Tightly Coupled LiDAR-Inertial Odometry
CN115344033A (zh) 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法
CN115355904A (zh) 一种针对地面移动机器人的Lidar-IMU融合的slam方法
CN114690230A (zh) 一种基于视觉惯性slam的自动驾驶车辆导航方法

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