[发明专利]基于语义点云配准的激光雷达三维建图方法

权利要求书

1.一种基于语义点云配准的激光雷达三维建图方法，其特征在于，从采集的激光雷达点云数据中提取语义特征点，根据三角形相似原理，对特征点进行基于语义的点云配准，该方法的步骤包括如下：

(1)获取激光雷达点云数据：

(1a)将激光雷达固定到电机控制的旋转平台上，遵循右手定则，以激光雷达当前所在的位置为雷达坐标系的原点，激光雷达的正前方为z轴建立雷达坐标系；

(1b)利用余弦公式，计算激光雷达扫描的每个点在雷达坐标系中每个轴的坐标值，输出带有坐标信息的点云；

(1c)以激光雷达第一帧扫描时雷达坐标系的原点作为世界坐标系的原点，将第一帧带有坐标信息的点云存入世界坐标系中，构建初始三维点云图；

(2)从采集的激光雷达点云数据中提取语义特征点：

(2a)利用点距离公式，计算点云中每个点与雷达坐标系中y轴正负方向所有与其相邻的距离中最近的正负两个点之间距离和的平均值，提取点云中任意两点间距离等于平均值的点，将提取的所有点组成规范点云；

(2b)沿着与雷达坐标系中z轴平行的方向，将规范点云中与z轴平行的每条线上的点组成一条水平线；

(2c)将每条水平线上使柱面方程成立的任意两个点组成一条线段，取每条线段的中心点为极点；将每条水平线上使平面垂直线段方程成立的任意两个点组成一条线段，取每条线段的起点和终点为顶点，取每条线段与其他水平线相交的交点为交点，将所有线段的极点、顶点、交点组成垂直特征点云；

(2d)沿着与雷达坐标系中y轴平行的方向，提取垂直特征点云中和y轴平行的每条线上的点，将每次提取的点组成垂直特征线；

(2e)将每条垂直特征线中极点、顶点、交点最多的一类作为该线的类别；

(2f)提取每条垂直特征线中的起点和终点，将每条垂直特征线的类别作为所提的起点和终点的类别，将所有的起点、终点组成语义特征点云；

(3)根据三角形相似原理，对特征点进行基于语义的点云配准：

(3a)将上一时刻的语义特征点云投影到当前时刻的雷达坐标系中，将投影后的语义特征点云中的任意三个点组成三角形，将上一时刻的所有三角形组成源点云三角形集合；

(3b)将当前时刻的语义特征点云中的任意三个点组成三角形，将当前时刻的所有三角形组成生长点云三角形集合；

(3c)从生长点云三角形集合中任意提取一个三角形，从源点云三角形集合中任意提取两个三角形；

(3d)分别计算每次提取的从生长点云三角形集合中提取的一个三角形的三个顶点与源点云三角形集合中提取的两个三角形的三个顶点的对应距离，将两个距离分别求和后相除，得到商值；

(3e)判断是否提取完生长点云三角形集合中的所有三角形，若是，则执行步骤(3f)；否则，执行步骤(3c)；

(3f)提取所有商值中大于等于阈值的商值，将每个商值对应的生长点云三角形集合中提取的一个三角形，及其源点云三角形集合中提取的两个三角形中对应距离小的三角形组成一个三角形对；

(3g)提取使每个三角形对的三个对应顶点类别都相同的每个对应顶点作为配准点云对；

(4)利用配准点云对构建三维点云图：

(4a)构建非线性优化目标函数如下：

f＝(s_k-(R×u_k+τ))^T(s_k-(R×u_k+τ))

其中，f表示非线性优化目标函数，s_k表示当前时刻第k个配准点云对在世界坐标系中z轴的坐标值，R表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的旋转矩阵,u_k表示上一时刻第k个配准点云对在雷达坐标系中x轴的坐标值，τ表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的平移矩阵，T表示转置操作；

(4b)使用高斯牛顿公式，计算当前时刻激光雷达的运动位姿；

(4c)用上一时刻的雷达坐标系中点云中每一点的坐标值与更新后的旋转矩阵相乘，用其乘积与更新后的平移矩阵相加，得到当前时刻的点在世界坐标系中的坐标值，将当前时刻的所有点组成三维点云图。