本发明公开一种基于激光雷达的三维建图的方法，主要解决现有的2D激光雷达在不借助其他传感器辅助的情况下，难以实时绘制精细三维地图的问题。其实现步骤如下：首先获得带有坐标信息的点云；然后使用双边滤波算法对点云进行滤波处理；接着由激光雷达测距算法计算激光雷达点云中每一个特征点的位移；最后用绘图算法将扫描到的点云配准到世界坐标系上，构成三维点云图。本发明仅使用激光雷达和电机组合的平台而不使用其他传感器辅助即可完成实时地构建高质量的三维点云图，可用于移动机器人对地形数据的测绘和对未知环境的探知。
1.一种基于激光雷达的三维建图的方法，包括如下步骤：
(1)将激光雷达安装在由电机控制的旋转平台上，且以激光雷达当前所在的位置为雷达坐标系的原点，激光雷达的正前方为x轴，雷达坐标系遵循右手定则；
(2)激光雷达输出扫描到的点到原点的距离d、点与原点的连线l和zOy平面的夹角θ1，电机输出l与x轴的夹角θ2；
(3)使用(2)中扫描到的点到原点的距离d、l与zOy平面的夹角θ1和l与x轴的夹角θ2，解算激光雷达点云中每一个点在激光雷达坐标系中的坐标，输出带有坐标信息的点云Vn；
(4)使用双边滤波算法对点云Vn进行滤波处理，输出滤波后的点云Cn；
(5)用激光雷达测距算法对滤波后的点云Cn进行处理，即计算两次连续扫描之间激光雷达点云中每一个特征点的位移，按如下步骤进行：(5.1)令α是点云Cn中的点，Μ是点云Cn中所有点的集合,定义变量h作为α的局部平滑度:其中， 是双边滤波后的点α的坐标， 是点云Cn第i个点的坐标，i∈(1,n)，n为Cn中所有点的个数；
(5.2)对点云Cn中每一点的h值进行排序，将h值最大的选为边缘特征点，h值最小的选为平面特征点；
(5.3)令tn为第n次扫描开始的时刻，在第n-1次扫描结束时，将(n-2,n-1)期间感知的点云Cn-1投射到时刻tn处，得到投射后的点云(5.4)在第n次扫描期间，利用 和Cn一起估计所有特征点的位移量与旋转角度：(5.4a)将(5.2)的边缘特征点的集合设为An，令b是An上的点，在点云 中找到点b的最邻近点设为j，在点云Cn中找到点b的最邻近点设为v，用j和v确定一条边缘线(j,v)，计算b点到边缘线(j,v)的对应距离，即边缘特征点的位移量dA：其中， 和 分别是点b、j和v的在雷达坐标系中的坐标；
令边缘线(j,v)的中点是点e，雷达坐标系的原点设为点o，将线段bo的长度设为lbo，线段eo的长度设为leo，线段be的长度设为lbe，通过余弦定理计算出bo与eo的夹角，即边缘特征点旋转角度(5.4b)将(5.2)的平面特征点的集合设为Bn，令r为Bn上的点，在点云 中找到点r的最邻近点设为η，在点云Cn中找到r的两个最邻近点设为u和m，用η、u和m确定平面块(η,u,m)，计算r点到平面块(η,u,m)的对应距离，即平面特征点的位移量dB：其中， 和 分别是点r、y、u和m在雷达坐标系中的坐标；
令平面块(η,u,m)的中心是点g，将线段go的长度设为lgo，线段ro的长度设为lro，线段gr的长度设为lgr，通过余弦定理计算出go与ro的夹角，即平面特征点的旋转角度(6)令开始扫描时激光雷达所在的位置为世界坐标系的原点，利用激光雷达绘图算法将Cn配准到世界坐标系上，构成三维点云图。
2.根据权利要求1所述的方法，其中步骤(4)中使用双边滤波算法对点云Vn进行滤波处理，按如下步骤进行：(4.1)建立k-邻域：
令点云Vn中所有点的集合为：Cloud2＝{p1,p2,…pi…,pn}，其中pi为点云Vn中第i个点，i∈(1,n)，n为点云Vn中所有点的个数；
假设有一点p∈Cloud2，将与p点最邻近的k个点设为p的k-邻域k(p)；
(4.2)使用最小二乘法在k-邻域k(p)内构造一个平面，称为点p在k(p)上的切平面S(p)；
(4.3)计算切平面S(p)的法向量 用 作为S(p)的法矢量估计；
(4.4)将点云Vn中每一个点的坐标通过下式进行滤波：
其中， 表示滤波后的点坐标；f表示Vn中需要改正的点的坐标；表示其对应的法矢量估计；L为双边滤波因子，式中，δc为点的邻域半径，δs为邻域点的标准偏差， 是δc的高斯核函数，是δs的高斯核函数，fi是Vn中第i点的坐标。
3.根据权利要求1所述的方法，其中步骤(6)中利用激光雷达绘图算法将Cn配准到世界坐标系上，按如下步骤进行：(6.1)令第n-1次扫描之前世界坐标系中的点云为Dn-1，将点云Cn转换到世界坐标系中，用Dn表示；
(6.2)利用(5.4a)中得到的边缘特征点的位移量dA和边缘特征点的旋转角度 将每一个边缘特征点沿着其旋转角度的方向减去它的位移量，去掉Dn中与Dn-1重合的边缘特征点，输出点云Kn；
(6.3)利用(5.4b)中得到的平面特征点的位移量dB和平面特征点的旋转角度 将每一个平面特征点沿着其旋转角度的方向减去它的位移量，去掉Dn中与Dn-1重合的平面特征点，输出点云Jn；
(6.4)合并(6.2)和(6.3)中输出的点云Kn和点云Jn，构成三维点云图。
基于激光雷达的三维建图的方法
技术领域
[0001] 本发明属于激光雷达点云处理技术领域，特别涉及激光雷达测绘领域中的一种2D 激光雷达实时地进行三维绘图的方法，可用于移动机器人对地形数据的测绘和对未知环境的探知。
背景技术
[0002] 由于激光雷达对环境光照和场景中的光学纹理不敏感，目前大多数应用使用激光雷达进行定位及绘图。当激光雷达的扫描速率远高于其自身运动时，扫描期间由于运动造成的失真通常可以忽略，在这种情况下，可用标准的ICP算法对激光雷达进行三维建图。然而，在移动的平台上使用激光雷达三维建图是困难的，如果激光雷达的自身运动较快，由于扫描期间激光雷达的运动造成的失真就会严重影响建图。因此在构建点云时，必须同步解算扫描期间移动平台的运动轨迹，否则点云结构将严重失真并且可能无法识别。
[0003] J.Hertzberg等人提出了一种采用频繁停止平台运动的方法以进行固定扫描，见 Nuchter,A.,Lingemann,K.,Hertzberg,J.,&Surmann,H.6D SLAM-3D mapping outdoor environments[J].Journal of Field Robotics,2007.16(4):337-340。这种方法对于大多数的实际应用会很大程度的降低效率。
[0004] 另一类减少失真的方法是使用其他传感器，比如用惯性测量单元IMU来提供速度和加速度的测量。M.Bosse和R.Zlot等人的方法即使用了IMU辅助建图，见Michael Bosse, Robert Zlot,&Paul Flick.Zebedee:Design of a Spring-Mounted 3-D Range Sensor with Application to Mobile Mapping[J].IEEE Transactions on Robotics,2012 28(5):1104-1119。他们的平台由2D激光雷达和由弹簧连接到手杆的IMU组成，每次手动点击设备记为一段扫描。该方法使用批量处理优化每段扫描的点云数据集进行绘图，IMU的测量用于记录激光雷达在每段扫描之间的运动轨迹。但是该方法由于需要批量处理来构建高质量的地图，无法实现实时建图，不适用于需要在线建图的应用。
发明内容
[0005] 本发明的目的在于针对上述已有的技术的不足，提出一种基于激光雷达的三维建图方法，以实时地构建高质量的三维点云图。
[0006] 本发明的基本思路是避免借助其他传感器的辅助，仅使用2D激光雷达和电机实现三维建图，其方案是：首先获得带有坐标信息的点云Vn，然后使用双边滤波算法对点云进行滤波，接着激光雷达测距算法估算激光雷达点云中每一个特征点的位移；最后绘图算法将扫描到的点云配准到世界坐标系上并输出三维点云图。其实现方案包括如下：
[0007] (1)将激光雷达安装在由电机控制的旋转平台上，且以激光雷达当前所在的位置为雷达坐标系的原点，激光雷达的正前方为x轴，雷达坐标系遵循右手定则；
[0008] (2)激光雷达输出扫描到的点到原点的距离d、点与原点的连线l和zOy平面的夹角θ1，电机输出l与x轴的夹角θ2；
[0009] (3)使用(2)中扫描到的点到原点的距离d、l与zOy平面的夹角θ1和l与x轴的夹角θ2，解算激光雷达点云中每一个点在激光雷达坐标系中的坐标，输出带有坐标信息的点云Vn；
[0010] (4)使用双边滤波算法对点云Vn进行滤波处理，输出滤波后的点云Cn；
[0011] (5)用激光雷达测距算法对滤波后的点云Cn进行处理，即计算两次连续扫描之间激光雷达点云中每一个特征点的位移；
[0012] (6)令开始扫描时激光雷达所在的位置为世界坐标系的原点，利用激光雷达绘图算法将Cn配准到世界坐标系上，构成三维点云图。
[0013] 本发明具有如下优点：
[0014] 1)本发明由于使用激光雷达进行三维建图，解决了视觉里程计对环境光照和场景中的光学纹理敏感的问题。
[0015] 2)本发明由于在三维建图时不需要任何传感器的辅助，仅使用激光雷达和电机组合的平台即可完成三维建图，因此能够极大的降低成本。
[0016] 3)本发明由于提取激光雷达点云中的特征点较少，所以三维建图的方法运算量较低，具有良好的实时性，适合激光雷达在线地对周围环境三维重建。
附图说明
[0017] 图1是本发明的实现总流程图；
[0018] 图2是本发明中提取出来的匹配特征点示意图；
[0019] 图3是用本发明平台静止不动情况下，使用激光雷达构建的三维点云图；
[0020] 图4是本发明在以0.5m/s的速度移动的情况下使用激光雷达构建的三维点云图。
[0021] 以下结合附图对本发明作进一步详细描述。
具体实施方式
[0022] 参照图1，本发明的实现步骤如下：
[0023] 步骤1，获得带有坐标信息的点云Vn。
[0024] (1a)将激光雷达安装在由电机控制的旋转平台上，且以激光雷达当前所在的位置为雷达坐标系的原点，激光雷达的正前方为x轴，雷达坐标系遵循右手定则；
[0025] (1b)激光雷达输出扫描到的点到原点的距离d、点与原点的连线l和zOy平面的夹角θ1，当电机带动激光雷达转动的时候，电机输出l与x轴的夹角θ2；
[0026] (1c)使用(1b)中扫描到的点到原点的距离d、l与zOy平面的夹角θ1和l与x轴的夹角θ2，解算激光雷达点云中每一个点在激光雷达坐标系中的坐标，输出带有坐标信息的点云Vn。
[0027] 步骤2，使用双边滤波算法对点云Vn进行滤波处理。
[0028] (2a)令点云Vn中所有点的集合为：Cloud2＝{p1,p2,…pi…,pn}，其中pi为点云Vn中第i个点，i∈(1,n)，n为点云Vn中所有点的个数；
[0029] 假设有一点p∈Cloud2，将与p点最邻近的k个点设为p的k-邻域k(p)；
[0030] (2b)使用最小二乘法在k-邻域k(p)内构造一个平面，称为点p在k(p)上的切平面S(p)；
[0031] (2c)计算切平面S(p)的法向量 用 作为S(p)的法矢量估计；
[0032] (2d)将点云Vn中每一个点的坐标通过下式进行滤波：
[0033]
[0034] 其中， 表示滤波后的点坐标；f表示Vn中需要改正的点的坐标；表示其对应的法矢量估计；L为双边滤波因子，
[0035]
[0036] 式中，δc为点的邻域半径，δs为邻域点的标准偏差， 是δc的高斯核函数， 是δs的高斯核函数，fi是Vn中第i点的坐标，||f-fi||是f-fi的2-范数，是 的2-范数。
[0037] 步骤3，用激光雷达测距算法计算两次连续扫描期间每一个特征点的位移量与旋转角度。
[0038] (3a)令α是点云Cn中的点，M是点云Cn中所有点的集合,定义变量h作为α的局部平滑度:
[0039]
[0040] 其中， 是双边滤波后的点α的坐标， 是点云Cn第i个点的坐标，i∈(1,n)， n为Cn中所有点的个数， 是 的2-范数， 是 的2-范数；
[0041] (3b)对点云Cn中每一点的h值进行排序，将h值最大的点选为边缘特征点，h值最小的点选为平面特征点；
[0042] (3c)令tn为第n次扫描开始的时刻，在第n-1次扫描结束时，将(n-2,n-1)期间感知的点云Cn-1投射到时刻tn处，得到投射后的点云
[0043] (3d)在第n次扫描期间，利用 和Cn一起估计所有特征点的位移量与旋转角度：
[0044] (3d1)将(3b)的边缘特征点的集合设为An，如图(2a)所示，令b是An上的点，在点云中找到点b的最邻近点设为j，在点云Cn中找到点b的最邻近点设为 v，用j和v确定一条边缘线(j,v)，计算b点到边缘线(j,v)的对应距离，即边缘特征点的位移量dA：
[0045]
[0046] 其中， 和 分别是点b、j和v的在雷达坐标系中的坐标；
[0047] 令边缘线(j,v)的中点是点e，雷达坐标系的原点设为点o，将线段bo的长度设为lbo，线段eo的长度设为leo，线段be的长度设为lbe，通过余弦定理计算出bo与eo的夹角，即边缘特征点旋转角度
[0048]
[0049] (3d2)将(3b)的平面特征点的集合设为Bn，如图(2b)所示，令r为Bn上的点，在点云中找到点r的最邻近点设为η，在点云Cn中找到r的两个最邻近点设为 u和m，用η、u和m确定平面块(η,u,m)，计算r点到平面块(η,u,m)的对应距离，即平面特征点的位移量dB：
[0050]
[0051] 其中， 和 分别是点r、y、u和m在雷达坐标系中的坐标；
[0052] 令平面块(η,u,m)的中心是点g，将线段go的长度设为lgo，线段ro的长度设为lro，线段gr的长度设为lgr，通过余弦定理计算出go与ro的夹角，即平面特征点的旋转角度[0053]
[0054] 步骤4，用激光雷达绘图算法将Cn配准到世界坐标系上，构成三维点云图。
[0055] (4a)令第n-1次扫描之前世界坐标系中的点云为Dn-1，将点云Cn转换到世界坐标系中，用Dn表示；
[0056] (4b)利用(3d1)中得到的边缘特征点的位移量dA和边缘特征点的旋转角度将每一个边缘特征点沿着其旋转角度的方向减去它的位移量，去掉Dn中与Dn-1重合的边缘特征点，输出点云Kn；
[0057] (4c)利用(3d2)中得到的平面特征点的位移量dB和平面特征点的旋转角度将每一个平面特征点沿着其旋转角度的方向减去它的位移量，去掉Dn中与Dn-1重合的平面特征点，输出点云Jn；
[0058] (4d)合并(4b)和(4c)中输出的点云Kn和点云Jn，得到三维点云图。
[0059] 本发明的效果可以通过以下实验进一步说明。
[0060] 1.实验条件
[0061] 本发明采用激光雷达Hokuyo UTM-30LX、电机Dynamixel MX-28分别通过USB连接到笔记本电脑上，在机器人操作系统ROS上运行我们的程序。
[0062] 2.实验步骤
[0063] 本发明的实验在室内进行，对两种场景进行三维建图实验：
[0064] (2.1)在平台静止不动情况下，使用激光雷达构建三维点云图，结果如图3，其中，图3(a)是从上向下观察的三维点云图，
[0065] 图3(b)是从后向前观察的三维点云图，
[0066] 图3(c)是从相同场景拍摄的实景照片。
[0067] 图3(a)点云上方凸起的部分和图3(b)点云中间密集的部分是大门的轮廓，使用点云图中大门两边门框对应的坐标测算出大门的宽度作为测量值，大门的实际宽度作为实际值。
[0068] (2.2)在以0.5m/s的速度移动的情况下，使用激光雷达构建三维点云图，结果如图4，其中
[0069] 图4(a)是从上向下观察的三维点云图，
[0070] 图4(b)和4(c)是从相同场景拍摄的实景照片。
[0071] 从图4(a)点云图中可以清楚的观察图4(b)左上方的窗户和图4(c)中左边的门框的轮廓。使用点云图两边墙壁特征点对应的坐标，测算出点云图中两边墙壁的距离作为测量值，墙壁的实际距离作为真实值。
[0072] 将(2.1)和(2.2)中的测量值与真实值进行比较，如表1所示。
[0073] 表1三维点云图中距离测量值与真实值对比
[0074]测试场景 测量值(cm) 真实值(cm) 相对误差
大门宽度 163.57 166.50 1.7％
墙壁距离 173.75 176.30 1.4％
[0075] 由表1所示的实验结果可以看出在室内环境中，构建的三维点云图的精度约为2％，说明本发明可以实时地构建高质量的三维点云图。
