CN115265523B - 机器人同时定位与建图方法、装置及可读介质 - Google Patents

机器人同时定位与建图方法、装置及可读介质 Download PDF

Info

Publication number
CN115265523B
CN115265523B CN202211178092.3A CN202211178092A CN115265523B CN 115265523 B CN115265523 B CN 115265523B CN 202211178092 A CN202211178092 A CN 202211178092A CN 115265523 B CN115265523 B CN 115265523B
Authority
CN
China
Prior art keywords
laser
frame
pose
ground
factor
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.)
Active
Application number
CN202211178092.3A
Other languages
English (en)
Other versions
CN115265523A (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.)
Quanzhou Institute of Equipment Manufacturing
Original Assignee
Quanzhou Institute of Equipment Manufacturing
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 Quanzhou Institute of Equipment Manufacturing filed Critical Quanzhou Institute of Equipment Manufacturing
Priority to CN202211178092.3A priority Critical patent/CN115265523B/zh
Publication of CN115265523A publication Critical patent/CN115265523A/zh
Application granted granted Critical
Publication of CN115265523B publication Critical patent/CN115265523B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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
    • G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Abstract

本发明公开了一种机器人同时定位与建图方法、装置及可读介质，涉及机器人定位领域。通过惯性测量单元预积分得到IMU里程计；基于IMU里程计通过时间线性插值方法将一帧点云内的每一个激光点变换到初始时刻激光点坐标系，得到去除运动畸变的激光点云；对去除运动畸变的激光点云提取特征点，以IMU里程计作为当前激光帧的初始位姿，通过特征匹配得到当前激光帧与局部地图的相对变换关系，优化当前激光帧的位姿，得到激光里程计；选取部分激光帧作为关键帧，通过IMU预积分因子、激光里程计因子及GPS因子、回环因子、地面约束因子构建因子图优化调整关键帧位姿，解决随着建图时间增加导致的定位误差累积问题，获取全局一致定位并生成点云地图。

Description

