三维景深数据采集和图像重构系统研究

吴 健，吴星明，陈伟海

（北京航空航天大学自动化科学与电气工程学院，北京 100191）

0 引 言

移动机器人三维景深信息数据采集和图像重构技术是实现自主导航、路径规划、机器人自定位和地图构建的基础[1]。对移动机器人来说，三维景深信息的处理直接影响到机器人的智能化程度和自主水平[2-3]。

德国学者Wulf[4]提出利用电机驱动激光测距仪运动来增加第三维方法得到了机器人小车周围环境三维重构图。Christian Brenneke[5]也提出在室外环境利用三维激光扫描数据来实现定位与图形绘制的方法。David M.Cole[6]提出了利用激光扫描数据对室外环境进行三维定位与图形绘制的方法。

国内卿慧玲[7]等也提出了利用二维激光测距仪得到三维重构的方法。施华[8]等提出了采用RT-Linux操作系统改进机器人控制实时性效果的方法。

该文提出了另一种系统结构，一个2D的激光测距仪与一个旋转的六棱柱获取距离数据，DSP采集距离数据与电机角度数据，使用ARM与嵌入式Linux系统来对采集到的距离数据与电机角度数据进行处理并显示。

1 三维景深数据处理系统硬件设计

1.1 三维激光景深数据采集系统

项目采用的三维景深数据采集系统由二维激光测距仪LMS291、一个不断旋转的六棱柱和一块DSP板卡构成。在这套系统中，激光源LMS291被垂直摆放并固定，通过电机旋转光源前的六棱柱来获取三维景深数据。同传统的三维数据采集系统相比，由于该系统的光源是固定的，避免了光源移动所带来的误差，因此获得的数据更精准，而且数据量大幅增加。

通过LMS291得到的数据只是一些二维的距离数据，如果想得到带有三维特征的景深数据必须要得到六棱柱的位置信息也就是电机的旋转角度。项目中采用了TI公司的专门用于电机控制的TMS320F2812作为核心芯片获得二维的景深数据和电机角度数据。该芯片具有高达150 MHz的工作主频和丰富的外设资源，片上集成了串口模块、PWM输出和码盘检测的EV模块、AD采样模块等，驱动电路采用3A H桥驱动芯片LMD18200。2812的事件管理器模块通过驱动放大电路控制直流有刷电机运动带动六棱镜旋转从而获得三维景深数据，控制电机选用瑞士Maxon RE40直流有刷电机，功率为150W，完全满足系统的需求，光电码盘采用HEDS 5540，准确度为500线，经DSP4倍频后为2 000线，基本能满足系统的精度要求。

1.2 三维景深图形重构系统

DSP2812获得的数据只是具有三维特征的二维景深数据，如果想要对三维图形进行重构必须得到具有三维坐标的景深数据，并且三维图形的显示需要有一个显示器。根据这些需求系统采用了Intel公司的XScale，采用的是ARM V5TE架构。主要负责整个系统的调度、图形的显示、人机接口以及三维坐标的转换。ARM进行三维坐标的转换需要得到原始的二维景深数据和电机角度数据，因此ARM与DSP2812之间的通信问题也是需要仔细考虑的。项目中采用了64 KB DPRAM（Dual-port static RAM）CY7C028进行ARM与2812之间的通信。

2 三维景深数据处理系统软件设计

2.1 三维激光景深数据采集系统

这部分的软件代码主要考虑的问题是LMS291与DSP2812之间数据通信协议的制定以及DSP2812向双口RAM发送数据程序的编写。综合考虑数据精确度和数据显示实时性2个因素，最终设定了LMS291的工作模式为MM模式，测量距离为8192mm，测量角度范围为100°，测量点的角度分辨率为1°，完成一次线扫描的周期为13.33ms。制定的协议如图1所示。

图1 数据通信协议

DSP2812得到的数据需要传输给ARM做进一步的坐标转换和显示工作，这里采用了64KB DPRAM（Dual-port static RAM）CY7C028作为DSP2812与ARM之间通信的接口。为了提高实时性，采用pingpong传输机制，将双口RAM分成2个分区，每个分区存储一帧也就是一个镜面扫描的数据，在2812写一个分区时，ARM读另一个分区的数据。这样就有效地提高了数据传输的效率，并且避免了由于不同CPU的读写时序不同所带来的数据错误等问题。

