ZZ 自制低成本3D激光扫描测距仪(3D激光雷达)，第二部分

转自：http://www.csksoft.net/blog/post/lowcost_3d_laser_ranger_2.html

1. 设备设计

核心元件原型

在第一篇文章的原理介绍[1]中，已经大致提到了本次制作的核心元件：摄像头、激光器以及进行扫描的伺服电机的 选型要求。

对于我期望的精度和性能，一般市面常见的USB VGA摄像头即可满足要求。

图：本制作使用的USB摄像头(已经拆除外壳)

对于激光器的选择，主要是考虑他的发射波长和功率。由于我的制作并不用像产品那样考虑激光器功率安全[2]问题 ，因此，采用了200mW的红外一字线激光器。较大功率的优势是可以通过缩短摄像机曝光速率，从而从画面上过滤到环 境光的干扰，同时也可以扫描较远的距离。当然，200mW的激光器功率的确有点太大了，在使用时注意不能用 眼睛直视，并且红外激光器人肉眼不可见，所以需要额外的当心。

图：制作所使用的红外一字线激光器

在使用了红外激光器后，可以通过给摄像头加装红外滤光片。它可以将肉眼可见光过滤，仅允许激光器发出的红外 光进入摄像头。从而有效地过滤环境光带来的干扰。对于红外滤光片，最佳的选择是使用与激光器发射波长相匹配的滤 光片，比如如果使用的是808nm的激光器，那么滤光片选择808nm的窄带滤光片最合适，这样做可以最大程度的降低干扰 。因为现实中，日光、白炽灯、遥控器也都会发出红外光谱。

但是这样的滤光片一般价格偏贵，在本制作中，我使用了800nm截至的低通滤光片。它允许任何波长低于800nm的红 外光通过。不过实际效果还是不错的。

图：本制作采用的低通红外光滤光片

对于用于扫描的伺服电机，由于摄像头的帧率是30fps。扫描速度不需要很快，因此这里使用了普通的标准舵机。他 的优势是可以直接控制定位到特定角度，驱动也相对容易。不过精度不高，对于0.3度的角度定位，已经有些吃力了。 这也是值得改进的地方。

图：本制作选用的舵机

这里统一列出他们的参数：

摄像头：VGA画质的USB摄像头，30fps (市面普遍可以购买的型号)。非广角

激光器：50mW 红外一字线激光 808nm

滤光片：10mm直径红外低通滤光片

舵机：HS-322hd 43g标准舵机

安装考虑

这里主要针对原理[1]中提到的几个参数的选择，决定激光器、摄像头的安装方式。在[1]中，我提到了摄像头焦距 和摄像头-激光器距离(s)的乘积f*s应当满足：

fs>=700

一般市面USB非广角镜头的摄像头的焦距在4.5mm左右，因此，s一般选择160mm左右。也就是说，摄像头和激光器的 间距在160mm或者以上。当然，如果觉得这间距太大了，也可以稍微缩小，正如[1]提到的，目前摄像头的像素尺寸一般 比较小。

另外一个在安装中要考虑的参数是激光器夹角beta。[1]中同样提到他的值在83deg左右。对此，安装的时候不必也 不能死板的测量角度并安装，这是因为除非使用了工业精工级别的激光器，否则激光器发出的激光射线也存在夹角。

beta角度可以在安装完成后进行多次修正，保证在较远处，画面中仍然可以看到激光轨迹，再进行固定。

2. 机械和结构部分

对摄像头的改装

由于我们使用了红外激光器，因此需要对摄像头做一些修改。

首先是要移除摄像头镜片中的红外截止滤光片。该滤光片的作用与前文提到的红外滤光片功能恰好相反：它将红外 光谱过滤。一般摄像头内都会含有这种滤光片。如果不移除，则只能够感受到很微弱的红外信号。（题外话，可以用拆 除截至滤光片的摄像头观察kinect投射出来的红外图案)

图：位于摄像头镜头中的红外截止滤光片

其次就是将前面提到的红外低通/带通滤光片安装到摄像头中，这里采用比较山寨的组装方式:用胶布固 定在镜头前面，不过效果也可以接受。

图：将红外低通滤光片安装在摄像头上

制作激光器、摄像头的固定平台

这里使用了轻质的木板来安装摄像头和激光器。选择它的主要原因是容易加工。如果有条件，可以考虑使用热缩涨 比率小的金属或者塑料材料来固定。

图：使用木板作为固定摄像头、激光器的材料

在木板上打孔，保证激光器和摄像头能正好装入，2个孔之间的距离是前文提到的160mm左右。不过不必 很精确，因为它的精确数值可以通过校正得到。这样安装的时候就比较省心。在给激光器打孔的时候要注意角度的问题 。

