自制低成本3D激光扫描测距仪(3D激光雷达)，第二部分
http://www.csksoft.net/blog/post/lowcost_3d_laser_ranger_2.html
版权信息：
本文采用CreativeCommons2.5授权许可，欢迎转载，但请保留原始作者信息以及原文链接。
在开始介绍原理前，先给出一些扫描得到的3D模型以及演示视频，给大家一个直观的认识。
相关的图片：
扫描得到的房间一角(点击查看原始尺寸)
扫描的我(点击查看原始尺寸)
扫描仪实物
本文结构
- 简单介绍了激光雷达产品的现状
- 激光三角测距原理
- 线状激光进行截面测距原理
- 3D激光扫描仪的制作考虑
- 参考文献
简介-激光扫描仪/雷达
这里所说的激光扫描测距仪的实质就是3D激光雷达。如上面视频中展现的那样，扫描仪可以获取各转角情况下目标物体扫描截面到扫描仪的距离，由于这类数据在可视化后看起来像是由很多小点组成的云团，因此常被称之为：点云(Point Clould)。
在获得扫描的点云后，可以在计算机中重现扫描物体/场景的三维信息。
这类设备往往用于如下几个方面：
1) 机器人定位导航
目前机器人的SLAM算法中最理想的设备仍旧是激光雷达(虽然目前可以使用kinect，但他无法再室外使用且精度相对较低)。机器人通过激光扫描得到的所处环境的2D/3D点云，从而可以进行诸如SLAM等定位算法。确定自身在环境当中的位置以及同时创建出所处环境的地图。这也是我制作他的主要目 的之一。
2) 零部件和物体的3D模型重建
3) 地图测绘
现状
目前市面上单点的激光测距仪已经比较常见，并且价格也相对低廉。但是它只能测量目标上特定点的距离。当然，如果将这类测距仪安装在一个旋转平台上，旋转扫描一周，就变成了2D激光雷达 （LIDAR）。相比激光测距仪，市面上激光雷达产品的价格就要高许多：
图片: Hokuyo 2D激光雷达
上图为Hokuyo这家公司生产的2D激光雷达产品，这类产品的售价都是上万元的水平。其昂贵的原因之一在于他们往往采用了高速的光学振镜进行大角度范围(180-270)的激光扫描，并且测距使用了计算发射/反射激光束相位差的手段进行。当然他们的性能也是很强的，一般扫描的频率都在10Hz以上，精度也在几个毫米的级别。
2D激光雷达使用单束点状激光进行扫描，因此只能采集一个截面的距离信息。如果要测量3D的数据 ，就需要使用如下2种方式进行扩充：
- 采用线状激光器
- 使用一个2D激光雷达扫描，同时在另一个轴进行旋转。从而扫描出3D信息。
第一种方式是改变激光器的输出模式，由原先的一个点变成一条线型光。扫描仪通过测量这束线型光在待测目标物体上的反射从而一次性获得一个扫描截面的数据。这样做的好处是扫描速度可以很快 ，精度也比较高。但缺点是由于激光变成了一条线段，其亮度(强度)将随着距离大幅衰减，因此测距范围很有限。对于近距离(<10m)的测距扫描而言，这种方式还是很有效并且极具性价比的，本文介绍的激光雷达也使用这种方式，
图：一字线红色激光器
对于第二种方式，优点是可以很容易用2D激光雷达进行改造，相对第一种做法来说，他在相同的激光器输出功率下扫描距离更远。当然，由于需要控制额外自由度的转轴，其误差可能较大，同时扫描速度也略低。
这类激光雷达产品目前在各类实验室、工业应用场景中出现的比较多，但对于个人爱好着或者家用 设备中，他们的价格实在是太高了。当然，目前也有了一个替代方案，那就是kinect，不过他的成像 分辨率和测距精度相比激光雷达而言低了不少，同时无法在室外使用。
低成本的方案
造成激光雷达设备高成本的因素为
- 使用测量激光相位差/传播时间差测距
- 高速振镜的高成本
- 矫正算法和矫正人工成本
对于个人DIY而言，第三个因素可以排除，所谓知识就是力量这里就能体现了:-) 对于前2个因素，如果要实现完全一样的精度和性能，那恐怕成本是无法降低的。但是，如果我们对精度、性能要求稍 微降低，那么成本将可以大幅的下降。
首先要明确的是投入的物料成本与能达成的性能之间并非线型比例的关系，当对性能要求下降到一 定水平后，成本将大幅下降。对于第一个因素，可以使用本文将介绍的三角测距方式来进行。而对于 扫锚用振镜，则可以使用普通的电机机构驱动激光器来替代。
本文介绍的低成本3D激光扫描仪实现了如下的成本/性能：
成本：~￥150
测量范围：最远6m
测量精度：（测量距离与实际距离的误差）最远6m出最大80mm误差，近距离(<1m)，误差水平在 5mm以内
扫描范围：180度
扫描速度：30 samples/sec (比如以1度角度增量扫描180度，耗时6秒)
对于精度而言，这个低成本方案足以超过kinect，不过扫描速度比较慢，但是对于一般业余用途而言已经足够。不过，该扫描速度是很容易提升的，本文将在分析其制约因素后介绍提高扫描速度的方 法。
原理和算法
这里先介绍测量目标上一个点所涉及的算法。3D扫描将采用类似的方式进行扩充。
使用单点激光进行三角测距
除了使用相位差和时间差进行TOF测距外，另一种测距方式就是三角测距。这也是实现低成本激光测距的关键，因为这种方式不需要具备其他测距方式所要求的特殊硬件。并且，在一定距离范围内， 三角测距也可以达到与TOF测距媲美的测量精度和分辨率。
图片(来源自[3]): 激光三角测距原理
目前有不少爱好者[1][2]基于激光三角测距制作了激光雷达或者测距仪，本制作也采用了这个方式。除了本文外，参考论文[3]也给出了较多的细节。（该论文的作者所在的公司正是将低成本激光雷达用于家用机器人XV-11的开发商，这里就不扯开了:-)
这里摘录了论文中的示意图，要进行激光三角测距，所需的设备很简单： 点状激光器、摄像头。因此，能做到多少的成本大家现在应该比较清楚了。
图中展现了测量对象Object距离激光器的距离d的示意图。图中的Imager部分是对摄 像头的一种抽象表达(针孔摄像机模型)。标有s的线段实际可以是一个固定摄像头和激光器的平面。摄像头成像平面与该固定平面平行，而激光器发出的射线与该平面夹角beta仅存在于图中的视图中。
要测量距离d，首先要求激光射线射到了Object上，他的反射光在摄像头的感光平面上成像。对于不同远近的物体，当被测距激光照射后，摄像头上的成像光点的x值将变化。这里涉及到如下几个参数
Beta:激光器夹角
s:激光器中心与摄像头中心点距离
f:摄像头的焦距
如果这些参数在测距设备安装后不再改变（固定）且数值已知，则物体距离激光器距离可由如下公式求得：
q=fs/x .... (1)
d=q/sin(beta) .... (2)
其中，x是测量中唯一需要获得的变量。它的含义是待测物体上激光光点在摄像头感光元件(如CMOS)上的成像到一侧边缘的距离。该距离可以通过在摄像头画面中查找并计算激光点中心位置的像素坐标来求得。对于示意图
式(1)求出了目标物体与摄像头-激光器平面的垂直距离(实际上对于大尺度测距，该值可以近似认为是实际距离)。这一步就是三角测距的所有内容了，非常简单。
不过，在实际操作中，上述公式仍旧需要扩充。首先时对于变量x的求解，假设我们已经通过算法求出了画面中激光光点的像素坐标(px,py)，要求出公式中需要的x，首先需要将像素单位的坐标变换到实际的距离值。为了计算方便，在安装时，可以令摄像头画面的一个坐标轴与上图线 段s平行，这样做的好处是我们只需要通过光点像素坐标中的一个参量(px或者py)来求出实际投影距离 x。这里假设我们只用到了px。
那么，变量x可以由如下公式计算:
式(3)由引入了两个参数，PixelSize以及offset。其中PixelSize是摄像头感光部件上单个像素感光单元的尺寸，offset是通过像素点计算的投影距离和实际投影距离x的偏差量。这个偏 差量是由如下2个因素引入的：
-
x变量的原点（示意图中与激光射线平息的虚线和成像平面焦点）的位置未必在成像感光阵列的第一列（或排）上（实际上在第一排的概率非常低）
-
通过摄像头主光轴的光线在画面中的像素坐标未必是画面中点。
对于PixelSize，可以通过摄像头感光元件手册来确定其数值。对于offset，要在安 装上消除offset或者直接测量，在业余条件下几乎是不可能的，因此，需要通过后面介绍的矫正步骤求出。
到这里，我们得出了通过激光点像素坐标(pX)来求出对应光点实际距离的公式：
接下来的问题就是如何确定这些参数了。不过，实际操作中，还需要考虑性能指标问题：要达到某种精度要求，究竟需要怎样的摄像头，上述各类参数如何选择呢？
决定单点激光测距性能的因素
有公式(3)可知，参数px是一个离散量(虽然有算法可以求出连续的px,后文将介绍) 。因此，得到的距离数值也将会发生一定的跳变。该跳变的程度反映了测距的分辨率以及精度。
如果将式(1)改写为x=fs/q并按q进行求导，可以得出：dx/dq=-fs/(q^2)，或者写为 ：
式(5)的含义是，变量x每发生一次跳变，通过我们三角测距公式求出的距离值q跳变大小与当前实际待测距离的关系。可以看出，当待测距离边远后，从摄像机获得的像素点每移动一个单位距离，求出的距离值得跳变会大幅增大。也就是说：三角测距的精度和分辨率均随着距 离增加而变差。
因此，要决定我们希望实现的指标，只需要明确：
希望测距的最大距离
在最大距离下，分辨率(式(5))的数值
在论文[3]中给出了他的选取规则，这里直接给出一个结论，具体过程就不重复了：
假设对于激光光点定位能做到0.1个次像素单位，单位像素尺寸为6um。并要求在6m处分辨率(dq/dx)<=30mm。则要求：
在我们制作过程中，这个要求还是很容易做到的。另外目前的CMOS摄像头往往具有更小的单位像素尺寸（在同样大小的芯片上做出了更高的分辨率），因此实际fs的取值下限可以更低 。
而对于摄像头分辨率、激光器夹角beta，则决定了测距的范围（最近/最远距离）。 这里也同样不再重复了，可以参考[3]。对于使用pX进行测距的摄像头，其分辨率480x640即可做出比较好的效果，更高的分辨率更好（当然后文会提到缺点）。beta一般在83deg左右。
2D激光雷达的原理和性能制约因素
在实现了单点激光测距后，进行2D激光扫描就非常容易：进行旋转。这里讨论的他的性能问题：扫描速度。
对于采用三角测距的方式，从摄像头画面上识别出激光点到计算出实际距离对于目前的桌面计算机而言，几乎可以认为不需要时间。那么，制约扫描速度的因素就在于摄像头的祯率了 。对于目前市面常见的usb摄像头，其工作在640x480分辨率的模式下最高帧率都在30fps，那么，扫描速度就是30samples/sec。换言之就是每秒钟进行30次的测距计算。
对于一个180度范围的激光雷达，如果按照每1度进行一次测距计算，最短需要6秒。
如果要提高扫描速度，很自然的就是提高祯率。对于usb摄像头，有PS eye摄像头可 以做到60fps。但这也只能实现3秒180度扫描。需要更加高的速率，也就意味着更快的传输速度，对于USB2.0而言，保证640x480的分辨率，fps很难有所提升。在论文[3]中，他们采用了高速摄像芯片+DSP 的方式实现了1200fps的帧率。
由于本制作不需要很高的扫描速度，因此我仍旧采用了30fps的摄像头。
3D激光扫描的原理
由前文已经指出，这里采用了线状激光器一次对一条线而非单点的目标物体进行扫描测距。将扫描器进行旋转，从而可以实现3D扫描。下图展示了他的工作画面和捕获到的摄像头画面 ：
图：本制作早期使用的红色一字线激光器的工作画面
图：采用红色一字线激光器捕捉到的画面
对于线状激光器进行测距的问题，可以将它转化为前面单点激光测距的计算问题。 对于上图中的激光线条，算法将按照Y轴依次计算出当前Y轴高度下，激光光斑的X坐标值pX。并尝试通过先前的算法求处该点的距离。
为了简化问题，我们先考虑对于一个与摄像头感光面平行的平面上激光光斑各点的 距离问题：
图：激光线条光斑在平行平面上各点的距离问题抽象
如上图所示，远处平面为目标待测平面，上面有一条紫色的激光光斑。近处的平面 是摄像头的感光成像平面，经过了翻折后，他可以看作是目标平面到摄像头成像中心点组成的棱锥的一个截面。
图中的P1点位于摄像头投影画面高度的中点，按照针孔摄像机的定义，该点在画面上的投影P1'距离摄像头中心Camera Center的距离应当为摄像头的焦距F。因此，对于P1，可以直接带入式(4)求出实际距离。
现在的问题是，对于其他高度上的点，如P2，是否可以通过式(4)求得？
图：3D测距的原理
答案自然是肯定的，不过这里涉及到了额外的参数。如上图所示，设P2的投影点P2' 到摄像头中心距离为f'，则P2到baseline垂线距离d'可由如下公式得到:
而很容易知道，f'可以通过f求出：
其中的P2'.y以及P1'.y分别是点P2',P1'在成像元件上的实际高度，他们可由各自点像素坐标pY乘以像素高度求出。
在求出了垂线距离d'后，需要转化成实际的距离D，此时需要 知道P2-RotationCenter以及Baseline组成的夹角theta。该角度可以由立体几何知识通过激光器与 Baseline的夹角beta求出。具体的求解公式可以参考本制作配套源代码的计算部分。
在求出了平行平面上激光光斑任意点的坐标后，可以将问题一 般化，对于3D空间任意激光投影点，可以先构造出该点所在的一个平行平面，然后利用上述算法求解 。
对于每次测距采样，上述算法将产生一个数组dist[n]。其中 dist[i]为对应画面不同高度像素坐标i下激光点的距离。对于采用640x480分辨率的摄像头，n的取值 为480。
激光光点像素坐标确定和求解
这里讨论如何从摄像头画面中计算出光点的坐标信息，具体来说，要解决如下几个 问题：
-
识别并确定激光光点，排除干扰
-
确定光点中心的精确位置
先来看问题一，这个问题看似简单，不过实际会有很多问题，比如下面的几幅实际操作中遇到的画面：
图：不同环境和配置下摄像头捕获的画面
上面3幅图像分别是在使用红色激光器摄像头所拍摄到的。(a)的图像比较理想，在于画面中除了激光光点外没有别的内容，虽然可以看到上方有光电发射发出的干扰点，但激光光点仍旧可以通过求出画面中最亮点的方式获取。
(b)画面中出现了日光灯，由于日光灯亮度也较高，从画面上看与激光点中心亮度一致（均为纯白)，对于这个图像，一种办法是同时判断临近像素的色彩，红色激光点的外围均为红色。
(c)画面中，除了激光点外，出现了其他的红色物体，并且部分高光区域也在图像中表现为纯白，此时，上述通过色彩判断的算法也将失效。因此需要有另外的办法。
完美的激光提取算法几乎是不存在的，一个例子就是当画面中出现了2个类似的激光点（另一个来自别的测距仪或者激光笔)，此时单从一副图像上很难做出判断哪个才是正确的光点。
同时，较准确的识别光点也需要硬件设备以及光学设备的合作，具体的细节超过了本文的范畴。这里列举几种可行的办法：
1. 加装滤光片
在文献[3]和文献[4]中均提及使用滤光片的做法，仅保留激光器发射波长的光线进入，从而可以一定程度的避免光线干扰。
2. 调整摄像头曝光时间
调整摄像机曝光率也可以有效去除画面的干扰，例如上图(b)和(c)，对于5mW的激光器，一定距离内其单位光照强度仍旧比日光强[3](人肉眼可以在室外识别出激光笔照射在地面的光点) ，因此，只要将摄像头曝光率调整的足够短，完全由可能将画面中除了激光点之外的内容剔除。
3. 采用非可见光激光器
例如使用红外激光器，这个做法与遥控器使用红外LED理由一样，在人造环境中少有红外光干扰。配合红外滤光片，可以有效滤除来自诸如日光灯等的干扰。但是，对于日光和白炽灯， 其中也含有足够强的红外光，无法单纯采用此法。
4. 增加激光器功率
配合曝光率控制，增加激光器发射功率也足以使得画面中仅保留光点，但这样也有危险性，尤其采用点状激光时。
本制作采用了上述的所有方法，将在后文具体介绍。
对于问题(2)，最简单的做法是直接找出光电中最亮的像素的坐标。但是由于前面公式得知，这样的得到的pX值是整数，计算得到的q将会有比较大的跳变。因此这里介绍如何将pX变为更 加精确的"次像素"级别。
对于这个问题，学术界已有不少的研究，这里推荐参考论文[5]，其中介绍了几种次像素激光光点定位算法的介绍以及分析了他们的优劣。这里也不再重复了。
简单来说，可以认为激光光点的亮度是一个二维的Gauss函数经过了一次采样得到了画面上的激光点。那么，可以通过拟合或者简单的线性插值/求质心的手段，估计出光点的中心。
本制作使用了简单的质心法求取次像素的激光中心点。
图：采用滤光片后，从白色日光灯画面(右上图)中识别并计算出激光光点中心坐 标
可能有人会问这样的估算精确有效吗？一般而言，精确到0.1个像素单位是比较可靠的，也有文献指出他们做到了0.01个像素的可靠定位。
对于线状激光器的求解过程与点状激光类似，区别在于将按照图像的每行（或者每列）分别找出激光光斑的中心。可参考文献[6]，文献[7]给出了一个针对线状激光 更优的光点中心提取算法。
摄像头校正
进行激光测距的基本原理非常简单，但在实现中却有很多制约因素。除了前文提到的进行三角测距求解公式中的那些参数需要确定之外，校正摄像头从而得到理想的针孔摄像机模型下的图像也是很重要的环节。
首先要回答的一个问题是：为何要校正摄像头？校正什么参数？
校正的主要理由是实际上目前使用的摄像头并非是前文所提到的针孔摄像机模型。 所谓针孔摄像机，简单说原理就和小孔成像类似：光线通过一个小孔后再背后的感光部件上成像。但大家知道，现实的摄像机都是采用光学透镜聚光成像的，并且所用的透镜并非是抛物面的（很难加工 ），同时，感光芯片也透镜之间也非严格平行[8]。总之，现实就是产生的画面实际上存在扭曲和偏移 的。如果直接使用原始摄像机的画面进行测距，势必造成误差。因此需要进行相机的校正，通过校正 后获取消除上述画面扭曲和偏移的图像，再用来进行激光测距的相关操作。
图: 摄像头原始画面和经过相机校正后的修正画面
上图左侧图片是一种摄像头拍摄到的原始画面，可以明显看出图像存在着扭曲，对相机校正后，我们可以校正后的参数修正扭曲的画面，得到右侧图像的效果。
对于摄像机校正的具体原理、算法和过程超过了本文的介绍范围，具体信息可以参考如下的文献和教程:[8][9][10]。在本文后续的制作部分，也会介绍本次制作的校正过程和结果。
校正和求解三角测距所用参数
前文介绍的三角测距公式中涉及了如下的参数：
Beta:激光器夹角
s:激光器中心与摄像头中心点距离
f:摄像头的焦距
pixelSize:感光部件单位像素尺寸
offset:激光点成像位置补偿值
这些参数有些很难通过实际测量求出，有些很难再安装时就控制好精度。他们数值 的精确度会对测距精度有着非常大的影响。例如pixelSize一般都是微米级别的数值，很小的误差即可导致最终测距的偏差。
对于他们的求解，我们将在测距仪制作完成后进行的校正环节求出。这里的校正， 实际过程是在实现测量好的距离下采集出测距公式中用到的pX数值。然后通过曲线拟合的方式确定参 数。
这部分的具体操作将在后文的制作/校正过程中具体介绍。
制作低成本的3D激光雷达
这部分的内容请访问
自制低成本3D激光扫描测距仪(3D激光雷达)，第二部分
http://www.csksoft.net/blog/post/lowcost_3d_laser_ranger_2.html
参考文献
[1] Details of the Laser Range finder
http://www.eng.buffalo.edu/ubr/ff03laser.php
[2] Webcam Based DIY Laser Rangefinder
http://sites.google.com/site/todddanko/home/webcam_laser_ranger
[3] K. Konolige, J. Augenbraun, N. Donaldson, C. Fiebig, and P. Shah. A low-cost laser distance sensor. In Int. Conference on Robotics and Automation (ICRA), 2008.
[4] Kenneth Maxon. A Real-time Laser Range Finding Vision System
http://www.seattlerobotics.org/encoder/200110/vision.htm
[5] Fisher, R. B. and D. K. Naidu. A Comparison of Algorithms for Subpixel Peak Detection. Springer-Verlag, Heidelberg, 1996.
[6] Mertz, C., J. Kozar, J.R. Miller, and C. Thorpe. Eye-safe Laser Line Striper for Outside Use. Intelligent Vehicle Symposium, 2002.
[7] J. Forest, J. Salvi, E. Cabruja, C. Pous, Laser stripe peak detector for 3D scanners. A FIR filter approach, in: International Conference on Pattern Recognition, Cambridge, August 2004, pp. 646–649
[8] Learning OpenCV: Computer Vision with OpenCV Library, Gary Bradski and Adrian Kachlev, first edition , 2008 O’Reilly, ISBN 978-0-569-51613.
[9] 分享一些OpenCV实现立体视觉的经验
http://blog.csdn.net/scyscyao/article/details/5443341
[10] Camera Calibration Toolbox for Matlab
http://www.vision.caltech.edu/bouguetj/calib_doc/
.jpg)