机器人同时定位与建图方法、装置及可读介质
技术领域
本发明涉及机器人定位领域，具体涉及一种机器人同时定位与建图方法、装置及可读介质。
背景技术
机器人实现自主导航的重要前提条件就是实现自主定位，即确定自身相对于环境地图所处的位姿信息。常见的机器人所处环境表达方式有点云地图、栅格地图、语义地图、拓扑地图等地图形式。机器人实现自主定位离不开地图，地图的建立又离不开定位。事实上，机器人的自主定位和建图几乎是同时完成的，同时定位与建图（SLAM，simultaneouslocalization and mapping）技术作为机器人领域热点研究问题已有三十多年的历史。依据传感器不同可以分为视觉SLAM方法和激光SLAM方法。以摄像头为主要传感器的视觉SLAM方法对环境光照变化敏感并且在照明不亮的情况下无法工作；以激光雷达为主要传感器的激光SLAM方法，大多都是惯性测量单元（IMU，inertial measurement unit）与激光雷达松耦合方案，定位精度难以保证。无论是视觉SLAM还是激光SLAM都面临着随建图时间增加导致定位误差累积，地图漂移严重甚至建图失败问题。因此，发明一种新的准确可靠的机器人同时定位与建图方法非常具有现实意义。
现有技术实现方案如下：
1、ORB-SLAM方法，一种采用摄像头为主要传感器的基于ORB特征的机器人定位与地图构建方法，可以建立周围环境的稀疏点云地图描述；
2、Cartographer方法，一种融合激光雷达、IMU、轮式里程计的机器人定位与建图方法，可以输出2D栅格地图或3D栅格地图用于机器人导航；
3、LIO-SAM方法，一种采用IMU与3D激光雷达紧耦合的基于特征匹配的同时定位与建图方法，前端采用特征匹配的方法获得里程计，后端执行因子图优化以减少前端里程计的估计误差，同时建立环境的点云地图描述；
现有技术的缺点如下：
1、以摄像机为主要传感器的视觉SLAM方法，视场（FOV）小、受光照条件变化影响大，无法在夜间和照明不良等条件下作业；
2、输出2D栅格地图的SLAM方法，只保留了周围环境一个平面内的信息，环境中大部分有用的信息被忽略了，建立的2D栅格地图不能充分描述机器人所处的真实环境；
3、采用3D激光雷达为主要传感器的激光SLAM方案没有与IMU紧密耦合，不能有效抑制长时间建图导致的定位误差累积问题，而且在退化环境中的鲁棒性很差；
4、现有的SLAM方法没有充分利用地面信息，利用地面信息可以在不额外增加传感器的条件下构建地面约束，有效的抑制机器人位姿估计误差。
发明内容
针对上述现有的同时定位与建图方法面临的共同难题就是解决随着建图时间增加导致的定位误差累积问题。在机器人增量建图过程中，机器人上一时刻的定位误差会传递给当前时刻，这让机器人的位姿估计变得越来越困难，最终导致定位失败无法获得全局一致地图。本申请的实施例的目的在于提出了一种实时稳定的、精度高的激光雷达和惯性测量单元（IMU）紧耦合的机器人同时定位与建图方法、装置及可读介质，旨在为机器人实现自主导航提供超高精度的点云地图。
第一方面，本申请的实施例提供了一种机器人同时定位与建图方法，包括以下步骤：
S1，获取机器人周围环境的点云信息、GPS信号、三轴角速度和三轴加速度，通过三轴角速度和三轴加速度预积分得到IMU里程计；
S2，基于IMU里程计通过时间线性插值方法，将点云信息的一帧点云内的每一个激光点变换到初始时刻激光点坐标系，得到去除运动畸变的激光点云；
S3，对去除运动畸变的激光点云提取特征点，基于特征点确定局部地图，以IMU里程计作为当前激光帧的初始位姿，通过特征匹配得到当前激光帧与局部地图的相对变换关系，优化当前激光帧的位姿，得到激光里程计；
S4，从所有激光帧中提取关键帧，根据连续两帧关键帧的位姿变换关系构建激光里程计因子，根据两关键帧之间的预积分量所获得的位姿变换构建IMU预积分因子，根据两关键帧之间的GPS信号构建GPS因子，根据回环检测到的激光帧与历史关键帧之间的位姿关系构建回环因子，通过分割提取出每个关键帧对应的局部地图中的地面信息，采用激光里程计的帧间变换关系拼接相邻关键帧对应的局部地图中的地面信息得到全局地面信息，根据全局地面信息与其观测到的初始地面信息构建地面约束因子；利用IMU预积分因子、激光里程计因子、GPS因子、回环因子和地面约束因子构建因子图，优化调整关键帧位姿，并生成点云地图。
作为优选，步骤S1中通过三轴角速度和三轴加速度预积分得到IMU里程计，具体包括：
根据三轴角速度和三轴加速度分别计算i时刻到j时刻间隔时间Δt内旋转增量ΔRij和平移增量Δpij，如下式所示：
Figure 97849DEST_PATH_IMAGE001
；
Figure 482694DEST_PATH_IMAGE002
；
其中，pi、pj 为i时刻和j时刻的IMU平移增量，
Figure 390476DEST_PATH_IMAGE003
为将IMU平移增量转变到i时刻下的坐标系的旋转矩阵，vi为三轴加速度关于时间的积分，ωi为三轴角速度，g是重力加速度，
Figure 227982DEST_PATH_IMAGE004
为IMU变化噪声，
Figure 997355DEST_PATH_IMAGE005
为IMU白噪声，将旋转增量ΔRij和平移增量Δpij构成预积分量，根据预积分量获取不同时刻的机器人位姿变换矩阵，并构成IMU里程计。
作为优选，步骤S2具体包括：通过下式的变换关系得到去除运动畸变的点
Figure 751553DEST_PATH_IMAGE006
：
Figure 580969DEST_PATH_IMAGE007
；
假设当前激光帧的初始时刻的位姿变换矩阵为
Figure 169687DEST_PATH_IMAGE008
，点云信息中一帧点云内第k个激光点
Figure 742751DEST_PATH_IMAGE009
相对初始时刻的位姿变换矩阵为
Figure 836609DEST_PATH_IMAGE010
。
作为优选，特征点包括角点和平面点，步骤S3具体包括：
计算去除运动畸变的激光点云中每个激光点的曲率：
Figure 86193DEST_PATH_IMAGE011
；
其中，x为激光点的深度值，
Figure 632712DEST_PATH_IMAGE012
为该激光点前后邻近的第k个点的深度值；
根据每个激光点的曲率大小判断每个激光点的类型，若曲率大于阈值，则该激光点为角点，若曲率小于阈值，则该激光点为平面点；
根据当前激光帧维护的滑动窗口所包含的关键帧中的特征点构建局部地图：
Figure 727576DEST_PATH_IMAGE013
；
其中，
Figure 941520DEST_PATH_IMAGE014
表示关键帧
Figure 362006DEST_PATH_IMAGE015
对应的局部地图，
Figure 661400DEST_PATH_IMAGE016
表示滑动窗口所包含的关键帧中的角点集合，
Figure 576267DEST_PATH_IMAGE017
表示滑动窗口所包含的关键帧中的平面点集合；
通过当前激光帧与局部地图进行特征匹配，优化当前激光帧的初始位姿，得到更新后的激光里程计。
作为优选，通过当前激光帧与局部地图进行特征匹配，具体包括：
通过计算当前激光帧的特征点与局部地图
Figure 631335DEST_PATH_IMAGE014
对应的特征点之间的变换关系：
Figure 973454DEST_PATH_IMAGE018
；
Figure 9412DEST_PATH_IMAGE019
；
其中，
Figure 462390DEST_PATH_IMAGE020
为当前激光帧的特征点，pf、pg为局部地图
Figure 369035DEST_PATH_IMAGE014
中的角点，pv、pl、pm为局部地图
Figure 882056DEST_PATH_IMAGE014
中的平面点；
采用高斯-牛顿法优化求解当前激光帧与局部地图之间的角点变换关系
Figure 156043DEST_PATH_IMAGE021
、平面点变换关系
Figure 661979DEST_PATH_IMAGE022
，并构成当前激光帧与局部地图之间的相对变换关系。
作为优选，步骤S4具体包括：
采用随机抽样一致算法从每个关键帧
Figure 705022DEST_PATH_IMAGE015
对应的局部地图
Figure 388944DEST_PATH_IMAGE014
中提取到地面参数
Figure 402424DEST_PATH_IMAGE023
，其中，
Figure 197205DEST_PATH_IMAGE024
表示地平面的法向量，
Figure 344021DEST_PATH_IMAGE025
表示雷达坐标系原点到地平面的距离；
若连续两个关键帧对应的地面参数
Figure 198845DEST_PATH_IMAGE024
、
Figure 447423DEST_PATH_IMAGE025
的变化小于预设阈值，则两处关键帧对应的是同一地面，否则由连续两个关键帧之间的位姿变换关系
Figure 295162DEST_PATH_IMAGE026
和
Figure 312797DEST_PATH_IMAGE027
关联新平面：
Figure 338522DEST_PATH_IMAGE028
；
Figure 58085DEST_PATH_IMAGE029
；
其中，
Figure 460248DEST_PATH_IMAGE030
、
Figure 319007DEST_PATH_IMAGE031
表示关键帧Fa对应的地面参数HF(
Figure 515633DEST_PATH_IMAGE030
，
Figure 473224DEST_PATH_IMAGE031
)，其中，
Figure 662766DEST_PATH_IMAGE030
表示地平面的法向量，
Figure 389414DEST_PATH_IMAGE031
表示雷达坐标系原点到地平面的距离；
Figure 271788DEST_PATH_IMAGE032
为
Figure 716676DEST_PATH_IMAGE033
对应的坐标系到
Figure 195062DEST_PATH_IMAGE034
对应的坐标系的平移向量，
Figure 291063DEST_PATH_IMAGE026
、
Figure 829491DEST_PATH_IMAGE027
为关键帧
Figure 761675DEST_PATH_IMAGE035
、
Figure 284230DEST_PATH_IMAGE036
之间的位姿变换关系，利用两帧关键帧之间的位姿变换关系将不同关键帧得到的估计的地面参数关联起来，得到全局地面信息。
作为优选，步骤S4具体包括：
根据连续两帧关键帧位姿变换关系T构建激光里程计因子，具体如下：
Figure 985470DEST_PATH_IMAGE037
；
其中，R为新关键帧的旋转变化量，t为平移变化量；
根据两关键帧之间的预积分量所获得的位姿变换T作为IMU预积分因子；
当接收到GPS信号时将其转换至关键帧坐标系，将两关键帧之间的GPS信号差值作为GPS因子；
当机器人回到历史途经位置附近时检测到回环时，将当前激光帧与该关键帧之间的位姿关系T作为回环因子；
将全局地面信息中每个关键帧对应的地面参数以及观测到的初始地面参数两者之间的差值作为地面约束因子；
通过调整关键帧位姿u，使得F（u）取极小值，然后降采样保存所有关键帧的点云信息生成点云地图；
Figure 209647DEST_PATH_IMAGE038
；
其中，F（u）为各因子残差项之和，
Figure 97969DEST_PATH_IMAGE039
、
Figure 183736DEST_PATH_IMAGE040
为机器人位姿，
Figure 254329DEST_PATH_IMAGE041
和
Figure 134561DEST_PATH_IMAGE042
分别为机器人位姿
Figure 25025DEST_PATH_IMAGE039
、
Figure 914484DEST_PATH_IMAGE040
之间一个约束的均值和信息矩阵，C为存在约束关系的所有机器人状态集合。
第二方面，本申请的实施例提供了一种机器人同时定位与建图装置，包括：
IMU里程计计算模块，被配置为获取机器人周围环境的点云信息、GPS信号、三轴角速度和三轴加速度，通过三轴角速度和三轴加速度预积分得到IMU里程计；
运动畸变去除模块，被配置为基于IMU里程计通过时间线性插值方法，将点云信息的一帧点云内的每一个激光点变换到初始时刻激光点坐标系，得到去除运动畸变的激光点云；
激光里程计计算模块，被配置为对去除运动畸变的激光点云提取特征点，基于特征点确定局部地图，以IMU里程计作为当前激光帧的初始位姿，通过特征匹配得到当前激光帧与局部地图的相对变换关系，优化当前激光帧的位姿，得到激光里程计；
因子图优化模块，被配置为从所有激光帧中提取关键帧，根据连续两帧关键帧的位姿变换关系构建激光里程计因子，根据两关键帧之间的预积分量所获得的位姿变换构建IMU预积分因子，根据两关键帧之间的GPS信号构建GPS因子，根据回环检测到的激光帧与历史关键帧之间的位姿关系构建回环因子，通过分割提取出每个关键帧对应的局部地图中的地面信息，采用激光里程计的帧间变换关系拼接相邻关键帧对应的局部地图中的地面信息得到全局地面信息，根据全局地面信息与其观测到的初始地面信息构建地面约束因子；利用IMU预积分因子、激光里程计因子、GPS因子、回环因子和地面约束因子构建因子图，优化调整关键帧位姿，并生成点云地图。
第三方面，本申请的实施例提供了一种电子设备，包括一个或多个处理器；存储装置，用于存储一个或多个程序，当一个或多个程序被一个或多个处理器执行，使得一个或多个处理器实现如第一方面中任一实现方式描述的方法。
第四方面，本申请的实施例提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如第一方面中任一实现方式描述的方法。
相比于现有技术，本发明具有以下有益效果：
（1）本发明通过激光雷达和惯性测量单元紧耦合作用，可以短时间内获得高精度的定位，很大程度上减少了定位误差，限制了里程计漂移。
（2）本发明融合了GPS绝对定位，能在大尺度规模环境中依靠GPS提供的绝对位姿修正里程计误差，提高了定位精度和建图效果。
（3）本发明通过添加地面位姿约束，限制机器人相对全局地面的位姿，提高了机器人位姿估计精度，减少了定位误差，从而保证点云地图的精度。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本申请的一个实施例可以应用于其中的示例性装置架构图；
图2为本发明的实施例的机器人同时定位与建图方法的流程示意图；
图3为本发明的实施例的机器人同时定位与建图方法的整体流程示意图；
图4为本发明的实施例的机器人同时定位与建图方法的因子图优化的示意图；
图5为本发明的实施例的机器人同时定位与建图装置的示意图；
图6是适于用来实现本申请实施例的电子设备的计算机装置的结构示意图。
具体实施方式
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
图1示出了可以应用本申请实施例的机器人同时定位与建图方法或机器人同时定位与建图装置的示例性装置架构100。
如图1所示，装置架构100可以包括终端设备101、102、103，网络104和服务器105。网络104用以在终端设备101、102、103和服务器105之间提供通信链路的介质。网络104可以包括各种连接类型，例如有线、无线通信链路或者光纤电缆等等。
用户可以使用终端设备101、102、103通过网络104与服务器105交互，以接收或发送消息等。终端设备101、102、103上可以安装有各种应用，例如数据处理类应用、文件处理类应用等。
终端设备101、102、103可以是硬件，也可以是软件。当终端设备101、102、103为硬件时，可以是各种电子设备，包括但不限于智能手机、平板电脑、膝上型便携计算机和台式计算机等等。当终端设备101、102、103为软件时，可以安装在上述所列举的电子设备中。其可以实现成多个软件或软件模块（例如用来提供分布式服务的软件或软件模块），也可以实现成单个软件或软件模块。在此不做具体限定。
服务器105可以是提供各种服务的服务器，例如对终端设备101、102、103上传的文件或数据进行处理的后台数据处理服务器。后台数据处理服务器可以对获取的文件或数据进行处理，生成处理结果。
需要说明的是，本申请实施例所提供的机器人同时定位与建图方法可以由服务器105执行，也可以由终端设备101、102、103执行，相应地，机器人同时定位与建图装置可以设置于服务器105中，也可以设置于终端设备101、102、103中。
应该理解，图1中的终端设备、网络和服务器的数目仅仅是示意性的。根据实现需要，可以具有任意数目的终端设备、网络和服务器。在所处理的数据不需要从远程获取的情况下，上述装置架构可以不包括网络，而只需服务器或终端设备。
图2示出了本申请的实施例提供的一种机器人同时定位与建图方法，包括以下步骤：
S1，获取机器人周围环境的点云信息、GPS信号、三轴角速度和三轴加速度，通过三轴角速度和三轴加速度预积分得到IMU里程计。
在具体的实施例中，参考图3，步骤S1中基于三轴角速度和三轴加速度得到IMU里程计中的位姿变换矩阵，具体包括：
步骤S1中通过三轴角速度和三轴加速度预积分得到IMU里程计，具体包括：
根据三轴角速度和三轴加速度分别计算i时刻到j时刻间隔时间Δt内旋转增量ΔRij和平移增量Δpij，如下式所示：
Figure 590316DEST_PATH_IMAGE043
（1）
Figure 159225DEST_PATH_IMAGE002
（2）
其中，pi、pj 为i时刻和j时刻的IMU平移增量，
Figure 287718DEST_PATH_IMAGE003
为将IMU平移增量转变到i时刻下的坐标系的旋转矩阵，vi为三轴加速度关于时间的积分，ωi为三轴角速度，g是重力加速度，
Figure 715288DEST_PATH_IMAGE004
为IMU变化噪声，
Figure 494894DEST_PATH_IMAGE005
为IMU白噪声，将旋转增量ΔRij和平移增量Δpij构成预积分量，根据预积分量获取不同时刻的机器人位姿变换矩阵，并构成IMU里程计。
具体的，由移动机器人平台，搭载激光雷达、IMU、GPS信号接收装置遍历周围环境建立点云地图。首先，由激光雷达获取周围环境信息的点云信息，该激光点云信息可以由不同线束的旋转式激光雷达提供，如常见的16线激光雷达、32线激光雷达等。IMU提供三轴角速度和三轴加速度，该三轴角速度和三轴加速度用于计算机器人位姿变化和去除激光点云运动畸变。GPS信号由GPS信号接收装置提供。假设在时间Δt内三轴角速度和三轴加速度不变，平移增量由公式（1）给出，旋转增量由公式（2）给出。IMU里程计由平移增量和旋转增量组成，用于为后续的运动畸变去除提供激光雷达获取一帧点云时间内的旋转增量和平移增量，同时由上一时刻激光帧到当前时刻激光帧间隔时间内的IMU里程计作为当前激光帧的初始位姿。里程计是一段时间内机器人位姿变换的描述，激光里程计由激光匹配（当前帧与局部地图匹配）得到的当前帧位姿变换矩阵组成，激光里程计=T0*T1*T2*T3*……*Tn。其中，Tn是n时刻的位姿变换。IMU里程计是由IMU数据积分得到的不同时刻的机器人位姿变换描述集合。
S2，基于IMU里程计通过时间线性插值方法，将点云信息的一帧点云内的每一个激光点变换到初始时刻激光点坐标系，得到去除运动畸变的激光点云。
在具体的实施例中，点云信息是激光雷达扫描周围环境的点获取的，激光雷达获取一帧点云时间内机器人自身的运动会导致激光雷达获取的点云发生运动畸变，产生运动畸变的点云是对环境的失真描述，对定位精度和点云地图构建都有极大影响。利用高频率的IMU数据计算位姿变换用于去除激光产生的运动畸变，并为激光里程计提供初始值。因此采用IMU提供的三轴角速度和三轴加速度来计算IMU里程计，IMU里程计可用于去畸变和做为激光里程计的初始值，实现紧耦合。
步骤S2中根据激光雷达当前帧起止时刻之间的IMU里程计中的旋转增量和平移增量利用时间线性插值方法将一帧点云内的每一激光点变换到初始时刻激光点坐标系下，实现点云运动畸变的校正。利用公式（3）变换关系得到去除运动畸变的点
Figure 716928DEST_PATH_IMAGE006
，将每个激光点去除畸变，即得到去除运动畸变的一帧激光点云，其中，位姿变换矩阵T由IMU里程计提供。
具体的，通过下式的变换关系得到去除运动畸变的点
Figure 332717DEST_PATH_IMAGE006
：
Figure 78825DEST_PATH_IMAGE007
（3）
假设当前激光帧的初始时刻的位姿变换矩阵为
Figure 463670DEST_PATH_IMAGE008
，点云信息中一帧点云内第k个激光点
Figure 856605DEST_PATH_IMAGE009
相对初始时刻的位姿变换矩阵为
Figure 208958DEST_PATH_IMAGE010
。
S3，对去除运动畸变的激光点云提取特征点，基于特征点确定局部地图，以IMU里程计作为当前激光帧的初始位姿，通过特征匹配得到当前激光帧与局部地图的相对变换关系，优化当前激光帧的位姿，得到激光里程计。
在具体的实施例中，对经过运动畸变去除之后的激光点云提取特征点，特征点包括角点和平面点，根据曲率计算公式（4）计算每个点的曲率，提取的特征点用于执行特征匹配计算两个激光帧相对位姿变换，得到激光里程计。通过计算当前时刻激光帧的点、面特征与上一段时间内激光帧特征点构成的子图所对应的特征变换关系得到激光里程计，即包含误差待优化的机器人定位。
具体的，步骤S3具体包括：
计算去除运动畸变的激光点云中每个激光点的曲率：
Figure 978331DEST_PATH_IMAGE011
（4）
其中，x为激光点的深度值，
Figure 735459DEST_PATH_IMAGE012
为该激光点前后邻近的第k个点的深度值；
根据每个激光点的曲率大小判断每个激光点的类型，若曲率大于阈值，则该激光点为角点，若曲率小于阈值，则该激光点为平面点；
根据当前激光帧维护的滑动窗口所包含的关键帧中的特征点构建局部地图：
Figure 564875DEST_PATH_IMAGE013
（5）
局部地图由当前激光帧维护的滑动窗口包含的固定数目关键帧构成，局部地图中包含了当前激光帧之前的若干关键帧的特征点集合，如公式（5）所示，其中，
Figure 155256DEST_PATH_IMAGE014
表示关键帧
Figure 977587DEST_PATH_IMAGE015
对应的局部地图，
Figure 71445DEST_PATH_IMAGE016
表示滑动窗口所包含的关键帧中的角点集合，
Figure 71762DEST_PATH_IMAGE017
表示滑动窗口所包含的关键帧中的平面点集合。
通过当前激光帧与局部地图进行特征匹配，优化当前激光帧的初始位姿，得到当前激光帧的位姿，进一步得到更新后的激光里程计。
在具体的实施例中，通过当前激光帧与局部地图进行特征匹配，具体包括：
基于IMU里程计获取当前激光帧的初始位姿；
通过计算当前激光帧的特征点与局部地图
Figure 398707DEST_PATH_IMAGE014
对应的特征点之间的变换关系：
Figure 775462DEST_PATH_IMAGE018
（6）
Figure 989406DEST_PATH_IMAGE044
（7）
其中，
Figure 144313DEST_PATH_IMAGE020
为当前激光帧的特征点，pf、pg为局部地图
Figure 443707DEST_PATH_IMAGE014
中的角点，pv、pl、pm为局部地图
Figure 345191DEST_PATH_IMAGE014
中的平面点；
采用高斯-牛顿法优化求解当前激光帧与局部地图之间的角点变换关系
Figure 413641DEST_PATH_IMAGE021
、平面点变换关系
Figure 21340DEST_PATH_IMAGE022
，并构成当前激光帧与局部地图之间的相对变换关系。
通过当前激光帧与局部地图之间的相对变换关系，优化由IMU里程计提供的当前激光帧的初始位姿，然后更新激光里程计。
S4，从所有激光帧中提取关键帧，根据连续两帧关键帧的位姿变换关系构建激光里程计因子，根据两关键帧之间的预积分量所获得的位姿变换构建IMU预积分因子，根据两关键帧之间的GPS信号构建GPS因子，根据回环检测到的激光帧与历史关键帧之间的位姿关系构建回环因子，通过分割提取出每个关键帧对应的局部地图中的地面信息，采用激光里程计的帧间变换关系拼接相邻关键帧对应的局部地图中的地面信息得到全局地面信息，根据全局地面信息与其观测到的初始地面信息构建地面约束因子；利用IMU预积分因子、激光里程计因子、GPS因子、回环因子和地面约束因子构建因子图，优化调整关键帧位姿，并生成点云地图。
具体的，如果每一个激光帧的位姿都进行优化不仅会造成计算资源的浪费，而且实时性难以保证，因此选取部分激光帧作为关键帧，进行关键帧位姿优化。关键帧的选取方式如下：当机器人移动一定的距离或者旋转一定的角度就定义一个包含足够特征的激光帧为关键帧。通过因子图优化调整关键帧位姿，减少长定位误差累积，获取全局一致定位，最终通过降采样保留所有关键帧的特征点云信息，即为目标点云地图。
在具体的实施例中，参考图4，x0~xn+1中的x表示机器人关键帧位姿，F0~Fn+1中的F表示关键帧，步骤S4具体包括：
采用随机抽样一致算法从每个关键帧
Figure 57298DEST_PATH_IMAGE015
对应的局部地图
Figure 510276DEST_PATH_IMAGE014
中提取到地面参数
Figure 698812DEST_PATH_IMAGE023
，其中，
Figure 461101DEST_PATH_IMAGE024
表示地平面的法向量，
Figure 735087DEST_PATH_IMAGE025
表示雷达坐标系原点到地平面的距离；
若连续两个关键帧对应的地面参数
Figure 257335DEST_PATH_IMAGE024
、
Figure 549645DEST_PATH_IMAGE025
的变化小于预设阈值，则两处关键帧对应的是同一地面，否则由连续两个关键帧之间的位姿变换关系
Figure 233568DEST_PATH_IMAGE026
和
Figure 993187DEST_PATH_IMAGE027
关联新平面：
Figure 53547DEST_PATH_IMAGE028
（8）
Figure 951096DEST_PATH_IMAGE029
（9）
其中，
Figure 320766DEST_PATH_IMAGE030
、
Figure 569345DEST_PATH_IMAGE031
表示关键帧Fa对应的地面参数HF(
Figure 167816DEST_PATH_IMAGE030
，
Figure 169139DEST_PATH_IMAGE031
)，其中，
Figure 460443DEST_PATH_IMAGE030
表示地平面的法向量，
Figure 930739DEST_PATH_IMAGE031
表示雷达坐标系原点到地平面的距离；
Figure 582169DEST_PATH_IMAGE032
为
Figure 454310DEST_PATH_IMAGE033
对应的坐标系到
Figure 916515DEST_PATH_IMAGE034
对应的坐标系的平移向量，
Figure 126304DEST_PATH_IMAGE026
、
Figure 66579DEST_PATH_IMAGE027
为关键帧
Figure 42494DEST_PATH_IMAGE035
、
Figure 675600DEST_PATH_IMAGE036
之间的位姿变换关系，利用两帧关键帧之间的位姿变换关系将不同关键帧得到的估计的地面参数关联起来，得到全局地面信息，用于构建地面约束因子。
在具体的实施例中，因子图由机器人位姿和因子构成，关键帧位姿受五种因子共同约束，分别为激光里程计因子、IMU预积分因子、GPS因子、回环因子和地面约束因子，通过GTSAM因子图优化求解器，更新关键帧位姿，得到最佳机器人状态分布，完成定位。
根据连续两帧关键帧位姿变换关系T构建激光里程计因子：
Figure 854909DEST_PATH_IMAGE037
（10）
其中，R为新关键帧的旋转变化量，t为平移变化量，具体的，激光里程计因子由连续两帧关键帧位姿变换关系T构建，当产生一帧新关键帧时，该帧与上一时刻关键帧之间存在位姿变换关系T；
根据两关键帧之间的预积分量所获得的位姿变换T作为IMU预积分因子；
GPS因子由GPS信号获得，当接收到GPS信号时将其转换至关键帧坐标系，将两关键帧之间的GPS信号差值作为GPS因子；
当机器人回到历史途经位置附近时检测到回环，将当前激光帧与该关键帧之间的位姿关系T作为回环因子；
将全局地面信息中每个关键帧对应的地面参数以及观测到的初始地面参数两者之间的差值作为地面约束因子；
通过调整关键帧位姿x，使得F（x）取极小值，然后降采样保存所有关键帧的点云信息生成点云地图；
Figure 848142DEST_PATH_IMAGE038
（11）
其中，F（u）为各因子残差项之和，
Figure 960454DEST_PATH_IMAGE039
、
Figure 233304DEST_PATH_IMAGE040
为机器人位姿，
Figure 414755DEST_PATH_IMAGE041
和
Figure 696832DEST_PATH_IMAGE042
分别为机器人位姿
Figure 384690DEST_PATH_IMAGE039
、
Figure 359599DEST_PATH_IMAGE040
之间一个约束的均值和信息矩阵，C为存在约束关系的所有机器人状态集合。最终得到的点云地图为机器人在建图过程中遍历的周围环境点云描述。
进一步参考图5，作为对上述各图所示方法的实现，本申请提供了一种机器人同时定位与建图装置的一个实施例，该装置实施例与图2所示的方法实施例相对应，该装置具体可以应用于各种电子设备中。
本申请实施例提供了一种机器人同时定位与建图装置，包括：
IMU里程计计算模块1，被配置为获取机器人周围环境的点云信息、GPS信号、三轴角速度和三轴加速度，通过三轴角速度和三轴加速度预积分得到IMU里程计；
运动畸变去除模块2，被配置为基于IMU里程计通过时间线性插值方法，将点云信息的一帧点云内的每一个激光点变换到初始时刻激光点坐标系，得到去除运动畸变的激光点云；
激光里程计计算模块3，被配置为对去除运动畸变的激光点云提取特征点，基于特征点确定局部地图，以IMU里程计作为当前激光帧的初始位姿，通过特征匹配得到当前激光帧与局部地图的相对变换关系，优化当前激光帧的位姿，得到激光里程计；
因子图优化模块4，被配置为从所有激光帧中提取关键帧，根据连续两帧关键帧的位姿变换关系构建激光里程计因子，根据两关键帧之间的预积分量所获得的位姿变换构建IMU预积分因子，根据两关键帧之间的GPS信号构建GPS因子，根据回环检测到的激光帧与历史关键帧之间的位姿关系构建回环因子，通过分割提取出每个关键帧对应的局部地图中的地面信息，采用激光里程计的帧间变换关系拼接相邻关键帧对应的局部地图中的地面信息得到全局地面信息，根据全局地面信息与其观测到的初始地面信息构建地面约束因子；利用IMU预积分因子、激光里程计因子、GPS因子、回环因子和地面约束因子构建因子图，优化调整关键帧位姿，并生成点云地图。
下面参考图6，其示出了适于用来实现本申请实施例的电子设备（例如图1所示的服务器或终端设备）的计算机装置600的结构示意图。图6示出的电子设备仅仅是一个示例，不应对本申请实施例的功能和使用范围带来任何限制。
如图6所示，计算机装置600包括中央处理单元（CPU）601和图形处理器（GPU）602，其可以根据存储在只读存储器（ROM）603中的程序或者从存储部分609加载到随机访问存储器（RAM）604中的程序而执行各种适当的动作和处理。在RAM 604中，还存储有装置600操作所需的各种程序和数据。CPU 601、GPU602、ROM 603以及RAM 604通过总线605彼此相连。输入/输出（I/O）接口606也连接至总线605。
以下部件连接至I/O接口606：包括键盘、鼠标等的输入部分607；包括诸如、液晶显示器（LCD）等以及扬声器等的输出部分608；包括硬盘等的存储部分609；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分610。通信部分610经由诸如因特网的网络执行通信处理。驱动器611也可以根据需要连接至I/O接口606。可拆卸介质612，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器611上，以便于从其上读出的计算机程序根据需要被安装入存储部分609。
特别地，根据本公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分610从网络上被下载和安装，和/或从可拆卸介质612被安装。在该计算机程序被中央处理单元（CPU）601和图形处理器（GPU）602执行时，执行本申请的方法中限定的上述功能。
需要说明的是，本申请所述的计算机可读介质可以是计算机可读信号介质或者计算机可读介质或者是上述两者的任意组合。计算机可读介质例如可以是——但不限于——电、磁、光、电磁、红外线或半导体的装置、装置或器件，或者任意以上的组合。计算机可读介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器（RAM）、只读存储器（ROM）、可擦式可编程只读存储器（EPROM或闪存）、光纤、便携式紧凑磁盘只读存储器（CD-ROM）、光存储器件、磁存储器件或者上述的任意合适的组合。在本申请中，计算机可读介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行装置、装置或者器件使用或者与其结合使用。而在本申请中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行装置、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。
可以以一种或多种程序设计语言或其组合来编写用于执行本申请的操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，也可以连接到外部计算机（例如利用因特网服务提供商来通过因特网连接）。
附图中的流程图和框图，图示了按照本申请各种实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，该模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的装置来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本申请实施例中所涉及到的模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的模块也可以设置在处理器中。
作为另一方面，本申请还提供了一种计算机可读介质，该计算机可读介质可以是上述实施例中描述的电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被该电子设备执行时，使得该电子设备：获取机器人周围环境的点云信息、GPS信号、三轴角速度和三轴加速度，通过三轴角速度和三轴加速度预积分得到IMU里程计；基于IMU里程计通过时间线性插值方法，将点云信息的一帧点云内的每一个激光点变换到初始时刻激光点坐标系，得到去除运动畸变的激光点云；对去除运动畸变的激光点云提取特征点，基于特征点确定局部地图，以IMU里程计作为当前激光帧的初始位姿，通过特征匹配得到当前激光帧与局部地图的相对变换关系，优化当前激光帧的位姿，得到激光里程计；从所有激光帧中提取关键帧，根据连续两帧关键帧的位姿变换关系构建激光里程计因子，根据两关键帧之间的预积分量所获得的位姿变换构建IMU预积分因子，根据两关键帧之间的GPS信号构建GPS因子，根据回环检测到的激光帧与历史关键帧之间的位姿关系构建回环因子，通过分割提取出每个关键帧对应的局部地图中的地面信息，采用激光里程计的帧间变换关系拼接相邻关键帧对应的局部地图中的地面信息得到全局地面信息，根据全局地面信息与其观测到的初始地面信息构建地面约束因子；利用IMU预积分因子、激光里程计因子、GPS因子、回环因子和地面约束因子构建因子图，优化调整关键帧位姿，并生成点云地图。
以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本申请中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离上述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的（但不限于）具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (9)

