如何重建三维空间模型-电子发烧友网

第一部分设计概述

1.1 设计目的

近年来，机器学习，无人驾驶等领域是十分热门的研究话题。在这些领域中，电脑对环境的感知十分重要。因此三维的环境重建是必然的趋势，方便我们实现虚拟世界与现实世界之间的交互。

在三维重建中，主流的方式有视觉和激光雷达两种。视觉技术的测距方法是基于三角的测距法，最大范围是 5-8m，并不适用于较大的空间，且这种方法受光线的影响很大。相比之下，激光雷达可以用于的场景更为广泛。

1.2 应用领域

三维建模的应用很广泛，例如无人机、无人驾驶汽车的即时避障，路径规划。同时也可以与机器视觉、3D 打印等技术也可以融合。在文物复刻和重建也是十分有意义的应用。本次设计的基于 ZYNQ 的激光雷达三维建模系统，可以深入目标环境，进行点云采集，重建三维空间模型，测量等机器视觉方面的应用。

1.3 主要技术特点

首先，本系统采用 pcl 点云库中的 icp 算法，可将激光雷达返回的数据进行多次迭代计算，实现精确配准。

其次，本系统将激光雷达搭载在麦轮小车上，通过小车陀螺仪和电机编码器，实时传输小车的位置和速度信息，通过坐标系变换可得到激光雷达的实时精准定位。

本设计可以对距离为 1~5m，盲区较少的室内物体进行实时的三维重建，不易受光线的影响。激光雷达搭载在可远程控制的小车上，可以用于勘察人无法进入的环境。

1.4 关键性能指标

1.5 主要创新点

（1）可通过移动 app 对小车进行远程控制；

（2）算法基于 PCL 点云库；

（3）通过 Icp 算法可达到实时数据配准；

（4）利用 pynq 的 PL 模块对 icp 算法进行 HLS 加速。

第二部分系统组成及功能说明

2.1 整体介绍

图 2.1.1 是整个系统的实现框图。系统由激光雷达、基于 STM32 的麦轮小车和 Xilinx 公司 PYNQ-Z2 组成的。其中激光雷达通过网口将采集到的点云数据传给 PYNQ-Z2，麦轮小车上搭载了电机编码器、陀螺仪和蓝牙模块。转向和移动可y由手机蓝牙程序控制。在运动过程中，将位移和姿态信息传递给 STM32 单片机，通过 UART 协议 STM32 将这些信息传递给 PYNQ-Z2，ZYNQ 根据位移和姿态信息计算出激光雷达的位移和姿态偏移。ZYNQ 根据姿态和位置信息的偏移将点云数据利用 ICP 算法拼接起来，拼接后用网口传出数据。

在本设计中，我们采用小车搭载激光雷达进行移动扫描，采集左、右和顶三面的信息进行重构。

2.2 各模块介绍

2.2.1 R-Fans-16 激光雷达

本系统中激光雷达采集采用的是 R-Fans-16 导航型雷达，它通过 16 线 360°扫描实现三维探测成像。基于高精度激光回波信号测量技术，R-Fans-16 具备测程远（探测能力最远达到 200m），测量精度高（测距精度优于 2cm），回波强度准确（目标反射回波强度达到 8 位）等技术特点，同时兼顾了俯仰方向的角度覆盖和角分辨率。运行激光雷达时，通过网口将实时点云数据传输给 PYNQ-Z2。

2.2.2 基于 STM32 的麦轮小车

在这个麦轮小车上搭载了 STM32 单片机，本次实验中，利用了小车上的陀螺仪、编码器和蓝牙。小车上的陀螺仪和电机编码器通过 SPI 协议将数据传给 STM32 单片机，单片机通过计算得到小车的姿态和轮胎转速，然后利用 UART 协议，以 115200 的波特率将数据循环实时发送给 ZYNQ。小车的移动和转向利用蓝牙远程控制。

2.2.3 坐标系转换

本设计中采用的是 R-Fans-16 导航型雷达，它采集的数据是建立在自身的坐标系之中的，三维重建的本质是将激光雷达坐标系中的数据转换为大地绝对坐标系，即球坐标系转化为直角坐标系。

球坐标系是一种利用球坐标（r,θ,φ）表示一个点 P 在三维空间的位置的三维正交坐标系。如图 2.2.1 所示，原点与点 P 之间的“径向距离”为 r，原点到点 P 的连线与正 z 轴之间的“极角”为 θ,原点到点 P 的连线在 xy 平面的投影线与 x 轴之间的“方位角”为 φ。

球坐标系与直角坐标系之间的公式转化如下：

在本设计中，以车启动时的坐标为绝对坐标系的坐标原点，此后在每圈激光雷达的数据期间，以激光雷达为坐标原点建立一个个子坐标系，并记录此刻激光雷达在初始绝对坐标系下的位移偏转量。

xyz 三个坐标轴的方向与激光雷达坐标系的轴方向一致。借助电机的编码器，可测得激光雷达的水平平面移动速度和方向，即可得知激光雷达坐标系与绝对坐标之间的平移量；通过陀螺仪，可测得激光雷达的姿态角，以得知激光雷达坐标系与绝对坐标系之间的旋转量。借助以上测得的两个数值以及球坐标系与直角坐标系之间的转换公式，即可将激光雷达坐标系中的点映射到大地绝对坐标系中。

2.2.4 点云配准（ICP 算法）

ICP 算法配准就是要将两个不同坐标系下的点集，利用它们的几何特性匹配起来。需要求解目标点集和参考点集之间的刚体变换矩阵和平移矩阵，利用刚体变换矩阵作用在目标点集，使两个点集尽可能重合。对于目标点集 P 和参考点集 Q 来说，转换公式为：

上式不总是成立的，因此我们要最小化目标函数

求解 R 和 T 常用的方法有：SVD 和非线性优化。本设计中使用了 SVD 的方法。

ICP 算法问题通常转换为最小二乘最优解问题，将整个问题分为两部分，第一部分是第二部分的基础和输入。第一部分称为粗略配准或全局配准，粗略配准即通过计算两点集间的位姿，得出点集间的大致重合结果，为下一步的精确配准提供合适初值。第二部分称精确配准或局部配准，对两个距离足够接近的点集使用迭代优化策略以达到最终的配准结果。

第三部分完成情况及性能参数

3.1 总述

本系统本设计完成了激光雷达的点云采集、陀螺仪、编码器的姿态信息采集。PYNQ-Z2 开发板的控制芯片 ZYNQ 使用的 PS-PL 设计极大地提高了系统设计的便捷性、可行性，降低了系统的设计难度。PS-PL 主从设计在保留系统设计的简易性同时提高了系统的运行速度和处理能力。PL 端 IP 核的设计极大地加快了算法的计算速度。本设计的点云拼接部分通过 PL 端的 IP 核加速，增强了拼接效果，成功地实现了实时三维重建的功能。

3.2 完成情况

在室内走廊中，我们摆放了一排桌子、一个消防灭火器，两侧分别为墙壁和窗户，具体场景如下图 3.2.1 所示。