CN113781582B - 基于激光雷达和惯导联合标定的同步定位与地图创建方法 - Google Patents
基于激光雷达和惯导联合标定的同步定位与地图创建方法 Download PDFInfo
- Publication number
- CN113781582B CN113781582B CN202111110510.0A CN202111110510A CN113781582B CN 113781582 B CN113781582 B CN 113781582B CN 202111110510 A CN202111110510 A CN 202111110510A CN 113781582 B CN113781582 B CN 113781582B
- Authority
- CN
- China
- Prior art keywords
- point
- imu
- map
- laser radar
- points
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 31
- 230000001360 synchronised effect Effects 0.000 title claims abstract description 15
- 230000033001 locomotion Effects 0.000 claims abstract description 15
- 238000001514 detection method Methods 0.000 claims abstract description 11
- 238000001914 filtration Methods 0.000 claims abstract description 9
- 238000004364 calculation method Methods 0.000 claims abstract description 7
- 238000012545 processing Methods 0.000 claims abstract description 5
- 238000007781 pre-processing Methods 0.000 claims abstract description 4
- 238000005457 optimization Methods 0.000 claims description 13
- 230000005484 gravity Effects 0.000 claims description 9
- 230000008878 coupling Effects 0.000 claims description 8
- 238000010168 coupling process Methods 0.000 claims description 8
- 238000005859 coupling reaction Methods 0.000 claims description 8
- 230000010354 integration Effects 0.000 claims description 8
- 238000012216 screening Methods 0.000 claims description 8
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000006073 displacement reaction Methods 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 3
- 230000001133 acceleration Effects 0.000 claims description 2
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 230000000295 complement effect Effects 0.000 claims description 2
- 230000007704 transition Effects 0.000 claims description 2
- 206010034719 Personality change Diseases 0.000 claims 1
- 238000012804 iterative process Methods 0.000 claims 1
- 238000005516 engineering process Methods 0.000 description 8
- 238000013507 mapping Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 230000008859 change Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
-
- 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
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T3/00—Geometric image transformations in the plane of the image
- G06T3/08—Projecting images onto non-planar surfaces, e.g. geodetic screens
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10032—Satellite or aerial image; Remote sensing
- G06T2207/10044—Radar image
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Geometry (AREA)
- Software Systems (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Remote Sensing (AREA)
- Computer Graphics (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开了基于激光雷达和惯导联合标定的同步定位与地图创建方法，涉及地图创建领域，包括S1获取数据S2处理IMU数据S3预处理激光雷达数据，保留目标特征点云S4判断是否初始化S5初始化求IMU原始状态S6矫正运动畸变S7建立滑窗，投影目标特征点到中心帧，组成局部地图S8局部地图和目标特征点匹配，优化位姿和IMU状态量S9构建初始全局地图，体素滤波优化为第一全局地图S10使用Scan‑to‑Map优化里程计帧和第一全局地图得第二全局地图S11回环检测，优化设备位置和姿态S12提取判断边缘特征点是否在同一平面，若否删除，反之使用距离残差计算匹配到第二全局地图S13获取高质量地图，通过激光雷达和IMU估计IMU状态，并矫正运动畸变，提高系统对于地面点和运动物体的鲁棒性。
Description
技术领域
本发明涉及地图创建领域，尤其涉及一种基于激光雷达和惯导联合标定的同步定位与地图创建方法。
背景技术
即时定位与建图(Simultaneous Localization and Mapping，SLAM)是当前机器人导航定位领域具有研究前景的技术领域。即时建图与定位技术的意义在于，其并不依赖于外部测量给出的信息确定自身的位置，而是依赖于自身传感器去确定自身位置的增量，从而确定本体在环境地图中的位置，同时根据位置和当前位置的传感器数据建立环境点云地图。SLAM领域目前主要分为激光SLAM技术和视觉SLAM技术、以及多传感器融合SLAM技术。激光SLAM技术由于性能稳定，鲁棒性强，已经被广泛应用于无人驾驶、智能机器人、三维环境重建、智能农业等各个领域中。但是，SLAM技术仍然存在一系列问题，如何使SLAM技术适应各种复杂环境如低特征环境、高速移动环境，大尺度(千米级)环境，仍然是专家学者们研究的热点。
针对激光SLAM问题中的大尺度环境问题，已有的解决方案有如下几种：
方案1：文献(Zhang J,Kaess M,Singh S.On degeneracy ofoptimization-basedstate estimation problems.[C]//2016IEEE International Conference on Roboticsand Automation(ICRA).IEEE,2016)文献提出了LOAM算法。LOAM主要贡献在于，提出了基于边缘点和平面点特征的激光点云配准方法以及利用匀速运动假设来消除激光点云的运动畸变，取得了低漂移和低计算成本的平衡。但美中不足的是，LOAM并没有考虑时下流行的后端优化方法去做后端回环处理。值得一提的是，LOAM中有使用惯性导航器件作为点云配准的初始值，相对于原始版本并没有取得很好的结果。
方案2：文献(Shan T,Englot B.Lego-loam:Lightweight and ground-optimizedlidar odometry and mapping on variable terrain[C]//2018IEEE/RSJ InternationalConference on Intelligent Robots and Systems(IROS).IEEE,2018:4758-4765.)文献中提出的LEGO_LOAM算法针对地面无人平台对LOAM算法进行了改进，提出了利用点云地面分割技术提取出地面，从而能够估计地面无人平台的横滚角、俯仰角以及高度，再利用剩余的特征点进行点云配准，估计出地面无人平台的偏航角和二维位置，同时加入了后端回环优化的部分使得回环之后点云地图会更加精确。但是没有完全解决大尺度环境下激光SLAM算法高度漂移的问题。
方案3：文献(Shan T,Englot B,Meyers D,et al.LIO-SAM:Tightly-coupledLidar Inertial Odometry via Smoothing and Mapping[J].2020.)该文献提出了LIO-SAM算法，在LEGO_LOAM的基础上加入了惯性测量单元里程计，并用图优化的理念对激光里程计和惯性里程计进行紧耦合，使得里程计积分出的位姿更加精确，同时，在后端优化中，使用GPS对部分位姿节点进行约束，从而能获得更加贴近于现实的建图效果。但是算法需要高精度的惯性导航器件和激光雷达的精确标定，实现条件较为苛刻。
结合以上三个已有方案，如何实现针对地面无人平台的即时定位和建图，并避免纯激光SLAM算法中存在的高度误差累积问题，是目前亟待解决的问题。
发明内容
本发明的目的就在于为了解决上述问题设计了一种基于激光雷达和惯导联合标定的同步定位与地图创建方法。
本发明通过以下技术方案来实现上述目的：
基于激光雷达和惯导联合标定的同步定位与地图创建方法，包括：
S1、获取激光雷达数据和IMU数据，并导入激光雷达和惯导的联合标定设备；
S2、对IMU数据进行预积分处理；
S3、对激光雷达数据的点云进行预处理，计算每个点的曲率将其区分为平面特征点和边缘特征点，保留非地面目标的目标特征点云；
S4、判断激光雷达和惯导的联合标定设备是否初始化，若否，则进入S5；若是，则进入S6；
S5、设备进行松耦合初始化，求解出IMU的原始状态，进入S6；
S6、设备进行紧耦合矫正运动畸变；
S7、沿时间轴建立多个滑窗，在每个滑窗中假设一个中心帧，将该滑窗各目标特征点转投影到该中心帧下，滤波后组成局部地图；
S8、对局部地图和投影后的目标特征点进行匹配，构造非线性关系和系统残差模型进行位姿和IMU状态量的优化，并更新IMU状态量；
S9、选取众多中心帧中二者之间距离超过设定阈值的帧形成关键帧链，通过关键帧链构建初始全局地图，并采用体素滤波优化初始全局地图得到第一全局地图；
S10、使用Scan-to-Map优化里程计帧对第一全局地图的点云优化获得第二全局地图；
S11、对第二全局地图进行回环检测判定，借助因子图动态优化设备位置和姿态；
S12、使用KD树提取第二全局地图的五个边缘特征点，并计算5个边缘特征点的平面度，判断这5个边缘特征点是否位于同一个平面，若否，删除这5个边缘特征点；若是，使用距离残差公式计算匹配到第二全局地图；
S13、使用优化后的位姿点注册点云获取高质量地图信息。
本发明的有益效果在于：通过使用激光雷达和IMU构成的紧耦合优化框架，实时估计IMU状态，通过提取的关键帧构建全局地图，并使用因子图优化的回环模块完善地图，结合IMU和激光雷达的特性，并使用回环模块，建立精确的点云地图，供设备定位或路径规划；通过激光雷达和IMU的紧耦合，精确实时估计IMU状态，并通过IMU预积分值矫正激光雷达运动畸变，提高了系统对于地面点和运动物体的鲁棒性；通过添加基于激光帧描述子和基于轨迹的回环检测模块，并在回环模块中使用因子图优化，实现了点云地图的闭环优化。
附图说明
图1是本发明基于激光雷达和惯导联合标定的同步定位与地图创建方法的流程图；
图2是点云曲率示意图；
图3是扇形扫描区域内子区域及激光点重心图；
图4是子区域中心在射线平面投影示意图；
图5是稳定与不稳定特征点的区分；
图6是不稳定特征点剔除示意图；
图7是基于轨迹的回环示意图；
图8是基于轨迹的回环优化示意图；
图9是基于描述子的回环检测流程图；
图10是因子图优化示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。
因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
在本发明的描述中，需要理解的是，术语“上”、“下”、“内”、“外”、“左”、“右”等指示的方位或位置关系为基于附图所示的方位或位置关系，或者是该发明产品使用时惯常摆放的方位或位置关系，或者是本领域技术人员惯常理解的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的设备或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
此外，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
在本发明的描述中，还需要说明的是，除非另有明确的规定和限定，“设置”、“连接”等术语应做广义理解，例如，“连接”可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接连接，也可以通过中间媒介间接连接，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
下面结合附图，对本发明的具体实施方式进行详细说明。
如图1所示，基于激光雷达和惯导联合标定的同步定位与地图创建方法，其特征在于，包括：
S1、获取激光雷达数据和IMU数据，并导入激光雷达和惯导的联合标定设备，采集获取数据前，将激光雷达和IMU固定(确保激光雷达水平方向无遮挡)；设置激光雷达的扫描频率为10Hz，扫描角度为水平360°，垂直为±20°；IMU的采样频率设置为每秒100帧。
S2、由于IMU采样频率远高于激光雷达，对每一帧IMU数据都进行处理则造成计算量骤增，且无法找到对应的激光雷达数据与之匹配，所以对IMU数据进行预积分处理；具体包括：
S21、将IMU系转换为世界系，则表示j时刻IMU系向世界系的转换，/>表示IMU系下i时刻向j时刻的位姿态变换；
S22、预积分后的惯导运动模型为积分结果更新后为/>其中/>和/>分别代表第j帧和第i帧的姿态，Δtb是IMU数据帧时间间隔，ωk和bgk分别是中间时刻k的角速度和角速度零偏，/>和分别表示j时刻和i时刻IMU在世界坐标系下的位置，/>是使用零偏后更新的位置，/>是更新之前的位置，/>和/>分别是位置残差对加速度和陀螺仪零偏更新量的雅可比矩阵；
S23、非线性优化后对速度和位置进行更新，分别为其中/>是更新之前的位置，/>是更新之前的速度。
S3、对激光雷达数据的点云进行预处理，计算每个点的曲率将其区分为平面特征点和边缘特征点，保留非地面目标的目标特征点云，具体包括：
S31、计算每个点的曲率，其中Ck,i表示第k条扫描线的第i个点上的曲率，N表示用N个点进行当前点的曲率计算，/>表示测量点到激光雷达坐标系原点的距离,/>和表示/>与当前点处于同一扫描线上周围的点，如图2所示，根据曲率的大小提取特征点，并将其区分为平面特征点和边缘特征点，曲率描述了该点所在的部分扫描线的平整程度，当Ck,i越小，其所在平面越平，当Ck,i越大，当曲率超过某个预先设置的阈值时，则可以认为当前点处于边缘点上；
S32、将三维空间中提取出来的特征点云投影到地面，将投影后的平面看做一个圆，并将圆分为S等分，每一份为一个扇形区域，假设点Pi的坐标为(xi,yi,zi)，那么点Pi投影到第几块扇形计算为：antan2表示点Pi在投影面上的角度，floor为向下取整的函数；
S33、将扇形区域分为多个子区域，如图3所示，计算每个子区域的重心点 其中，重心点/>代表当前子区域的所有点，Pi为处于di这个子区域中的点，Ndi为处于子区域di中点的数量；
S34、计算相邻两个子区域的重心点的斜率，判断斜率的绝对值是否超过设定的阈值，如图4所示，若超过，则为非地面点，保留；若未超过，则为地面点，剔除；
S35、剔除地面点后的点云进行聚类形成多个点云块，小于设定阈值的点云块去除；
S36、剔除伪特征点：由于特征点的提取只关注了激光点之间的曲率大小，而曲率的计算公式只能处理一般的集合结构，当出现特殊的结构信息时(如物体之间产生了遮挡关系时)，就通常会提取出不稳定的特征点，当激光雷达运动的时候，由于遮挡关系的变化，这些特征点的位置也会出现变换，最终导致错误的帧间匹配关系；几种不稳定特征点的出现方式如图5所示，图中A,C是稳定特征点，B,D是不稳定特征点，在曲率计算中，A,B都被判断为边缘特征点，但是B是由另一个物体遮挡而形成的曲率较大点，当激光雷达欲动的时候，B的位置会变化，最终对帧间搜索和匹配特征点带来很大的误差；在曲率计算中，C,D两点被判断为平面特征点，但是C所在的平面与激光束呈一个很大的角度，D所在的平面几乎与激光束平行，在在激光雷达运动时，D点的位置也会发生很大的变化，造成特征点匹配上的误差；
因此设点A、B为边缘特征点，点C为任意一个平面特征点，O为激光雷达光源点，A位于扫描线第i个点，计算扫描线OA和扫描线OB之间的夹角，并判断∠AOB的角度是否超过5°，若不超过，则点A、B不被认为边缘点；若是，计算和/>的值，并判断二者的值均大于0.95时，则点A、B、C所在的平面与激光射线平行，则点A、B、C所在的平面上的点均为伪特征点，剔除，如图6所示。
S4、判断激光雷达和惯导的联合标定设备是否初始化，若否，则进入S5；若是，则进入S6。
S5、设备进行松耦合初始化，求解出IMU的原始状态，进入S6；
S6、设备进行紧耦合矫正运动畸变；
S7、沿时间轴建立多个滑窗，在每个滑窗中假设一个中心帧，将该滑窗各目标特征点转投影到该中心帧下，滤波后组成局部地图；
S8、对局部地图和投影后的目标特征点进行匹配，构造非线性关系和系统残差模型进行位姿和IMU状态量的优化，并更新IMU状态量，构造系统残差模型具体包括构造激光雷达残差和构造IMU残差；IMU残差的构建约束为激光雷达残差被IMU状态约束，采用舒尔补原理求解IMU残差，其中/>为bi向w变换的旋转矩阵，/>和/>分别表示IMU坐标系下IMU所处的位置和速度。
S9、选取众多中心帧中二者之间距离超过设定阈值的帧形成关键帧链，被选取的帧在当前帧的位姿与上一帧的位姿的位移量超过设定阈值，通过关键帧链构建初始全局地图，并采用体素滤波优化初始全局地图得到第一全局地图，体素滤波的主要思想就是将空间分为多个相邻的小三维立方体(即体素)，在每个体素中，计算所有包含点的重心，之后使用重心来代替表示体素中的所有点云，该方法需要对不同环境数据类型设置不同的体素大小，同时也相对于其他降采样方法更慢，但是其对平面和曲面的表示更加准确，体素滤波前的点云更加密集，但更加杂乱，这对于接下来要进行的点云最近见搜索来说不友好，因为在全局地图中搜索当前帧点云匹配的线特征点和面特征点时，杂乱的点云会带来更多的随机性。而且过于密集的点云代表搜索到的匹配特征点之间到距离很近，很小的噪声就能对点到平面法向量方向和点到边缘线法向量方向造成很大影响，在约束残差项上引入较大噪声，精细化的定位约束造成影响，而采用体素滤波的全局地图点云数据，相当于对多帧拼接的区域点云进行了求取位姿均值的操作，因此能够有效抑制多帧点云数据位姿误差带来的匹配点集的集合误差，为之后的匹配提供适当的约束条件。
S10、使用Scan-to-Map优化里程计帧对第一全局地图的点云优化获得第二全局地图。
S11、对第二全局地图进行回环检测判定，借助因子图动态优化设备位置和姿态；具体包括：
S1101、基于描述子对第二全局地图进行回环检测，并求取回环帧误差，如图7、图8、图9所示；从某一地点开始，设备第二次到达该地点，判断当前位置与第一次经过该点时的位置差值是否满足回环合理性，若不满足，则不构成回环，若满足，并使用KD树搜索点云最近帧与当前点云的距离残差小于设定阈值时，则检测到构成回环成为回环候选帧，使用ICP算法计算ICP迭代过程中两个点云的均方距离作为回环筛选门限，当距离残差小于回环筛选门限时，则搜索到的回环候选帧是真实的回环中心帧，与视觉回环的筛选方式不同，基于激光点云的回环筛选没法通过RANSC迭代构造外点，因此只能通过回环筛选过程中，ICP迭代的误差作为门线判断是否真的出现了回环，使用ICP迭代过程中两个点云的均方距离作为回环帧筛选标准，迭代过程中的误差函数为其中，xi和pi分别表示回环历史帧点云和当前帧点云，R和t代表回环检测ICP迭代中求解的当前帧到回环关键帧经过点云匹配之后的位姿变换；
S1102、基于因子图对设备位置和姿态进行回环优化，如图10所示；获取当前帧中产生特征点的物体作为观测数据节点，通过运动方程和当前观测数据节点的先验信息求解当前观测数据节点位姿，根据距离残差判断是否检测到回环求解下一次该观测数据节点的先验信息，矫正优化下一次的位姿，运动方程为 其中，/>表示第i帧IMU的所有状态，/>是世界系下IMU的位置，/>是IMU的速度，/>是世界系下IMU的姿态，/>第i帧的加速度计零偏，/>是第i帧的陀螺仪零偏。/>是IMU系向激光雷达系的转换(也就是外参)，/>是IMU向激光雷达的位移，/>是IMU向激光雷达的姿态变化。
S12、使用KD树提取第二全局地图的五个边缘特征点，并计算5个边缘特征点的平面度，判断这5个边缘特征点是否位于同一个平面，若否，删除这5个边缘特征点；若是，使用距离残差公式计算匹配到第二全局地图；五个边缘特征点的三维坐标为pi(x,y,z)，其中i＝1,2,3,4,5，则可以协方差矩阵为其中aix、aiy和aiz分别代表第i个点的x、y和z坐标值，计算所提取每个边缘特征点的特征值和特征向量，判断特征值中任意一个值是否大于其余任意两个值，或特征值中任意一个值与其余任意一个值之间的差值是否大于设定阈值，若均是，则五个边缘特征点不在同一个平面，剔除；反之则计算该平面的法向量，并设原点到该平面的距离为d，将五个边缘特征点与原点相连的线段投影到该平面的法向量上，投影后的总长度为L，计算L与d之间的差值是否超过设定阈值，若是，则剔除这5个边缘特征点，若否，使用距离残差公式计算匹配到第二全局地图。
S13、使用优化后的位姿点注册点云获取高质量地图信息。
本发明的技术方案不限于上述具体实施例的限制，凡是根据本发明的技术方案做出的技术变形，均落入本发明的保护范围之内。
Claims (7)
1.基于激光雷达和惯导联合标定的同步定位与地图创建方法，其特征在于，包括：
S1、获取激光雷达数据和IMU数据，并导入激光雷达和惯导的联合标定设备；
S2、对IMU数据进行预积分处理；
S3、对激光雷达数据的点云进行预处理，计算每个点的曲率将其区分为平面特征点和边缘特征点，保留非地面目标的目标特征点云；
S4、判断激光雷达和惯导的联合标定设备是否初始化，若否，则进入S5；若是，则进入S6；
S5、设备进行松耦合初始化，求解出IMU的原始状态，进入S6；
S6、设备进行紧耦合矫正运动畸变；
S7、沿时间轴建立多个滑窗，在每个滑窗中假设一个中心帧，将该滑窗各目标特征点转投影到该中心帧下，滤波后组成局部地图；
S8、对局部地图和投影后的目标特征点进行匹配，构造非线性关系和系统残差模型进行位姿和IMU状态量的优化，并更新IMU状态量；
构造系统残差模型具体包括构造激光雷达残差和构造IMU残差；IMU残差的构建约束为激光雷达残差被IMU状态约束，采用舒尔补原理求解IMU残差，其中/>为bi向w变换的旋转矩阵，/>和/>分别表示IMU坐标系下IMU所处的位置和速度；
S9、选取众多中心帧中二者之间距离超过设定阈值的帧形成关键帧链，通过关键帧链构建初始全局地图，并采用体素滤波优化初始全局地图得到第一全局地图；
S10、使用Scan-to-Map优化里程计帧对第一全局地图的点云优化获得第二全局地图；
S11、对第二全局地图进行回环检测判定，借助因子图动态优化设备位置和姿态；具体包括：
S1101、基于描述子对第二全局地图进行回环检测，并求取回环帧误差；
S1102、基于因子图对设备位置和姿态进行回环优化；具体为：
获取当前帧中产生特征点的物体作为观测数据节点，通过运动方程和当前观测数据节点的先验信息求解当前观测数据节点位姿，根据距离残差判断是否检测到回环求解下一次该观测数据节点的先验信息，矫正优化下一次的位姿，运动方程为其中，/>表示第i帧IMU的所有状态，/>是世界系下IMU的位置，/>是IMU的速度，/>是世界系下IMU的姿态，/>第i帧的加速度计零偏，/>是第i帧的陀螺仪零偏；/>是IMU系向激光雷达系的转换，/>是IMU向激光雷达的位移，/>是IMU向激光雷达的姿态变化；
S12、使用KD树提取第二全局地图的五个边缘特征点，并计算5个边缘特征点的平面度，判断这5个边缘特征点是否位于同一个平面，若否，删除这5个边缘特征点；若是，使用距离残差公式计算匹配到第二全局地图；
S13、使用优化后的位姿点注册点云获取高质量地图信息。
2.根据权利要求1所述的基于激光雷达和惯导联合标定的同步定位与地图创建方法，其特征在于，在S2中包括：
S21、将IMU系转换为世界系，则表示j时刻IMU系向世界系的转换，/>表示IMU系下i时刻向j时刻的位姿态变换；
S22、预积分后的惯导运动模型为积分结果更新后为/>其中/>和/>分别代表第j帧和第i帧的姿态，Δtb是IMU数据帧时间间隔，ωk和bgk分别是中间时刻k的角速度和角速度零偏，/>和/>分别表示j时刻和i时刻IMU在世界坐标系下的位置，/>是使用零偏后更新的位置，是更新之前的位置，/>和/>分别是位置残差对加速度和陀螺仪零偏更新量的雅可比矩阵；
S23、非线性优化后对速度和位置进行更新，分别为其中/>是更新之前的位置，/>是更新之前的速度。
3.根据权利要求1所述的基于激光雷达和惯导联合标定的同步定位与地图创建方法，其特征在于，在S3中包括：
S31、计算每个点的曲率，其中Ck,i表示第k条扫描线的第i个点上的曲率，N表示用N个点进行当前点的曲率计算，/>表示测量点到激光雷达坐标系原点的距离,/>和表示/>与当前点处于同一扫描线上周围的点，根据曲率的大小提取特征点，并将其区分为平面特征点和边缘特征点；
S32、将三维空间中提取出来的特征点云投影到地面，将投影后的平面看做一个圆，并将圆分为S等分，每一份为一个扇形区域，假设点Pi的坐标为(xi,yi,zi)，那么点Pi投影到第几块扇形计算为：antan2表示点Pi在投影面上的角度，floor为向下取整的函数；
S33、将扇形区域分为多个子区域，计算每个子区域的重心点 其中，重心点/>代表当前子区域的所有点，Pi为处于di这个子区域中的点，Ndi为处于子区域di中点的数量；
S34、计算相邻两个子区域的重心点的斜率，判断斜率的绝对值是否超过设定的阈值，若超过，则为非地面点，保留；若未超过，则为地面点，剔除；
S35、剔除地面点后的点云进行聚类形成多个点云块，小于设定阈值的点云块去除；
S36、剔除伪特征点，具体包括设点A、B为边缘特征点，点C为任意一个平面特征点，O为激光雷达光源点，A位于扫描线第i个点，计算扫描线OA和扫描线OB之间的夹角，并判断∠AOB的角度是否超过设定阈值，若不超过，则点A、B不被认为边缘特征点；若是，计算和的值，并判断二者的值均大于设定阈值时，则点A、B、C所在的平面与激光射线平行，则点A、B、C所在的平面上的点均为伪特征点，剔除。
4.根据权利要求3所述的基于激光雷达和惯导联合标定的同步定位与地图创建方法，其特征在于，在S36中，∠AOB角度的设定阈值为5°，和/>的设定阈值为0.95。
5.根据权利要求1所述的基于激光雷达和惯导联合标定的同步定位与地图创建方法，其特征在于，在S9中，被选取的帧在当前帧的位姿与上一帧的位姿的位移量超过设定阈值。
6.根据权利要求1所述的基于激光雷达和惯导联合标定的同步定位与地图创建方法，其特征在于，在S12中，五个边缘特征点的三维坐标为pi(x,y,z)，其中i＝1,2,3,4,5，则协方差矩阵可以为 其中aix、aiy和aiz分别代表第i个点的x、y和z坐标值，计算所提取每个边缘特征点的特征值和特征向量，判断特征值中任意一个值是否大于其余任意两个值，或特征值中任意一个值与其余任意一个值之间的差值是否大于设定阈值，若均是，则五个边缘特征点不在同一个平面，剔除；反之则计算该平面的法向量，并设原点到该平面的距离为d，将五个边缘特征点与原点相连的线段投影到该平面的法向量上，投影后的总长度为L，计算L与d之间的差值是否超过设定阈值，若是，则剔除这5个边缘特征点，若否，使用距离残差公式计算匹配到第二全局地图。
7.根据权利要求1所述的基于激光雷达和惯导联合标定的同步定位与地图创建方法，其特征在于，在S1101的回环检测具体包括：
从某一地点开始，设备第二次到达该地点，判断当前位置与第一次经过该点时的位置差值是否满足回环合理性，若不满足，则不构成回环，若满足，并使用KD树搜索点云最近帧与当前点云的距离残差小于设定阈值时，则检测到构成回环成为回环候选帧，使用ICP算法计算ICP迭代过程中两个点云的均方距离作为回环筛选门限，当距离残差小于回环筛选门限时，则搜索到的回环候选帧是真实的回环中心帧。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111110510.0A CN113781582B (zh) | 2021-09-18 | 2021-09-18 | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111110510.0A CN113781582B (zh) | 2021-09-18 | 2021-09-18 | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113781582A CN113781582A (zh) | 2021-12-10 |
| CN113781582B true CN113781582B (zh) | 2023-09-19 |
Family
ID=78852673
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111110510.0A Active CN113781582B (zh) | 2021-09-18 | 2021-09-18 | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113781582B (zh) |
Families Citing this family (22)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114283167B (zh) * | 2021-12-27 | 2024-07-05 | 上海智楹机器人科技有限公司 | 一种基于视觉的清扫区域检测方法 |
| CN114372914B (zh) * | 2022-01-12 | 2022-09-13 | 吉林大学 | 应用于矿用电铲上的机械式激光雷达点云预处理方法 |
| CN114397642B (zh) * | 2022-01-13 | 2024-10-29 | 北京工业大学 | 一种基于图优化的三维激光雷达与imu外参标定方法 |
| CN114565726A (zh) * | 2022-01-26 | 2022-05-31 | 浙江工业大学 | 一种非结构化动态环境下的同时定位与建图方法 |
| CN114459474B (zh) * | 2022-02-16 | 2023-11-24 | 北方工业大学 | 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法 |
| CN114608569B (zh) * | 2022-02-22 | 2024-03-01 | 杭州国辰机器人科技有限公司 | 三维位姿估计方法、系统、计算机设备及存储介质 |
| CN114593751B (zh) * | 2022-03-11 | 2024-06-18 | 北京京东乾石科技有限公司 | 外参标定方法、装置、介质及电子设备 |
| CN114674311B (zh) * | 2022-03-22 | 2024-05-24 | 中国矿业大学 | 一种室内定位与建图方法及系统 |
| CN114739409A (zh) * | 2022-03-23 | 2022-07-12 | 深圳市优必选科技股份有限公司 | 机器人定位方法、装置、计算机可读存储介质及机器人 |
| CN114777775B (zh) * | 2022-05-06 | 2024-06-14 | 浙江师范大学 | 一种多传感器融合的定位方法及系统 |
| CN115183778A (zh) * | 2022-07-01 | 2022-10-14 | 北京斯年智驾科技有限公司 | 一种基于码头石墩的建图方法、装置、设备以及介质 |
| CN115407357B (zh) * | 2022-07-05 | 2024-05-31 | 东南大学 | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 |
| CN115265546A (zh) * | 2022-08-12 | 2022-11-01 | 北京金坤科创技术有限公司 | 一种无人机室内slam定位方法 |
| CN115265523B (zh) * | 2022-09-27 | 2023-01-03 | 泉州装备制造研究所 | 机器人同时定位与建图方法、装置及可读介质 |
| CN115376051B (zh) * | 2022-10-25 | 2023-03-24 | 杭州华橙软件技术有限公司 | 关键帧的管理方法及管理装置、slam方法、电子设备 |
| CN115683122A (zh) * | 2022-10-25 | 2023-02-03 | 中国科学院自动化研究所 | 以世界为中心的定位与全局地图优化方法、装置及设备 |
| CN116299500B (zh) * | 2022-12-14 | 2024-03-15 | 江苏集萃清联智控科技有限公司 | 一种融合目标检测和跟踪的激光slam定位方法及设备 |
| CN116045833B (zh) * | 2023-01-03 | 2023-12-22 | 中铁十九局集团有限公司 | 一种基于大数据的桥梁施工变形监测系统 |
| CN116295507B (zh) * | 2023-05-26 | 2023-08-15 | 南京师范大学 | 一种基于深度学习的激光惯性里程计优化方法、系统 |
| CN117872330B (zh) * | 2024-03-11 | 2024-05-31 | 安徽大学 | 面向复杂环境的无人驾驶多激光雷达标定与融合建图方法 |
| CN118031983B (zh) * | 2024-04-11 | 2024-06-25 | 江苏集萃清联智控科技有限公司 | 自动驾驶融合定位方法及系统 |
| CN118518093B (zh) * | 2024-07-24 | 2024-10-01 | 南昌航空大学 | 一种基于多帧空间占用率的激光slam方法、设备及介质 |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108665540A (zh) * | 2018-03-16 | 2018-10-16 | 浙江工业大学 | 基于双目视觉特征和imu信息的机器人定位与地图构建系统 |
| CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
| CN111325794A (zh) * | 2020-02-23 | 2020-06-23 | 哈尔滨工业大学 | 一种基于深度卷积自编码器的视觉同时定位与地图构建方法 |
| CN111929699A (zh) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统 |
| CN112258600A (zh) * | 2020-10-19 | 2021-01-22 | 浙江大学 | 一种基于视觉与激光雷达的同时定位与地图构建方法 |
| CN112347840A (zh) * | 2020-08-25 | 2021-02-09 | 天津大学 | 视觉传感器激光雷达融合无人机定位与建图装置和方法 |
| CN112634451A (zh) * | 2021-01-11 | 2021-04-09 | 福州大学 | 一种融合多传感器的室外大场景三维建图方法 |
| CN113066105A (zh) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及系统 |
| CN113379841A (zh) * | 2021-06-21 | 2021-09-10 | 上海仙工智能科技有限公司 | 一种基于相位相关法与因子图的激光slam方法及其可读存储介质 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11940277B2 (en) * | 2018-05-29 | 2024-03-26 | Regents Of The University Of Minnesota | Vision-aided inertial navigation system for ground vehicle localization |
-
2021
- 2021-09-18 CN CN202111110510.0A patent/CN113781582B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108665540A (zh) * | 2018-03-16 | 2018-10-16 | 浙江工业大学 | 基于双目视觉特征和imu信息的机器人定位与地图构建系统 |
| CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
| CN111325794A (zh) * | 2020-02-23 | 2020-06-23 | 哈尔滨工业大学 | 一种基于深度卷积自编码器的视觉同时定位与地图构建方法 |
| CN111929699A (zh) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统 |
| CN112347840A (zh) * | 2020-08-25 | 2021-02-09 | 天津大学 | 视觉传感器激光雷达融合无人机定位与建图装置和方法 |
| CN112258600A (zh) * | 2020-10-19 | 2021-01-22 | 浙江大学 | 一种基于视觉与激光雷达的同时定位与地图构建方法 |
| CN112634451A (zh) * | 2021-01-11 | 2021-04-09 | 福州大学 | 一种融合多传感器的室外大场景三维建图方法 |
| CN113066105A (zh) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及系统 |
| CN113379841A (zh) * | 2021-06-21 | 2021-09-10 | 上海仙工智能科技有限公司 | 一种基于相位相关法与因子图的激光slam方法及其可读存储介质 |
Non-Patent Citations (5)
| Title |
|---|
| 3D激光雷达SLAM算法综述;周治国等;仪器仪表学报;第42卷(第09期);13-27 * |
| Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping;Tixiao Shan等;2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS);1-8 * |
| Unified multi-modal landmark tracking for tightly coupled lidar-visual-inertial odometry;David Wisth等;IEEE Robotics and Automation Letters;第6卷(第2期);1004 - 1011 * |
| 激光雷达惯导耦合的里程计与建图方法;庞帆等;计算机应用研究;第38卷(第07期);2188-2193+2199 * |
| 高适应性激光雷达SLAM;黄瑞等;电子科技大学学报;第50卷(第01期);52-58 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113781582A (zh) | 2021-12-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
| CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
| CN112987065B (zh) | 一种融合多传感器的手持式slam装置及其控制方法 | |
| CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| CN112254729B (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
| Chen et al. | Real-time 3D mapping using a 2D laser scanner and IMU-aided visual SLAM | |
| CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
| CN112967392A (zh) | 一种基于多传感器触合的大规模园区建图定位方法 | |
| CN112652001B (zh) | 基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统 | |
| CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
| CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
| CN114397642B (zh) | 一种基于图优化的三维激光雷达与imu外参标定方法 | |
| CN115479598A (zh) | 基于多传感器融合的定位与建图方法及紧耦合系统 | |
| CN113503872B (zh) | 一种基于相机与消费级imu融合的低速无人车定位方法 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN116380039A (zh) | 一种基于固态激光雷达和点云地图的移动机器人导航系统 | |
| Niewola et al. | PSD–probabilistic algorithm for mobile robot 6D localization without natural and artificial landmarks based on 2.5 D map and a new type of laser scanner in GPS-denied scenarios | |
| CN113639722B (zh) | 连续激光扫描配准辅助惯性定位定姿方法 | |
| Wu et al. | AFLI-Calib: Robust LiDAR-IMU extrinsic self-calibration based on adaptive frame length LiDAR odometry | |
| CN115049910A (zh) | 一种基于双目视觉里程计的足式机器人建图与导航方法 | |
| CN113379915B (zh) | 一种基于点云融合的行车场景构建方法 | |
| Anousaki et al. | Simultaneous localization and map building of skid-steered robots | |
| CN116972844A (zh) | 基于ArUco阵列的移动机器人室内定位系统及方法 | |
| US11992961B2 (en) | Pose determination method, robot using the same, and computer readable storage medium | |
| CN115290090A (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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |