CN113674412A - 基于位姿融合优化的室内地图构建方法、***及存储介质 - Google Patents

基于位姿融合优化的室内地图构建方法、***及存储介质 Download PDF

Info

Publication number
CN113674412A
CN113674412A CN202110924699.0A CN202110924699A CN113674412A CN 113674412 A CN113674412 A CN 113674412A CN 202110924699 A CN202110924699 A CN 202110924699A CN 113674412 A CN113674412 A CN 113674412A
Authority
CN
China
Prior art keywords
pose
data
robot
imu
fusion
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
Application number
CN202110924699.0A
Other languages
English (en)
Other versions
CN113674412B (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.)
Zhejiang Gongshang University
Original Assignee
Zhejiang Gongshang University
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 Zhejiang Gongshang University filed Critical Zhejiang Gongshang University
Priority to CN202110924699.0A priority Critical patent/CN113674412B/zh
Publication of CN113674412A publication Critical patent/CN113674412A/zh
Application granted granted Critical
Publication of CN113674412B publication Critical patent/CN113674412B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/251—Fusion techniques of input or preprocessed data
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Software Systems (AREA)
  • Geometry (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Artificial Intelligence (AREA)
  • Remote Sensing (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computer Graphics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

本发明提供一种基于位姿融合优化的室内地图构建方法、***及存储介质，所述方法包括：根据KINECT和IMU获取的实时数据解算机器人的位姿数据；根据IMU的线加速度数据和里程计数据判断机器人的当前运动状态；若机器人处于静止状态，则采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理，若机器人处于运动状态，则采用动态加权方法对所述位姿数据进行融合处理；根据融合处理结果构建室内地图。该方法具有更高的位姿估计精度、更高的二维地图建模精度以及更优的建模效果，能够应用于贫特征、高动态、弱光影的场景中。

Description

基于位姿融合优化的室内地图构建方法、***及存储介质
技术领域
本发明涉及一种室内地图构建方法，尤其涉及一种在室内环境卫星信号缺失的情况下， 在贫特征、高动态、弱光影的场景中，基于KINECT/IMU位姿融合优化的室内地图构建的 方法、***及存储介质。
背景技术
精确高效的位姿估计与建图***是机器人实现运动控制与路径规划的关键，在室内环境 卫星信号缺失的情况下，同时定位与地图构建(Simultaneous Localization andMapping， SLAM)技术是解决机器人环境认知与定位导航的有效手段之一。SLAM是指搭载特定传感 器的主体(一般为机器人等)，在没有环境先验信息的情况下，在运动中建立环境模型，同 时估计自身运动的过程。根据搭载传感器的不同，SLAM可以分为视觉SLAM、激光SLAM和多传感器融合SLAM。基于视觉传感器的SLAM(Visual SLAM)主要由传感器信息读取、 视觉里程计、后端优化、回环检测和建图等部分组成。随着计算机视觉、图像处理、人工智 能等技术的发展，视觉SLAM的可用性不断得到挖掘，并且尝试融合其他传感器信息用于 构建更加稳健的视觉SLAM***。随着RGB-D相机的出现，许多学者提出了基于KINECT 的视觉SLAM方法，并且运用到三维建模、人体识别、***交互等领域。
但是，视觉SLAM方法在贫特征、高动态、弱光影的场景时，算法受到很多限制。 IMU传感(惯性传感器)由于自身的特点，使得它与视觉SLAM方法形成了较好的互补性。 例如，视觉传感器在高速运动的状态下，基于特征匹配的位姿估计成功率较低，而IMU依 赖自身加速度和陀螺仪仍然能够保持较精确的位姿估计；视觉传感器可以有效限制运动过程 中IMU存在的累计漂移误差；相机数据与IMU数据的融合可以提高位姿估计算法的鲁棒性。 这种结合相机和IMU的方法被称为视觉惯性SLAM(Visual-Inertial SLAM，VI-SLAM)。与 视觉SLAM相似，VI-SLAM框架可以分为基于滤波和基于优化两种。基于滤波的VI-SLAM 方法将状态和参数作为在线计算的一部分，输入不同的滤波框架，更新校准并输出实时位姿。 基于优化的VI-SLAM方法也被称为平滑方法，它们将IMU数据作为先验信息，提取匹配图 像特征，再根据图像特征定义重投影误差，构造代价函数，解算机器人位姿。很多学者对此 方面进行了研究：Weiss等设计的SSF(Single-Sensor Fusion)算法以扩展卡尔曼滤波(Extended KalmanFilter，EKF)为基本框架，将IMU解算的位姿作为预测函数，将PTAM 算法计算的位姿作为测量数据，有效地提升了算法的性能，并且当视觉解算位姿短时间失败时，该算法中IMU的运动预测仍然能够提供精确的估计。刘振彬等使用一种改进的非线性方法优化单目惯导SLAM方案，该方案基于VINS-Mono***，在初始化的过程中添加了对 加速度贝叶斯偏差的初始化，并借鉴ORB-SLAM，使用ORB(一种快速特征点提取和描述 的算法)特征点在前端对图像进行特征检测匹配，在提高方案鲁棒性的同时实现了较高精度 的实时定位与稀疏地图构建。Campos等提出一种基于视觉惯导的SLAM***VIORB- SLAM，能够支持单目、双目、RGBD等多种相机类型。VI ORB-SLAM***基于两种传感 器设计了一种混合地图数据关联机制，能够对短期、中期、长期的传感器采集数据进行数据 关联。同时基于特征的紧耦合VIO***，VI ORB-SLAM算法依赖最大后验估计，对关键帧 进行几何一致性检测，提高了地图定位的精确性。
但是，融合两种传感器数据的位姿估计，虽然可以有效提高估计精度，其同样存在一些 技术缺陷：使用松耦合的方式融合位姿，主体仍然独立地使用视觉算法实现机器人的位姿估 算，IMU的测量数据仅仅被添加到一些独立的位姿估算过程当中，直接导致了相机与IMU 内部状态之间的相关性被忽略，缺乏对位姿估算有效的约束和补偿，甚至会出现对相机与 IMU位姿误差估算失败的结果，因此，松耦合方法并不能取得最优的位姿估计精度；紧耦 合的方法是将视觉算法和IMU数据融合在一起，共同构建运动方程与状态方程，对机器人 的位姿状态进行解算，但是采用全局优化或局部平滑的紧耦合方法过度依赖初始化的位姿估 计，计算相机与IMU数据的过程中复杂度较高，实时性有所欠缺；同时，单目相机无法直 接获取尺度信息，只能依赖IMU计算的尺度信息，紧耦合方法并不能有效增强位姿尺度的 估算精度。此外，还需要考虑到基于滤波的VI-SLAM方法的马尔科夫性，即在某一时刻的 状态与之前所有时刻的状态关系无法确定，算法框架存在缺陷，精度提升空间有限。
发明内容
为了解决上述技术问题，本发明的目的是提供一种基于KINECT/IMU位姿融合优化的 室内地图构建的方法，该方法具有更高的位姿估计精度、更高的二维地图建模精度以及更优 的建模效果，能够应用于贫特征、高动态、弱光影的场景中。
基于上述目的，本发明的一个方面，提供一种基于位姿融合优化的室内地图构建方法， 该方法包括：
根据KINECT和IMU获取的实时数据解算机器人的位姿数据；
获取机器人当前运动状态；
若机器人处于静止状态，则采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理，若 机器人处于运动状态，则采用动态加权方法对所述位姿数据进行融合处理；
根据融合处理结果构建室内地图。
作为优选，所述利用KINECT获取的实时数据解算机器人位姿数据的具体方法为：获 取图像数据；对两张部分重叠图像的同名特征点进行匹配，获得同名特征点之间的对应关系， 该对应关系与机器人位姿对应。
作为优选，所述利用IMU获取的实时数据解算机器人位姿数据的具体方法为：采用方 向余弦矩阵算法进行位姿解算。
作为优选，所述获取机器人当前运动状态的方法为：
若IMU的线加速度为0，且里程计数据没有增加，则判定机器人当前为静止状态；
若IMU的线加速度不为0，和/或里程计数据有增加，则判定机器人当前为运动状态。
作为优选，采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理的具体方法为：
获取***状态量，计算误差状态预测值和***状态噪声协方差矩阵；
由计算得到观测矩阵和噪声协方差矩阵，计算***的增益矩阵；
由增益矩阵，更新***状态量、***协方差矩阵；
将***状态量计算得到的四元数进行归一化处理；
将四元数转化为方便位姿显示的欧拉角，以此表示机器人的位姿状态。
作为优选，采用动态加权方法对所述位姿数据进行融合处理的方法为：
通过KINECT根据关键帧获取对应的图像；
根据KINECT获取的两个关键帧之间特征点的匹配成功率μ设置权重值阈值；若匹配 成功率μ降低，则提高IMU姿态的权重；若匹配成功率μ升高，则提高KINECT姿态的权 重。
作为优选，根据所述融合处理结果创建室内地图的具体方法为：
获取激光点与雷达本身经过距离与方位角的解算后获得相对坐标；
预先将融合优化后的位姿估计替换建图算法中的位姿估计，并将该位姿估计作为新的数 据源计算激光点与雷达的相对关系，以更新激光点的距离与方位角信息；
使用基于高斯牛顿的扫描匹配法匹配激光点坐标，建立高精度的室内二维地图。
本发明的另一个方面，提供一种采用上述的基于位姿融合优化的室内地图构建方法的进 行室内地图构建的***，其特征在于，包括：
位姿数据解算单元，根据KINECT和IMU获取的实时数据解算机器人的位姿数据；
运动状态判断单元，根据IMU的线加速度数据和里程计数据判断机器人当前运动状态；
位姿数据融合处理单元，分别与位姿数据解算单元和运动状态判断单元连接，当机器人 处于静止状态时，采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理，当机器人处于运 动状态时，采用动态加权方法对所述位姿数据进行融合处理；
二维地图构建单元，与位姿数据融合处理单元连接，根据所述融合处理结果创建室内地 图。
作为优选，所述位姿数据解算单元包括：
KINECT数据解算子单元，其用于获取图像数据；对两张部分重叠图像的同名特征点进 行匹配，获得同名特征点之间的对应关系，该对应关系与机器人位姿对应。
IMU数据解算子单元，其采用方向余弦矩阵算法对IMU获取的实时数据进行位姿解算。
本发明的再一个方面，提供一种存储介质，该存储介质内存储有计算机程序，所述计算 机程序被处理执行时实现如上所述的基于位姿融合优化的室内地图构建方法。
与现有技术相比，本发明的有益效果为：
该方法通过首先判定机器人的运动状态，针对机器人静止状态，使用扩展卡尔曼滤波算 法进行位姿数据的融合，针对机器人运动状态，采用特征匹配加权策略融合KINECT与 IMU位姿数据，这样，有效提高融合后的位姿估计精确度，使得二维地图建模精度更好且建 模效果更优，该方法可有效提高贫特征、高动态、弱光影的场景中的位姿估计精度。
附图说明
构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实 施例及其说明用于解释本申请，并不构成对本申请的限定。
图1是本发明实施例中基于位姿融合优化的室内地图构建方法的流程图；
图2(a)-(c)分别是是本发明实施例中过道、墙角和桌椅特征点选取匹配示意图；
图3是本发明实施例中部分自然场景与人为布设场景；
图4是本发明实施例中匹配成功率与位姿误差关系；
图5是本发明实施例中搭建的机器人平台与传感器；
图6是本发明实施例中x、y、z方向位移偏离图；
图7是本发明实施例中x、y方向位移偏离图；
图8(a)-(d)是本发明实施例中局部建图效果对比；
图9(a)是本发明实施例中Cartographer算法构建的二维地图；
图9(b)是本发明实施例中本发明算法构建的二维地图。
具体实施方式
下面结合附图与实施例对本发明作进一步说明。
应该指出，以下详细说明都是示例性的，旨在对本申请提供进一步的说明。除非另有指 明，本实施例使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理 解的相同含义。
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申 请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图 包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括” 时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
本实施例提供一种基于位姿融合优化的室内地图构建方法，如图1所示，该方法包括：
根据KINECT和IMU获取的实时数据解算机器人的位姿数据；
获取机器人当前运动状态；
若机器人处于静止状态，则采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理，若 机器人处于运动状态，则采用动态加权方法对所述位姿数据进行融合处理；
根据融合处理结果构建室内地图。
作为一种较优的实施方式，所述利用KINECT获取的实时数据解算机器人位姿数据的 具体方法为：获取图像数据；对两张部分重叠图像的同名特征点进行匹配，获得同名特征点 之间的对应关系，该对应关系与机器人位姿对应。具体地：基于KINECT获取的图像数据 解算机器人的位姿，通常根据两张部分重叠图像的同名特征点的匹配而实现。首先，使用 Oriented-Fast算法提取图像特征点；然后，使用快速近似最近邻方法(Fast Libraryfor Approximate Nearest Neighbors，FLANN)实现特征点之间的匹配。根据两张图像中的同名 特征点的对应关系，实现相机运动参数的解算。例如：假设相机运动的刚体变换量为(R， T)，其中，R为旋转向量，T为平移向量，那么特征点对应的坐标与运动过程的关系如式(1) 所示。
Ppm＝RPlm+T (1)
其中，Ppm，和RPlm为当前帧图像与上一帧图像第m个特征点的三维坐标，m＝1，2，3……n。使用最近邻方法匹配特征点的同时需要剔除误匹配的特征点对，随机选取至少4对特征点对，计算刚体变换量(R，T)，将其坐标代入||Ppm-(RPlm+T)||2，筛选小于阈值的点 代入式(1)解算。最后，通过迭代计算获得特征点对应坐标与运动过程关系的最小二乘值， 用于估计机器人的当前位姿。
作为一种较优的实施方式，所述利用IMU获取的实时数据解算机器人位姿数据的具体 方法为：采用方向余弦矩阵算法进行位姿解算。具体地：
IMU主要包含三轴加速度计与三轴陀螺仪，通过对加速度计测得的线速度进行二次积 分，可以获得IMU的速度与位置数据；对陀螺仪测得的角速度进行积分，可以获得IMU的 姿态数据。为避免当俯仰角为90°时，欧拉角微分方程会出现奇点的问题，导致无法获得IMU的全姿态解算，本实施例采用方向余弦矩阵算法(Direction Cosine Matrix，DCM)使用矢量的方向余弦表示姿态矩阵，有效地避免了使用欧拉角表示姿态遇到的奇点问题，具体 地，本实施例中IMU姿态的解算采用DCM算法，定义DCM矩阵为C，如式(2)所示。
Figure BDA0003208732120000061
其中，i′,j′,k′为机器人坐标的单位向量。对i′,j′,k′求导可得式(3)。
Figure BDA0003208732120000062
再对矩阵C求导可得
Figure BDA0003208732120000066
如式(4)所示。
Figure BDA0003208732120000063
求解式(4)方程，得到IMU与机器人的相对位姿变换关系。
此外，由于在实际操作中，IMU的高测量频率占用了大量的计算资源，为避免在每个 IMU测量时都添加一个新的状态量，通常在两帧之间加入重新参数化的过程，实现运动约 束，避免重复积分，这种重新参数化的过程称为IMU预积分。具体地：假设IMU测量得到的加速度与角速度分别表示为
Figure BDA0003208732120000064
那么，IMU测量模型如式(5)所示。
Figure BDA0003208732120000065
其中，W为世界坐标系，B为IMU坐标系，BωWB(t)为B相对于W的瞬时角速度，
Figure BDA0003208732120000071
为世界坐标系到IMU坐标系的旋转矩阵，wα(t)为世界坐标系下的瞬时加速度，bg(t)、 bα(t)为陀螺仪与加速度的偏差，ηg(t)、ηα(t)为随机噪声。引入动力学积分模型，模型包含旋 转关系
Figure BDA0003208732120000072
速度
Figure BDA0003208732120000073
和平移
Figure BDA0003208732120000074
如式(6)所示。
Figure BDA0003208732120000075
对式(6)进行积分可以得Δt时间内的旋转量RWB(t+Δt)、速度量wv(t+Δt)和平移量 WP(t+Δt)WP(t+Δt)，结合IMU测量模型中的加速度与角速度
Figure BDA0003208732120000076
得到IMU 在Δt时间内相对于世界坐标系的运动状态RWB(t+Δt)、Wv(t+Δt)和Wp(t+Δt)，即两个时刻 IMU数据之间的相对关系。IMU与KINECT的更新率并不同步，积分两个关键帧之间的多 帧IMU数据可以有效约束关键帧。考虑到旋转矩阵
Figure BDA0003208732120000077
随着时间的不断变化，因此相对运动 增量ΔRij、Δvij、Apij，如式(7)所示。
Figure BDA0003208732120000078
其中，
Figure BDA0003208732120000079
Figure BDA00032087321200000710
式(7)左侧为相对运动增量，右 侧为IMU数据，进一步将两个关键帧的相对运动增量ΔRij、Δvij、Δpij表示为式(8)。
Figure BDA00032087321200000711
如此一来，得到两帧之间的相对运动增量ΔRij、Δvij、Δpij，即两帧之间使用IMU预积分 重新参数化的结果，避免了重复积分IMU观测量，提高了算法的可靠性。
作为一种较优的实施方式，所述获取机器人当前运动状态的方法为：
若IMU的线加速度为0，且里程计数据没有增加，则判定机器人当前为静止状态；
若IMU的线加速度不为0，或里程计数据有增加，则判定机器人当前为运动状态。
作为一种较优的实施方式，采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理的具 体方法为：
获取***状态量，计算误差状态预测值和***状态噪声协方差矩阵；
由计算得到观测矩阵和噪声协方差矩阵，计算***的增益矩阵；
由增益矩阵，更新***状态量、***协方差矩阵；
将***状态量计算得到的四元数进行归一化处理；
将四元数转化为方便位姿显示的欧拉角，以此表示机器人的位姿状态。
具体地：
为了更加精确地估计机器人的位姿信息，可以使用扩展卡尔曼滤波融合IMU和KINECT位姿。EKF分为预测过程和更新过程。对于预测过程，定义误差状态向量
Figure BDA0003208732120000081
如式 (9)所示。
Figure BDA0003208732120000082
其中，
Figure BDA0003208732120000083
为状态向量期望值。同时，使用
Figure BDA0003208732120000084
表示状态噪声n，根据误差状态向 量
Figure BDA0003208732120000085
的定义，线性化处理IMU测量得到的加速度与角速度，得到误差状态方程中平移、旋转、 速度、加速度计和陀螺零漂的表示量
Figure BDA0003208732120000086
参照***的状态方程，通过 雅克比矩阵求解状态转移矩阵
Figure BDA0003208732120000087
如式(10)所示。
Figure BDA0003208732120000088
根据***噪声矩阵G和状态转移矩阵
Figure BDA0003208732120000089
可以求出***噪声协方差矩阵Qd，进一步计算扩 展卡尔曼滤波的误差状态预测值Xk+1|k和***状态协方差矩阵预测值Pk+1|k，如式(11)所示。
Figure BDA00032087321200000810
其中，Xk|k、Pk|k为上一时刻的最优估计值。随着时间推移，IMU误差不断累积，需要使用低频输出的KINECT位姿作为滤波器的观测值z，用于校正IMU解算的结果。一般而言， IMU与KINECT相对于机器人底盘的位姿转换矩阵已经确定，因此可以确定IMU与 KINECT相机之间的旋转平移矩阵
Figure BDA00032087321200000811
***观测方程zp、zq，如式(12)所示。
Figure BDA0003208732120000091
由观测矩阵和观测噪声协方差矩阵可以推导EKF的更新过程，计算***的增益矩阵K， 如式(13)所示。
K＝Pk+1|kHT(HPk+1|kHT+R)-1 (13)
获得增益矩阵K后，更新***状态量Xk+1|k+1，如式(14)所示。
Figure BDA0003208732120000092
更新***协方差矩阵Pk+1|k+1，如式(15)所示。
Pk+1|k+1＝(I15-KH)Pk+1|k(I15-KH)T+KRKT (15)
更新***状态量Xk+1|k+1和***协方差矩阵Pk+1|k+1之后，将***状态量计算得到的四元数 进行归一化处理，再将四元数转化为方便位姿显示的欧拉角，以此表示机器人的位姿状态， 实现扩展卡尔曼滤波融合传感器数据后的位姿输出。
作为一种优选的实施方式，采用动态加权方法对所述位姿数据进行融合处理的方法为：
通过KINECT根据关键帧获取对应的图像；
根据KINECT获取的两个关键帧之间特征点的匹配成功率μ设置权重值阈值；若匹配 成功率μ降低，则提高IMU姿态的权重；若匹配成功率μ升高，则提高KINECT姿态的权 重。
具体地：位姿的融合方式取决于机器人的位姿状态，机器人的位姿状态分为运动与静止 两种，本实施例使用IMU数据与里程计数据作为判断机器人运动与静止的条件。如果IMU 的线加速度为0，并且里程计数据没有增加，那么可以判定机器人为静止状态，位姿的融合 直接使用扩展卡尔曼滤波方法融合KINECT和IMU的位姿；如果IMU的线加速度不为0和 /或者里程计数据有增加，那么可以判定机器人为运动状态。在机器人的运动过程中，KINECT获取图像的纹理特征的不确定性较大，IMU误差虽有累积，但是相对而言较为稳定。因此，位姿融合过程中可以适当考虑两者之间的权重关系，分析关键帧特征点的匹配成功率， 用以确定KINECT和IMU位姿对于特征点匹配的依赖程度，进而实现面向不同位姿状态采 取不同的加权策略融合处理IMU和KINECT位姿。
当机器人处于运动状态时，IMU的测量值包括线速度、加速度和角速度，KINECT根据 关键帧获取对应的图像。根据两个KINECT获取的关键帧之间特征点匹配的成功率设置权 重值阈值；如果匹配成功率低，那么当前姿态和KINECT姿态的关联程度低，应当提高IMU姿态的权重；如果匹配成功率高，那么当前姿态和KINECT姿态的关联程度高，应当 提高KINECT姿态的权重。
假设两个关键帧匹配成功的特征点与总特征点的比值定义为匹配成功率μ，那么加权融 合位姿的关键在于，根据μ的取值区间，确定KINECT和IMU位姿融合的权重值，即位姿加 权融合的权参数。
作为一种较优的实施方式，根据所述融合处理结果创建室内地图的具体方法为：
获取激光点与雷达本身经过距离与方位角的解算后获得相对坐标；
预先将融合优化后的位姿估计替换建图算法中的位姿估计，并将该位姿估计作为新的数 据源计算激光点与雷达的相对关系，以更新激光点的距离与方位角信息；
使用基于高斯牛顿的扫描匹配法匹配激光点坐标，建立高精度的室内二维地图。
室内二维地图的构建过程中，非结构化的场景可能导致二维激光雷达的构图平面倾斜， 降低建图质量。本实施例采用激光三角测距技术实现LIDAR建图，测得的激光点与LIDAR 本身经过距离与方位角的解算后获得相对坐标，构图前将融合优化后的位姿估计替换建图算 法中的位姿估计，将其作为新的数据源计算激光点与雷达的相对关系，以此更新激光点的距 离与方位角信息，再使用基于高斯牛顿的扫描匹配法匹配激光点坐标，建立高精度的室内二 维地图。
室内环境大多呈现垂直结构，实验中需要将倾斜的雷达数据做近似投影处理。扫描获得 的激光数据(ρ，θ)按照地图坐标表达形式表示为(Px，Py，Pz)，如式(16)所示。
Figure BDA0003208732120000101
其中，(θx，θy，θz)表示翻滚角、俯仰角、偏航角。根据初始位姿与角度变换求得扫描激光 点的坐标(X，Y，Z)，如式(17)所示。
Figure BDA0003208732120000102
对应优化后的位姿估计与激光点相对坐标后，使用高斯牛顿法匹配激光点，提高建图精 度与效果。首先对刚体变换T取最小值T*，如式(18)所示。
Figure BDA0003208732120000111
其中，Si(T)为激光点的平面坐标，G(Si(T))为Si(T)的占用值，当所有激光点完全匹配时， G(Si(T))的值为1。之后优化一段时间内的雷达数据测量误差，如式(19)所示：
Figure BDA0003208732120000112
通过泰勒展开与求偏导，将求解ΔT转化为高斯牛顿的最小化问题，如式(20)所示。
Figure BDA0003208732120000113
其中，
Figure BDA0003208732120000114
输出基于牛顿高斯的最小二乘解后，根 据高斯牛顿法求得光束点匹配的结果，将与优化位姿对应点的距离与方位角作为数据源代入 建图过程，构建室内二维地图，有效去除了因二维激光雷达运动带来的畸变，提升了建图效 果。
根据上述内容，基于KINECT/IMU的位姿融合优化的室内地图构建方法根据机器人的 运动状况，选择不同的位姿融合策略。当机器人处于静止状态时，使用EKF方法融合KINECT和IMU的位姿；当机器人处于运动状态时，使用加权方法融合KINECT和IMU的 位姿。对于加权融合方法而言，确定KINECT位姿和IMU位姿以何种比例关系参与加权是 关键。本实施例依据两个关键帧匹配成功的特征点与总特征点的比值μ(简称匹配成功率μ) 确定KINECT位姿和IMU位姿的权重关系，即位姿加权融合的权参数λ。
为了得到准确的位姿加权融合的权参数λ，本实施例还设计了位姿加权融合权参数确定 实验。实验区域选取某实验室室内环境。实验设备选取KINECT V2相机与AH100B惯性测 量单元。由于惯性测量单元采集数据的频率过高，为同步相机与惯性测量单元采集数据的频 率，前期使用IMU预积分步骤，将相机与惯性测量单元的采集频率统一为KINECT V2相机 的30Hz。
实验步骤主要包括如下步骤：
(1)获取原始位姿数据。
分别选取26个室内环境场景，使用KINECT V2相机拍摄图像，匹配图像之间的特征点， 解算位姿；使用IMU积分记录加速度与角速度数据，解算位姿；常用的位姿解算方法都直 接使用扩展卡尔曼滤波方法获得机器人的位姿，因此，可以将用作实验比较的真值。
(2)计算均方根误差。
分别计算KINECT V2相机和IMU解算的位姿与扩展卡尔曼滤波解算的位姿之间的均方 根误差。
(3)分析匹配成功率μ与位姿均方根误差的关系。当匹配成功率μ取不同的值时，分析KINECT V2相机和IMU计算的均方根误差的关系，进而确定KINECT位姿和IMU位姿 的权重关系。
匹配成功率μ是指某一关键帧的两幅重叠图像的匹配成为同名特征点的数量与重叠图像 获取的总特征点之间的比值，本申请中匹配成功率μ的高低决定了环境特征的显著程度，进 而决定了位姿解算的精度。
本实施例首先选取了19个不同的场景，实验场景包含了具有代表性的过道、墙角、桌 椅等位置，基于ORB特征点检测使用快速近似最邻近方法匹配特征点。
如图2(a)-图2(c)所示，左侧图像分别表示实验室环境的过道、墙角、桌椅场景，右侧3幅图像表示从不同角度获取的过道、墙角、桌椅图像，其中图中横线连线表示两幅图像成功匹配的ORB特征点对。实验过程中，过道、墙角、桌椅场景分别检测到533、149、 653个特征点，其中过道场景成功匹配50对特征点对，墙角场景成功匹配4对特征点对， 桌椅场景成功匹配76对特征点对。过道、墙角、桌椅场景的匹配成功率分别为9.38％、 2.68％、11.63％，这说明纹理丰富、结构明显、特征性强的场景能够检测到更多的特征点并 且匹配成功率较高；相反，弱纹理、结构单一的场景检测到的特征点少并且匹配成功率低。 更为重要的是，通过这19个不同场景的实验，匹配成功率最大为43％，始终没有超过50％。 针对上述情况，本实施例增加7个布设了人为特征的实验场景，使得特征点匹配成功率达到 了87.37％，图3为部分自然场景和人为布设场景。
其次，如图4所示，分别计算KINECT V2相机和IMU解算的位姿与扩展卡尔曼滤波解算的位姿之间的均方根误差，实验结果表明：(1)当KINECT相机特征点匹配的成功率越 低时，KINECT相机与扩展卡尔曼滤波之间的位姿均方根误差就越高，说明此时KINECT相 机解算位姿的可靠性较低；(2)当KINECT相机特征点匹配的成功率越高时，KINECT相机 与扩展卡尔曼滤波之间的位姿均方根误差就越低，说明此时KINECT相机解算位姿的可靠 性较高；(3)但是，考虑到匹配成功率越高的原因是增加了许多人为的特征点，因此在适当 时候可能需要忽略这种影响。如果单纯观察匹配成功率低于50％的情况，还可以发现 KINECT解算的位姿精度逐渐降低，IMU解算的位姿精度始终没有太大变化，这种现象是符 合实际情况的。因此，当机器人处于运动状态时，本申请按照加权参数融合KINECT解算 的姿态和IMU解算的姿态，用作优化的融合位姿。
1)0.6≤μ≤1时，KINECT解算的姿态的权参数λ为0.5；IMU解算的姿态的权参数μ为0.5；
2)0.3≤μ<0.6时，KINECT解算的姿态的权参数λ为0.33；IMU解算的姿态的权参数μ为0.67；
3)0≤μ<0.3时，KINECT解算的姿态的权参数λ为0.2；IMU解算的姿态的权参数μ为0.8。
4KINECT/IMU位姿融合实验：
KINECT/IMU位姿融合实验的实验平台选用Autolabor机器人，它搭载了KINECT V2相机、AH100B惯性测量单元和RPLIDAR A2二维激光雷达，三个传感器与Autolabor机器 人之间采用固联的联接方式，如图5所示。
实验数据：
为了验证位姿融合加权策略的有效性、KINECT/IMU融合位姿的精度以及优化后的LIDAR建图效果，本实施例设计了两组不同实验，第一组实验选用了EuRoC公开数据集作 为数据来源，另一组实验采集某实验室环境数据作为数据来源。
EuRoC公开数据集是苏黎世联邦理工学院基于无人飞行器在礼堂与空房间两种环境内 采集的数据，无人飞行器搭载的传感器主要包括相机与惯性测量单元，相机频率为20Hz， 惯性测量单元频率为200Hz。实验数据分为4个文件夹，每个文件夹都包含传感器相对于坐 标系的变换情况。EuRoC公开数据集主要用于位姿精度的比较分析。
第二组数据集使用Autolabor机器人采集某实验室的环境数据。实验室场景为有障碍物 的室内平坦地面，面积为15m×10m。机器人搭载的传感器主要包括KINECT V2相机与 AH100B惯性测量单元，相机频率为30Hz，惯性测量单元频率为37Hz。实时采集的数据集通过bag数据包记录，然后根据不同的话题将bag数据包导出对应的位姿数据集。
实验步骤：
(1)实验数据获取
第一组实验选用了EuRoC公开数据集作为数据来源，第二组数据集使用Autolabor机器 人采集实验室的环境数据。
(2)位姿解算
提取并匹配两帧重叠图像的ORB特征点，根据特征点对的匹配关系解算相机的运动参 数。二次积分IMU记录的线速度，计算速度与位置信息，积分角速度计算姿态信息，获得 速度、位置、姿态信息之后，使用方向余弦矩阵计算IMU与实验平台的相对位姿关系。
(3)位姿估计优化
判断机器人运动状态。如果机器人为静止状态，那么使用扩展卡尔曼滤波融合相机与惯 导传感器解算的位姿；如果机器人为运动状态，那么通过特征点匹配的依赖程度，使用加权 策略融合位姿，输出优化后的位姿估计。
(4)二维地图构建
优化更新后的位姿纠正RPLIDAR A2获取激光点的距离与方位角信息，使用基于高斯 牛顿的扫描匹配，构建二维环境地图。
(5)实验分析
首先，基于EuRoC公开数据集，比较ROVIO算法和本实施例算法在位姿融合优化方面 的精度。ROVIO算法紧耦合视觉信息和IMU信息，通过迭代卡尔曼滤波融合数据，更新滤波状态与位姿，在算法框架和数据融合方式方面与本实施例类似。其次，基于实验室场景的采集数据，同样比较VIORB-SLAM算法和比本实施例算法在位姿融合优化方面的精度。VIORB-SLAM算法由ORB-SLAM和IMU模块组成，通过最大后验估计，对关键帧进行几 何一致性检测，得到位姿最优估计，因此，VIORB-SLAM算法和本实施例算法具有较高的 对比度。最后，在二维地图构建方面，比较Cartographer算法和本实施例算法的建模精度和 效果。Cartographer算法是基于二维激光雷达构建地图的经典算法，根据IMU和里程计信息 通过滤波的方式预测位姿，将扫描点进行体素滤波转换为栅格点坐标构建地图，建图效果稳定，鲁棒性高。
第一组实验基于EuRoC公开数据集，比较分析ROVIO算法和本实施例算法的位姿估计 精度。实验时间总长为149.96s，共选取了29993个位姿。实验误差如表5所示，分别选取最大误差(Max)、最小误差(Min)、平均值(Mean)、均方根误差(RMSE)和标准差 (Std)作为精度衡量的指标。同时分析了机器人在x轴方向、y轴方向和z轴方向的偏移图 (图6)。
表5本实施例算法与ROVIO实验误差分析(pose:29993)
Figure BDA0003208732120000151
从实验结果可以看出：本实施例算法与ROVIO算法对比，均方根绝对误差为2.5944， 标准差绝对误差为1.5223，同时对比x、y、z方向位移偏离，说明本实施例算法相较于 ROVIO算法能够有效过滤多余的偏移与振动，借助融合的优化机制有效提升机器人的位姿 估计精度。
第二组实验基于机器人实时采集的实验室场景数据，比较分析VIORB-SLAM算法和本 实施例算法的位姿估计精度。实验时间总长为367.30s，共选取了3648个位姿。实验误差如 表6所示，同样选取最大误差(Max)、最小误差(Min)、平均值(Mean)、均方根误差 (RMSE)和标准差(Std)作为精度衡量的指标。同时分析了机器人在x轴方向、y轴方向 的偏移图(图7)。
表6本申请算法与VIORB-SLAM实验误差分析(pose:3648)
Figure BDA0003208732120000152
在3648个位姿估计中，本申请算法与VIORB-SLAM算法对比，均方根误差减少0.26，标准差减少0.27。相较于VIORB-SLAM算法的位姿解算结果，本实施例算法的融合位姿优 化为机器人提供了更高精度的位姿估计，误差分析如表6所示。由图7所示，运动过程中， x、y方向上通过融合位姿优化的机器人在相同的运动轨迹中位移偏差逐渐拟合，位置随时 间的漂移减少，说明融合位姿优化在现地场景中也能有效提升机器人的定位精度。
通过两组实验表明，基于KINECT/IMU的融合优化位姿，一方面解决了室内环境卫星 信号缺失情况下机器人位姿估计定位困难的问题，同时提高了机器人位姿估计精度；另一方 面也解决了KINECT相机输出频率低、可依赖性差的问题，融合位姿优化使用的IMU高频 输出也满足了高动态特性条件下的高精度位姿估计需求。
地图构建分析：
室内环境二维地图构建通过机器人操作***实现。实验比较分析了基于KINECT/IMU 融合位姿优化的室内地图构建方法和Cartographer算法的建模精度和效果。
首先，对比分析局部建图效果，如图8所示。图8(a)和图8(b)分别为Cartographer算法 和本实施例算法在该场景中扫描到的激光点分布，图8(c)和图8(d)分别为Cartographer算法 和本实施例算法基于该场景构建的局部地图。从对比结果中可以看出，在同一局部场景非重 复扫描的条件下，Cartographer算法扫描的激光点在光滑铁皮面等反射率较高的物体处出现 局部激光点不连续的情况，如图8(a)所示，导致Cartographer算法根据二维激光点构建的实 际二维地图的边缘无法拟合，如图8(c)方框中所示，进而影响地图精度与构建效果；而使用 IMU/KINECT融合位姿补偿的本实施例算法可以修正二维激光雷达扫描的激光点位姿，产生 根据机器人自身的位姿形成对激光点的距离与方位角的优化，使得激光点相对于真值的偏差 减小，如图8(b)所示，使得二维图像边缘产生的缝隙封闭拟合，提高了扫描匹配的精度，最 终通过地图构建算法构建场景二维地图，提升地图局部细节的精度与可视化效果，如图8(d) 所示。采用本实施例算法构建完整的室内环境地图，实验中机器人共行驶68.96m，产生 3648个标记点，全局建图效果如图9(a)、图9(b)所示。
其次，从整体的建图效果分析，本实施例算法构建地图的边缘细节保留更完整(图9(b))，实际墙面在二维平面地图上的映射直线相互垂直，边与边的相对关系基本没有畸变。
因此，从建图精度与可视化效果的角度来说，本实施例算法相较于Cartographer算法都 有一定程度的提升。
本实施例以室内环境为实验背景，结合机器人室内建图的位姿精度需求与室内环境特点， 提出了一种基于KINECT/IMU融合位姿优化的方法，将其运用于二维地图的建模。首先， 通过KINECT、IMU解算机器人的位姿；其次，通过判定机器人的运动状态，使用扩展卡尔 曼滤波或特征匹配加权策略融合IMU与KINECT数据，形成新的位姿估计；再次，将融合优化后的高精度位姿和同一时刻测得的激光点一一对应，纠正激光点在坐标系内的距离与方 位角信息，将其代入基于牛顿高斯扫面点的匹配过程，构建室内二维地图。最后，设计了KINECT/IMU位姿融合实验，分别运用EuRoc公开数据集和实测数据集验证算法的有效性，实验结果表明，算法较ROBVIO算法和VI ORB-SLAM算法具有更高位姿估计精度，较Cartographer算法具有更高的二维地图建模精度和建模效果。
本发明的另一个方面，提供一种采用上述的基于位姿融合优化的室内地图构建方法的进 行室内地图构建的***，其特征在于，包括：
位姿数据解算单元，根据KINECT和IMU获取的实时数据解算机器人的位姿数据；
运动状态判断单元，根据所述位姿数据判断机器人当前运动状态；
位姿数据融合处理单元，分别与位姿数据解算单元和运动状态判断单元连接，当机器人 处于静止状态时，采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理，当机器人处于运 动状态时，采用动态加权方法对所述位姿数据进行融合处理；
二维地图构建单元，与位姿数据融合处理单元连接，根据所述融合处理结果创建室内地 图。
作为一种较优的实施方式，所述位姿数据解算单元包括：
KINECT数据解算子单元，其用于获取图像数据；对两张部分重叠图像的同名特征点进 行匹配，获得同名特征点之间的对应关系，该对应关系与机器人位姿对应。
IMU数据解算子单元，其采用方向余弦矩阵算法对IMU获取的实时数据进行位姿解算。
本发明的再一个方面，提供一种存储介质，该存储介质内存储有计算机程序，所述计算 机程序被处理执行时实现如上所述的基于位姿融合优化的室内地图构建方法。
为了提高室内地图构建的精度，通常可以融合KINECT、IMU等多种传感器获得的高精 度数据。本申请通过判断机器人的运动状态，根据特征点匹配的依赖程度，分别使用EKF和加权融合两种方式实现KINECT和IMU两种传感器位姿的优化，实现了室内地图的高精 度建模。这一过程中，首先判定机器人的运动状态，针对机器人静止状态，使用扩展卡尔曼 滤波算法进行位姿数据的融合，针对机器人运动状态，采用特征匹配加权策略融合KINECT 与IMU位姿数据，这样，有效提高融合后的位姿估计精确度，使得二维地图建模精度更好 且建模效果更优，该方法可有效提高贫特征、高动态、弱光影的场景中的位姿估计精度。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的， 不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下 在本发明的范围内可以对上述实施例进行变化、修改、替换和变型，凡是依据本发明的技术 实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围 内。

