CN114199235A - 一种基于扇面深度相机的定位系统及定位方法 - Google Patents

Abstract

本发明公开一种基于扇面深度相机的定位系统，定位系统包括：扇面深度相机，用于采集水平方向上预设角度的扇面范围内的三维点云数据并传输至定位优化模块；图像采集装置，用于采集图像数据并传输至定位优化模块；惯性传感器，用于采集IMU数据并传输至定位优化模块；定位优化模块，用于接收扇面深度相机传输的三维点云数据、图像采集装置传输的图像数据以及惯性传感器传输的IMU数据，并基于图像数据和IMU数据对三维点云数据进行优化，获取优化后的位置信息。本发明采用扇面深度相机实现激光导航定位，同时结合视觉图像数据和IMU数据对扇面深度相机数据进行补偿优化，提高基于扇面深度相机的定位系统的定位精度和可靠性。

Description

一种基于扇面深度相机的定位系统及定位方法

技术领域

本发明涉及视觉定位技术领域，具体涉及一种基于扇面深度相机的定位系统及定位方法。

背景技术

目前现有技术中移动机器人通常具备自主移动定位导航功能，大部分移动机器人采用视觉、激光或惯性导航等技术实现自主定位导航，在目前支持移动机器人的定位导航技术中，360度旋转激光导航技术由于定位精度高和地图构建效果佳得到较多消费者的肯定，但是360度旋转激光导航技术需要在移动机器人上装配凸起的的旋转激光机构，增加机器人开模成本，且旋转激光机构由于存在旋转的机械结构导致其存在寿命较短的问题。

发明内容

为解决上述问题，本发明提供了一种基于扇面深度相机的定位系统及定位方法，采用不存在旋转机械结构的扇面深度相机实现激光导航定位，结合视觉图像数据和IMU数据对扇面深度相机数据进行补偿优化，在解决旋转激光导航技术短板的同时提高定位精度和可靠性。本发明的具体技术方案如下：

一种基于扇面深度相机的定位系统，所述基于扇面深度相机的定位系统具体包括：扇面深度相机，用于采集水平方向上预设角度的扇面范围内的三维点云数据并传输至定位优化模块；图像采集装置，用于采集图像数据并传输至定位优化模块；惯性传感器，用于采集IMU数据并传输至定位优化模块；定位优化模块，用于接收扇面深度相机传输的三维点云数据、图像采集装置传输的图像数据以及惯性传感器传输的IMU数据，并基于图像数据和IMU数据对三维点云数据进行优化，获取优化后的位置信息。

进一步地，所述定位优化模块具体包括：图像前端处理单元，用于接收所述图像采集装置传输的图像数据，并对图像数据进行视觉处理，以获取第一待优化位姿并传输至后端优化单元；点云前端处理单元，用于接收所述扇面深度相机传输的三维点云数据和所述惯性传感器传输的IMU数据，并对三维点云数据和IMU数据进行点云处理，以获取第二待优化位姿并传输至后端优化单元；后端优化单元，用于接收所述图像前端处理单元传输的第一待优化位姿、所述点云前端处理单元传输的第二待优化位姿和惯性传感器传输的IMU数据，并采用IMU数据和第一待优化位姿对第二待优化位姿进行后端优化处理，获取优化后的位置信息。

进一步地，所述图像前端处理单元还将所述第一待优化位姿传输至所述点云前端处理单元，以作为补偿数据优化所述点云前端处理单元基于三维点云数据和IMU数据获取的第二待优化位姿。

本发明还公开一种基于扇面深度相机的定位方法，所述基于扇面深度相机的定位方法是基于如前所述的基于扇面深度相机的定位系统实现，所述基于扇面深度相机的定位方法包括：图像处理步骤：图像采集装置获取当前帧图像并传输至图像前端处理单元，图像前端处理单元对当前帧图像进行图像处理，获取当前帧图像的相对位姿作为第一待优化位姿，图像前端处理单元将第一待优化位姿传输至后端优化单元；点云处理步骤：扇面深度相机获取当前帧三维点云并传输至点云前端处理单元，点云前端处理单元对当前帧三维点云进行最优匹配筛选，获取当前帧三维点云的最优匹配位姿作为第二待优化位姿，点云前端处理单元将第二待优化位姿传输至后端优化单元；定位优化处理步骤：后端优化单元基于惯性传感器传输的IMU数据和图像前端处理单元传输的第一待优化位姿对点云前端处理单元传输的第二待优化位姿进行定位优化处理，获取优化后的位置信息。

进一步地，所述图像处理步骤具体包括：图像采集装置获取当前帧图像并传输至图像前端处理单元；图像前端处理单元从之前记录的图像中确定参考帧图像；图像前端处理单元对参考帧图像和当前帧图像进行特征提取，获取参考帧图像的特征和当前帧图像的特征；图像前端处理单元对参考帧图像的特征和当前帧图像的特征进行特征匹配，获取参考帧图像的特征与当前帧图像的特征的重投影误差；图像前端处理单元对参考帧图像的特征与当前帧图像的特征的重投影误差进行最小化处理，获取参考帧图像与当前帧图像的相对位姿作为当前帧图像的第一相对位姿；图像前端处理单元将当前帧图像的第一相对位姿作为第一待优化位姿传输至后端优化单元。

进一步地，所述图像前端处理单元从之前记录的图像中确定参考帧图像的方法，具体包括：所述图像前端处理单元从之前记录的图像中确定当前帧图像的上一帧图像为参考帧图像，和/或，所述图像前端处理单元从之前记录的图像中确定其中一帧关键帧图像为参考帧图像。