图：木板打孔后，将摄像头和激光器装入

图：在木板底部打孔，安装螺丝柱，用于固定在舵机转盘上

底座和舵机安装

我是用了用于安装仪器的塑料盒子作为扫描仪的底座，这类盒子可以在taobao上找到。

图：选用的塑料盒子和舵机

将盒子顶板开孔，使得舵机能够放进去：

图：将盒子开孔

图：将舵机嵌入盒子顶板

最后将之前的摄像头固定支架安装于舵机上：

3. 电子系统制作

这里的电子系统主要功能是从PC接受指令并控制舵机转到指定角度的。由于摄像头已经是USB接口了，所以这里的电 子系统不用去处理摄像头的信号。

目前PC外设使用USB几乎已经成为了一种标准，为了使得设备使用尽可能方便，如果能做到免驱动，那就更加完美了 。

这里我才用了我们RoboPeak机器人团队设计的一款AVR开源控制版：RoboPeak Usb Connector[2]来实现这部分功能。它使用单片AVR(Atmega88)芯片通过软 件方式模拟出USB协议栈，并且使用了HID协议使得无需在PC上安装驱动程序。对于这部分的细节属于固件实现部分，将 在后文提到。

图: 我们RoboPeak团队的开源USB方案：RP USB Connector（蓝色PCB）

这里列出本制作中电子系统所实现的功能

实现了一个HID类的USB设备，无需第三方驱动程序支持

支持通过USB接受指令，控制舵机运转到指定角度

可通过USB指令，开启/关闭激光器

通过一个双米字LED显示屏，显示当前舵机的角度

支持用户通过设备上按钮手工设置舵机角度

对于USB设备的实现、接收处理USB指令等属于固件范畴的职责，这里将不做讨论。这里使用了RP USB Connector， 因此其固件已经支持了此部分的基础操作，也可以在他的介绍页面[2]查看详情。

米字LED数码管的驱动

由于2个米字形LED含有16多个独立的LED需要独立控制，RP USB Connector上并没有如此多空闲IO口。因此这里的做 法是很传统的采用74595串行转并行输出芯片。RP USB Connector上通过SPI总线进行控制。

图：使用2个74595芯片驱动双米字LED数码管

扩充RP Usb Connector

由于RP Usb Connector主要功能是做为AVR芯片编程器和通用USB开发版的，上面并没有舵机控制和激光 器控制功能（需要三极管扩流）。因此需要做一块额外的电路扩充它的功能。电路比较简单，这里就不介绍了，可以参 考本章节模块给出的电路图。

图：在RP USB Connector基础上做扩充电路

制作角度手工控制面板

这部分做的比较粗糙，主要就是在塑料盒子外边加装按钮，就不多介绍了，给一些制作的照片。

图：按钮(正面)

图：按钮(背面)

总装

最后将所有线路连接起来，并关闭盒子，设备的安装就大功告成。

这里给出一个早期的视频：

电子系统电路原理图：

RoboPeak USB Connector电路图：

这部分请访问位于google code托管的开源项目：http://code.google.com/p/rp-usb-connector/

扩展电路/LED驱动电路原理图：

目前本制作的所有源代码和电路也已经托管于Googe code, 请访问如下地址下载：

http://code.google.com/p/rp-3d- scanner

4. 固件以及PC通讯

这里的固件自然就是运行在RoboPeak USB Connector AVR芯片上的固件代码了。这里只大致列出实现的思路和原理 。具体的细节以及基础知识不做介绍。源代码可以在本制作的google code项目页面[3]上找到。

固件实现的功能

如前文所述，固件实现了：

1. 通过软件方式模拟出USB1.1协议栈(使用了v-usb库[4])
2. 实现了HID(Human Input Device)类的USB设备进行通讯，在PC上无需外驱动
3. 控制舵机角度
4. 驱动LED数码管显示

同时，固件代码采用了我之前写的Arduino-Lite[5]轻量级AVR固件库。因此如果需要使用这里的固件代码，请在 google code上下载并配置Arduino-Lite[6]。

接下来我将跳出一些典型问题介绍

HID-USB设备的模拟及与PC通讯

RoboPeak USB Connector使用的是不含有USB接口的AVR芯片。因此，USB通讯支持是采用软件方式进行的。索性目前 v-usb开源库[4]已经提供了很优秀的封装。

