相关的图片：
扫描得到的房间一角
扫描的我
扫描仪实物
本文结构
1 简单介绍了激光雷达产品的现状
2 激光三角测距原理
3 线状激光进行截面测距原理
这里所说的激光扫描测距仪的实质就是3D 激光雷达。如上面视频中展现的那样，扫描仪可以获取各转角情况下目标物体扫描截面到扫描仪的距离，由于这类数据在可视化后看起来像是由很多小点组成的云团，因此常被称之为：点云(Point Clould)。
在获得扫描的点云后，可以在计算机中重现扫描物体/场景的三维信息。
这类设备往往用于如下几个方面：
1) 机器人定位导航
目前机器人的SLAM 算法中最理想的设备仍旧是激光雷达(虽然目前可以使用kinect，但他
无法再室外使用且精度相对较低)。机器人通过激光扫描得到的所处环境的2D/3D 点云，从
而可以进行诸如SLAM 等定位算法。确定自身在环境当中的位置以及同时创建出所处环境
的地图。这也是我制作他的主要目的之一。
2) 零部件和物体的3D 模型重建
3) 地图测绘
现状
目前市面上单点的激光测距仪已经比较常见，并且价格也相对低廉。但是它只能测量目标上
特定点的距离。当然，如果将这类测距仪安装在一个旋转平台上，旋转扫描一周，就变成了
2D 激光雷达(LIDAR)。相比激光测距仪，市面上激光雷达产品的价格就要高许多：
图片: Hokuyo 2D 激光雷达
上图为Hokuyo 这家公司生产的2D 激光雷达产品，这类产品的售价都是上万元的水平。其昂贵的原因之一在于他们往往采用了高速的光学振镜进行大角度范围(180-270)的激光扫描，并且测距使用了计算发射/反射激光束相位差的手段进行。当然他们的性能也是很强的，一般扫描的频率都在10Hz 以上，精度也在几个毫米的级别。
2D 激光雷达使用单束点状激光进行扫描，因此只能采集一个截面的距离信息。如果要测量3D 的数据，就需要使用如下2种方式进行扩充：
5 采用线状激光器
6 使用一个2D 激光雷达扫描，同时在另一个轴进行旋转。从而扫描出3D 信息。
第一种方式是改变激光器的输出模式，由原先的一个点变成一条线型光。扫描仪通过测量这束线型光在待测目标物体上的反射从而一次性获得一个扫描截面的数据。这样做的好处是描速度可以很快，精度也比较高。但缺点是由于激光变成了一条线段，其亮度(强度)将随着距离大幅衰减，因此测距范围很有限。对于近距离(<10m)的测距扫描而言，这种方式还是很有效并且极具性价比的，本文介绍的激光雷达也使用这种方式
图：一字线红色激光器
对于第二种方式，优点是可以很容易用2D 激光雷达进行改造，相对第一种做法来说，他在相同的激光器输出功率下扫描距离更远。当然，由于需要控制额外自由度的转轴，其误差可能较大，同时扫描速度也略低。
这类激光雷达产品目前在各类实验室、工业应用场景中出现的比较多，但对于个人爱好着或者家用设备中，他们的价格实在是太高了。当然，目前也有了一个替代方案，那就是kinect，不过他的成像分辨率和测距精度相比激光雷达而言低了不少，同时无法在室外使用。
低成本的方案
造成激光雷达设备高成本的因素为
使用测量激光相位差/传播时间差测距
高速振镜的高成本
矫正算法和矫正人工成本
对于个人DIY 而言，第三个因素可以排除，所谓知识就是力量这里就能体现了:-) 对于前2个因素，如果要实现完全一样的精度和性能，那恐怕成本是无法降低的。但是，如果我们对精度、性能要求稍微降低，那么成本将可以大幅的下降。
首先要明确的是投入的物料成本与能达成的性能之间并非线型比例的关系，当对性能要求下降到一定水平后，成本将大幅下降。对于第一个因素，可以使用本文将介绍的三角测距方式来进行。而对于扫锚用振镜，则可以使用普通的电机机构驱动激光器来替代。
本文介绍的低成本3D 激光扫描仪实现了如下的成本/性能：
对于精度而言，这个低成本方案足以超过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个因素引入的：
x 变量的原点(示意图中与激光射线平息的虚线和成像平面焦点)的位置未必在成像感光阵列的第一列(或排)上(实际上在第一排的概率非常低)，通过摄像头主光轴的光线在画面中的像素坐标未必是画面中点。
对于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 进行测距的摄像头，其分辨率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"可由如下公式得到:
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 下激光点的距离。对于采用640x480分辨率的摄像头，n 的取值为480。
如果进行180度，步进为1度的3D 扫描，则可得到分辨率为180x480的点云阵列。如果采用0.3度步进，扫描180度，则得到600x480的点云阵
激光光点像素坐标确定和求解
这里讨论如何从摄像头画面中计算出光点的坐标信息，具体来说，要解决如下几个问题：
识别并确定激光光点，排除干扰
确定光点中心的精确位置
先来看问题一，这个问题看似简单，不过实际会有很多问题，比如下面的几幅实际操作中遇到的画面：
图：不同环境和配置下摄像头捕获的画面
上面3幅图像分别是在使用红色激光器摄像头所拍摄到的。(a)的图像比较理想，在于画面中除了激光光点外没有别的内容，虽然可以看到上方有光电发射发出的干扰点，但激光光点仍旧可以通过求出画面中最亮点的方式获取。
(b)画面中出现了日光灯，由于日光灯亮度也较高，从画面上看与激光点中心亮度一致(均为纯白)，对于这个图像，一种办法是同时判断临近像素的色彩，红色激光点的外围均为红色。
(c)画面中，除了激光点外，出现了其他的红色物体，并且部分高光区域也在图像中表现为纯白，此时，上述通过色彩判断的算法也将失效。因此需要有另外的办法。
完美的激光提取算法几乎是不存在的，一个例子就是当画面中出现了2个类似的激光点(另一个来自别的测距仪或者激光笔)，此时单从一副图像上很难做出判断哪个才是正确的光点。
同时，较准确的识别光点也需要硬件设备以及光学设备的合作，具体的细节超过了本文的范畴。这里列举几种可行的办法：
1. 加装滤光片
在使用滤光片的做法，仅保留激光器发射波长的光线进入，从而可以一定程度的避免光线干扰。
2. 调整摄像头曝光时间
调整摄像机曝光率也可以有效去除画面的干扰，例如上图(b)和(c)，对于5mW 的激光器，一定距离内其单位光照强度仍旧比日光强[3](人肉眼可以在室外识别出激光笔照射在地面的光点) ，因此，只要将摄像头曝光率调整的足够短，完全由可能将画面中除了激光点之外的内容剔除。
3. 采用非可见光激光器
例如使用红外激光器，这个做法与遥控器使用红外LED 理由一样，在人造环境中少有红外光干扰。配合红外滤光片，可以有效滤除来自诸如日光灯等的干扰。但是，对于日光和白炽灯， 其中也含有足够强的红外光，无法单纯采用此法。
4. 增加激光器功率
配合曝光率控制，增加激光器发射功率也足以使得画面中仅保留光点，但这样也有危险性，尤其采用点状激光时。
本制作采用了上述的所有方法，将在后文具体介绍。
对于问题(2)，最简单的做法是直接找出光电中最亮的像素的坐标。但是由于前面公式得知，这样的得到的pX 值是整数，计算得到的q 将会有比较大的跳变。因此这里介绍如何将pX变为更加精确的"次像素"级别。
对于这个问题，学术界已有不少的研究，这里推荐参考论文[5]，其中介绍了几种次像素激光光点定位算法的介绍以及分析了他们的优劣。这里也不再重复了。
简单来说，可以认为激光光点的亮度是一个二维的Gauss 函数经过了一次采样得到了画面上的激光点。那么，可以通过拟合或者简单的线性插值/求质心的手段，估计出光点的中心。
本制作使用了简单的质心法求取次像素的激光中心点。
图：采用滤光片后，从白色日光灯画面(右上图)中识别并计算出激光光点中心坐标
可能有人会问这样的估算精确有效吗?一般而言，精确到0.1个像素单位是比较可靠的，他们做到了0.01个像素的可靠定位。
对于线状激光器的求解过程与点状激光类似，区别在于将按照图像的每行(或者每列)分别找出激光光斑的中心。可参考文献[6]，文献[7]给出了一个针对线状激光更优的光点中心提取算法。
摄像头校正
进行激光测距的基本原理非常简单，但在实现中却有很多制约因素。除了前文提到的进行三角测距求解公式中的那些参数需要确定之外，校正摄像头从而得到理想的针孔摄像机模型下的图像也是很重要的环节。
首先要回答的一个问题是：为何要校正摄像头?校正什么参数?
校正的主要理由是实际上目前使用的摄像头并非是前文所提到的针孔摄像机模型。所谓针孔摄像机，简单说原理就和小孔成像类似：光线通过一个小孔后再背后的感光部件上成像。但大家知道，现实的摄像机都是采用光学透镜聚光成像的，并且所用的透镜并非是抛物面(很难加工)，同时，感光芯片也透镜之间也非严格平行[8]。总之，现实就是产生的画面实际上存在扭曲和偏移的。如果直接使用原始摄像机的画面进行测距，势必造成误差。因此需要进行相机的校正，通过校正后获取消除上述画面扭曲和偏移的图像，再用来进行激光测距的相关操作。
图: 摄像头原始画面和经过相机校正后的修正画面
上图左侧图片是一种摄像头拍摄到的原始画面，可以明显看出图像存在着扭曲，对相机校正后，我们可以校正后的参数修正扭曲的画面，得到右侧图像的效果。
对于摄像机校正的具体原理、算法和过程超过了本文的介绍范围，具体信息可以参考如下的，在本文后续的制作部分，也会介绍本次制作的校正过程和结果。
校正和求解三角测距所用参数
前文介绍的三角测距公式中涉及了如下的参数：
Beta:激光器夹角
s:激光器中心与摄像头中心点距离
f:摄像头的焦距
pixelSize:感光部件单位像素尺寸
offset:激光点成像位置补偿值
这些参数有些很难通过实际测量求出，有些很难再安装时就控制好精度。他们数值的精确度会对测距精度有着非常大的影响。例如pixelSize 一般都是微米级别的数值，很小的误差即可导致最终测距的偏差。
对于他们的求解，我们将在测距仪制作完成后进行的校正环节求出。这里的校正， 实际过程是在实现测量好的距离下采集出测距公式中用到的pX 数值。然后通过曲线拟合的方式确定参数。
设备设计
核心元件原型
在第一篇文章的原理介绍[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 UsbConnector[2]来实现这部分功能。它使用单片AVR(Atmega88)芯片通过软件方式模拟出USB 协议栈，并且使用了HID 协议使得无需在PC 上安装驱动程序。对于这部分的细节属于固件实现部分，将在后文提到。
图: 我们RoboPeak 团队的开源USB 方案：RP USB Connector(蓝色PCB)
这里列出本制作中电子系统所实现的功能：
1、实现了一个HID 类的USB 设备，无需第三方驱动程序支持
2、支持通过USB 接受指令，控制舵机运转到指定角度
3、可通过USB 指令，开启/关闭激光器
4、通过一个双米字LED 显示屏，显示当前舵机的角度
5、支持用户通过设备上按钮手工设置舵机角度
对于USB 设备的实现、接收处理USB 指令等属于固件范畴的职责，这里将不做讨论。这里使用了RP USB Connector， 因此其固件已经支持了此部分的基础操作，也可以在他的介绍页面[2]查看详情。
米字LED 数码管的驱动
由于2个米字形LED 含有16多个独立的LED 需要独立控制，RP USB Connector 上并没有如此多空闲IO 口。因此这里的做法是很传统的采用74595串行转并行输出芯片。RPUSB Connector 上通过SPI 总线进行控制。
图：使用2个74595芯片驱动双米字LED 数码管
扩充RP Usb Connector
由于RP Usb Connector 主要功能是做为AVR 芯片编程器和通用USB 开发版的，上面并没有舵机控制和激光器控制功能(需要三极管扩流)。因此需要做一块额外的电路扩充它的功能。电路比较简单，这里就不介绍了，可以参考本章节模块给出的电路图。
图：在RP USB Connector 基础上做扩充电路
制作角度手工控制面板
这部分做的比较粗糙，主要就是在塑料盒子外边加装按钮，就不多介绍了，给一些制作的照片。
图：按钮(正面)
图：按钮(背面)
总装
最后将所有线路连接起来，并关闭盒子，设备的安装就大功告成。
这里给出一个早期的视频：
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。
接下来我将跳出一些典型问题介绍
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 类设备。理由如下：
协议比较简单，通讯单位也是数据包
支持USB1.1低速率规范，可以用v-usb 实现
在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。它的信息请参考第一篇文章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 p
ixel axes = 90.00000 ? 0.00000 degrees
Distortion: kc = [ - 0.13196 -0.05787 -0.00358 -0.011
49 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 standa
rd 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
性能分析
这里主要看看扫描精度实现的情况。如果之前校正用的基准数据没有任何误差(实际不可能)，激光光点提取算法没有问题，那么实际工作时刻的误差主要就体现在拟合的曲线与实际的函数曲线的差距。换句话说，就是拟合得到的参数于实际正确的参数(我们并不知道)。
当然，上面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。相比这个，对于近距离的数据，误差也比较大。对于第二个现象，在前一篇文章猜测这是由于镜头扭曲造成的。但是，大家可能会有疑问了，我的实现不是已经做过相机校正了吗?为何还会有镜头扭曲?这里给出几个可能的解释
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 团队，大家也可今后在我们团队动态中了解我正在做的事情。
版权所有