Rotating 3D laser mapping system for multi-rotor drones

2.3.1. FAST-LIO algorithm

FAST-LIO is a filter-based, tightly coupled laser SLAM algorithm that uses the lidar and IMU as sensors and does not require an external reference between them. Firstly, the state prediction is performed by forward propagation of IMU measurements, along with point cloud feature extraction in the same way as in the literature ^[9]. Secondly, all the newly formed feature points are transformed to the end moment of each lidar scan frame using the IMU data to remove motion distortion during backpropagation of the state and residual calculation. Then, the Error-sate Iteration Kalman Filter (ESIEKF) sub-module is judged to converge; if not, the updated state is used to continue the iteration to obtain new global points for feature matching again until convergence to the optimal state; otherwise, the global map and odometer will be updated. This tightly coupled approach significantly improves the localization accuracy and map-building effect.

2.3.2. Rotating 3D laser SLAM algorithm

In order to enable the multi-rotor UAV to obtain more information about its surrounding environment, this paper proposes a rotating 3D laser SLAM algorithm based on FAST-LIO, whose workflow is in Figure 3. First of all, the line surface features are extracted from the lidar output and additional motion compensation using the encoder angle and IMU measurement data, respectively; the IMU measurement data are used as the state prediction of the system; then, the structural parameters and external parameters between sensors are combined to update the state and feature matching using the error-sate iteration Kalman filter, and the optimal state is obtained at the end of the iteration as the odometry output while the global map is updated; finally, the odometry output frequency is increased using IMU pre-integration, and the odometer data is smoothed by a first-order low-pass filter to achieve the condition of stable flight of the rotorcraft UAV.

Figure 3. Rotational laser SLAM workflow diagram. IMU: Inertial Measurement Unit; SLAM: simultaneous localization and mapping.

(1) Angular and IMU-based motion compensation

Before the state estimation, the raw data obtained from sensors are firstly preprocessed, and the data preprocessing of the proposed algorithm in this paper mainly contains three parts. The first part is to extract the line and surface features from the original point cloud of the lidar output; the second part obtains the real-time rotation angle information of the lidar through the encoder to complete the first motion compensation. Subsequently, the real-time measurement data of IMU is used for the second motion compensation and state prediction.

(a) Extraction of point cloud feature

The feature point extraction in the rotating 3D lidar SLAM method is the same as in the literature ^[10]. The feature extraction is based on the curvature value of points; the points with larger curvature are considered edge points, and the points with smaller curvature are considered plane points, where $$ S_{p} $$ represents a set of points that are in the same frame as the point $$ X_{(k,i)}^{L}, and X_{(k,j)}^{L} $$ are points that are in $$ S_{p} $$. The curvature $$ c $$ is computed by

(b) Motion compensation

The rotational 3D laser SLAM algorithm requires two motion compensations for the acquired raw point cloud. Firstly, real-time rotation angle information is used to compensate for the motion compensation brought to the lidar by the rotation. In each frame of the lidar scan, the angle values of the start and end moments are obtained, so all feature points during this time should have an angle value, which is obtained by linear interpolation, thus converting the lidar feature points under the rotational coordinate system $$ S $$ to the motor axis coordinate system $$ B $$. The calculation formula is described as

where $$ st $$ represents the beginning of the time period, $$ et $$ represents the end of the time period, $$ ct $$ represents the time of the current feature point, $$ angle-po $$ represents the angle corresponding to each feature point, $$ es-angle $$ represents the angle at the beginning of the current frame, and $$ last-angle $$ represents the angle at the end of the current frame.

Each frame of data acquired by the lidar is from a period in the past rather than a moment in time during which the lidar or its carrier usually moves and results in inconsistencies in the origin of the current frame, so it is necessary to borrow the IMU measurement data to compensate for the second motion and convert the current frame to the last moment in the coordinate system, thereby converting the feature points from the motor axis coordinate system $$ B $$ to the IMU coordinate system $$ I $$. The calculation can be described as

where $$ q_{slerp} $$ represents the quaternion pose of the feature point at the current moment, $$ q_{0} $$ represents the quaternion pose of the first feature point in the time, $$ q_{1} $$ represents the quaternion pose of the last feature point in the period. The process involves converting the quaternion into a rotation matrix and then transforming the feature points into the lidar coordinate system. The two motion compensation schematics are in Figure 4.

Figure 4. Motion compensation process. IMU: Inertial Measurement Unit.

(2) State estimation with rotational external parameters

In this paper, the algorithm of the literature ^[15] is used as the main framework, and the rotation angle information of the lidar is written into the state equation on its original basis. The discrete equation of the state can be described as follows

where $$ x $$ is the system state $$ _{}^{W}R_{I} $$, $$ _{}^{W}P_{I} $$ represents the rotation matrix and position of the IMU in the world coordinate system, $$ _{}^{W}V_{I} $$ represents the linear velocity of the IMU in the world coordinate system, $$ b_{w} $$ and $$ b_{a} $$ represent the gyroscope and accelerometer bias, respectively, $$ g $$is the gravity vector in the world coordinate system $$ _{}^{I}R_{B} $$, $$ _{}^{I}t_{B} $$ represents the rotation and translation of the rotary motor axis and the IMU, respectively, and $$ _{}^{S} \varphi _{L} $$ and $$ _{}^{S} t_{L} $$ represent the rotation and translation between the lidar and the rotary motor, respectively. Note that there are five coordinate systems in the rotary 3D laser SLAM algorithm: the lidar coordinate system $$ L $$, the rotary motor coordinate system $$ S $$, the motor axis coordinate system $$ B $$, the IMU coordinate system $$ I $$, and the world coordinate system $$ W $$.

