[发明专利]高精地图点云位姿优化方法、装置、设备及介质

权利要求书

1.一种点云位姿优化方法，包括：

获取目标环境的至少一个点云对中每帧点云的第一位姿；所述点云对包括所述目标环境对应的多帧点云中相邻的两帧点云；

获取每个点云对中两帧点云之间的相对位姿；所述相对位姿是里程计模块估计出的；

根据所述第一位姿和所述相对位姿，对所述至少一个点云对的位姿轨迹进行优化，得到所述每帧点云的第二位姿。

2.根据权利要求1所述的点云位姿优化方法，其中，所述根据所述第一位姿和所述相对位姿，对所述至少一个点云对的位姿轨迹进行优化，得到所述每帧点云的第二位姿，包括：

对每个点云对中的第一帧点云和第二帧点云执行以下操作：

根据所述第一帧点云和所述第二帧点云之间的相对位姿、以及所述第二帧点云的第一位姿，构建当前点云对的第一位姿残差；

对所述第一位姿残差进行优化，得到优化后的所述第一帧点云和所述第二帧点云的第二位姿。

3.根据权利要求2所述的点云位姿优化方法，其中，所述根据所述第一帧点云和所述第二帧点云之间的相对位姿、以及所述第二帧点云的第一位姿，构建当前点云对的第一位姿残差，包括：

根据所述第一帧点云和所述第二帧点云之间的相对位姿，构建当前点云对的第一位姿残差中的第一残差项；

根据所述第二帧点云的第一位姿，构建当前点云对的第一位姿残差中的第二残差项。

4.根据权利要求1-3中任一项所述的点云位姿优化方法，还包括：

根据所述第二位姿和所述相对位姿，对所述位姿轨迹进行优化，得到所述每帧点云的第三位姿。

5.根据权利要求4所述的点云位姿优化方法，其中，所述根据所述第二位姿和所述相对位姿，对所述位姿轨迹进行优化，得到所述每帧点云的第三位姿，包括：

对每个点云对中的第一帧点云和第二帧点云执行以下操作：

在所述第二帧点云的第二位姿和第一位姿的距离差的绝对值大于预设距离阈值的情况下，根据所述第一帧点云和所述第二帧点云的第二位姿、所述第一帧点云和所述第二帧点云之间的相对位姿、以及所述第二帧点云的第一位姿，构建当前点云对的第二位姿残差；

对所述第二位姿残差进行优化，得到优化后的所述第一帧点云和所述第二帧点云的第三位姿。

6.根据权利要求5所述的点云位姿优化方法，其中，所述第一帧点云和所述第二帧点云的第二位姿、所述第一帧点云和所述第二帧点云之间的相对位姿、以及所述第二帧点云的第一位姿，构建当前点云对的第二位姿残差，包括：

根据所述第一帧点云和所述第二帧点云的第二位姿、以及所述第一帧点云和所述第二帧点云之间的相对位姿，构建当前点云对的第二位姿残差中的第一残差项；

根据所述第二帧点云的第一位姿和第二位姿，构建当前点云对的第二位姿残差中的第二残差项。

7.根据权利要求4所述的点云位姿优化方法，还包括：

确定所述第三位姿形成的位姿轨迹的平滑度是否满足预设的平滑度条件；

在所述平滑度不满足所述平滑度条件的情况下，对所述第三位姿形成的位姿轨迹继续进行至少一次优化，直至优化后的位姿轨迹的平滑度满足所述平滑度条件。

8.一种点云位姿优化装置，包括：

初始位姿获取模块，用于获取目标环境对应的至少一个点云对中每帧点云的第一位姿；所述点云对包括所述目标环境对应的多帧点云中相邻的两帧点云；

相对位姿获取模块，用于获取每个点云对中两帧点云之间的相对位姿；所述相对位姿是里程计模块估计出的；

第一优化模块，用于根据所述第一位姿和所述相对位姿，对所述至少一个点云对的位姿轨迹进行优化，得到所述每帧点云的第二位姿。