ZZ made low cost 3D laser scanning rangefinder (3D laser radar), the first portion

Why 80% of the code can not do farming architect? >>>  

From CSK low-cost 3D scanner is. Very Impressive!

Before starting introduce the principle, first give some 3D models and video presentations scanned, give us an intuitive understanding.

Video Links

Related pictures:

Scanned corner of the room (Click to view original size)

I scanned (Click to view original size)

Scanners in kind

The paper is organized

1. It introduces the status quo of laser radar products
2. Laser triangulation principle
3. A cross-sectional linear laser ranging principle
4. 3D laser scanners to consider making
5. references

Profile - Laser Scanner / radar

The essence here is 3D laser scanning laser range finder radar. As above, as the video presentation, the scanner scans the target object can be acquired from the scanner to the cross-section, since such data cloud appears many small dots in the visual angle in each such case, it is often called it is: a point cloud (point Clould).

After obtaining the point cloud scan, you can reproduce three-dimensional information of the scanned object / scene in a computer.

Such devices are often used in the following aspects:

1) robot navigation

Currently SLAM algorithm robot is still the best equipment laser radar (although you can use kinect, but he can no longer be used outdoors and relatively low accuracy). Robot, which can be positioned, such as a SLAM algorithm the like 2D / 3D point cloud environment in which the laser beam scanned. Determine their position in the environment and at the same time to create them map their environment. This is what I made one of his main purposes.

2) 3D object model reconstruction component and

3) ground mapping

status quo

Currently available single-point laser rangefinders have been more common, and the price is relatively low. But it can only measure the distance from a specific point on the target. Of course, if such a rangefinder mounted on a rotating platform, rotating scanning one week, it becomes a 2D laser radar (LIDAR). Compared to laser range finder, laser radar products, the market price will much higher:

Picture: Hokuyo 2D laser radar

上图为Hokuyo这家公司生产的2D激光雷达产品，这类产品的售价都是上万元的水平。其昂贵的原因 之一在于他们往往采用了高速的光学振镜进行大角度范围(180-270)的激光扫描，并且测距使用了计算 发射/反射激光束相位差的手段进行。当然他们的性能也是很强的，一般扫描的频率都在10Hz以上，精 度也在几个毫米的级别。

2D激光雷达使用单束点状激光进行扫描，因此只能采集一个截面的距离信息。如果要测量3D的数据 ，就需要使用如下2种方式进行扩充：

1. 采用线状激光器
2. 使用一个2D激光雷达扫描，同时在另一个轴进行旋转。从而扫描出3D信息。

第一种方式是改变激光器的输出模式，由原先的一个点变成一条线型光。扫描仪通过测量这束线型 光在待测目标物体上的反射从而一次性获得一个扫描截面的数据。这样做的好处是扫描速度可以很快 ，精度也比较高。但缺点是由于激光变成了一条线段，其亮度(强度)将随着距离大幅衰减，因此测距 范围很有限。对于近距离(<10m)的测距扫描而言，这种方式还是很有效并且极具性价比的，本文介 绍的激光雷达也使用这种方式，

图：一字线红色激光器

对于第二种方式，优点是可以很容易用2D激光雷达进行改造，相对第一种做法来说，他在相同的激 光器输出功率下扫描距离更远。当然，由于需要控制额外自由度的转轴，其误差可能较大，同时扫描 速度也略低。

这类激光雷达产品目前在各类实验室、工业应用场景中出现的比较多，但对于个人爱好着或者家用 设备中，他们的价格实在是太高了。当然，目前也有了一个替代方案，那就是kinect，不过他的成像 分辨率和测距精度相比激光雷达而言低了不少，同时无法在室外使用。

低成本的方案

造成激光雷达设备高成本的因素为

1. 使用测量激光相位差/传播时间差测距
2. 高速振镜的高成本
3. 矫正算法和矫正人工成本

对于个人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可以由如下公式计算:

x=PixelSize*px+offset …. (3)

式(3)由引入了两个参数，PixelSize以及offset。其中PixelSize是摄像头感光部件 上单个像素感光单元的尺寸，offset是通过像素点计算的投影距离和实际投影距离x的偏差量。这个偏 差量是由如下2个因素引入的：

1. x变量的原点（示意图中与激光射线平息的虚线和成像平面焦点）的位置未必 在成像感光阵列的第一列（或排）上（实际上在第一排的概率非常低）

2. 通过摄像头主光轴的光线在画面中的像素坐标未必是画面中点。

对于PixelSize，可以通过摄像头感光元件手册来确定其数值。对于offset，要在安 装上消除offset或者直接测量，在业余条件下几乎是不可能的，因此，需要通过后面介绍的矫正步骤 求出。

到这里，我们得出了通过激光点像素坐标(pX)来求出对应光点实际距离的公式：

d=fs/(PixelSize*px+offset)/sin(beta) …. (4)