Claims (10)

1.一种基于位姿融合优化的室内地图构建方法，其特征在于，包括：
根据KINECT和IMU获取的实时数据解算机器人的位姿数据；
获取机器人当前运动状态；
若机器人处于静止状态，则采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理，若机器人处于运动状态，则采用动态加权方法对所述位姿数据进行融合处理；
根据融合处理结果构建室内地图。
2.根据权利要求1所述的一种基于位姿融合优化的室内地图构建方法，其特征在于，所述利用KINECT获取的实时数据解算机器人位姿数据的具体方法为：获取图像数据；对两张部分重叠图像的同名特征点进行匹配，获得同名特征点之间的对应关系，该对应关系与机器人位姿对应。
3.根据权利要求1所述的一种基于位姿融合优化的室内地图构建方法，其特征在于，所述利用IMU获取的实时数据解算机器人位姿数据的具体方法为：采用方向余弦矩阵算法进行位姿解算。
4.根据权利要求1所述的一种基于位姿融合优化的室内地图构建方法，其特征在于，所述获取机器人当前运动状态的方法为：
若IMU的线加速度为0，且里程计数据没有增加，则判定机器人当前为静止状态；
若IMU的线加速度不为0，或里程计数据有增加，则判定机器人当前为运动状态。
5.根据权利要求1所述的一种基于位姿融合优化的室内地图构建方法，其特征在于，采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理的具体方法为：
获取***状态量，计算误差状态预测值和***状态噪声协方差矩阵；
由计算得到观测矩阵和噪声协方差矩阵，计算***的增益矩阵；
由增益矩阵，更新***状态量、***协方差矩阵；
将***状态量计算得到的四元数进行归一化处理；
将四元数转化为方便位姿显示的欧拉角，以此表示机器人的位姿状态。
6.根据权利要求1所述的一种基于位姿融合优化的室内地图构建方法，其特征在于，采用动态加权方法对所述位姿数据进行融合处理的方法为：
通过KINECT根据关键帧获取对应的图像；
根据KINECT获取的两个关键帧之间特征点的匹配成功率μ设置权重值阈值；若匹配成功率μ降低，则提高IMU姿态的权重；若匹配成功率μ升高，则提高KINECT姿态的权重。
7.根据权利要求1所述的一种基于位姿融合优化的室内地图构建方法，其特征在于，根据所述融合处理结果创建室内地图的具体方法为：
获取激光点与雷达本身经过距离与方位角的解算后获得相对坐标；
预先将融合优化后的位姿估计替换建图算法中的位姿估计，并将该位姿估计作为新的数据源计算激光点与雷达的相对关系，以更新激光点的距离与方位角信息；
使用基于高斯牛顿的扫描匹配法匹配激光点坐标，建立高精度的室内二维地图。
8.一种采用如权利要求1-7中任一项所述的基于位姿融合优化的室内地图构建方法的进行室内地图构建的***，其特征在于，包括：
位姿数据解算单元，根据KINECT和IMU获取的实时数据解算机器人的位姿数据；
运动状态判断单元，根据IMU的线加速度数据和里程计数据判断机器人当前运动状态；
位姿数据融合处理单元，分别与位姿数据解算单元和运动状态判断单元连接，当机器人处于静止状态时，采用扩展卡尔曼滤波算法对所述位姿数据进行融合处理，当机器人处于运动状态时，采用动态加权方法对所述位姿数据进行融合处理；
二维地图构建单元，与位姿数据融合处理单元连接，根据所述融合处理结果创建室内地图。
9.根据权利要求8所述的基于位姿融合优化的室内地图构建的***，其特征在于，所述位姿数据解算单元包括：
KINECT数据解算子单元，其用于获取图像数据；对两张部分重叠图像的同名特征点进行匹配，获得同名特征点之间的对应关系，该对应关系与机器人位姿对应。
IMU数据解算子单元，其采用方向余弦矩阵算法对IMU获取的实时数据进行位姿解算。
10.一种存储介质，该存储介质内存储有计算机程序，其特征在于，所述计算机程序被处理执行时实现如权利要求1-7中任一项所述的基于位姿融合优化的室内地图构建方法。
CN202110924699.0A 2021-08-12 2021-08-12 基于位姿融合优化的室内地图构建方法、***及存储介质 Active CN113674412B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110924699.0A CN113674412B (zh) 2021-08-12 2021-08-12 基于位姿融合优化的室内地图构建方法、***及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110924699.0A CN113674412B (zh) 2021-08-12 2021-08-12 基于位姿融合优化的室内地图构建方法、***及存储介质