进一步地，所述图像处理步骤还包括：图像前端处理单元判断是否能够从之前记录的全部关键帧图像中选取至少一帧与所述当前帧图像的特征相匹配的关键帧图像；若能够从之前记录的全部关键帧图像中选取至少一帧与所述当前帧图像的特征相匹配的关键帧图像，则获取其中一帧与所述当前帧图像的特征相匹配的关键帧图像与当前帧图像的相对位姿作为当前帧图像的第二相对位姿，图像前端处理单元将当前帧图像的第二相对位姿作为第一待优化位姿传输至后端优化单元；若不能够从之前记录的全部关键帧图像中选取至少一帧与所述当前帧图像的特征相匹配的关键帧图像，则图像前端处理单元将当前帧图像的第一相对位姿作为第一待优化位姿传输至后端优化单元。

进一步地，所述点云处理步骤具体包括：扇面深度相机获取当前帧三维点云并传输至点云前端处理单元；点云前端处理单元将当前帧三维点云与当前子图进行匹配，获取当前帧三维点云与当前子图的最优匹配位姿；点云处理前端将当前帧三维点云与当前子图的最优匹配位姿作为第二待优化位姿。

进一步地，所述点云前端处理单元将当前帧三维点云与当前子图进行匹配，获取当前帧三维点云与当前子图的最优匹配位姿的方法，具体包括：点云前端处理单元基于之前获取的三维点云进行三维点云叠加匹配，生成当前子图；点云前端处理单元采用迭代最近点算法将当前帧三维点云与当前子图中全部三维点云分别进行一一匹配，获取当前帧三维点云与当前子图的匹配误差；点云前端处理单元对当前帧三维点云与当前子图的匹配误差进行最小化处理，获取当前帧三维点云与当前子图的最优匹配位姿。

进一步地，所述点云处理步骤还包括：点云前端处理单元判断最小化处理后的当前帧三维点云与当前子图的匹配误差是否大于或等于预设误差阈值；若所述最小化处理后的当前帧三维点云与当前子图的匹配误差大于或等于预设误差阈值，则将所述当前子图作为历史子图，并重新构建当前子图；若所述最小化处理后的当前帧三维点云与当前子图的匹配误差小于预设误差阈值，则将当前帧三维点云叠加匹配到当前子图中，更新当前子图。

进一步地，所述点云处理步骤还包括：点云前端处理单元选取最邻近帧的第一待优化位姿作为当前帧三维点云的初始位姿，以补偿优化当前帧三维点云；其中，所述最邻近帧的第一待优化位姿是指与所述当前帧三维点云所在帧间隔帧数最少的一帧第一待优化位姿。

进一步地，所述点云处理步骤还包括：点云前端处理单元获取惯性传感器传输的IMU数据；点云前端处理单元判断当前帧三维点云的初始位姿与当前帧三维点云是否为同一帧；若当前帧三维点云的初始位姿与当前帧三维点云为同一帧，则采用当前帧三维点云的初始位姿对当前帧三维点云进行配准；若当前帧三维点云的初始位姿与当前帧三维点云不为同一帧，则点云前端处理单元基于IMU数据对所述当前帧三维点云的初始位姿和所述当前帧三维点云进行等比例对齐处理，采用等比例对齐处理后的当前帧三维点云的初始位姿对等比例对齐处理后的当前帧三维点云进行配准。

进一步地，所述点云前端处理单元基于IMU数据对当前帧三维点云的初始位姿和当前帧三维点云进行等比例对齐处理的方法具体包括：点云前端处理单元获取与所述当前帧三维点云的初始位姿最邻近的两帧IMU数据，将与所述当前帧三维点云的初始位姿最邻近的两帧IMU数据中的前一帧IMU数据作为第一对齐参考数据，将与所述当前帧三维点云的初始位姿的两帧IMU数据中的后一帧IMU数据作为第二对齐参考数据；点云前端处理单元计算所述当前帧三维点云的初始位姿的采集时刻与所述第一对齐参考数据的采集时刻相距的第一时刻差值；点云前端处理单元计算所述当前帧三维点云的初始位姿的采集时刻与所述第二对齐参考数据的采集时刻相距的第二时刻差值；点云前端处理单元根据第一对齐参考数据和第二对齐参考数据之间的位姿、第一时间差值和第二时间差值，确定所述当前帧三维点云的初始位姿与第一对齐参考数据之间的位姿；点云前端处理单元获取与所述当前帧三维点云最邻近的两帧IMU数据，将与所述当前帧三维点云最邻近的两帧IMU数据中的前一帧IMU数据作为第三对齐参考数据，将与所述当前帧三维点云最邻近的两帧IMU数据的后一帧IMU数据作为第四对齐参考数据；点云前端处理单元计算所述当前帧三维点云的采集时刻与所述第三对齐参考数据的采集时刻相距的第三时刻差值；点云前端处理单元计算所述当前帧三维点云的采集时刻与所述第四对齐参考数据的采集时刻相距的第四时刻差值；点云前端处理单元根据所述第三对齐参考数据和所述第四对齐参考数据之间的位姿、所述第三时间差值和所述第四时间差值，确定所述当前帧三维点云与第三对齐参考数据之间的位姿；点云前端处理单元计算所述第一对齐参考数据的采集时刻与所述第三对齐参考数据的采集时刻相距的第五时刻差值；点云前端处理单元根据所述第一对齐参考数据和所述第三对齐参考数据之间的位姿、第五时间差值、所述当前帧三维点云的初始位姿与第一对齐参考数据之间的位姿，以及所述当前帧三维点云与第三对齐参考数据之间的位姿，确定所述当前帧三维点云的初始位姿与所述当前帧三维点云之间的位姿。

进一步地，所述定位优化处理步骤具体包括：后端优化单元接收图像前端处理单元传输的第一待优化位姿以及点云前端处理单元传输的第二待优化位姿；后端优化单元对第一待优化位姿和第二待优化位姿进行误差最小化计算，获取最佳节点图；后端优化单元接收惯性传感器传输的IMU数据，并基于IMU数据结合最佳节点图进行滤波融合优化，获取优化后的位置信息。