DSP2812对双口RAM写数据涉及到2812对外扩RAM的操作以及对数据位置的精确控制，也就是说2812在写数据到双口RAM时，必须保证每个分区的首地址是六棱镜初始位置对应的电机角度，这样才能保证双口RAM每个分区内保存的内容是一帧完整的画面所对应的数据。在2812中，对外部器件的读、写访问都是通过外部接口模块XINTF来实现的[9]，XINTF是一个非多路复用的异步总线接口，它可以将2812的外部接口分为5个存储器映射区。其中XZCS6区对应的内存空间大小为512KB，而使用的双口RAM是64 KB的CY7C028，因此选用XZCS6分区的前64 KB的内存空间作为双口RAM的映射地址。2812对双口RAM写数据的程序流程图如图2所示。

由于ARM上使用了Linux操作系统，在Linux中，所有的硬件设备都像常规文件一样看待，它们可以使用和操作文件相同的、标准的系统调用来进行打开、关闭和读写。用户程序通过相应的系统调用来访问硬件设备，设备驱动程序的作用正是为这些系统调用提供接口。根据功能上的要求，编写了双口RAM的驱动程序。实现的主要功能如下：（1）将双口RAM的内存空间分为2个分区；（2）实现用户空间读取双口RAM内存地址中的数据；（3）选择不同的分区。

图2 双口RAM写数据程序流程图

2.2 三维景深图形重构系统

这部分所要完成的功能主要包括三维景深数据直角坐标系的转换和三维图形的显示。根据功能的需求采用了XScale作为主控制器，使用Linux2.6.9作为操作系统，使用QtE与TinyGL相结合的方法编写了上层应用程序。Qt/E（Qt/Embedded）是用于嵌入式Linux系统的Qt版本。Qt/E去掉了X Lib的依赖而直接工作于Frame Buffer上，因而效率更高，但它并不是Qt的子集，而应该是超集，部分机制（如QCOP等）不能用于Qt/X11中。OpenGL是一套绘图函数的标准，TinyGL是OpenGL的程序库。

首先ARM要从DSP中得到原始的距离数据和电机角度数据。前文提到，为了提高效率，该文将双口RAM分成了2个分区并采用了一种ping-pong传输的机制。在DSP里是对数据进行判断并直接写双口RAM的内存地址实现的，在ARM里由于使用了Linux操作系统，不能直接操作硬件，因此编写了双口RAM的驱动。在应用程序中，由于使用的是多线程的编程方式，为了保证共享数据的完整性，使用了互斥锁来对2个分区进行读取操作，并得到了很好的效果。

如果想显示三维的数据，必须要得到每个扫描点在直角坐标系下的坐标。这样就需要一个将二维的距离数据和电机角度数据转换成直角坐标系下的三维坐标的程序。根据激光源和六棱柱的几何关系推导出了三维直角坐标系的坐标转换公式[10]：根据这一公式在ARM上编写了坐标转换程序。可以看到，这个公式具有大量的三角函数运算，而使用的主控制芯片是Intel的XScale，这款新型高性能、低功耗的微构架兼容ARM V5TE指令集，不过不支持浮点指令集。这是为了节省处理器芯片体积和降低运行功耗，XScale体系结构没有实现昂贵的浮点运算部件和除法部件，这些是嵌入式应用中不常用的运算。当需要这类运算时，要通过软件方法实现，即在上位机交叉编译ARM上运行的程序时，使用支持浮点运算的交叉编译工具链交叉编译程序，这样使用了浮点运算功能的程序才能在ARM上正常地运行。但毕竟ARM不是专门用来做运算的芯片，这种方法的效率与使用浮点运算部件做运算的效率相比是比较低的，需要实时显示三维数据，因此提高计算的效率是十分必要的。这里将正弦函数第一象限的函数值制作一个函数表，在使用到三角函数时使用直接查表的方法，并根据不同象限下的计算公式在程序中进行相应的处理，这样就节省了大量的运算时间，从而使系统的实时性能得到了很大的提高。优化前后延迟时间比较如表1所示。