Publications (2)

Publication Number Publication Date
CN113674412A true CN113674412A (zh) 2021-11-19
CN113674412B CN113674412B (zh) 2023-08-29

Family

ID=78542345

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110924699.0A Active CN113674412B (zh) 2021-08-12 2021-08-12 基于位姿融合优化的室内地图构建方法、***及存储介质

Country Status (1)

Country Link
CN (1) CN113674412B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114442808A (zh) * 2022-01-26 2022-05-06 深圳市慧鲤科技有限公司 一种位姿跟踪模块的测试方法、装置、设备、***和介质
CN116258769A (zh) * 2023-05-06 2023-06-13 亿咖通(湖北)技术有限公司 一种定位校验方法、装置、电子设备和存储介质
CN117437290A (zh) * 2023-12-20 2024-01-23 深圳市森歌数据技术有限公司 一种多传感器融合的自然保护区无人机三维空间定位方法
CN117554984A (zh) * 2023-11-08 2024-02-13 广东科学技术职业学院 一种基于图像理解的单线激光雷达室内slam定位方法及***

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105651242A (zh) * 2016-04-05 2016-06-08 清华大学深圳研究生院 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法
CN106056664A (zh) * 2016-05-23 2016-10-26 武汉盈力科技有限公司 一种基于惯性和深度视觉的实时三维场景重构***及方法
CN108827301A (zh) * 2018-04-16 2018-11-16 南京航空航天大学 一种改进误差四元数卡尔曼滤波机器人姿态解算方法
CN109631888A (zh) * 2019-01-04 2019-04-16 北京卡路里信息技术有限公司 动作轨迹识别方法、装置、可穿戴设备及存储介质
CN109785428A (zh) * 2019-01-21 2019-05-21 苏州大学 一种基于多态约束卡尔曼滤波的手持式三维重建方法
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110163909A (zh) * 2018-02-12 2019-08-23 北京三星通信技术研究有限公司 用于获得设备位姿的方法、装置和存储介质
WO2019205853A1 (zh) * 2018-04-27 2019-10-31 腾讯科技（深圳）有限公司 相机姿态追踪过程的重定位方法、装置、设备及存储介质
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
EP3715785A1 (en) * 2019-03-29 2020-09-30 Trimble Inc. Slam assisted ins
WO2020253854A1 (zh) * 2019-06-21 2020-12-24 台州知通科技有限公司 移动机器人姿态角解算方法
KR20210063723A (ko) * 2019-11-25 2021-06-02 한국전자기술연구원 Hmd를 착용한 사용자의 자세 추정 방법

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105651242A (zh) * 2016-04-05 2016-06-08 清华大学深圳研究生院 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法
CN106056664A (zh) * 2016-05-23 2016-10-26 武汉盈力科技有限公司 一种基于惯性和深度视觉的实时三维场景重构***及方法
CN110163909A (zh) * 2018-02-12 2019-08-23 北京三星通信技术研究有限公司 用于获得设备位姿的方法、装置和存储介质
CN108827301A (zh) * 2018-04-16 2018-11-16 南京航空航天大学 一种改进误差四元数卡尔曼滤波机器人姿态解算方法
WO2019205853A1 (zh) * 2018-04-27 2019-10-31 腾讯科技（深圳）有限公司 相机姿态追踪过程的重定位方法、装置、设备及存储介质
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109631888A (zh) * 2019-01-04 2019-04-16 北京卡路里信息技术有限公司 动作轨迹识别方法、装置、可穿戴设备及存储介质
CN109785428A (zh) * 2019-01-21 2019-05-21 苏州大学 一种基于多态约束卡尔曼滤波的手持式三维重建方法
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
EP3715785A1 (en) * 2019-03-29 2020-09-30 Trimble Inc. Slam assisted ins
WO2020253854A1 (zh) * 2019-06-21 2020-12-24 台州知通科技有限公司 移动机器人姿态角解算方法
KR20210063723A (ko) * 2019-11-25 2021-06-02 한국전자기술연구원 Hmd를 착용한 사용자의 자세 추정 방법

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
李鲁明;赵鲁阳;唐晓红;何为;李凤荣;: "基于模糊卡尔曼滤波的姿态估计算法", 仪表技术与传感器, no. 04 *
江杰;朱君;岂伟楠;: "四旋翼无人飞行器姿态数据采集处理***", 计算机测量与控制, no. 06 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114442808A (zh) * 2022-01-26 2022-05-06 深圳市慧鲤科技有限公司 一种位姿跟踪模块的测试方法、装置、设备、***和介质
CN116258769A (zh) * 2023-05-06 2023-06-13 亿咖通(湖北)技术有限公司 一种定位校验方法、装置、电子设备和存储介质
CN117554984A (zh) * 2023-11-08 2024-02-13 广东科学技术职业学院 一种基于图像理解的单线激光雷达室内slam定位方法及***
CN117437290A (zh) * 2023-12-20 2024-01-23 深圳市森歌数据技术有限公司 一种多传感器融合的自然保护区无人机三维空间定位方法
CN117437290B (zh) * 2023-12-20 2024-02-23 深圳市森歌数据技术有限公司 一种多传感器融合的自然保护区无人机三维空间定位方法

