基于语义点云配准的激光雷达三维建图方法技术

一种基于语义点云配准的激光雷达三维建图方法，其实现步骤为：(1)获取待扫描物体的点云数据；(2)从采集的激光雷达点云数据中提取语义特征点；(3)根据三角形相似原理，对特征点进行基于语义的点云配准；(4)利用配准点云对构建三维点云图。本发明专利技术提取的特征点有极点、交点和顶点三种类别，配准过程中误匹配概率降低，匹配的复杂度降低，构建地图实时性提高，利用配准点云对计算激光雷达运动位姿的准确性提高，利用运动位姿纠正点云位置信息，投影到世界坐标系构建的地图更加精准，本发明专利技术仅利用激光雷达扫描数据完成在线地绘制高质量的三维点云图，具有精确误匹配所致误差的优点。

【技术实现步骤摘要】
基于语义点云配准的激光雷达三维建图方法
本专利技术属于通信
，更进一步涉及激光雷达测绘
中的一种基于语义点云配准的激光雷达三维建图方法。本专利技术可以应用于移动机器人对地形的测绘和对三维环境的感知，针对已知空间点位置的相邻点云进行配准，从而构建完整的三维点云图。
技术介绍
点云配准是激光雷达同步定位与三维建图应用中的关键技术。在点云配准过程中，如果激光雷达本身运动过快，则扫描期间自身造成的运动失真会导致配准错误，从而影响三维点云图的构建。此外，目前提出的点云配准方法复杂度过高，影响构建三维点云图的实时性。江苏中科院智能科学技术应用研究院在其申请的专利文献“基于三维激光雷达的无人机多场景定位建图方法”(专利申请号：201810597661.5，申请公开号：108303710A)中公开了基于三维激光雷达的无人机多场景定位建图方法。该方法包括以下步骤：将三维激光雷达采集的每一帧点云数据进行提取边缘点和平面点，依据提取的边缘点特征集及平面点特征集采用迭代最近邻算法进行配准，即找到每一个待配准点距它最近的点构成配准点对，并求解相邻两帧的位姿变换矩阵。该方法只通过提取的边缘点和平面点进行配准，存在高概率的误匹配，并且该方法因为在配准时寻找的是距离待配准点最近的点，超出阈值便会存在误差，因此该方法存在的不足之处是，误匹配概率高，在计算激光雷达的运动位姿变换矩阵时存在偏差。GreenspanM,YurickM在其发表的论文“ApproximateK-DTreeSearchforEfficientICP”(IEEE,2003:442-448)中提出了使用K-D树优化后的最近邻迭代配准方法。该方法在寻找待配准点的对应点时，选择的是K阶最近邻点而不是空间上的最近邻点，减少了点云配准过程中局部优化问题的出现。由于该方法在寻找对应的最近邻点步骤上需要遍历点云中的所有点，导致计算最优的变换矩阵复杂度为O(m3,n3)，对于百万数量级的三维点云来说，这种方法复杂度太高，因此该方法存在的不足之处是，点云配准过程中复杂度太高，对于大规模点云的配准效率难以达到实时三维建图。
技术实现思路
本专利技术的目的在于针对上述已有技术的不足，提出了一种基于语义点云配准的激光雷达三维建图方法。实现本专利技术目的的思路是，利用激光雷达扫描获得的点云数据，使用余弦定理获得在雷达坐标系中带有坐标信息的点云，将点云投影到世界坐标系中建图；取点云中的极点、交点和顶点作为语义特征点，分别将连续两帧的语义特征点中的任意三点组成三角形，取任意两个三角形，利用三角形相似原理获得配准三角形对，并根据配准三角形对的对应点都满足语义特征点类别提取配准点云对，利用配准点云对计算连续两帧间激光雷达的运动位姿，通过运动位姿更新构建的三维点云图，完成建图。从采集的激光雷达点云数据中提取语义特征点，根据三角形相似原理，对特征点进行基于语义的点云配准，本专利技术的具体步骤包括如下：(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＝(zk-(R×xk+τ))T(zk-(R×xk+τ))其中，f表示非线性优化目标函数，zk表示当前时刻第k个配准点云对在世界坐标系中的坐标值，R表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的旋转矩阵,xk表示上一时刻第k个配准点云对在雷达坐标系中的坐标值，τ表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的平移矩阵，T表示转置操作；(4b)使用高斯牛顿公式，计算当前时刻激光雷达的运动位姿；(4c)用上一时刻的雷达坐标系中点云中每一点的坐标值与更新后的旋转矩阵相乘，用其乘积与更新后的平移矩阵相加，得到当前时刻的点在世界坐标系中的坐标值，将当前时刻的所有点组成三维点云图。本专利技术与现有技术相比具有如下优点：第一，由于本专利技术从采集的激光雷达点云数据中提取语义特征点时，提取了极点、交点和顶点三种，克服了现有技术点云配准时，只提取边缘点和平面点进行配准，误匹配概率较大的问题，使得本专利技术点云配准方法可以更好的配准点云，提高了配准结果可靠性。第二，由于本专利技术根据三角形相似原理，对特征点进行基于语义的点云配准时，仅使用三角形相似原理配准，克服了现有技术点云配准时，采集的样本点较多，计算的变换矩阵复杂度太高的问题，使得本专利技术在点云配准过程中配准复杂度低于现有方法，提高了配准的效率。第三，由于本专利技术根据三角形相似原理，对特征点进行基于语义的点云配准时，利用语义特征点的类别作为配准点云的约束条件，克服了现有技术点云配准时，使用本文档来自技高网...

【技术保护点】
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＝(zk‑(R×xk+τ))T(zk‑(R×xk+τ))其中，f表示非线性优化目标函数，zk表示当前时刻第k个配准点云对在世界坐标系中的坐标值，R表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的旋转矩阵,xk表示上一时刻第k个配准点云对在雷达坐标系中的坐标值，τ表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的平移矩阵，T表示转置操作；(4b)使用高斯牛顿公式，计算当前时刻激光雷达的运动位姿；(4c)用上一时刻的雷达坐标系中点云中每一点的坐标值与更新后的旋转矩阵相乘，用其乘积与更新后的平移矩阵相加，得到当前时刻的点在世界坐标系中的坐标值，将当前时刻的所有点组成三维点云图。...

【技术特征摘要】
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＝(zk-(R×xk+τ))T(zk-(R×xk+τ))其中，f表示非线性优化目标函数，zk表示当前时刻第k个配准点云对在世界坐标系中的坐标值，R表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的旋转矩阵,xk表示上一时刻第k个配准点云对在雷达坐标系中的坐标值，τ表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的平移矩阵，T表...

【专利技术属性】
技术研发人员：孙伟，纪晓凤，
申请(专利权)人：西安电子科技大学，
类型：发明
国别省市：陕西,61