表1 优化前后延迟时间比较

经过坐标变换得到的数据是原始的三维景深信息，这种信息没有颜色、纹理的渲染，很难识别，给景深图像的处理增加了难度。在计算机图形学中，多边形表面模型因其具有简单、灵活等特性，是最常用的一种三维物体模型表示方法[11]，而三角形表面模型是多边形表面模型中最基本的表示形式。显示的每帧画面都是有很多个小三角形拼接而成的，而平面的法向量能够体现出不同平面的位置关系，根据这一特点按照每个平面法向量的不同值对各个点进行着色，这样就能有效地将不同平面的特征表现出来。

在程序实时运行时，为了能够使画面连续显示，使用了Qt中的定时器功能，每隔1s读取一次双口RAM中的数据，并进行一次窗口重绘，这样就能将重构后的三维图像连续的显示出来。

3 实验与分析

选定了如图3所示的实验场景，在走廊的拐角站定一个人，让系统扫描前方的空间，三维激光测距仪获取到景深信息以后，在嵌入式平台上重构图像，通过嵌入式控制平台的LCD可以观察到重构的图像，通过Qt的界面实现不同工作模式的切换功能。重构后的三维景深图像点云图如图4所示。三维景深图像三角网格图如图5所示。

可以看出，三维景深重构图像被实时处理，并做了颜色的渲染。从图像中可以清晰地分辨出两面墙壁、地面、右面墙壁的门以及站立的人像。

图3 实验场景拐角的走廊

图4 三维场景重构点云图

图5 三维场景重构三角网格图

4 结束语

为了能够实时地采集三维景深数据，该文设计了以TMS320F2812为核心芯片的数据采集卡，数据采集卡与数据处理和图像重构模块之间的通信使用双口RAM并采用了一种ping-pong传输的机制，有效地提高了传输速率。图像重构模块使用TinyGL对原始图像进行了着色，并使用归类算法对重构图像中特征比较明显的平面进行了归类。实验结果证明算法是可行的。

[1]Liu Y，Williams II，Robert L，et al.Integrated control and navigation for omni-directional mobile robot based on trajectory linearization [C]∥ Proceedings of the 2007 American Control Conference，ACC.2007：2153-2158.

[2]周庆瑞，原魁，刘俊承.移动机器人视觉导航控制研究[J].计算机工程与应用，2005（15）：5-8.

[3]陈光伟，向中凡，齐彦龙.基于DSP的全自主机器人运动控制系统的研究[J].西华大学学报：自然科学版，2009，28（6）：29-31.

[4]Wulf O，Wagner B.Fast 3D Scanning methods for laser measurement systems[C]∥14th International Conference on Control Systems and Computer Science Bucharest：Editura Politehnica Press，2003.

[5]Brenneke C，Wulf O，Wagner B.Using 3D laser range data for SLAM in outdoor environments[C]∥IEEE/RSJ International conference on Intelligent Robots and Systems，2003.

[6]Cole D M，Newman P M.Using laser range data for 3D SLAM in outdoor environments[C]∥Robotics and Automation，IEEE International Conference，2006.

[7]卿慧玲，蔡自兴.移动机器人激光雷达数据的三维可视化方法[J].吉林大学学报：信息科学版，2004，22（4）：411-414.

[8]施华，陈一民.基于RT-Linux的实时机器人控制器研究[J].计算机工程与科学，2002，24（6）：91-95.

[9]李志军，宋江鹏，马军.TMS320F2812慢速外设接口的时序控制[J].单片机及嵌入式系统应用，2005（11）：20-22.

[10]Rong L X，Chen W H，Yu S Q，et al.Mathematical model of coordinate transformations for 3D Depth-offield information collection system [C]∥Proceedings of the 6th IEEE InternationalConference on Industrial Informatics.Daejeon，2008：80-85.

[11]Kim J，Lee S，Kobbelt L.View-dependent streaming of progressive meshes[C]∥Proc of the Shape Modeling International Conference，2004：209-220.