本发明提供的基于扇面深度相机的定位系统采用扇面深度相机实现激光定位导航，相较于目前现有技术中常用的360度旋转激光，本发明采用的扇面深度相机无需配置旋转机械结构，延长激光装置寿命，且采用视觉定位技术采集的图像数据与惯性传感器采集的IMU数据对扇面深度相机采集的数据进行补偿优化，从而解决扇面深度相机受限于采集角度导致无法获取足够的匹配信息的短板问题，实现结合视觉定位技术和扇面深度相机的优势，提升定位系统整体定位精度和可靠性。

附图说明

图1为本发明第一实施例所述基于扇面深度相机的定位系统的模块示意图。

图2为本发明第二实施例所述基于扇面深度相机的定位系统的模块示意图。

图3为本发明第三实施例所述基于扇面深度相机的定位系统的模块示意图。

图4为本发明第四实施例所述基于扇面深度相机的定位方法的步骤示意图。

图5为本发明一种实施例所述图像处理的步骤示意图。

图6为本发明一种实施例所述点云处理的步骤示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清晰，以下将结合附图及实施例，对本发明进行描述和说明。应当理解，下面所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。此外还可以理解的是，对于本领域普通技术人员而言，在本发明揭露的技术内容上进行一些设计、制造或者生产等变更只是常规的技术手段，不应理解为本申请公开的内容不充分。

除非另作定义，本发明所涉及的技术术语或科学术语应当为本申请所属技术领域内普通技术人员所理解的通常意义。本申请所涉及的术语“包含”、“包括”、“具有”以及它们任何变形，意图在于覆盖不排他的包含，如：包含一系列步骤或模块的过程、方法、系统产品或者设备，不限定于已列出的步骤或模块，而是还可以包括没有列出的步骤或模块，或者还可以包括对于这些过程、方法、产品或设备固有的其他步骤或模块。

作为本发明一种较优的实施例，本发明的第一实施例提供一种基于扇面深度相机的定位系统，采用扇面深度相机以解决目前360旋转激光技术所需旋转激光结构成本高、寿命短的问题，同时结合视觉图像数据和IMU数据对扇面深度相机采集的三维点云数据进行补偿优化，以解决扇面深度相机相较于360度旋转激光获取的三维点云数据量少的短板，提高定位系统整体定位精度和可靠性。

需要说明的是本发明所提供的基于扇面深度相机的定位系统适用于各类具备移动功能的智能机器人，如：扫地机器人、拖地机器人和巡逻机器人等。

具体地，如图1所示，所述基于扇面深度相机的定位系统包括：

扇面深度相机，用于采集水平方向上预设角度的扇面范围内的三维点云数据并传输至定位优化模块；具体地，水平方向上所能采集三维点云数据的范围被限定为预设角度的扇面范围内，所述扇面深度相机的预设角度可以是但不限于30°、60°、90°或120°等小于180°的角度。

图像采集装置，用于采集图像数据并传输至定位优化模块；具体地，所述图像采集装置是指具备图像采集功能的装置，本发明中图像采集装置可以是但不限于单目摄像头、双目摄像头或鱼眼相机等具有图像采集功能的装置。

惯性传感器，用于采集IMU数据并传输至定位优化模块；具体地，所述惯性传感器是用于检测和测量加速度和倾斜的传感器，本发明中惯性传感器包括加速度传感器和角度传感器；所述惯性传感器采集的IMU数据具体包括加速度和角速度。

定位优化模块，用于接收扇面深度相机传输的三维点云数据、图像采集装置传输的图像数据以及惯性传感器传输的IMU数据，并基于图像数据和IMU数据对三维点云数据进行优化，获取优化后的位置信息。

基于上述实施例，作为本发明一种较优的实施例，如图2所示，本发明的第二实施例中所述定位优化模块包括：图像前端处理单元、点云前端处理单元和后端优化单元。

具体地，所述图像前端处理单元，用于接收所述图像采集装置传输的图像数据，并对图像数据进行视觉处理，以获取第一待优化位姿并传输至后端优化单元；具体地，所述图像前端处理单元用于实现基于图像数据获取用于实现补偿扇面深度相机的第一待优化位姿。

所述点云前端处理单元，用于接收所述扇面深度相机传输的三维点云数据和所述惯性传感器传输的IMU数据，并对三维点云数据和IMU数据进行点云处理，以获取第二待优化位姿并传输至后端优化单元；具体地，所述点云前端处理单元用于基于扇面深度相机获取的三维点云数据结合IMU数据处理获取第二待优化位姿，以作为扇面深度相机提供的定位参考位姿。

所述后端优化单元，用于接收所述图像前端处理单元传输的第一待优化位姿、所述点云前端处理单元传输的第二待优化位姿和惯性传感器传输的IMU数据，并采用IMU数据和第一待优化位姿对第二待优化位姿进行后端优化处理，获取优化后的位置信息。具体地，所述后端优化单元用于采用IMU数据和第一待优化位姿对基于扇面深度相机获取的第二待优化位姿进行后端优化处理，实现基于视觉定位技术采集的图像数据与惯性传感器采集的IMU数据解决扇面深度相机受限于采集角度导致无法获取足够的匹配信息的短板，提高定位系统的定位精度。

基于上述实施例，作为本发明一种较优的实施例，如图3所示，本发明的第三实施例中所述图像前端处理单元还将所述第一待优化位姿传输至所述点云前端处理单元，作为初始位姿以补偿优化所述点云前端处理单元基于三维点云数据和IMU数据获取的第二待优化位姿。本实施例通过将基于视觉定位技术获取的第一待优化位姿作为基于扇面深度相机获取第二待优化位姿的初始位姿，以实现第一待优化位姿对第二待优化位姿的补偿优化，提高扇面深度相机获取的第二待优化位姿的精度。

