基于最大似然估计平滑的激光里程计方法、系统、装置制造方法及图纸

本发明专利技术属于机器人技术领域，具体涉及一种基于最大似然估计平滑的激光里程计方法、系统、装置，旨在解决现有直接利用原始点云信息的激光里程计方法应用固定滞后平滑时数据关联较差的问题。本方法包括：估计出激光雷达传感器当前帧坐标系在世界坐标系下的第一位姿；计算得到平滑前激光雷达传感器当前帧坐标系在世界坐标系下的位姿，作为第二位姿，如果当前帧为奇数帧，将第二位姿作为基于最大似然估计平滑的激光里程计方法当前帧对应的输出位姿，否则，将该帧加入平滑窗口，计算出平滑后的当前帧位姿，作为对应的输出位姿；进行平滑窗口更新。本发明专利技术保证了直接利用原始点云信息的激光里程计方法应用固定滞后平滑时有效的数据关联。据关联。据关联。

【技术实现步骤摘要】
基于最大似然估计平滑的激光里程计方法、系统、装置


[0001]本专利技术涉及机器人
，具体涉及一种基于最大似然估计平滑的激光里程计方法、系统、装置。

技术介绍

[0002]随着感知、控制、人工智能等技术的快速发展，机器人已经进入人们的日常生产生活中。机器人实时估计自身的位姿是其顺利完成任务的重要前提。根据机器人自身携带的传感器进行主动定位的里程计方法受到普遍关注，其中，基于视觉传感器的视觉里程计方法和基于激光雷达的激光里程计方法成为主流。与视觉里程计相比，激光里程计对光照变化更加鲁棒。激光里程计最为简单的实现策略是通过匹配激光雷达当前帧的点云和前一帧的点云来估计当前帧的位姿，称之为帧帧匹配。帧帧匹配速度较快，但存在较大的累计误差，精度较低。一种改进方案是匹配当前帧的点云和当前帧之前多帧点云构成的局部地图来优化当前帧的位姿，称之为帧图匹配。相比于帧帧匹配，帧图匹配考虑了当前帧之前多帧点云的信息，从而提高了数据关联的准确性，改善了激光里程计的精度，但同时也造成了效率的降低。为了平衡效率和精度，研究人员考虑将帧帧匹配和帧图匹配相结合，获得了实时、低漂移的激光里程计结果，代表性的方法包括LOAM(LiDAR Odometry and Mapping)、A
‑
LOAM(Advanced LiDAR Odometry and Mapping)等。在位姿估计过程中一般需要用到非线性优化算法，包括Gauss
‑
Newton算法等；同时，对于激光雷达传感器获取的三维点云数据，一种常见的方式是直接利用PCL点云库进行点云数据的处理(包括体素滤波、聚类等)，而对于涉及到的矩阵计算则通常用Eigen计算库进行处理(包括特征值分解、指数映射等)。
[0003]需要说明的是，帧帧匹配和帧图匹配相结合的激光里程计方法只优化当前帧的位姿，未考虑局部区域内里程计轨迹的一致性，难以应对长时间的位姿估计。为了解决该问题，研究人员开始考虑将固定滞后平滑(Fixed
‑
Lag Smoothing)引入到激光里程计中，通过同时优化平滑窗口内的多帧位姿来提高估计轨迹的局部一致性。对于现有直接利用原始点云信息的激光里程计方法，例如基于GICP(Generalized Iterative Closest Point)配准的激光里程计方法等，应用固定滞后平滑时数据关联较差，导致难以在平滑窗口内的多帧之间构建约束，也就难以实现固定滞后平滑提高估计轨迹局部一致性的目标。因此，如何提出一种解决上述问题的方案是本领域技术人员目前需要解决的问题。

技术实现思路