软件模拟usb的优势在于可以降低设备成本(带有USB的AVR芯片成本较高），缺点是稳定性和速率上不如硬件实现。 对于本制作，稳定性和速度并不关键。

在使用USB与PC通讯时有一个烦恼的问题是驱动。直接为系统编写驱动程序会提升本制作的难度，同时对于64位 Windows系统，微软要求驱动程序进行签名。目前也有一种可以在用户态实现usb驱动的方案：libusb。但是，同样在64 位Windows系统上，这个做法就不管用了。

这里采用了另一种思路：利用那些OS自带驱动支持的USB类设备。(USB协议中预先定义了几类USB设备的种类，比如 Mass Storage就是平时用的移动硬盘、UVC设备常用于实现摄像头、HID设备用于实现键盘鼠标。OS往往会自带驱动支持 这类设备）。这里我也使用了HID类设备。理由如下：

1. 协议比较简单，通讯单位也是数据包
2. 支持USB1.1低速率规范，可以用v-usb实现
3. 在Windows、Linux、Mac上均可以通过OS提供的用户态API与HID设备通讯

采用了HID设备后，及时在计算机上第一次使用本扫描仪，OS也能直接识别，不会要求安装驱动或者配置文件。

图：本扫描仪在PC上识别为HID设备

接下来的问题是如何利用HID设备进行通讯了。这里的做法千差万别。为了提高通讯质量，这里我使用 了带有checksum交验的，基于数据包的通讯协议，将它运行在HID协议之上。因为本固件是基于RP USB Connector固件 的缘故，我直接采用了用于AVR芯片烧录器的STK500v2协议。

图：本制作的通讯构架

舵机驱动逻辑

在AVR上有很多舵机驱动库，这里我使用的是RoboPeak对Arduino自带舵机库的修改版本，该版本使用 Arduino-Lite进行了重写，精度上也较高。但这部分不影响具体效果，因此就不多介绍了。

数码管驱动逻辑

在RP USB Connector上引出了SPI总线，因此很自然的我使用SPI协议驱动74595芯片，直接通过AVR控制 数码管的每个LED，从而支持显示任意的图案。

要注意的是，由于双米字管的每个字符上相同位置的LED公用信号线，因此在驱动上需要采用传统的扫 描方式，轮流点亮LED。

5. 图像处理和渲染

图像处理部分就是第一篇文章[1]所介绍的几个步骤：

1. 获取摄像头原始画面
2. 通过摄像头校正参数，消除画面扭曲
3. 提取和识别激光光斑的位置
4. 通过激光光斑的位置，代入距离求解公式，算出对应点真实距离

本制作使用了OpenCV库简化图像计算的开发难度。同时这样也有利于代码的跨平台移植。对于摄像头的校正，会在 后文介绍。

这部分的细节已经在第一篇文章中详细介绍了，因此这里给出程序运作中的截图作为示意：

图：对摄像头画面进行扭曲校正并识别激光点的过程

由于采用了红外激光器，在加装滤光片后，背景光干扰被有效的去处。画面上除了激光光斑外，几乎看不到别的内 容。红外激光在摄像头中以偏紫色的色彩显示。

上图中显示了画面中心点的激光中心坐标值。这样做的目的是用于后期进行测距参数的校正。而使用中心点的原因 在第一篇文章中已经介绍。

渲染点云

在视频中看到了实时扫描并显示点云的画面。在明白了扫描原理后，这点就没什么特别的了。基本上都是基本的3D 渲染得技巧。我使用了Irrlicht[7]开源3D引擎简化了这部分的实现工作。

除了自行编写软件外，也有很多工具可以用来查看3D点云。比如开源的MeshLab[8]，Blender[9]以及Matlab。在后 文我将给出可以在Meshlab中观看的点云数据。

要产生可被这类软件读取的文件很容易，像MeshLab支持如下格式的文本点云数据

x1,y1,z1

x2,y2,z2

…

xn,yn,zn

6. 校正

当完成所有部分制作和PC端软件后，就可以进行校正工作了。校正主要分为：相机校正和测距校正。

相机校正

我使用自己打印的Chessboard图案，分别在不同距离和位置下拍摄了不同画面。校正工具使用了matlab的Camera Calibration Toolbox。它的信息请参考第一篇文章的参考文献[10]。OpenCV也包含了相机校正功能，也可以直接使用 。

图：用棋盘图在不同位置下拍摄画面

图：使用matlab进行校正时识别出的Inner Corner

图：校正计算的外部参数(对本制作无用)

在完成校正后，将得到如下的校正参数：

Calibration results (with uncertainties):

Focal Length:          fc = [ 935.44200   929.73860 ] ? [ 11.29945   10.64268 ]
Principal point:       cc = [ 149.00014   233.25474 ] ? [ 17.13538   11.11605 ]
Skew:             alpha_c = [ 0.00000 ] ? [ 0.00000  ]   => angle of pixel axes = 90.00000 ? 0.00000 degrees
Distortion:            kc = [ - 0.13196   -0.05787   -0.00358   -0.01149  0.00000 ] ? [ 0.04542   0.12717   0.00195   0.00565  0.00000 ]
Pixel error:          err = [ 0.24198   0.25338 ]

Note: The numerical errors are approximately three times the standard deviations (for reference).

对于这里的应用，之需要关心Focal Length、Principal point、Distortion的几组参数。可以使用OpenCV的 cvInitUndistortMap/cvRemap读取校正参数并完成画面扭曲消除。

测距参数校正

需要校正的参数请参考前一篇原理文章。要开始测距校正，首先要求PC客户端软 件已经能够得到激光光斑中心位置了。这里给出对画面中心位置激光光斑测距参数校正的过程。至于这样做的理由，已 经在第一篇文章中详细介绍了。其他的参数可以用相同的思路进行。

理想的校正环境是比较空旷的区域，前方有垂直的白墙用于反射激光光斑。并配 备高精度的测距仪器参考。我没有这样的条件，也没有必要如此，因此采用了比较山寨的参考设备：卷尺。

图:进行测距参数校正

上图正是在制作本测距仪时进行校正所拍摄的照片。所需要的设备就是一把卷尺 ，当然最好能够足够长，有5-6m。这样可以校正到较远的距离。一般校正到5米是必须的。

校正至少需要采集2个参数：实际的距离值，激光光斑中心点位置。校正采集到的数据自然是越多也好 ，但一般6个以上的点即可。

如下是本制作校正采集的数据：

Dist X
146.8 14.79
246.8 259.63
346.8 356.72
446.8 420.24
5 46.8 457.9
746.8 503.58
946.8 528.9
1146.8 546.71
1546.8 567.54
1846.8 576.87
2246.8 586.08
3046.8 597.04
4046.8 604.6
5546.8 611.9

在完成了数据采集后，可以在matlab等工具帮助下，进行曲线拟合。拟合的曲线公式正式前一篇文章提 到的式(4)。可以看出采集的数据和理论曲线非常吻合：

图：对校正数据进行的拟合

7. 结果和讨论

本制作的结果在文章开始的时候已经给出了，简单说，就是达到了我的预期:-)

指标如下：

绝对测距精度：1m内-/+10mm与实际值的偏差，5m处最大80mm与实际值的偏差
扫描角度: 0-180度
最小步进 ：0.3度
扫描分辨率: 480 points per sample
扫描速度：30 samples per sec (180度，1度步进需时6秒)
成本：~￥150

这里给出前面图像和视频中出现的我的扫描像的点云数据，可以在MeshLab中导入察看：

http://www.csksoft.net/data/pic/3d scanner/sample/face_scan.zip

同时也给出一些额外的图片和视频：

图：在Matlab中查看点云(点击查看原始图像)

视频：另一段实时扫面渲染

视频：matlab中观察点云

性能分析

这里主要看看扫描精度实现的情况。如果之前校正用的基准数据没有任何误差（实际不可能），激光光点提取算法没有问题，那么实际工作时刻的误差主要就体现在拟合的曲线与实际的函数曲线的差距。换句话说，就是拟合得到的参数于实际正确的参数（我们并不知道）。

当然，上面2个假设实际都是不成立的，不过，我们先加设他们都没有误差，先来分析校正曲线与实际曲线的误差：

Dist   X      Calc        Diff
146.8  14.79  123.3103843 23.48961566
246.8  259.63 231.8752505 14.92474953
346.8  356.72 328.8075565 17.99244355
446.8  420.24 440.8004392 5.99956081
546.8  457.9  546.254414  0.545586001
746.8  503.58 758.5428124 -11.74281238
946.8  528.9  958.9149139 -12.11491386
1146.8 546.71 1172.910412 -26.11041169
1546.8 567.54 1578.227294 -31.42729368
1846.8 576.87 1862.988106 -16.18810582
2246.8 586.08 2262.96604  -16.16604012
3046.8 597.04 3030.93851  15.86149048
4046.8 604.6  3948.153444 98.64655635
5546.8 611.9  5564.223928 -17.42392847

上表是在之前校正中收集的数据基础上得到的，其中Calc列的数据是通过校正后的拟合曲线，通过式(4)的计算得到的测距数据，而Diff就是计算得到的距离和真实距离(Dist)的差值。这里的单位除了X列外都是毫米。

从数据上很直观的可以看到在4046mm处进行测距时，计算结果和实际值相差了98.64mm。相比这个，对于近距离的数据，误差也比较大。对于第二个现象，在前一篇文章的文献[3]猜测这是由于镜头扭曲造成的。但是，大家可能会有疑问了，我的实现不是已经做过相机校正了吗？为何还会有镜头扭曲？这里给出几个可能的解释

1. 相机校正结果存在误差，造成仍旧有细微扭曲
2. 相机校正也是基于实现设计出来的数学模型，实际情况有很多其他因素均能导致画面扭曲，但他们无法通过目前的校正修正
3. 红外光的折射率与自然光不同，校正是针对自然光频率范围进行的，因此对于红外光，扭曲依然存在

实际的情况可能是这些原因中的几种组合。

另外，别忘了我们之前做的2个假设，实际他们也是不成立的。在得到了这个误差表后，接下来的问题是：可否继续校正来弥补这个误差？前一篇文章的文献[3]表示这也是可以做得，至少有了上面的表格后，可以在对应的距离下直接校正出正确结果。不过这样做的有效性有待验证。

除了测距精度外，这里也提一下扫描的分辨率。在前文中我提到过目前的舵机可以实现0.3度的角度定位精度。但实际上对于近距离物体扫描，这是不够的。目前的设备比较适合进行大范围扫描，这也比较符合他作为激光雷达的用途。

8. 下一步工作

目前本制作已搞一段落。这里说说我的制作动机和下一步打算。

其实动机在前一篇文章中已经点到，就是用于我们RoboPeak团队的机器人，进行SLAM。这也是接下来我即将进行的事情。当然，其实能做的事情还有很多，比如：

进行多视角扫描，并合成为一个全局点云

其实商业的扫描仪都会支持这个应用。目前一次3D扫描只能采集物体的一个表面，而他的背面则无法扫描。如果对物体的背面也进行3D扫描，那就能得到完整的3D模型了。

要实现它，可以有2种办法：

1. 旋转物体本身，扫描仪固定
2. 扫描仪在不同方位扫描目标物体

1的实现和原理很简单，2的核心问题是如何将2次扫描的点云对应起来？除了人肉拼接外，实际上也有比较成熟的算法，这类算法成为Surface Registration。如ICP-Slam也是采用了这样的算法。

提高扫描精度和速度

更高的精度和速度永远是对这个作品的需求，这就不多解释了。我在原理部分也提到了改善性能的方法。

基于3D点云进行物体识别

这个应用就很类似于Kinect。其实实现的算法也是相同的。

这里也提一下一个可用的工具和库：PCL (Point Cloud Library)[10]。他也是又机器人公司WillowGarage推出的开源库。其中包含了可以实现上述扩展的基础库。可以尝试。

好了，这个系列的文章就告一个段落了，感谢能耐住性子看到这里的朋友们:-) 如果你们愿意，可以在我Blog上的本文留个言让我知道:-) 每次写好这类文章，都感慨写文章要比制作复杂漫长，其中要把自己的想法转化成大家能懂得语言，对我这个表达能力较差的人来说还是蛮辛苦的。