基于上述实施例所述的基于扇面深度相机的定位系统，作为本发明一种较优的实施例，本发明的第四实施例提供一种基于扇面深度相机的定位方法，所述基于扇面深度相机的定位方法包括图像处理步骤、点云处理步骤和定位优化处理步骤，需要说明的是，如图4所示，所述图像处理步骤和所述点云处理步骤同步进行，所述图像处理步骤的输出结果和所述点云处理步骤的输出结果都在定位优化处理步骤中进行优化。

具体地，所述图像处理步骤包括：图像采集装置获取当前帧图像并传输至图像前端处理单元，图像前端处理单元对当前帧图像进行图像处理，获取当前帧图像的相对位姿作为第一待优化位姿，图像前端处理单元将第一待优化位姿传输至后端优化单元；所述点云处理步骤包括：扇面深度相机获取当前帧三维点云并传输至点云前端处理单元，点云前端处理单元对当前帧三维点云进行最优匹配筛选，获取当前帧三维点云的最优匹配位姿作为第二待优化位姿，点云前端处理单元将第二待优化位姿传输至后端优化单元；所述定位优化处理步骤包括：后端优化单元基于惯性传感器传输的IMU数据和图像前端处理单元传输的第一待优化位姿对点云前端处理单元传输的第二待优化位姿进行定位优化处理，获取优化后的位置信息。本实施例通过采用图像处理步骤输出的第一待优化位姿对点云处理步骤输出的第二待优化位姿进行优化处理，以实现通过视觉补偿的方式提高基于扇面深度相机的定位精度。

基于上述实施例所述的基于扇面深度相机的定位方法，作为本发明一种较优的实施例，本发明的第五实施例中，如图5所示，所述图像处理步骤具体包括：

图像采集装置获取当前帧图像并传输至图像前端处理单元；具体地，所述图像采集装置可以是但不限于单目摄像头、双目摄像头、鱼眼相机或深度相机等具备图像采集功能的装置；

图像前端处理单元从之前记录的图像中确定参考帧图像；具体地，所述确定参考帧图像的方式是根据实际优化精度要求进行调整，所述图像前端处理单元从之前记录的图像中确定参考帧图像的方法，具体包括：所述图像前端处理单元从之前记录的图像中确定当前帧图像的上一帧图像为参考帧图像，和/或，所述图像前端处理单元从之前记录的图像中确定其中一帧关键帧图像为参考帧图像。

图像前端处理单元对参考帧图像和当前帧图像进行特征提取，获取参考帧图像的特征和当前帧图像的特征；具体地，所述图像前端处理单元对参考帧图像和当前帧图像进行特征提取的方式可以是但不限于Harris算法、SIFT（Scale Invariant FeatureTransform)算法、SURF算法或FAST算法等。

图像前端处理单元对参考帧图像的特征和当前帧图像的特征进行特征匹配，获取参考帧图像的特征与当前帧图像的特征的重投影误差；具体地，所述特征匹配是指对两幅图像中的特征进行一一比对；所述获取参考帧图像的特征于当前帧图像的特征的重投影误差是指将所述参考帧图像的特征点在所述当前帧图像中的投影与所述当前帧图像中一一对应匹配的特征点的误差。

图像前端处理单元对参考帧图像的特征与当前帧图像的特征的重投影误差进行最小化处理，获取参考帧图像与当前帧图像的相对位姿作为当前帧图像的第一相对位姿；具体地，所述最小化处理是指用于优化所述参考帧图像的特征与所述当前帧图像的重投影误差的处理手段，以使得所述参考帧图像与所述当前帧图像的相对位姿最小，所述最小化处理的方法可以是但不限于能够使得重投影误差变小的方法，如：调节图像采集装置位姿等。

图像前端处理单元将当前帧图像的第一相对位姿作为第一待优化位姿传输至后端优化单元。

本实施例提供的图像处理步骤通过获取当前帧图像与参考帧图像的重投影误差，并通过最小化重投影误差的方式提高第一待优化位姿的精度，确保视觉定位技术能够提供高精度的第一待优化位姿以供基于扇面深度相机获取的第二待优化位姿进行补偿优化。

基于上述实施例所述的基于扇面深度相机的定位方法，作为本发明一种较优的实施例，本发明的第六实施例中，如图5所示，所述图像处理步骤还包括：图像前端处理单元判断是否能够从之前记录的全部关键帧图像中选取至少一帧与所述当前帧图像的特征相匹配的关键帧图像；若能够从之前记录的全部关键帧图像中选取至少一帧与所述当前帧图像的特征相匹配的关键帧图像，则获取其中一帧与所述当前帧图像的特征相匹配的关键帧图像与当前帧图像的相对位姿作为当前帧图像的第二相对位姿，图像前端处理单元将当前帧图像的第二相对位姿作为第一待优化位姿传输至后端优化单元；若不能够从之前记录的全部关键帧图像中选取至少一帧与所述当前帧图像的特征相匹配的关键帧图像，则图像前端处理单元将当前帧图像的第一相对位姿作为第一待优化位姿传输至后端优化单元。具体地，所述与当前帧图像的特征相匹配的关键帧图像是指该关键帧图像中的特征与当前帧图像的特征重合度满足预设匹配阈值；所述预设匹配阈值可以根据实际匹配精度需求进行调节。本实施例通过在关键帧图像中选取相匹配的一帧关键帧图像并将该一帧关键帧图像与当前帧图像的相对位姿作为第一待优化位姿送至位姿优化处理步骤，提高了第一待优化位姿的精准度，并将关键帧图像与当前帧图像的相对位姿设置为优先于参考帧图像与当前帧图像的相对位姿的第一待优化位姿，优化图像处理步骤输出的第一待优化位姿的精度，提高定位方法整体可靠性。

优选地，所述图像处理步骤执行过程中，还包括：若能够从之前记录的全部关键帧图像中选取出至少一帧与所述当前帧图像的特征相匹配的关键帧图像，则将当前帧图像标记并存储为关键帧图像。本步骤通过特征匹配结果实时记录新的关键帧图像。

