[发明专利]基于柱坐标的NDT单帧点云配准定位方法

说明书

本发明公开了一种基于柱坐标的NDT单帧点云配准定位方法，包括如下步骤：S1、确定配准目标区域，S2、划分柱坐标格栅，S3、向栅格中分配地图点云，S4、生成各格栅概率密度函数，S5、单帧点云预转换，S6、单帧点云分配栅格，S7、计算定位得分，S8、非线性优化求解，S9、迭代求最优解；本发明根据单帧点云的分布特性设计一种柱坐标栅格，相比与传统立方体栅格，该方法可以让单帧点云更加均匀的分布在各个栅格中，大大减小了无效栅格的数量，提高了定位精度；同时，柱坐标栅格可以用更少的栅格覆盖更大的范围，减小了查询栅格时的资源消耗。

技术领域

本发明涉及一种定位导航技术领域，尤其涉及一种基于柱坐标的NDT单帧点云配准定位方法。

背景技术

目前，NDT定位都基于笛卡尔坐标系均匀地划分定位栅格，当使用机械式激光雷达的单帧(360du)点云与地图匹配时，由于机械激光雷达的扫描特性，会存在点云在各栅格中分布极不均匀的问题，导致定位精度下降。

发明内容

本发明所要解决的技术问题是提供一种点云分布均匀、无效栅格较少、定位精度较高的基于柱坐标的NDT单帧点云配准定位方法。

为解决上述技术问题，本发明的技术方案是：基于柱坐标的NDT单帧点云配准定位方法，包括如下步骤：

S1、确定配准目标区域：确定柱坐标的坐标中心O；

S2、划分柱坐标格栅：以坐标中心O为圆心，以激光雷达扫描距离为半径做圆柱，截取定位地图在圆柱内的单帧点云为配准的目标点云Q，按照(α，r，h)三个维度将目标点云Q划分为多个栅格；

S3、向栅格中分配地图点云：根据目标点云Q中所有点的三维坐标(x，y，z)求出对应的柱坐标(α，r，h)，进而找到对应的栅格；

S4、生成各格栅概率密度函数：计算各个格栅的三维正态分布参数；

S5、单帧点云预转换：将待配准的单帧点云通过标定参数转换至惯导坐标系中，然后再通过初始姿态转换至定位地图坐标系中；

S6、单帧点云分配栅格：根据步骤S3中的定义找到单帧点云中每个点所在的栅格；

S7、计算定位得分：根据步骤S4计算的正态分布参数，计算待配准的单帧点云中每个点的概率密度，将每个点计算出的结果相加得到柱坐标NDT配准得分score；

S8、非线性优化求解：使用高斯牛顿法对socre函数进行优化，找到最优姿态R与最优位置T使得socre的值最大；

S9、迭代求最优解：将计算出的R、T带入至步骤S5中，重复进行步骤S5-S7，直到RT的变化量小于阈值为止，最终获得的R、T就是单帧点云定位的结果。

作为优选的技术方案，步骤S3中栅格计算方法为：

h＝z

其中(x，y，z)为三维坐标，(α，r，h)为柱坐标。

作为优选的技术方案，步骤S4中格栅的三维正态分布参数的具体计算方法为：

其中，p_i为栅格中第i个点的坐标，p_q为均值坐标，∑为协方差矩阵。

作为优选的技术方案，步骤S5中，坐标变换公式为：

P'＝RP+T

其中，P'为变换后点云。

作为优选的技术方案，步骤S7中柱坐标NDT配准得分score的计算方法为：