ZYNQ 激光视觉SLAM算法移植与设计

Posted 2022-05-24 信迈科技DSP+ARM+FPGA

ZYNQ上基于FPGA加速的 双目立体视觉 三维重建 生成 实时深度图彩虹图 。
1.在ZYNQ上完成双目采集成像，PS+PL协同处理。
2.在ZYNQ上完成相机标定，镜头校准。
3.在ZYNQ上完成立体匹配算法，目前已实现局部立体匹配BM以及半全局立体匹配SGBM
4.在ZYNQ上完成了立体匹配后的深度图转换彩虹图，即彩色深度图
5.在ZYNQ上也完成了测距算法，但是目前没有识别算法，只能取图像一点计算距离。精度在10mm内。有效距离目前测试在5m内。
6.优化了所有的算法，目前接近720p的图像质量，并且帧率在30fps左右。

详细的实现过程会出一个专栏，每周更一次，从零开始的项目搭建ZYNQ上的实时三维重建。

部分实验过程记录：
标定过程：

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

在三维重建中，主流的方式有视觉和激光雷达两种。视觉技术的测距方法是基于三角的测距法，最大范围是 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 整体介绍

系统由激光雷达、基于 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 完成情况

在室内走廊中，我们摆放了一排桌子、一个消防灭火器，两侧分别为墙壁和 窗户。

每采集 200 帧作为一组数据，存入 pcd 文件，得到每组数据生成的原始图像如图 3.2.2 所示，图像右侧为摆放桌子一侧，可看到明显桌面和桌脚的细节，左侧较低侧为摆放的消防灭火器。

每相邻两组点云进行 icp 配准，得到完整的小车驶过走廊的三维模型。

图片右侧离散的小点云为走廊中的窗户投射到室外返回的激光，若走廊两侧都为墙面，则可返回得到完整的三维模型。

3.3 性能参数

激光雷达点云采集时距离越远分辨率越低，在有效探测距离内对物体的宽度和深度的测量误差取决于设备的稳定性（这里指激光雷达的抖动程度）。宽度测量误差在 2cm 内，倾斜物体的测量误差为 6cm，倾斜角度的误差为 4°。由于测量时小车自身会有轻微抖动，所以结果有一定的误差。

第四部分 总结

可扩展之处

（1）使用 python 编程。本次设计是 ZYNQ 上搭载了 Utunbu18.04，基于 C++++ 语言的编程。使用过程中只能调用 C 语言基本的库中的函数以及 PCL 库函数， 显示并不方便，对使用者并不友好。可将 PCL 移植到 Jupyter 平台，利用 Python 更方便更快捷。

（2）全方位的立体扫描。对于搭载激光雷达的小车来说，只能扫描小车的 左、右和顶三面进行单向的扫描。想要扫描一个特定的物体，则无法了解其立体的信息，可以选择更灵活的搭载物，例如无人机。