基于上述实施例所述的基于扇面深度相机的定位方法，作为本发明一种较优的实施例，本发明的第七实施例中，所述点云处理步骤具体包括：扇面深度相机获取当前帧三维点云并传输至点云前端处理单元；点云前端处理单元将当前帧三维点云与当前子图进行匹配，获取当前帧三维点云与当前子图的最优匹配位姿；具体地，所述当前子图由多帧三维点云数据配准叠加而成。点云处理前端将当前帧三维点云与当前子图的最优匹配位姿作为第二待优化位姿。本实施例通过对当前帧三维点云与当前子图进行匹配，以获取当前帧三维点云与当前子图的最优匹配位姿，从而将其作为第二待优化位姿传输以执行后端优化处理步骤，本实施例公开的点云处理步骤实现对基于三维点云数据获取的位姿进行优化，以获取最优匹配位姿，实现三维点云数据的前端优化步骤，在执行视觉对扇面深度相机的补偿优化前，先对扇面深度相机获取的三维点云进行前端优化，通过前后端分别优化的方式提高定位方法整体的定位精度和可靠性。

基于上述实施例所述的基于扇面深度相机的定位方法，作为本发明一种较优的实施例，如图6所示，本发明的第八实施例中，所述点云前端处理单元将当前帧三维点云与当前子图进行匹配，获取当前帧三维点云与当前子图的最优匹配位姿的方法，具体包括：

点云前端处理单元基于之前获取的三维点云进行三维点云叠加匹配，生成当前子图；点云前端处理单元采用迭代最近点算法将当前帧三维点云与当前子图中全部三维点云分别进行一一匹配，获取当前帧三维点云与当前子图的匹配误差；需要说明的是，本实施例中采用的迭代最近点算法是一种用于图像配准领域，对点集与点集进行配准的算法。

点云前端处理单元对当前帧三维点云与当前子图的匹配误差进行最小化处理，获取当前帧三维点云与当前子图的最优匹配位姿；具体地，所述最小化处理是指所述迭代最近点算法对当前帧三维点云与所述当前子图进行配准后，对匹配误差进行最小化，以使得当前帧三维点云与所述当前子图尽可能匹配重叠，缩小其匹配误差，实现优化三维点云数据的前端结果，从而优化整体定位效果。

基于上述实施例所述的基于扇面深度相机的定位方法，作为本发明一种较优的实施例，本发明的第九实施例中，所述点云处理步骤还包括：点云前端处理单元判断最小化处理后的当前帧三维点云与当前子图的匹配误差是否大于或等于预设误差阈值；需要说明的是，所述预设误差阈值是根据实际应用过程中扇面深度相机所能水平方向上覆盖预设角度范围、匹配精度要求等多种因素综合考量设置的阈值，用于限定匹配误差的大小，以确定是否重新构建子图，从而确保后续三维点云数据与当前子图配准的准确性和可靠性。

若所述最小化处理后的当前帧三维点云与当前子图的匹配误差大于或等于预设误差阈值，则将所述当前子图作为历史子图，并重新构建当前子图；若所述最小化处理后的当前帧三维点云与当前子图的匹配误差小于预设误差阈值，则将当前帧三维点云叠加匹配到当前子图中，更新当前子图。本实施例通过对点云处理步骤中最小化处理后的匹配误差与预设误差阈值进行比较，以确定三维点云数据的变化情况，提高小角度点云处理步骤输出的第二待优化位姿的精度，确保定位方法整体的精度和可靠性。

基于上述实施例所述的基于扇面深度相机的定位方法，作为本发明一种较优的实施例，本发明的第十实施例中，所述点云处理步骤还包括：点云前端处理单元选取最邻近帧的第一待优化位姿作为当前帧三维点云的初始位姿，以补偿优化当前帧三维点云；其中，所述最邻近帧的第一待优化位姿是指与所述当前帧三维点云所在帧间隔帧数最少的一帧第一待优化位姿。本实施例通过采用基于图像获取的第一待优化位姿作为三维点云的初始位姿，实现在点云处理时对基于扇面深度相机获取的第二待优化位姿进行前端优化，在第一待优化位姿作为初始位姿的情况下，提高第二待优化位姿的精度。

基于上述实施例所述的基于扇面深度相机的定位方法，作为本发明一种较优的实施例，本发明的第十一实施例中，所述点云处理步骤还包括：

点云前端处理单元获取惯性传感器传输的IMU数据；具体地，所述惯性传感器是指用于检测和测量加速度和旋转运动的传感器，所述惯性传感器包括但不限于加速度计和陀螺仪，所述IMU数据是指所述惯性传感器基于加速度计和陀螺仪获取的加速度和角速度等数据。

点云前端处理单元判断当前帧三维点云的初始位姿与当前帧三维点云是否为同一帧；需要说明的是，判断所述当前帧三维点云的初始位姿与所述当前帧三维点云是否为同一帧是指判断所述当前帧三维点云的初始位姿的采集时刻与所述当前帧三维点云的采集时刻是否相同。

若当前帧三维点云的初始位姿与当前帧三维点云为同一帧，则采用当前帧三维点云的初始位姿对当前帧三维点云进行配准；若当前帧三维点云的初始位姿与当前帧三维点云不为同一帧，则点云前端处理单元基于IMU数据对所述当前帧三维点云的初始位姿和所述当前帧三维点云进行等比例对齐处理，采用等比例对齐处理后的当前帧三维点云的初始位姿对等比例对齐处理后的当前帧三维点云进行配准。需要说明的是，所述IMU数据的采集频率通常为三维点云数据的采集频率或图像的采集频率若干倍，所述IMU数据的采集频率远远大于所述三维点云或所述图像的采集频率，如：所述IMU数据的采集频率为1000帧每秒，所述三维点云数据的采集频率为30帧每秒，所述图像的采集频率为10帧每秒，此处的举例仅仅用于说明IMU数据、三维点云和图像的采集频率存在频率相差较大的情况，并不代表着本发明中IMU数据、三维点云数据和图像按照上述采集频率执行，本发明中IMU数据、三维点云数据和图像的采集频率根据实际应用过程中采用的采集装置不同和实际精度需求不同进行调节。