In this paper, the state is predicted and updated using an error-state iterative extended Kalman filter. The state prediction is first started when the IMU data is received, at which there is the $$ i_{th} $$ propagation of the state $$ \hat{x} $$, and $$ f(x,u,w) $$ is the first-order derivative of the state $$ \hat{x_{i}} $$ to time, assuming that the IMU noise is zero at the start moment. They are both discrete models and can be described as

where $$ \hat{x} $$ is the propagation value and $$ \bar{x} $$ is the update value. $$ w $$ is the IMU Gaussian white noise, and $$ u $$ is the IMU measurement, which can be written as

in this instance, the system state covariance matrix $$ \hat{P}_{i} $$ is calculated by

where $$ \hat{P}_{i} $$ is the covariance matrix of the previous state, and $$ Q $$ is the covariance matrix of the IMU Gaussian white noise, please refer to the literature ^[21] for the detailed derivation process, and $$ F_{\tilde{x}}, F_{w} $$ can be calculated by

the error $$ \tilde{x} $$ in the ground-truth $$ x $$ and propagated values $$ \hat{x} $$ is written as

where $$ G(\tilde{x}_{i},g(\tilde{x}_{i},w_{i})) $$ and $$ g(\tilde{x}_{i},w_{i}) $$ are described as

Then, based on the error between the ground truth at the previous moment and the predicted value at the current moment, an iterative state update is performed and the updated state is output when the update error is less than a threshold or reaches the maximum number of iterations. The residual $$ z $$, Kalman gain $$ K $$, and the Jacobi matrix $$ H $$ are calculated during the state update process by

Where $$ G_{j} $$ and $$ u_{j} $$ are the normal vector to the plane or edge, $$ p_{}^{G} $$ is the point on the normal vector, and $$ p $$ is the plane or edge point. Additionally, $$ h_{j}(x_{k},_{}^{L_{j}}n_{f_{j}}) $$ is residual and can be described as

where $$ _{}^{L_{j}}n_{f_{j}}^{} $$ is noise.

At this point, the updated state $$ \bar{x_{k} } $$, the propagation state $$ \hat{x}_{k}^{k+1} $$, and the covariance matrix $$ \bar{P}_{k} $$ are updated by

where $$ J $$ is the derivative of $$ (\hat{x}_{k}^{k}\oplus \tilde{x}_{k}^{k})-\hat{x}_{k}^{k} $$ with respect to $$ \tilde{x}_{k}^{k} $$. At the first iteration, $$ J=I $$(unit matrix). When the state is updated, each feature point is projected from the lidar coordinate system $$ I $$ to the world coordinate system $$ W $$, and at the same time, these feature points are added to the map that has been generated previously. The conversion formula can be written as

where $$ T $$ is the transformation matrix between coordinate systems (e.g., $$ ^{W}T_{I} $$ the transformation matrix between the IMU coordinate system and the world coordinate system).

(3) High frequency odometry based on IMU and filter

The odometer frequency after the error iterative Kalman filtering optimization is far from the normal flight conditions of the rotorcraft. The IMU has a high output frequency, so this paper first improves the overall interval frequency of the odometry through IMU pre-integration, which can be calculated by

In this case, $$ R $$ represents the rotation matrix between moments $$ i $$ and $$ j $$, $$ v $$ represents velocity, $$ p $$ represents pose, $$ a $$ is the IMU acceleration, $$ w $$ is the IMU angular velocity, $$ b^{a} $$ is the IMU accelerometer zero bias noise, $$ b^{g} $$ is the IMU gyroscope zero bias noise, $$ \eta^{ad} $$ is the accelerometer white noise, $$ \eta^{gd} $$ is the gyroscope white noise, and $$ \bigtriangleup t $$ is the sampling time. The data obtained after boosting the odometer frequency often has fluctuations, so a first-order low-pass filter is used to smooth the odometer data between the formal output odometers, calculated by

where $$ \lambda $$ is the filter factor.

3.3.1. Indoor positioning data validation

The rectangular motion was performed under the indoor motion capture system, and the displacement x-axis, y-axis, and z-axis output results of the rotating 3D laser SLAM odometer, fixed-angle FAST-LIO, and rotating FAST-LIO (Ours, F-FAST-LIO, R-FAST-LIO) were compared with the positioning results (Nokov) of the motion capture system, respectively, as in Figure 8A and B. From the experimental data, it is clear that the proposed method in the maximum positioning error is 0.08 m; the average error is 0.04 m, and the minimum is 0.005 m. The maximum positioning error of fixed angle FAST-LIO is 0.147 m; the average error is 0.062, and the minimum error is 0.005 m. The absolute attitude error (ATE) and root mean square error (RMSE) are also compared; the results are detailed in Table 2.

Figure 8. Comparison of ground-truth location data.

3.3.2. Indoor positioning and mapping experiment

To further verify the effect of the rotating 3D lidar SLAM algorithm in indoor localization and mapping, the localization and mapping experiments were conducted in the underground parking lot in Figure 9A and compared with the FAST-LIO localization and mapping effect in the same scene with a linear motion. The experimental localization data results are in Figure 9B; the mapping effects are in Figure 10A and B below, from which it can be seen that the proposed algorithm can build a complete environmental map to provide more environmental information for the UAV compared with the unrotated laser mapping.

Figure 9. Indoor environment and comparison of indoor positioning data.

Figure 10. Comparison of indoor mapping effect.