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

权利要求书

1.基于柱坐标的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就是单帧点云定位的结果。

2.如权利要求1所述的基于柱坐标的NDT单帧点云配准定位方法，其特征在于：步骤S3中栅格计算方法为：

h＝z

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

3.如权利要求2所述的基于柱坐标的NDT单帧点云配准定位方法，其特征在于：步骤S4中格栅的三维正态分布参数的具体计算方法为：

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

4.如权利要求3所述的基于柱坐标的NDT单帧点云配准定位方法，其特征在于：步骤S5中，坐标变换公式为：

P'＝RP+T

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

5.如权利要求4所述的基于柱坐标的NDT单帧点云配准定位方法，其特征在于：步骤S7中柱坐标NDT配准得分score的计算方法为：

score＝∑PDF

其中，p_i为P′中的点。