接下来的问题就是如何确定这些参数了。不过，实际操作中，还需要考虑性能指标 问题：要达到某种精度要求，究竟需要怎样的摄像头，上述各类参数如何选择呢？

决定单点激光测距性能的因素

有公式(3)可知，参数px是一个离散量(虽然有算法可以求出连续的px,后文将介绍) 。因此，得到的距离数值也将会发生一定的跳变。该跳变的程度反映了测距的分辨率以及精度。

如果将式(1)改写为x=fs/q并按q进行求导，可以得出：dx/dq=-fs/(q^2)，或者写为 ：

dq/dx=-q^2/fs …. (5)

式(5)的含义是，变量x每发生一次跳变，通过我们三角测距公式求出的距离值q跳变 大小与当前实际待测距离的关系。可以看出，当待测距离边远后，从摄像机获得的像素点每移动一个 单位距离，求出的距离值得跳变会大幅增大。也就是说：三角测距的精度和分辨率均随着距 离增加而变差。

因此，要决定我们希望实现的指标，只需要明确：

希望测距的最大距离

在最大距离下，分辨率(式(5))的数值

在论文[3]中给出了他的选取规则，这里直接给出一个结论，具体过程就不重复了：

假设对于激光光点定位能做到0.1个次像素单位，单位像素尺寸为6um。并 要求在6m处分辨率(dq/dx)<=30mm。则要求：

fs>=700

在我们制作过程中，这个要求还是很容易做到的。另外目前的CMOS摄像头往往具有 更小的单位像素尺寸（在同样大小的芯片上做出了更高的分辨率），因此实际fs的取值下限可以更低 。

而对于摄像头分辨率、激光器夹角beta，则决定了测距的范围（最近/最远距离）。 这里也同样不再重复了，可以参考[3]。对于使用pX进行测距的摄像头，其分辨率480×640即可做出比 较好的效果，更高的分辨率更好（当然后文会提到缺点）。beta一般在83deg左右。

2D激光雷达的原理和性能制约因素

在实现了单点激光测距后，进行2D激光扫描就非常容易：进行旋转。这里讨论的他 的性能问题：扫描速度。

对于采用三角测距的方式，从摄像头画面上识别出激光点到计算出实际距离对于目 前的桌面计算机而言，几乎可以认为不需要时间。那么，制约扫描速度的因素就在于摄像头的祯率了 。对于目前市面常见的usb摄像头，其工作在640×480分辨率的模式下最高帧率都在30fps，那么，扫描 速度就是30samples/sec。换言之就是每秒钟进行30次的测距计算。

对于一个180度范围的激光雷达，如果按照每1度进行一次测距计算，最短需要6秒。

如果要提高扫描速度，很自然的就是提高祯率。对于usb摄像头，有PS eye摄像头可 以做到60fps。但这也只能实现3秒180度扫描。需要更加高的速率，也就意味着更快的传输速度，对于 USB2.0而言，保证640×480的分辨率，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’可由如下公式得到:

d’=f’baseline/x …. (6)

而很容易知道，f’可以通过f求出：

f’=f/cos(arctan((P2′.y-P1′.y)/f)) …. (7)

其中的P2′.y以及P1′.y分别是点P2′,P1′在成像元件上的实际高度，他们可由各自点 像素坐标pY乘以像素高度求出。

在求出了垂线距离d’后，需要转化成实际的距离D，此时需要 知道P2-RotationCenter以及Baseline组成的夹角theta。该角度可以由立体几何知识通过激光器与 Baseline的夹角beta求出。具体的求解公式可以参考本制作配套源代码的计算部分。

在求出了平行平面上激光光斑任意点的坐标后，可以将问题一 般化，对于3D空间任意激光投影点，可以先构造出该点所在的一个平行平面，然后利用上述算法求解 。

对于每次测距采样，上述算法将产生一个数组dist[n]。其中 dist[i]为对应画面不同高度像素坐标i下激光点的距离。对于采用640×480分辨率的摄像头，n的取值 为480。

如果进行180度，步进为1度的3D扫描，则可得到分辨率为 180×480的点云阵列。

如果采用0.3度步进，扫描180度，则得到600×480的点云阵列

激光光点像素坐标确定和求解

这里讨论如何从摄像头画面中计算出光点的坐标信息，具体来说，要解决如下几个 问题：

1. 识别并确定激光光点，排除干扰

2. 确定光点中心的精确位置

先来看问题一，这个问题看似简单，不过实际会有很多问题，比如下面的几幅实际 操作中遇到的画面：

图：不同环境和配置下摄像头捕获的画面

上面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激光雷达

这部分的内容将在下一篇文章开始介绍。

参考文献

[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

[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] Mertz, C., J. Kozar, J.R. Miller, and C. Thorpe. Eye-safe Laser Line Striper for Outside Use. Intelligent Vehicle Symposium, 2002.

[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/

Reproduced in: https: //my.oschina.net/dake/blog/196657