1.一种机器人同时定位与建图方法，其特征在于，包括以下步骤：
S1，获取机器人周围环境的点云信息、GPS信号、三轴角速度和三轴加速度，通过所述三轴角速度和三轴加速度预积分得到IMU里程计；
S2，基于所述IMU里程计通过时间线性插值方法，将所述点云信息的一帧点云内的每一个激光点变换到初始时刻激光点坐标系，得到去除运动畸变的激光点云；
S3，对所述去除运动畸变的激光点云提取特征点，基于所述特征点确定局部地图，以所述IMU里程计作为当前激光帧的初始位姿，通过特征匹配得到当前激光帧与局部地图的相对变换关系，优化当前激光帧的位姿，得到激光里程计；
S4，从所有激光帧中提取关键帧，根据连续两帧关键帧的位姿变换关系构建激光里程计因子，根据两关键帧之间的预积分量所获得的位姿变换构建IMU预积分因子，根据两关键帧之间的GPS信号构建GPS因子，根据回环检测到的激光帧与历史关键帧之间的位姿关系构建回环因子，通过分割提取出每个关键帧对应的局部地图中的地面信息，采用激光里程计的帧间变换关系拼接相邻关键帧对应的局部地图中的地面信息得到全局地面信息，具体包括：
采用随机抽样一致算法从每个关键帧
Figure DEST_PATH_IMAGE001
对应的局部地图
Figure 206716DEST_PATH_IMAGE002
中提取地面参数
Figure DEST_PATH_IMAGE003
，其中，
Figure 683834DEST_PATH_IMAGE004
表示地平面的法向量，
Figure DEST_PATH_IMAGE005
表示雷达坐标系原点到地平面的距离；
若连续两个关键帧对应的地面参数
Figure 753945DEST_PATH_IMAGE004
、
Figure 723038DEST_PATH_IMAGE005
的变化小于预设阈值，则两处关键帧对应的是同一地面，否则由连续两个关键帧之间的位姿变换关系
Figure 2710DEST_PATH_IMAGE006
和
Figure DEST_PATH_IMAGE007
关联新平面：
Figure 599914DEST_PATH_IMAGE008
；
Figure DEST_PATH_IMAGE009
；
其中，
Figure 837997DEST_PATH_IMAGE010
、
Figure DEST_PATH_IMAGE011
表示关键帧Fa对应的地面参数HF(
Figure 559965DEST_PATH_IMAGE010
，
Figure 646257DEST_PATH_IMAGE011
)，其中，
Figure 238913DEST_PATH_IMAGE010
表示地平面的法向量，
Figure 788843DEST_PATH_IMAGE011
表示雷达坐标系原点到地平面的距离；
Figure 60424DEST_PATH_IMAGE012
为关键帧Fa的第r个点，
Figure DEST_PATH_IMAGE013
为关键帧Fb的第r个点，
Figure 213057DEST_PATH_IMAGE007
为关键帧Fb到关键帧Fa的平移向量，
Figure 191377DEST_PATH_IMAGE006
为关键帧Fb到关键帧Fa的旋转矩阵，利用两帧关键帧之间的位姿变换关系将不同关键帧得到的估计的地面参数关联起来，得到全局地面信息；
根据所述全局地面信息与其观测到的初始地面信息构建地面约束因子；利用所述IMU预积分因子、激光里程计因子、GPS因子、回环因子和地面约束因子构建因子图，优化调整关键帧位姿，并生成点云地图。
2.根据权利要求1所述的机器人同时定位与建图方法，其特征在于，所述步骤S1中通过所述三轴角速度和三轴加速度预积分得到IMU里程计，具体包括：
根据所述三轴角速度和三轴加速度分别计算i时刻到j时刻间隔时间Δt内旋转增量ΔRij和平移增量Δpij，如下式所示：
Figure 912208DEST_PATH_IMAGE014
；
Figure DEST_PATH_IMAGE015
；
其中，pi、pj 为i时刻和j时刻的IMU平移增量，
Figure 939595DEST_PATH_IMAGE016
为将IMU平移增量转变到i时刻下的坐标系的旋转矩阵，
Figure 364760DEST_PATH_IMAGE018
为i时刻和j时刻的时间间隔，vi为三轴线速度，由三轴加速度积分得到，
Figure 932007DEST_PATH_IMAGE020
为三轴角速度，g是重力加速度，
Figure DEST_PATH_IMAGE021
为IMU变化噪声，
Figure 682795DEST_PATH_IMAGE022
为IMU白噪声，将所述旋转增量ΔRij和平移增量Δpij构成预积分量，根据所述预积分量获取不同时刻的机器人位姿变换矩阵，并构成所述IMU里程计。
3.根据权利要求2所述的机器人同时定位与建图方法，其特征在于，所述步骤S2具体包括：通过下式的变换关系得到去除运动畸变的点
Figure DEST_PATH_IMAGE023
：
Figure 194547DEST_PATH_IMAGE024
；
假设当前激光帧的初始时刻的位姿变换矩阵为
Figure DEST_PATH_IMAGE025
，所述点云信息中一帧点云内第k个激光点
Figure 703631DEST_PATH_IMAGE026
相对初始时刻的位姿变换矩阵为
Figure DEST_PATH_IMAGE027
。
4.根据权利要求3所述的机器人同时定位与建图方法，其特征在于，所述特征点包括角点和平面点，所述步骤S3具体包括：
计算所述去除运动畸变的激光点云中每个激光点的曲率：
Figure 718861DEST_PATH_IMAGE028
；
其中，x为激光点的深度值，
Figure DEST_PATH_IMAGE029
为该激光点前后邻近的第k个点的深度值；
根据每个激光点的曲率大小判断每个激光点的类型，若曲率大于阈值，则该激光点为角点，若曲率小于阈值，则该激光点为平面点；
根据当前激光帧维护的滑动窗口所包含的关键帧中的特征点构建局部地图：
Figure 843811DEST_PATH_IMAGE030
；
其中，
Figure DEST_PATH_IMAGE031
表示关键帧
Figure 577281DEST_PATH_IMAGE032
对应的局部地图，
Figure DEST_PATH_IMAGE033
表示滑动窗口所包含的关键帧中的角点集合，
Figure 878337DEST_PATH_IMAGE034
表示滑动窗口所包含的关键帧中的平面点集合；
通过当前激光帧与局部地图进行特征匹配，优化当前激光帧的初始位姿，得到更新后的激光里程计。
5.根据权利要求4所述的机器人同时定位与建图方法，其特征在于，所述通过当前激光帧与局部地图进行特征匹配，具体包括：
通过计算当前激光帧的特征点与局部地图
Figure 748073DEST_PATH_IMAGE031
对应的特征点之间的变换关系：
Figure DEST_PATH_IMAGE035
；
Figure 512766DEST_PATH_IMAGE036
；
其中，
Figure DEST_PATH_IMAGE037
为当前激光帧的特征点，pf、pg为局部地图
Figure 999111DEST_PATH_IMAGE031
中的角点，pv、pl、pm为局部地图
Figure 304191DEST_PATH_IMAGE031
中的平面点；
采用高斯-牛顿法优化求解当前激光帧与局部地图之间的角点变换关系
Figure 700537DEST_PATH_IMAGE038
、平面点变换关系
Figure DEST_PATH_IMAGE039
，并构成当前激光帧与局部地图之间的相对变换关系。
6.根据权利要求1所述的机器人同时定位与建图方法，其特征在于，所述步骤S4具体包括：
根据连续两帧关键帧位姿变换关系T构建激光里程计因子，具体如下：
Figure 966958DEST_PATH_IMAGE040
；
其中，R为新关键帧的旋转变化量，t为平移变化量；
根据两关键帧之间的预积分量所获得的位姿变换T作为IMU预积分因子；
当接收到GPS信号时将其转换至关键帧坐标系，将两关键帧之间的GPS信号差值作为GPS因子；
当机器人回到历史途经位置附近时检测到回环时，将当前激光帧与该关键帧之间的位姿关系T作为回环因子；
将所述全局地面信息中每个关键帧对应的地面参数以及观测到的初始地面参数两者之间的差值作为地面约束因子；
通过调整关键帧位姿u，使得F（u）取极小值，然后降采样保存所有关键帧的点云信息生成点云地图；
Figure DEST_PATH_IMAGE041
；
其中，F（u）为各因子残差项之和，
Figure 347124DEST_PATH_IMAGE042
、
Figure DEST_PATH_IMAGE043
为机器人位姿，
Figure 721473DEST_PATH_IMAGE044
和
Figure DEST_PATH_IMAGE045
分别为机器人位姿
Figure 831381DEST_PATH_IMAGE042
、
Figure 672298DEST_PATH_IMAGE043
之间一个约束的均值和信息矩阵，C为存在约束关系的所有机器人状态集合。
7.一种机器人同时定位与建图装置，其特征在于，包括：
IMU里程计计算模块，被配置为获取机器人周围环境的点云信息、GPS信号、三轴角速度和三轴加速度，通过所述三轴角速度和三轴加速度预积分得到IMU里程计；
运动畸变去除模块，被配置为基于所述IMU里程计通过时间线性插值方法，将所述点云信息的一帧点云内的每一个激光点变换到初始时刻激光点坐标系，得到去除运动畸变的激光点云；
激光里程计计算模块，被配置为对所述去除运动畸变的激光点云提取特征点，基于所述特征点确定局部地图，以所述IMU里程计作为当前激光帧的初始位姿，通过特征匹配得到当前激光帧与局部地图的相对变换关系，优化当前激光帧的位姿，得到激光里程计；
因子图优化模块，被配置为从所有激光帧中提取关键帧，根据连续两帧关键帧的位姿变换关系构建激光里程计因子，根据两关键帧之间的预积分量所获得的位姿变换构建IMU预积分因子，根据两关键帧之间的GPS信号构建GPS因子，根据回环检测到的激光帧与历史关键帧之间的位姿关系构建回环因子，通过分割提取出每个关键帧对应的局部地图中的地面信息，采用激光里程计的帧间变换关系拼接相邻关键帧对应的局部地图中的地面信息得到全局地面信息，具体包括：
采用随机抽样一致算法从每个关键帧
Figure 70918DEST_PATH_IMAGE032
对应的局部地图
Figure 923992DEST_PATH_IMAGE031
中提取地面参数
Figure 294930DEST_PATH_IMAGE046
，其中，
Figure DEST_PATH_IMAGE047
表示地平面的法向量，
Figure 759279DEST_PATH_IMAGE048
表示雷达坐标系原点到地平面的距离；
若连续两个关键帧对应的地面参数
Figure 317299DEST_PATH_IMAGE047
、
Figure 33451DEST_PATH_IMAGE048
的变化小于预设阈值，则两处关键帧对应的是同一地面，否则由连续两个关键帧之间的位姿变换关系
Figure DEST_PATH_IMAGE049
和
Figure 843582DEST_PATH_IMAGE050
关联新平面：
Figure DEST_PATH_IMAGE051
；
Figure 823040DEST_PATH_IMAGE052
；
其中，
Figure DEST_PATH_IMAGE053
、
Figure 992990DEST_PATH_IMAGE054
表示关键帧Fa对应的地面参数HF(
Figure 247254DEST_PATH_IMAGE053
，
Figure 61626DEST_PATH_IMAGE054
)，其中，
Figure 8722DEST_PATH_IMAGE053
表示地平面的法向量，
Figure 809844DEST_PATH_IMAGE054
表示雷达坐标系原点到地平面的距离；
Figure DEST_PATH_IMAGE055
为关键帧Fa的第r个点，
Figure 133378DEST_PATH_IMAGE056
为关键帧Fb的第r个点，
Figure 333415DEST_PATH_IMAGE050
为关键帧Fb到关键帧Fa的平移向量，
Figure 920254DEST_PATH_IMAGE049
为关键帧Fb到关键帧Fa的旋转矩阵，利用两帧关键帧之间的位姿变换关系将不同关键帧得到的估计的地面参数关联起来，得到全局地面信息；
根据所述全局地面信息与其观测到的初始地面信息构建地面约束因子；利用所述IMU预积分因子、激光里程计因子、GPS因子、回环因子和地面约束因子构建因子图，优化调整关键帧位姿，并生成点云地图。
8.一种电子设备，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-6中任一所述的方法。
9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-6中任一所述的方法。
CN202211178092.3A 2022-09-27 2022-09-27 机器人同时定位与建图方法、装置及可读介质 Active CN115265523B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211178092.3A CN115265523B (zh) 2022-09-27 2022-09-27 机器人同时定位与建图方法、装置及可读介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211178092.3A CN115265523B (zh) 2022-09-27 2022-09-27 机器人同时定位与建图方法、装置及可读介质

