PCB Blog - Analysis of 3D modeling of lidar based on ZYNQ

In 3D reconstruction, there are two main methods: vision and laser radar. The distance measurement method of vision technology is based on triangulation. The maximum range is 5-8m, which is not suitable for large space, and this method is greatly affected by light. In contrast, lidar can be used in a wider range of scenes. in 3D reconstruction

1.2 Application field

The application of 3D modeling is very extensive, such as the real-time obstacle avoidance and path planning of unmanned aerial vehicles and driverless vehicles. At the same time, it can also be integrated with machine vision, 3D printing and other technologies. It is also a very meaningful application in the reproduction and reconstruction of cultural relics. The laser radar 3D modeling system based on ZYNQ designed this time can go deep into the target environment, collect point clouds, reconstruct 3D space models, measure and other machine vision applications.

1.3 Main technical characteristics

First of all, the system uses the icp algorithm in the pcl point cloud database, which can iterate the data returned by the lidar for many times to achieve accurate registration.

Secondly, the system carries the laser radar PCB on the wheat wheel trolley, transmits the position and speed information of the trolley in real time through the trolley gyroscope and motor encoder, and obtains the real-time accurate positioning of the laser radar through coordinate system transformation.

This design can conduct real-time 3D reconstruction for indoor objects with a distance of 1~5m and less blind areas, and is not easily affected by light. The lidar is mounted on a remotely controlled car and can be used to survey the environment that is inaccessible to people.

1.4 Key performance indicators

1.5 Main innovation points

(1) The trolley can be remotely controlled through mobile app;

(2) The algorithm is based on PCL point cloud library;

(3) Real time data registration can be achieved through Icp algorithm;

(4) Use the PL module of pynq to accelerate the icp algorithm with HLS.

Part II System Composition and Function Description

2.1 Overall introduction

The system is composed of laser radar, STM32 based wheat wheel trolley and Xilinx PYNQ-Z2. The laser radar transmits the collected point cloud data to PYNQ-Z2 through the Internet port, and the McLun trolley is equipped with motor encoder, gyroscope and Bluetooth module. The steering and movement can be controlled by the mobile phone Bluetooth program. During the movement, the displacement and attitude information is transmitted to the STM32 microcontroller, and these information is transmitted to PYNQ-Z2 through the UART protocol STM32. ZYNQ calculates the displacement and attitude offset of the laser radar according to the displacement and attitude information. ZYNQ splices the point cloud data with ICP algorithm according to the offset of attitude and position information, and then transmits the data through the network port.

In this design, we use a small car equipped with a laser radar to carry out mobile scanning, and collect the left, right and top information for reconstruction.

2.2 Introduction to each module

2.2.1 R-Fans-16 lidar

In this system, R-Fans-16 navigation radar is used for lidar acquisition, which realizes 3D detection imaging through 16 line 360 ° scanning. Based on high-precision laser echo signal measurement technology, R-Fans-16 has such technical characteristics as long measurement range (detection capability up to 200m), high measurement accuracy (ranging accuracy better than 2cm), accurate echo intensity (target reflected echo intensity up to 8 bits), and gives consideration to angle coverage and angular resolution in the pitch direction. When running the lidar, the real-time point cloud data is transmitted to PYNQ-Z2 through the network port.

2.2.2 Wheat wheel trolley based on STM32

The wheat wheel trolley is equipped with STM32 single-chip microcomputer. In this experiment, the gyroscope, encoder and Bluetooth on the trolley are used. The gyroscope and motor encoder on the trolley transmit the data to the STM32 single-chip microcomputer through SPI protocol. The single-chip microcomputer calculates the attitude of the trolley and the tire speed, and then uses UART protocol to send the data cycle to ZYNQ in real time at 115200 baud rate. The movement and steering of the car are controlled remotely by Bluetooth.

2.2.3 Coordinate system conversion

The R-Fans-16 navigation radar is used in this design. The data it collects is based on its own coordinate system. The essence of 3D reconstruction is to convert the data in the laser radar coordinate system to the geodetic absolute coordinate system, that is, the spherical coordinate system to the rectangular coordinate system.

