点云配准方法、智能设备及存储介质与流程

本申请涉及自动驾驶，具体涉及一种点云配准方法、智能设备及存储介质。

背景技术：

1、点云配准是求解两帧点云之间相对位姿的一种计算方法，可以根据匹配好的两帧点云计算获得两帧点云之间的相对位姿。根据计算过程中约束形式的不同，可以分为点对点的配准、点对分布(p2d：point to distribution))和分布对分布(d2d：distribution todistribution)的配准。不同约束形式的配准在位姿求解精度和算力上存在差异。这里的distribution一般表示为点云的分辨率格子内的点的高斯分布。其中d2d方法，考虑了两帧点云之间局部形状一致性的约束，精度会更高些，因此业内通常采用d2d形式的约束来迭代优化初始位姿，以此来提高点云配准的精度。

2、但是业内采用的d2d约束形式的点云配准方法，为了避免直接计算雅克比矩阵和海森堡矩阵，一般是采用第三方非线性优化库（比如google开发的ceres）进行求导计算，同时ceres这种通用的非线性优化库，为了通用性，有一些额外的计算，因此导致使用ceres等第三方非线性优化库求解的算力一般比较大。

3、由此可见，现有技术的点云配准方法存在精度低或对算力要求较高的缺陷。

技术实现思路

1、为了克服现有技术中存在的缺陷，本申请提出了一种轻量级的、高精度的点云配准方案。

2、在第一方面，本申请提供一种点云配准方法，所述方法包括：

3、获取源点云、目标点云以及源点云与目标点云之间的初始位姿；

4、根据所述初始位姿将所述源点云转换到所述目标点云所在坐标系；

5、基于所述目标点云所在坐标系下的所述源点云的高斯分布和所述目标点云的高斯分布构建代价函数；

6、基于所述代价函数计算雅克比矩阵和海森堡矩阵；

7、基于所述雅克比矩阵和所述海森堡矩阵对所述代价函数进行优化迭代获得位姿增量。

8、优选地，所述根据所述初始相对位姿将所述源点云转换到所述目标点云所在坐标系，具体包括：

9、将源点云进行栅格划分得到多个源栅格，根据所述初始位姿，将所述源栅格投影到所述目标点云所在坐标系，并在目标点云中确定与所述源栅格对应的目标栅格。

10、优选地，所述基于所述目标点云所在坐标系下的所述源点云的高斯分布和所述目标点云的高斯分布构建代价函数具体包括：

11、根据所述源栅格的高斯分布和所述目标栅格的高斯分布分别确定源栅格的协方差和目标栅格的协方差；

12、基于位姿参数构建所述源栅格与所述目标栅格的匹配关系函数；所述位姿参数包括旋转矩阵和平移向量；

13、根据所述匹配关系函数、所述源栅格的协方差和所述目标栅格的协方差构建关于位姿的代价函数。

14、优选地，所述基于所述雅克比矩阵和所述海森堡矩阵对所述代价函数进行优化迭代获得位姿增量具体为：

15、基于所述雅克比矩阵和所述海森堡矩阵，利用牛顿法迭代求解所述代价函数直至满足收敛条件，获得位姿增量。

16、进一步地，所述迭代求解过程还包括：基于所述海森堡矩阵、所述雅克比矩阵，以及预先设定的匹配栅格点对噪声确定位姿协方差。

17、进一步地，所述迭代求解过程还包括：当不满足收敛条件时，判断所述位姿增量是否大于预设阈值；是则基于所述位姿增量更新所述初始位姿，并基于更新后的所述初始位姿对所述源点云和所述目标点云重新进行匹配；否则基于所述位姿增量重新计算海森堡矩阵和雅可比矩阵，并进行下一次迭代。

18、优选地，所述基于所述位姿增量重新计算海森堡矩阵和雅可比矩阵具体包括：

19、基于所述位姿增量更新高斯分布参数，所述高斯分布参数包括均值和方差；

20、基于更新后的高斯分布参数重新计算海森堡矩阵和雅克比矩阵。

21、优选地，所述方法还包括：判断所述海森堡矩阵是否符合正定矩阵，并在所述海森堡矩阵不符合正定矩阵时对所述海森堡矩阵进行修订的步骤。

22、进一步地，所述判断所述海森堡矩阵是否符合正定矩阵，并在所述海森堡矩阵不符合正定矩阵时对所述海森堡矩阵进行修正的步骤具体包括：

23、对所述海森堡矩阵进行奇异值分解获得多个奇异值，从所述多个奇异值中选定最小奇异值，判断所述最小奇异值小于预设值，是则所述海森堡矩阵为非正定矩阵，否则所述海森堡矩阵为正定矩阵；

24、当所述海森堡矩阵为非正定矩阵时，将所述多个奇异值中每个奇异值都增加所述预设值得到修改后的多个奇异值，并基于修改后的多个奇异值重新确定海森堡矩阵。