Publications (2)

Publication Number Publication Date
CN115265523A CN115265523A (zh) 2022-11-01
CN115265523B true CN115265523B (zh) 2023-01-03

Family

ID=83756836

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211178092.3A Active CN115265523B (zh) 2022-09-27 2022-09-27 机器人同时定位与建图方法、装置及可读介质

Country Status (1)

Country Link
CN (1) CN115265523B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115755901A (zh) * 2022-11-14 2023-03-07 杭州蓝芯科技有限公司 一种移动机器人的停障控制方法和装置
CN115561731B (zh) * 2022-12-05 2023-03-10 安徽蔚来智驾科技有限公司 位姿优化方法、点云地图建立方法、计算机设备及介质
CN115979248B (zh) * 2023-03-17 2023-08-18 上海仙工智能科技有限公司 基于定位位姿为约束的地图更新方法及系统、存储介质
CN115984504B (zh) * 2023-03-21 2023-07-04 上海仙工智能科技有限公司 一种地图自动更新方法及系统、存储介质
CN116222544B (zh) * 2023-05-09 2023-08-04 浙江大学湖州研究院 一种面向饲养场的投料车自动导航定位方法及装置
CN116359938B (zh) * 2023-05-31 2023-08-25 未来机器人（深圳）有限公司 对象检测方法、装置及运载装置
CN116929338B (zh) * 2023-09-15 2024-01-09 深圳市智绘科技有限公司 地图构建方法、设备及存储介质
CN117128957B (zh) * 2023-10-27 2024-01-02 湖南光华防务科技集团有限公司 一种多源信息融合的无人机火源定位方法及系统
CN117475092B (zh) * 2023-12-27 2024-03-19 安徽蔚来智驾科技有限公司 位姿优化方法、设备、智能设备和介质
CN117495968A (zh) * 2024-01-02 2024-02-02 苏州中德睿博智能科技有限公司 基于3d激光雷达的移动机器人位姿跟踪方法及装置

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105631939A (zh) * 2015-12-28 2016-06-01 集美大学 一种基于曲率滤波的三维点云畸变校正方法及其系统
CN107168331A (zh) * 2017-06-20 2017-09-15 南京阿凡达机器人科技有限公司 基于光电鼠标传感器位移检测的机器人室内地图创建方法
CN107193279A (zh) * 2017-05-09 2017-09-22 复旦大学 基于单目视觉和imu信息的机器人定位与地图构建系统
CN110298914A (zh) * 2019-05-29 2019-10-01 江苏大学 一种建立果园中果树冠层特征地图的方法
CN112268559A (zh) * 2020-10-22 2021-01-26 中国人民解放军战略支援部队信息工程大学 复杂环境下融合slam技术的移动测量方法
WO2021089844A1 (en) * 2019-11-08 2021-05-14 Outsight Rolling environment sensing and gps optimization
CA3108547A1 (en) * 2020-02-11 2021-08-11 Raytheon Company Collaborative 3d mapping and surface registration
CN114526745A (zh) * 2022-02-18 2022-05-24 太原市威格传世汽车科技有限责任公司 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN115077519A (zh) * 2022-06-27 2022-09-20 南京航空航天大学 基于模板匹配与激光惯导松耦合的定位建图方法和装置

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113781582B (zh) * 2021-09-18 2023-09-19 四川大学 基于激光雷达和惯导联合标定的同步定位与地图创建方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105631939A (zh) * 2015-12-28 2016-06-01 集美大学 一种基于曲率滤波的三维点云畸变校正方法及其系统
CN107193279A (zh) * 2017-05-09 2017-09-22 复旦大学 基于单目视觉和imu信息的机器人定位与地图构建系统
CN107168331A (zh) * 2017-06-20 2017-09-15 南京阿凡达机器人科技有限公司 基于光电鼠标传感器位移检测的机器人室内地图创建方法
CN110298914A (zh) * 2019-05-29 2019-10-01 江苏大学 一种建立果园中果树冠层特征地图的方法
WO2021089844A1 (en) * 2019-11-08 2021-05-14 Outsight Rolling environment sensing and gps optimization
CA3108547A1 (en) * 2020-02-11 2021-08-11 Raytheon Company Collaborative 3d mapping and surface registration
CN112268559A (zh) * 2020-10-22 2021-01-26 中国人民解放军战略支援部队信息工程大学 复杂环境下融合slam技术的移动测量方法
CN114526745A (zh) * 2022-02-18 2022-05-24 太原市威格传世汽车科技有限责任公司 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN115077519A (zh) * 2022-06-27 2022-09-20 南京航空航天大学 基于模板匹配与激光惯导松耦合的定位建图方法和装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于滤波与图优化的定位与建图系统;华智等;《电子测量技术》;20220228;第45卷(第04期);第99-106页 *