Also Published As

Publication number Publication date
CN113674412B (zh) 2023-08-29

Similar Documents

Publication Publication Date Title
CN111258313B (zh) 多传感器融合slam***及机器人
CN111024066B (zh) 一种无人机视觉-惯性融合室内定位方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及***
CN111156998B (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
CN112902953B (zh) 一种基于slam技术的自主位姿测量方法
CN113674412B (zh) 基于位姿融合优化的室内地图构建方法、***及存储介质
Weiss Vision based navigation for micro helicopters
CN111561923A (zh) 基于多传感器融合的slam制图方法、***
CN110095116A (zh) 一种基于lift的视觉定位和惯性导航组合的定位方法
CN111076733A (zh) 一种基于视觉与激光slam的机器人室内建图方法及***
CN113551665B (zh) 一种用于运动载体的高动态运动状态感知***及感知方法
CN113052908A (zh) 一种基于多传感器数据融合的移动机器人位姿估计算法
CN111932674A (zh) 一种线激光视觉惯性***的优化方法
CN112734765A (zh) 基于实例分割与多传感器融合的移动机器人定位方法、***及介质
Tomažič et al. Fusion of visual odometry and inertial navigation system on a smartphone
CN111161337A (zh) 一种动态环境下的陪护机器人同步定位与构图方法
CN115479598A (zh) 基于多传感器融合的定位与建图方法及紧耦合***
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN113516692A (zh) 一种多传感器融合的slam方法和装置
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台
CN114648584B (zh) 一种用于多源融合定位的鲁棒性控制方法和***
Huai et al. Real-time large scale 3D reconstruction by fusing Kinect and IMU data
CN115355904A (zh) 一种针对地面移动机器人的Lidar-IMU融合的slam方法
CN115218906A (zh) 面向室内slam的视觉惯性融合定位方法及***
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach

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