25、优选地，所述源点云具体为基于激光雷达采集获取的实时点云数据；所述目标点云具体为与所述源点云相同的实时点云数据形态，或者具体为划分为多个目标栅格的离线点云图像序列形态。

26、优选地，所述初始位姿具体为基于全球卫星导航系统采集的信号获得，或者是基于一个或多个车辆传感器采集的信号获得。

27、在第二方面，本申请提供一种智能设备，该智能设备包括处理器和存储装置，所述存储装置适于存储多条程序代码，所述程序代码适于由所述处理器加载并运行以执行上述点云配准方法的技术方案中任一项技术方案所述的点云配准方法。

28、在第三方面，本申请提供一种计算机可读存储介质，该计算机可读存储介质其中存储有多条程序代码，所述程序代码适于由处理器加载并运行以执行点云配准方法的技术方案中任一项技术方案所述的点云配准方法。

29、本申请上述一个或多个技术方案，至少具有如下一种或多种有益效果：在实施本申请的技术方案中，首先根据所述初始位姿将所述源点云转换到所述目标点云所在坐标系，然后基于目标点云所在坐标系下的所述源点云的高斯分布和所述目标点云的高斯分布构建代价函数，基于所述代价函数计算雅克比矩阵和海森堡矩阵；基于所述雅克比矩阵和所述海森堡矩阵对所述代价函数进行优化迭代获得位姿增量。本申请基于源点云的高斯分布和目标点云的高斯分布，使用雅克比矩阵和海森堡矩阵实现整个非线性优化迭代过程，较大程度上降低了算力。并且本申请的点云配准方法在位姿协方差计算中考虑了匹配栅格点对噪声，可以较为精确地描述位姿计算结果的不确定度。

技术特征：

1.一种点云配准方法，其特征在于，所述方法包括：

2.根据权利要求1所述的方法，其特征在于，所述根据所述初始相对位姿将所述源点云转换到所述目标点云所在坐标系，具体包括：

3.根据权利要求2所述的方法，其特征在于：所述基于所述目标点云所在坐标系下的所述源点云的高斯分布和所述目标点云的高斯分布构建代价函数具体包括：

4.根据权利要求1所述的方法，其特征在于：所述基于所述雅克比矩阵和所述海森堡矩阵对所述代价函数进行优化迭代获得位姿增量具体为：

5.根据权利要求4所述的方法，其特征在于：所述迭代求解过程还包括：基于所述海森堡矩阵、所述雅克比矩阵，以及预先设定的匹配栅格点对噪声确定位姿协方差。

6.根据权利要求4或5所述的方法，其特征在于，所述迭代求解过程还包括：

7.根据权利要求6所述的方法，其特征在于，所述基于所述位姿增量重新计算海森堡矩阵和雅可比矩阵具体包括：

8.根据权利要求1所述的方法，其特征在于，所述方法还包括：判断所述海森堡矩阵是否符合正定矩阵，并在所述海森堡矩阵不符合正定矩阵时对所述海森堡矩阵进行修订的步骤。

9.根据权利要求8所述的方法，其特征在于，所述判断所述海森堡矩阵是否符合正定矩阵，并在所述海森堡矩阵不符合正定矩阵时对所述海森堡矩阵进行修正的步骤具体包括：

10.根据权利要求1所述的方法，其特征在于：所述源点云具体为基于激光雷达采集获取的实时点云数据；所述目标点云具体为与所述源点云相同的实时点云数据形态，或者具体为划分为多个目标栅格的离线点云图像序列形态。

11.根据权利要求1所述的方法，其特征在于：所述初始位姿具体为基于全球卫星导航系统采集的信号获得，或者是基于一个或多个车辆传感器采集的信号获得。

12.一种智能设备，其特征在于，包括：

13.一种计算机可读存储介质，其中存储有多条程序代码，其特征在于，所述程序代码适于由处理器加载并运行以执行权利要求1至8中任一项所述的点云配准方法。

技术总结
本申请公开的点云配准方法、智能设备及存储介质，涉及自动驾驶技术领域。该方法包括：获取源点云、目标点云以及源点云与目标点云之间的初始位姿；根据所述初始位姿将所述源点云转换到所述目标点云所在坐标系；基于所述目标点云所在坐标系下的所述源点云的高斯分布和所述目标点云的高斯分布构建代价函数；基于所述代价函数计算雅克比矩阵和海森堡矩阵；基于所述雅克比矩阵和所述海森堡矩阵对所述代价函数进行优化迭代获得位姿增量。本申请的点云配准方法基于源点云的高斯分布和目标点云的高斯分布，使用雅克比矩阵和海森堡矩阵实现整个非线性优化迭代过程，较大程度上降低了算力。

技术研发人员：游振兴,李琦,孙立,袁弘渊,任少卿
受保护的技术使用者：安徽蔚来智驾科技有限公司
技术研发日：
技术公布日：2024/3/12