Also Published As

Publication number Publication date
CN115265523A (zh) 2022-11-01

Similar Documents

Publication Publication Date Title
CN115265523B (zh) 机器人同时定位与建图方法、装置及可读介质
CN109521403B (zh) 多线激光雷达的参数标定方法及装置、设备及可读介质
CN109461208B (zh) 三维地图处理方法、装置、介质和计算设备
CN109146938B (zh) 动态障碍物的位置校准方法、装置、设备及存储介质
CN110660098B (zh) 基于单目视觉的定位方法和装置
CN110349212B (zh) 即时定位与地图构建的优化方法及装置、介质和电子设备
CN115371699B (zh) 视觉惯性里程计方法、装置及电子设备
JP7369847B2 (ja) 自動運転車両に対するデータ処理方法及び装置、電子機器、記憶媒体、コンピュータプログラム、ならびに自動運転車両
CN112781594A (zh) 基于imu耦合的激光雷达迭代最近点改进算法
CN114993328B (zh) 车辆定位评估方法、装置、设备和计算机可读介质
CN113932796A (zh) 高精地图车道线生成方法、装置和电子设备
CN114022561A (zh) 一种基于gps约束和动态校正的城区单目测图方法和系统
CN116929343A (zh) 位姿估计方法、相关设备及存储介质
CN115900697B (zh) 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN115937449A (zh) 高精地图生成方法、装置、电子设备和存储介质
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
US11908198B2 (en) Contextualization and refinement of simultaneous localization and mapping
CN114170300A (zh) 高精地图点云位姿优化方法、装置、设备及介质
CN114998406A (zh) 一种自监督多视图深度估计方法、装置
CN115032672A (zh) 一种基于定位子系统的融合定位方法及系统
CN115147561A (zh) 位姿图生成方法、高精地图生成方法和装置
CN113970754A (zh) 一种自主可行驶设备的定位方法和装置
CN112880675B (zh) 用于视觉定位的位姿平滑方法、装置、终端和移动机器人
CN116817928B (zh) 基于因子图优化的卫导/惯导列车多源融合定位的方法
CN109255095B (zh) Imu数据的积分方法、装置、计算机可读介质及电子设备

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