时隔几个月，我blog终于迎来一次比较大的更新。希望自己也能坚持下去，不过，业余时间的确也比以前少了些。不知道我下一篇文章是何时发布呢？当然，我业余时间的另一个投入就是我们RoboPeak团队，大家也可今后在我们团队动态中了解我正在做的事情:-)

目前本制作已经开源，代码托管于google code。请访问项目页面下载

http://code.google.com/p/rp-3d- scanner

参考文献

[1] 自制低成本3D激光扫描测距仪(3D激光雷达)，第一部分
http://www.csksoft.net/blog/post/lowcost_3d_laser_ranger_1.html

[2]: Driverless USB AVR/51 ISP Programmer powered by RoboPeak
http://code.google.com/p/rp-usb-connector/

[3]: 本制作在Google Code的开源项目页面
http://code.google.com/p/rp-3d-scanner

[4]: V-USB, Virtual USB port for AVR microcontrollers
http://www.obdev.at/products/vusb/index.html

[5]: Arduino-Lite, Lightweight AVR library developed by RoboPeak
http://www.robopeak.net/blog/?p=131

[6]: Arduino-Lite Project
http://code.google.com/p/arduino-lite/

[7]: Irrlicht Engine – A free open source 3d engine
http://irrlicht.sourceforge.net/

[8]: MeshLab OpenSource Project
http://meshlab.sourceforge.net/

[9]: Blender OpenSource Project
http://www.blender.org/

[10]: PCL – Point Cloud Library
http://pointclouds.org/