本实施判断的目的是通过确定所述三维点云的初始位姿是否与当前帧三维点云为相同帧，若为相同帧则可以直接采用当前帧三维点云的初始位姿与当前帧三维点云进行配准，反之，若为不同帧则需要通过IMU数据进行时间轴等比例对齐后再进行配准，以确保其配准的精度和有效性。

基于上述实施例所述的基于扇面深度相机的定位方法，作为本发明一种较优的实施例，本发明的第十二实施例中，所述点云前端处理单元基于IMU数据对当前帧三维点云的初始位姿和当前帧三维点云进行等比例对齐处理的方法具体包括：

点云前端处理单元获取与所述当前帧三维点云的初始位姿最邻近的两帧IMU数据，将与所述当前帧三维点云的初始位姿最邻近的两帧IMU数据中的前一帧IMU数据作为第一对齐参考数据，将与所述当前帧三维点云的初始位姿的两帧IMU数据中的后一帧IMU数据作为第二对齐参考数据；需要说明的是，所述与当前帧三维点云的初始位姿最邻近的两帧IMU数据中的前一帧IMU数据的采集时刻在所述当前帧三维点云的初始位姿的采集时刻之前，所述与当前帧三维点云的初始位姿最邻近的两帧IMU数据中的后一帧IMU数据的采集时刻在所述当前帧激光点的初始位姿的采集时刻之后。

点云前端处理单元计算所述当前帧三维点云的初始位姿的采集时刻与所述第一对齐参考数据的采集时刻相距的第一时刻差值；点云前端处理单元计算所述当前帧三维点云的初始位姿的采集时刻与所述第二对齐参考数据的采集时刻相距的第二时刻差值；具体地，根据两个数据的采集时刻不同确定两个数据的采集时刻相距的时间差值。

点云前端处理单元根据第一对齐参考数据和第二对齐参考数据之间的位姿、第一时间差值和第二时间差值，确定所述当前帧三维点云的初始位姿与第一对齐参考数据之间的位姿；具体地，所述根据第一对齐参考数据和第二对齐参考数据之间的位姿、第一时间差值和第二时间差值，确定所述当前帧三维点云的初始位姿与第一对齐参考数据之间的位姿的方法具体为：由于第一对齐参考数据和第二对齐参考数据之间的位姿为已知数据，将第一对齐参考数据和第二对齐参考数据之间的位姿平均分至每一个时刻，实现等时间比例均分，从而确定当前帧三维点云的初始位姿与第一对齐参考数据之间的位姿等于第一对齐参考数据和第二对齐参考数据之间的位姿与第一时间差值的乘积除以第一时间差值和第二时间差值的和值。

点云前端处理单元获取与所述当前帧三维点云最邻近的两帧IMU数据，将与所述当前帧三维点云最邻近的两帧IMU数据中的前一帧IMU数据作为第三对齐参考数据，将与所述当前帧三维点云最邻近的两帧IMU数据的后一帧IMU数据作为第四对齐参考数据；需要说明的是，所述与当前帧三维点云最邻近的两帧IMU数据中的前一帧IMU数据的采集时刻在所述当前帧三维点云的采集时刻之前，所述与当前帧三维点云最邻近的两帧IMU数据中的后一帧IMU数据的采集时刻在所述当前帧激光点的采集时刻之后。

点云前端处理单元计算所述当前帧三维点云的采集时刻与所述第三对齐参考数据的采集时刻相距的第三时刻差值；点云前端处理单元计算所述当前帧三维点云的采集时刻与所述第四对齐参考数据的采集时刻相距的第四时刻差值；点云前端处理单元根据所述第三对齐参考数据和所述第四对齐参考数据之间的位姿、所述第三时间差值和所述第四时间差值，确定所述当前帧三维点云与第三对齐参考数据之间的位姿；具体地，本步骤中确定所述当前帧三维点云与第三对齐参考数据之间的数据的方法与上述确定当前帧三维点云的初始位姿与第一对齐参考数据之间的位姿的方法相同。

点云前端处理单元计算所述第一对齐参考数据的采集时刻与所述第三对齐参考数据的采集时刻相距的第五时刻差值；点云前端处理单元根据所述第一对齐参考数据和所述第三对齐参考数据之间的位姿、第五时间差值、所述当前帧三维点云的初始位姿与第一对齐参考数据之间的位姿，以及所述当前帧三维点云与第三对齐参考数据之间的位姿，确定所述当前帧三维点云的初始位姿与所述当前帧三维点云之间的位姿。需要说明的是，所述当前帧三维点云的初始位姿的采集时刻在所述当前帧三维点云的采集时刻之前。本实施例中通过将当前帧三维点云的初始位姿与当前帧三维点云通过IMU数据进行等比例对齐处理，以实现通过当前帧三维点云的初始位姿达到对当前帧三维点云配准，以提高当前帧三维点云精度的目的。

基于上述实施例所述的基于扇面深度相机的定位方法，作为本发明一种较优的实施例，本发明的第十三实施例中，所述定位优化处理步骤具体包括：后端优化单元接收图像前端处理单元传输的第一待优化位姿以及点云前端处理单元传输的第二待优化位姿；后端优化单元对第一待优化位姿和第二待优化位姿进行误差最小化计算，获取最佳节点图；后端优化单元接收惯性传感器传输的IMU数据，并基于IMU数据结合最佳节点图进行滤波融合优化，获取优化后的位置信息。所述滤波融合优化可以是但不限于采用卡尔曼滤波等具备优化误差功能的滤波，具体地，卡尔曼滤波是一种通过预测值和观测值优化消除数据误差的滤波。具体地，卡尔曼滤波由两个过程组成：预测和校正，在预测阶段，卡尔曼滤波采用基于上一帧数据的估计，对当前状态做出预测，在校正阶段，卡尔曼滤波利用当前帧的观测值修正预设阶段获取的当前状态预测值，从而获取一个更接近真实数据的估值。在本发明中，将视觉和扇面深度相机解算得到的位姿数据作为预测值，将IMU数据得到的位姿数据作为校正值，从而获取优化后的定位信息。本实施例通过多次误差最小化处理，层层递进的优化位姿信息，从而提高整体定位精度，且本实施例中采用卡尔曼滤波进行滤波融合处理是由于卡尔曼滤波的计算需求较小，且其估算结构预测较为精准，确保定位结果的精度和可靠性。