[0004]为了解决现有技术中的上述问题，即为了解决现有直接利用原始点云信息的激光里程计方法应用固定滞后平滑时数据关联较差的问题，本专利技术提出了一种基于最大似然估计平滑的激光里程计方法，该方法包括以下步骤：
[0005]步骤S10，机器人通过激光雷达传感器感知环境，获取周围环境在激光雷达传感器当前帧坐标系下的三维点云数据；基于所述三维点云数据，使用基于GICP配准的激光里程计方法估计出激光雷达传感器当前帧坐标系在世界坐标系下的位姿，作为第一位姿；
[0006]步骤S20，根据所述第一位姿，计算得到平滑前激光雷达传感器当前帧坐标系在世界坐标系下的位姿，作为第二位姿；如果当前帧为奇数帧，该帧不加入平滑窗口，将所述第二位姿作为基于最大似然估计平滑的激光里程计方法当前帧对应的输出位姿，并返回步骤S10；如果当前帧为偶数帧，将该帧加入平滑窗口，并将其对应的三维点云数据变换到世界坐标系后注册到体素地图中，跳转步骤S30；其中，所述平滑窗口由l帧激光雷达帧组成，l≤l
max
，l
max
为平滑窗口中的激光雷达帧的最大数量；
[0007]步骤S30，遍历所述体素地图中的各体素，若体素点数大于设定的数目阈值且体素对应的几何度大于设定的几何度阈值，则将该体素作为合格体素；计算每个合格体素中所有点的对数似然，通过最大化所有合格体素中所有点的对数似然和，构建基于最大似然估计平滑的位姿优化目标函数，并利用Gauss
‑
Newton算法对基于最大似然估计平滑的位姿优化目标函数求解，得到最优的位姿矫正量；基于所述最优的位姿校正量，结合所述第二位姿，计算出平滑后的当前帧位姿，作为基于最大似然估计平滑的激光里程计方法当前帧对应的输出位姿；
[0008]步骤S40，判断所述平滑窗口是否满l
max
帧，若满l
max
帧，则从平滑窗口中移除最旧帧，并在所述体素地图中边缘化其对应的点云，实现平滑窗口更新，然后返回步骤S10，不满l
max
帧则直接返回步骤S10。
[0009]在一些优选的实施方式中，如果当前帧为奇数帧且为第一帧，在获取基于最大似然估计平滑的激光里程计方法当前帧对应的输出位姿后，在返回步骤S10前，先构建体素地图，构建过程为：
[0010]使用将第一帧的三维点云数据变换到世界坐标系G下获得点云然后注册到平滑窗口的体素地图中；其中，为第一帧对应的第二位姿；注册到的体素地图的过程为：遍历所有包含中的点的体素，对于体素地图中包含点云中的点的一个体素，将该体素中属于点云的点构成的点集的点数、均值和协方差矩阵添加到该体素；
[0011]注册到体素地图中后，进行边缘化处理；边缘化处理过程如下：遍历所有包含中的点的体素，对于第j个体素，将该体素中存储的平滑窗口内第一帧所对应的点数均值和协方差矩阵分别赋值给该体素中的和然后删除和其中，和分别表示第j个体素中边缘化点构成的点集对应的点数、均值和协方差矩阵。
[0012]在一些优选的实施方式中，体素对应的几何度，其计算方法为：
[0013]遍历候选体素集合中的所有体素，计算各体素的均值μ
b
和协方差矩阵Σ
b
；其中，b表示体素索引，所述候选集合为基于体素点数大于设定的数目阈值的体素构建的集合；
[0014]使用特征值分解算法对协方差矩阵Σ
b
进行特征值分解，得到Σ
b
的三个特征值λ1,λ2,λ3；其中λ1≥λ2≥λ3；
[0015]基于三个特征值，计算几何度σ
b
：
[0016]在一些优选的实施方式中，所述最优的位姿校正量，其获取方法为：
[0017]遍历候选体素集合中的所有体素，计算各体素的几何度；将所述候选体素集合中几何度大于设定的几何度阈值的体素作为合格体素；
[0018]计算每个合格体素中所有点的对数似然，通过最大化所有合格体素中所有点的对数似然和，构建基于最大似然估计平滑的位姿优化目标函数；
[0019]对所述基于最大似然估计平滑的位姿优化目标函数中的位姿矫正量ΔT
k
添加扰动δT
k
，并利用Gauss
‑
Newton算法进行求解，得到最优扰动进而通过更新位姿矫正量ΔT
k
；其中，本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于最大似然估计平滑的激光里程计方法，其特征在于，该方法包括以下步骤：步骤S10，机器人通过激光雷达传感器感知环境，获取周围环境在激光雷达传感器当前帧坐标系下的三维点云数据；基于所述三维点云数据，使用基于GICP配准的激光里程计方法估计出激光雷达传感器当前帧坐标系在世界坐标系下的位姿，作为第一位姿；步骤S20，根据所述第一位姿，计算得到平滑前激光雷达传感器当前帧坐标系在世界坐标系下的位姿，作为第二位姿；如果当前帧为奇数帧，该帧不加入平滑窗口，将所述第二位姿作为基于最大似然估计平滑的激光里程计方法当前帧对应的输出位姿，并返回步骤S10；如果当前帧为偶数帧，将该帧加入平滑窗口，并将其对应的三维点云数据变换到世界坐标系后注册到体素地图中，跳转步骤S30；其中，所述平滑窗口由l帧激光雷达帧组成，l≤l
max
，l
max
为平滑窗口中的激光雷达帧的最大数量；步骤S30，遍历所述体素地图中的各体素，若体素点数大于设定的数目阈值且体素对应的几何度大于设定的几何度阈值，则将该体素作为合格体素；计算每个合格体素中所有点的对数似然，通过最大化所有合格体素中所有点的对数似然和，构建基于最大似然估计平滑的位姿优化目标函数，并利用Gauss
‑
Newton算法对基于最大似然估计平滑的位姿优化目标函数求解，得到最优的位姿矫正量；基于所述最优的位姿校正量，结合所述第二位姿，计算出平滑后的当前帧位姿，作为基于最大似然估计平滑的激光里程计方法当前帧对应的输出位姿；步骤S40，判断所述平滑窗口是否满l
max
帧，若满l
max
帧，则从平滑窗口中移除最旧帧，并在所述体素地图中边缘化其对应的点云，实现平滑窗口更新，然后返回步骤S10，不满l
max
帧则直接返回步骤S10。2.根据权利要求1所述的基于最大似然估计平滑的激光里程计方法，其特征在于，如果当前帧为奇数帧且为第一帧，在获取基于最大似然估计平滑的激光里程计方法当前帧对应的输出位姿后，在返回步骤S10前，先构建体素地图，构建过程为：使用将第一帧的三维点云数据变换到世界坐标系G下获得点云然后注册到平滑窗口的体素地图中；其中，为第一帧对应的第二位姿；注册到的体素地图的过程为：遍历所有包含中的点的体素，对于体素地图中包含点云中的点的一个体素，将该体素中属于点云的点构成的点集的点数、均值和协方差矩阵添加到该体素；注册到体素地图中后，进行边缘化处理；边缘化处理过程如下：遍历所有包含中的点的体素，对于第j个体素，将该体素中存储的平滑窗口内第一帧所对应的点数均值和协方差矩阵分别赋值给该体素中的和然后删除和其中，和分别表示第j个体素中边缘化点构成的点集对应的点数、均值和协方差矩阵。3.根据权利要求1所述的基于最大似然估计平滑的激光里程计方法，其特征在于，体素对应的几何度，其计算方法为：遍历候选体素集合中的所有体素，计算各体素的均值μ
b
和协方差矩阵Σ
b
；其中，b表示
体素索引，所述候选集合为基于体素点数大于设定的数目阈值的体素构建的集合；使用特征值分解算法对协方差矩阵Σ
b
进行特征值分解，得到Σ
b
的三个特征值λ1,λ2,λ3；其中λ1≥λ2≥λ3；基于三个特征值，计算几何度σ
b
：4.根据权利要求1所述的基于最大似然估计平滑的激光里程计方法，其特征在于，所述最优的位姿校正量，其获取方法为：遍历候选体素集合中的所有体素，计算各体素的几何度；将所述候选体素集合中几何度大于设定的几何度阈值的体素作为合格体素；计算每个合格体素中所有点的对数似然，通过最大化所有合格体素中所有点的对数似然和，构建基于最大似然估计平滑的位姿优化目标函数；对所述基于最大似然估计平滑的位姿优化目标函数中的位姿矫正量ΔT
k
添加扰动δT
k
，并利用Gauss
‑
Newton算法进行求解，得到最优扰动进而通过更新位姿矫正量ΔT
...

【专利技术属性】
技术研发人员：曹志强，王诚鹏，李建杰，管培育，谭民，
申请(专利权)人：中国科学院自动化研究所，
类型：发明
国别省市：