基于语义点云配准的激光雷达三维建图方法与流程

本发明属于通信
技术领域：
，更进一步涉及激光雷达测绘
技术领域：
中的一种基于语义点云配准的激光雷达三维建图方法。本发明可以应用于移动机器人对地形的测绘和对三维环境的感知，针对已知空间点位置的相邻点云进行配准，从而构建完整的三维点云图。
背景技术：
：点云配准是激光雷达同步定位与三维建图应用中的关键技术。在点云配准过程中，如果激光雷达本身运动过快，则扫描期间自身造成的运动失真会导致配准错误，从而影响三维点云图的构建。此外，目前提出的点云配准方法复杂度过高，影响构建三维点云图的实时性。江苏中科院智能科学技术应用研究院在其申请的专利文献“基于三维激光雷达的无人机多场景定位建图方法”(专利申请号：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是本发明的流程图；图2是本发明提取点云的语义特征点步骤的示意图；图3是本发明仿真一的仿真图；图4是本发明仿真二的仿真图。具体实施方式下面结合附图对本发明作进一步的描述。参照图1，对本发明的具体步骤作进一步的描述。步骤1，获取激光雷达点云数据。将激光雷达固定到电机控制的旋转平台上，遵循右手定则，以激光雷达当前所在的位置为雷达坐标系的原点，激光雷达的正前方为z轴建立雷达坐标系。利用余弦公式，计算激光雷达扫描的每个点在雷达坐标系中每个轴的坐标值，输出带有坐标信息的点云。所述的余弦公式如下：al＝bl×cosθl其中，al表示点云中第l个点在雷达坐标系的三个轴中坐标值对应轴的坐标值，bl表示点云中第l个点到原点的距离，cos表示取余弦操作，θl表示点云中第l个点和原点的连线与雷达坐标系的三个轴中对应轴的夹角。以激光雷达第一帧扫描时雷达坐标系的原点作为世界坐标系的原点，将第一帧带有坐标信息的点云存入世界坐标系中，构建初始三维点云图。步骤2，从采集的激光雷达点云数据中提取语义特征点。利用点距离公式，计算点云中每个点与雷达坐标系中y轴正负方向所有与其相邻的距离中最近的正负两个点之间距离和的平均值，提取点云中任意两点间距离等于平均值的点，将提取的所有点组成规范点云。所述的点距离公式如下：其中，hi表示点云中第i个点与在雷达坐标系中y轴正负方向上所有与其相邻的距离中最近的正负两个点之间距离和的平均值，(xi,yi,zi)表示点云中第i个点在雷达坐标系中的坐标值，(xi,yi+1,zi)、(xi,yi-1,zi)分别表示在雷达坐标系中y轴正负方向上所有与其相邻的距离中最近的两个点在雷达坐标系中的坐标值。沿着与雷达坐标系中z轴平行的方向，将规范点云中与z轴平行的每条线上的点组成一条水平线。将每条水平线上使柱面方程成立的任意两个点组成一条线段，取每条线段的中心点为极点；将每条水平线上使平面垂直线段方程成立的任意两个点组成一条线段，取每条线段的起点和终点为顶点，取每条线段与其他水平线相交的交点为交点，将所有线段的极点、顶点、交点组成垂直特征点云。所述的柱面方程如下：||(p-q)×ca||-r＝0其中，||·||表示取距离操作，p表示水平线中第p个点在雷达坐标系中的坐标值，q表示水平线中第q个点在雷达坐标系中的坐标值，ca表示雷达坐标系中y轴方向的单位长度坐标值；r表示在雷达坐标系中水平线中第p个点与第q个点之间的距离。所述的平面垂直线段方程如下：其中，xm表示水平线中第m个点在雷达坐标系中x轴的坐标值，xn表示水平线中第n个点在雷达坐标系中x轴的坐标值，a表示水平线在雷达坐标系中x轴方向上的方向分量；ym表示水平线中第m个点在雷达坐标系中y轴的坐标值，yn表示水平线中第n个点在雷达坐标系中y轴的坐标值，b表示水平线在雷达坐标系中y轴方向上的方向分量；zm表示水平线中第m个点在雷达坐标系中z轴的坐标值，zn表示水平线中第n个点在雷达坐标系中z轴的坐标值，c表示水平线在雷达坐标系中z轴方向上的方向分量。沿着与雷达坐标系中y轴平行的方向，提取垂直特征点云中和y轴平行的每条线上的点，将每次提取的点组成垂直特征线。将每条垂直特征线中极点、顶点、交点最多的一类作为该线的类别。提取每条垂直特征线中的起点和终点，将每条垂直特征线的类别作为所提的起点和终点的类别，将所有的起点、终点组成语义特征点云。下面结合图2对本步骤做进一步的说明图2(a)是本发明提取的语义特征点示意图，图2(b)是本发明的特征垂直线示意图。图2(a)中最底端矩形框内部的点为极点；最底端矩形框内部整体形状如圆柱，使柱面方程成立的任意两个点，一个分布在圆柱轴上，另一个分布在圆柱体表面，将两点连成的线段取中点便是极点。图2(a)中最右端矩形框内部的点为交点；使平面垂直线段方程成立的任意两个点都位于水平线上，取水平线的顶点与其他水平线相交的交点便是交点。图2(a)中最左端矩形框内部的点为顶点，使平面垂直线段方程成立的任意两个点都位于水平线上，取水平线中与其他水平线不相交的顶点便是顶点。图2(b)中的每一条特征垂直线都是沿着与雷达坐标系中y轴平行的方向，提取图2(a)中和y轴平行的每条线上的点，将每次提取的点组成垂直特征线。步骤3，根据三角形相似原理，对特征点进行基于语义的点云配准。(3.1)将上一时刻的语义特征点云投影到当前时刻的雷达坐标系中，将投影后的语义特征点云中的任意三个点组成三角形，将上一时刻的所有三角形组成源点云三角形集合。(3.2)将当前时刻的语义特征点云中的任意三个点组成三角形，将当前时刻的所有三角形组成生长点云三角形集合。(3.3)从生长点云三角形集合中任意提取一个三角形，从源点云三角形集合中任意提取两个三角形。(3.4)分别计算每次提取的从生长点云三角形集合中提取的一个三角形的三个顶点与源点云三角形集合中提取的两个三角形的三个顶点的对应距离，将两个距离分别求和后相除，得到商值。(3.5)判断是否提取完生长点云三角形集合中的所有三角形，若是，则执行步骤(3.6)；否则，执行步骤(3.3)。(3.6)提取所有商值中大于等于阈值的商值，将每个商值对应的生长点云三角形集合中提取的一个三角形，及其源点云三角形集合中提取的两个三角形中对应距离小的三角形组成一个三角形对。所述的阈值是指在小于等于1的范围内取的一个任意值。(3.7)提取使每个三角形对的三个对应顶点类别都相同的每个对应顶点作为配准点云对。步骤4，利用配准点云对构建三维点云图。构建非线性优化目标函数如下：f＝(zk-(r×xk+τ))t(zk-(r×xk+τ))其中，f表示非线性优化目标函数，zk表示当前时刻第k个配准点云对在世界坐标系中的坐标值，r表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的旋转矩阵,xk表示上一时刻第k个配准点云对在雷达坐标系中的坐标值，τ表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的平移矩阵，t表示转置操作。所述的激光雷达运动位姿的旋转矩阵是指，在t-1时刻和t时刻对激光雷达运动旋转角度取正余弦操作后获得的3×3矩阵。所述的激光雷达运动位姿的平移矩阵是指，在t-1时刻和t时刻对激光雷达运动平移量取转置操作后获得的3×1矩阵。使用高斯牛顿公式，计算当前时刻激光雷达的运动位姿。所述的高斯牛顿公式如下：t′←[t-(jt×j)-1jt(zk-(r×xk+τ))]其中，t′表示当前时刻更新后的激光雷达运动位姿，t表示当前时刻的激光雷达运动位姿，←表示更新操作，j表示第k个配准点云对的对应点误差对上一时刻点的导数，-1表示取逆操作。用上一时刻的雷达坐标系中点云中每一点的坐标值与更新后的旋转矩阵相乘，用其乘积与更新后的平移矩阵相加，得到当前时刻的点在世界坐标系中的坐标值，将当前时刻的所有点组成三维点云图。下面结合仿真实验对本发明的效果做进一步的说明1.仿真条件：本发明的仿真实验采用激光雷达hokuyoutm-30lx、电机dynamixelmx-28分别通过接口usb连接到笔记本电脑上，在ubuntu14.04的ros上进行的。2.仿真内容与结果分析：本发明的仿真实验是在室内，对两种不同的场景分别进行三维建图仿真。仿真实验1：在长走廊场景下，使用激光雷达构建三维点云图，仿真如图3所示。图3(a)是长走廊环境实景图，图3(b)是激光雷达扫描时某一帧的生长点云图，图3(c)是激光雷达扫描时当前帧的三维点云图，图3(d)是从后向前观察的激光雷达构建的三维点云图，图3(e)是激光雷达运动位姿图，箭头所指方向代表激光雷达坐标系中x轴方向。由图3(d)可见，点云中突出部分与周围平整处为内凹的门的轮廓，最前方为激光雷达构建的长走廊的大门。使用三维点云图中长走廊的大门两边点云对应的点坐标相减得到大门的宽度作为测量值，长走廊实景图最前方大门的实际宽度作为实际值。仿真实验2：在旋转楼梯场景下，使用激光雷达构建三维点云图，仿真如图4所示。图4(a)是旋转楼梯实景图，图4(b)是激光雷达扫描时某一帧的生长点云图，图4(c)是激光雷达扫描时当前帧的三维点云图，图4(d)是从后向前观察的三维点云图，图4(e)是激光雷达运动位姿图，箭头所指方向代表激光雷达坐标系中x轴方向。由图4(d)可见，激光雷达构建的三维点云最底端为楼梯开始处，旋转向上到楼梯结束处。从开始到结束两处的点云对应点坐标取长度得到激光雷达构建的旋转楼梯的楼梯长度作为测量值，旋转楼梯实景图的楼梯实际长度作为实际值。将仿真实验1和仿真实验2中的测量值与真实值进行比较，如表1所示。表1.三维点云图中墙壁距离测量值与实际值对比表测试场景测量值(cm)实际值(cm)相对误差大门200.6203.51.4％走廊183.2186.31.6％由表1所示的实验结果可以看出在室内环境中，构建的三维点云图的精度约为2％，说明本发明可以精确估计激光雷达的运动轨迹，并实时地构建高质量的三维点云图。当前第1页12