显然，上述的实施例仅仅是本发明一部分实施例，而不是全部的实施例，各个实施例之间的技术方案可以相互结合。在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。在本发明所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本发明实施方式对各种可能的组合方式不再另行说明。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (14)

1.一种基于扇面深度相机的定位系统，其特征在于，所述基于扇面深度相机的定位系统具体包括：

扇面深度相机，用于采集水平方向上预设角度的扇面范围内的三维点云数据并传输至定位优化模块；

图像采集装置，用于采集图像数据并传输至定位优化模块；

惯性传感器，用于采集IMU数据并传输至定位优化模块；

定位优化模块，用于接收扇面深度相机传输的三维点云数据、图像采集装置传输的图像数据以及惯性传感器传输的IMU数据，并基于图像数据和IMU数据对三维点云数据进行优化，获取优化后的位置信息。

2.根据权利要求1所述的基于扇面深度相机的定位系统，其特征在于，所述定位优化模块具体包括：

图像前端处理单元，用于接收所述图像采集装置传输的图像数据，并对图像数据进行视觉处理，以获取第一待优化位姿并传输至后端优化单元；

点云前端处理单元，用于接收所述扇面深度相机传输的三维点云数据和所述惯性传感器传输的IMU数据，并对三维点云数据和IMU数据进行点云处理，以获取第二待优化位姿并传输至后端优化单元；

后端优化单元，用于接收所述图像前端处理单元传输的第一待优化位姿、所述点云前端处理单元传输的第二待优化位姿和惯性传感器传输的IMU数据，并采用IMU数据和第一待优化位姿对第二待优化位姿进行后端优化处理，获取优化后的位置信息。

3.根据权利要求2所述的基于扇面深度相机的定位系统，其特征在于，所述图像前端处理单元还将所述第一待优化位姿传输至所述点云前端处理单元，以作为补偿数据优化所述点云前端处理单元基于三维点云数据和IMU数据获取的第二待优化位姿。

4.一种基于扇面深度相机的定位方法，所述基于扇面深度相机的定位方法是基于权利要求1至3任意一项所述的基于扇面深度相机的定位系统实现，其特征在于，所述基于扇面深度相机的定位方法包括：

图像处理步骤：图像采集装置获取当前帧图像并传输至图像前端处理单元，图像前端处理单元对当前帧图像进行图像处理，获取当前帧图像的相对位姿作为第一待优化位姿，图像前端处理单元将第一待优化位姿传输至后端优化单元；

点云处理步骤：扇面深度相机获取当前帧三维点云并传输至点云前端处理单元，点云前端处理单元对当前帧三维点云进行最优匹配筛选，获取当前帧三维点云的最优匹配位姿作为第二待优化位姿，点云前端处理单元将第二待优化位姿传输至后端优化单元；

定位优化处理步骤：后端优化单元基于惯性传感器传输的IMU数据和图像前端处理单元传输的第一待优化位姿对点云前端处理单元传输的第二待优化位姿进行定位优化处理，获取优化后的位置信息。

5.根据权利要求4所述的基于扇面深度相机的定位方法，其特征在于，所述图像处理步骤具体包括：

图像采集装置获取当前帧图像并传输至图像前端处理单元；

图像前端处理单元从之前记录的图像中确定参考帧图像；

图像前端处理单元对参考帧图像和当前帧图像进行特征提取，获取参考帧图像的特征和当前帧图像的特征；

图像前端处理单元对参考帧图像的特征和当前帧图像的特征进行特征匹配，获取参考帧图像的特征与当前帧图像的特征的重投影误差；

图像前端处理单元对参考帧图像的特征与当前帧图像的特征的重投影误差进行最小化处理，获取参考帧图像与当前帧图像的相对位姿作为当前帧图像的第一相对位姿；

图像前端处理单元将当前帧图像的第一相对位姿作为第一待优化位姿传输至后端优化单元。

6.根据权利要求5所述的基于扇面深度相机的定位方法，其特征在于，所述图像前端处理单元从之前记录的图像中确定参考帧图像的方法，具体包括：所述图像前端处理单元从之前记录的图像中确定当前帧图像的上一帧图像为参考帧图像，和/或，所述图像前端处理单元从之前记录的图像中确定其中一帧关键帧图像为参考帧图像。

7.根据权利要求6所述的基于扇面深度相机的定位方法，其特征在于，所述图像处理步骤还包括：

图像前端处理单元判断是否能够从之前记录的全部关键帧图像中选取至少一帧与所述当前帧图像的特征相匹配的关键帧图像；

若能够从之前记录的全部关键帧图像中选取至少一帧与所述当前帧图像的特征相匹配的关键帧图像，则获取其中一帧与所述当前帧图像的特征相匹配的关键帧图像与当前帧图像的相对位姿作为当前帧图像的第二相对位姿，图像前端处理单元将当前帧图像的第二相对位姿作为第一待优化位姿传输至后端优化单元；

若不能够从之前记录的全部关键帧图像中选取至少一帧与所述当前帧图像的特征相匹配的关键帧图像，则图像前端处理单元将当前帧图像的第一相对位姿作为第一待优化位姿传输至后端优化单元。