Spherical coordinate system is a kind of system that uses spherical coordinates (r, θ，φ） A three-dimensional orthogonal coordinate system representing the position of a point P in three-dimensional space. As shown in Figure 2.2.1, the "radial distance" between the origin and point P is r, and the "polar angle" between the line from the origin to point P and the positive z axis is θ， The "azimuth" between the projection line of the line from the origin to point P on the xy plane and the x axis is φ。

The formula between spherical coordinate system and rectangular coordinate system is converted as follows:

In this design, the coordinates at the start of the car are taken as the coordinate origin of the absolute coordinate system. Later, during the lidar data cycle, a sub coordinate system is established with the lidar as the coordinate origin, and the displacement deflection of the lidar under the initial absolute coordinate system is recorded.

The directions of the three coordinate axes of xyz are consistent with those of the laser radar coordinate system. With the help of the encoder of the motor, the moving speed and direction of the horizontal plane of the laser radar can be measured, and the translation between the laser radar coordinate system and the absolute coordinate can be known; The attitude angle of the laser radar can be measured through the gyroscope to know the rotation between the laser radar coordinate system and the absolute coordinate system. With the help of the two values measured above and the conversion formula between the spherical coordinate system and the rectangular coordinate system, the points in the laser radar coordinate system can be mapped to the geodetic absolute coordinate system.

2.2.4 Point cloud registration (ICP algorithm)

ICP algorithm registration is to match the point sets under two different coordinate systems with their geometric characteristics. It is necessary to solve the rigid body transformation matrix and translation matrix between the target point set and the reference point set, and use the rigid body transformation matrix to act on the target point set to make the two point sets coincide as much as possible. For target point set P and reference point set Q, the conversion formula is:

The above equation is not always true, so we should minimize the objective function

The common methods to solve R and T are SVD and nonlinear optimization. SVD method is used in this design.

The ICP algorithm problem is usually converted into the least squares optimal solution problem, and the whole problem is divided into two parts. The first part is the basis and input of the second part. The first part is called rough registration or global registration. Rough registration is to obtain the approximate coincidence result between point sets by calculating the position and pose between two point sets, so as to provide the appropriate initial value for the next accurate registration. The second part is called precise registration or local registration. For two point sets that are close enough to each other, an iterative optimization strategy is used to achieve the final registration result.

Part III Completion and Performance Parameters

3.1 General

This system has completed the point cloud acquisition of laser radar, the attitude information acquisition of gyroscope and encoder. The PS-PL design used by ZYNQ, the control chip of PYNQ-Z2 development board, greatly improves the convenience and feasibility of system design, and reduces the difficulty of system design. PS-PL master-slave design keeps the simplicity of system design while improving the running speed and processing capacity of the system. The design of the PL IP core greatly speeds up the calculation of the algorithm. The point cloud splicing part of this design is accelerated by the IP core at the PL end, which enhances the splicing effect and successfully realizes the function of real-time 3D reconstruction.

3.2 Completion

In the indoor corridor, we put a row of tables and a fire extinguisher, with walls and windows on both sides.

Every 200 frames are collected as a group of data and stored in the pcd file. The original images generated by each group of data are shown in Figure 3.2.2. The right side of the image is the side where the table is placed. The details of the table top and table feet can be seen. The lower side of the left side is the fire extinguisher placed.

Icp registration is performed for each two adjacent groups of point clouds to obtain a complete three-dimensional model of the car passing through the corridor.

The discrete small point cloud on the right side of the picture is the laser that is projected from the window in the corridor to the outside and returned. If both sides of the corridor are walls, the complete 3D model can be returned.

3.3 Performance parameters

The longer the distance is, the lower the resolution is. The measurement error of object width and depth within the effective detection range depends on the stability of the equipment (here refers to the jitter degree of the laser radar). The measurement error of width is within 2cm, the measurement error of inclined object is 6cm, and the error of inclined angle is 4 °. The trolley itself will shake slightly during the measurement, so the result has certain error.