8.根据权利要求4所述的基于扇面深度相机的定位方法，其特征在于，所述点云处理步骤具体包括：

扇面深度相机获取当前帧三维点云并传输至点云前端处理单元；

点云前端处理单元将当前帧三维点云与当前子图进行匹配，获取当前帧三维点云与当前子图的最优匹配位姿；

点云处理前端将当前帧三维点云与当前子图的最优匹配位姿作为第二待优化位姿。

9.根据权利要求8所述的基于扇面深度相机的定位方法，其特征在于，所述点云前端处理单元将当前帧三维点云与当前子图进行匹配，获取当前帧三维点云与当前子图的最优匹配位姿的方法，具体包括：

点云前端处理单元基于之前获取的三维点云进行三维点云叠加匹配，生成当前子图；

点云前端处理单元采用迭代最近点算法将当前帧三维点云与当前子图中全部三维点云分别进行一一匹配，获取当前帧三维点云与当前子图的匹配误差；

点云前端处理单元对当前帧三维点云与当前子图的匹配误差进行最小化处理，获取当前帧三维点云与当前子图的最优匹配位姿。

10.根据权利要求9所述的基于扇面深度相机的定位方法，其特征在于，所述点云处理步骤还包括：

点云前端处理单元判断最小化处理后的当前帧三维点云与当前子图的匹配误差是否大于或等于预设误差阈值；

若所述最小化处理后的当前帧三维点云与当前子图的匹配误差大于或等于预设误差阈值，则将所述当前子图作为历史子图，并重新构建当前子图；

若所述最小化处理后的当前帧三维点云与当前子图的匹配误差小于预设误差阈值，则将当前帧三维点云叠加匹配到当前子图中，更新当前子图。

11.根据权利要求8所述的基于扇面深度相机的定位方法，其特征在于，所述点云处理步骤还包括：点云前端处理单元选取最邻近帧的第一待优化位姿作为当前帧三维点云的初始位姿，以补偿优化当前帧三维点云；其中，所述最邻近帧的第一待优化位姿是指与所述当前帧三维点云所在帧间隔帧数最少的一帧第一待优化位姿。

12.根据权利要求11所述的基于扇面深度相机的定位方法，其特征在于，所述点云处理步骤还包括：

点云前端处理单元获取惯性传感器传输的IMU数据；

点云前端处理单元判断当前帧三维点云的初始位姿与当前帧三维点云是否为同一帧；

若当前帧三维点云的初始位姿与当前帧三维点云为同一帧，则采用当前帧三维点云的初始位姿对当前帧三维点云进行配准；

若当前帧三维点云的初始位姿与当前帧三维点云不为同一帧，则点云前端处理单元基于IMU数据对所述当前帧三维点云的初始位姿和所述当前帧三维点云进行等比例对齐处理，采用等比例对齐处理后的当前帧三维点云的初始位姿对等比例对齐处理后的当前帧三维点云进行配准。

13.根据权利要求12所述的基于扇面深度相机的定位方法，其特征在于，所述点云前端处理单元基于IMU数据对当前帧三维点云的初始位姿和当前帧三维点云进行等比例对齐处理的方法具体包括：

点云前端处理单元获取与所述当前帧三维点云的初始位姿最邻近的两帧IMU数据，将与所述当前帧三维点云的初始位姿最邻近的两帧IMU数据中的前一帧IMU数据作为第一对齐参考数据，将与所述当前帧三维点云的初始位姿的两帧IMU数据中的后一帧IMU数据作为第二对齐参考数据；

点云前端处理单元计算所述当前帧三维点云的初始位姿的采集时刻与所述第一对齐参考数据的采集时刻相距的第一时刻差值；

点云前端处理单元计算所述当前帧三维点云的初始位姿的采集时刻与所述第二对齐参考数据的采集时刻相距的第二时刻差值；

点云前端处理单元根据第一对齐参考数据和第二对齐参考数据之间的位姿、第一时间差值和第二时间差值，确定所述当前帧三维点云的初始位姿与第一对齐参考数据之间的位姿；

点云前端处理单元获取与所述当前帧三维点云最邻近的两帧IMU数据，将与所述当前帧三维点云最邻近的两帧IMU数据中的前一帧IMU数据作为第三对齐参考数据，将与所述当前帧三维点云最邻近的两帧IMU数据的后一帧IMU数据作为第四对齐参考数据；

点云前端处理单元计算所述当前帧三维点云的采集时刻与所述第三对齐参考数据的采集时刻相距的第三时刻差值；

点云前端处理单元计算所述当前帧三维点云的采集时刻与所述第四对齐参考数据的采集时刻相距的第四时刻差值；

点云前端处理单元根据所述第三对齐参考数据和所述第四对齐参考数据之间的位姿、所述第三时间差值和所述第四时间差值，确定所述当前帧三维点云与第三对齐参考数据之间的位姿；

点云前端处理单元计算所述第一对齐参考数据的采集时刻与所述第三对齐参考数据的采集时刻相距的第五时刻差值；

点云前端处理单元根据所述第一对齐参考数据和所述第三对齐参考数据之间的位姿、第五时间差值、所述当前帧三维点云的初始位姿与第一对齐参考数据之间的位姿，以及所述当前帧三维点云与第三对齐参考数据之间的位姿，确定所述当前帧三维点云的初始位姿与所述当前帧三维点云之间的位姿。

14.根据权利要求4至13任意一项所述的基于扇面深度相机的定位方法，其特征在于，所述定位优化处理步骤具体包括：

后端优化单元接收图像前端处理单元传输的第一待优化位姿以及点云前端处理单元传输的第二待优化位姿；

后端优化单元对第一待优化位姿和第二待优化位姿进行误差最小化计算，获取最佳节点图；

后端优化单元接收惯性传感器传输的IMU数据，并基于IMU数据结合最佳节点图进行滤波融合优化，获取优化后的位置信息。

Images