A precise localization algorithm for unmanned aerial vehicles integrating visual-internal odometry and cartographer

2.1. Coordinate transformation

The construction of precise maps using the Cartographer algorithm requires data information from multiple different sensors, which often requires a certain coordinate transformation before being used by the algorithm. Coordinate transformation helps UAVs align multiple data coordinate systems in different environments, achieving precise positioning and map building functions. Taking the inertial odometer, two-dimensional radar, and depth camera as examples in unmanned aerial vehicles, the coordinate relationship in centimeter length units is shown in Fig. 1.

Fig. 1Schematic diagram of sensor coordinate system

The Cartographer algorithm uses a transformation matrix to achieve coordinate transformation. The algorithm converts the data of each sensor based on the given position and direction of each sensor. Taking the coordinate transformation of radar sensors as an example, its transformation matrix relationship can be represented by Eq. (1):

where $Lidar$ is a point in the LiDAR coordinate system, $map$ is a point in the map coordinate system, $T$ is the translation matrix, and $R$ is the rotation matrix. By using this transformation matrix, the coordinate information of various sensors at different spatial positions is integrated into the same coordinate system. After coordinate unification, the algorithm can continuously calculate the robot’s position based on data from multiple sensors, improving the accuracy of position estimation.

2.2. Local SLAM and global SLAM

The Cartographer algorithm can be treated as two relatively independent and interrelated subsystems. The first is the local SLAM (front-end), which is responsible for establishing a series of submaps, outputting and storing odometers and submaps. The second is global SLAM (back-end), which matches the submaps generated by the front-end according to the timestamp, and then optimizes the global submaps and odometers. The Cartographer algorithm first processes the LiDAR data in the front-end stage. The radar provides a frame of point cloud information every time it scans, and is converted into point cloud data in the radar coordinate system according to Eq. (2):

where $P$ is the position of the three-dimensional point on the radar coordinate system, $\theta$ is the radian required to rotate around the $z$-axis to return to the radar coordinate system. Matching the converted radar data with the previous submap, the position of the point beam falls to the optimal position on the submap after conversion. As the data frames are continuously inserted, they are combined to form a new submap. Subsequently, in order to eliminate sensor measurement errors and improve the accuracy of the map, $T_{\xi }{h}_k$ was optimized through matching algorithms and outputing the optimal pose for matching. This least squares problem can be solved by using the Ceres library in Cartographer according to Eq. (3):

where $M_{smooth}$is a linear evaluation function, $T_{\xi }$ is the position in the submap coordinate system, and $h_k$ is a subset of the square radius of the Submap. Through this optimization method, Cartographer has higher accuracy than raster maps. Therefore, the adjacent two submaps obtained through scan matching during the mapping process are relatively accurate, but there are still cumulative errors compared to the global SLAM. The Cartographer algorithm adopts a branch and bound optimization method for optimization search, which can efficiently search for the best match in the global position space. For candidate positions, only one matching score needs to be calculated, and the pre-calculated values can be used to accelerate this calculation, thereby improving search efficiency and improving the accuracy of mapping.

2.3. Visual inertia odometer (VIO) algorithm

At present, the main navigation systems for UAVs include GPS satellite navigation systems, integrated navigation systems with LIDAR and inertial navigation. However, when the GPS signal is weak or there is significant environmental interference during the navigation process, the effectiveness of the data of these navigation systems is greatly reduced. The VIO algorithm for tightly coupled binocular camera IMU data and visual data can effectively solve the above problems. By aligning the IMU pose sequence with the camera pose sequence, the true scale of the camera trajectory can be obtained. The VIO-SlAM is divided into three parts in this paper: front-end visual inertial navigation odometer, back-end nonlinear optimization, and loop detection, as shown in Fig. 2.

Fig. 2Schematic diagram of VIO-SLAM structure

Adopting the binocular LK optical flow method for feature tracking and built-in IMU pre-integration processing to optimize the tightly coupled VIO algorithm theory, the sliding window is optimized for tightly coupled backend nonlinearity, and loop detection and repositioning are used to eliminate accumulated errors and improve positioning accuracy.

2.4. Improved cartographer algorithm integrating VIO

The improved Cartographer algorithm based on VIO proposed in this paper tightly couples the IMU of binocular cameras and the visual data of binocular fish eyes with VIO, and outputs the odom data information after fusion with the flight control IMU and LiDAR multi-sensor. After preprocessing, it is converted into robot_local_position and then used Kalman filtering to denoise the incoming data, followed by normalization processing. Finally, the processed data is input into the Kalman filter for state estimation, and the position estimation value is calculated based on the state estimation results, greatly improving robustness and accuracy of position estimation. Applying the processed position information to the Cartographer algorithm for mapping can improve the accuracy of the map.

2.5. Optimized tightly coupled VIO position output

Using LK optical flow method for feature tracking in VIO tightly coupled binocular fish eye visual data and pre-integrated binocular camera IMU data, the optimized variables are calculated using Jacobian matrix and covariance matrix through minimum edge residual, visual residual, etc. Finally, loop detection is performed to improve the accuracy of the output position information. The LK optical flow method tracks the movement of image pixels, while $x$ and $y$ represent the position of pixels in the image. By using the least squares method, the position movement of pixels within $\mathrm{\Delta }t$ time can be calculated, as shown in Eq. (4):

The feature quantity to be estimated is the spatial three-dimensional coordinates ${\left(x,y,z\right)}^T$, and the observed values ${\left(u,v\right)}^T$are the coordinates of the feature points on the camera normalization plane. Therefore, the observed values cannot be directly output as position information. It is necessary to transform the matrix and perform inverse depth parameterization to meet the Gaussian system, and calculate the three-dimensional coordinate output. The equation is shown in Eq. (5):

where $\lambda =1/z$ is the inverse depth. Then, the optimized visual position data is tightly coupled with the IMU preprocessed pose data for VIO, thereby outputting more stable position information.

2.6. Position output after fusion of LiDAR and flight control IMU

The Cartographer algorithm integrates local SLAM and global SLAM, and both SLAM processes require optimized position information, which is the scanned position data obtained from radar observations $\left(x,y,\theta \right)$. When the radar is applied to UAVs during testing, the platform cannot achieve decisive stability, so it integrates the IMU (Inertial Measurement Unit) of flight control. IMU will provide the direction of gravity, which is used to project radar data onto the 2D plane, provide the radar with an attitude, and determine the 2D plane scanned by the radar. Mapping navigation requires submap matching, and the construction of submap is an iterative process of continuously aligning Scans with the coordinate system of submap. Scans' data is obtained from each frame of the radar using Eq. (6):

where $R$ is the radius size of the submap. When matching and aligning the obtained Scans’ data with the submap, it needs to be converted into the position $T_{\xi }$ of the radar frame in the submap coordinate system $\xi$. The conversion can be done through Eq. (7):

Through the transformed position $T_{\xi }$ , the constructed submap can be matched to obtain more accurate maps.

2.7. Optimization of position information output using Kalman filtering

The front-end of the Cartographer is responsible for inputting and storing position information data. In order to provide accurate position information for it, this paper constructs a sequence by pruning to ensure that all data timestamps are in order before tightly coupled VIO pose data and LiDAR fusion flight control IMU data fusion. Then, the first and last LiDAR odometer data and VIO odometer data are taken out, and the position transformation relationship at two time points is calculated using Eq. (8):

where $ODO{M}_{old}^{new}$ is the position transformation from the last data to the first data, and $ODO{M}_{new}^{first}$ is the first received VIO or Lidar position data. $ODO{M}_{old}^{first}$ is the VIO or Lidar position data received at the last moment. Calculate the translation from the last data to the first data, and then divide by the elapsed time to obtain the average linear velocity of the $X$, $Y$, and $Z$ axes for that time period. The initial prediction of odom and point cloud position by Cartographer is based on the uniform velocity model, which is often difficult to meet in practical application scenarios. For UAVs, the shorter the running distance, the more capable the uniform velocity model can replace the linear and angular velocities of UAVs. Therefore, using the closest two odometer data for estimation can obtain more accurate odometer data. The position of each timestamp of the UAV can be obtained through an inference device, and its displacement inference mainly relies on the latest line velocity from odom, which is calculated using Eq. (9):

By calculation, Lidar_linear^local and VIO_linear^local can be obtained, both of which can be used to calculate and infer the latest speed information and odometer data in UAV coordinate system. However, due to the different accuracy of sensors in different environments, the calculated UAV line speed is weighted and integrated according to Eq. (10) to obtain odometer data:

where robot_local_position is the latest odometer data, and $M$ and $N$ are the weights of trust for the two postures. The weight of $M$ and $N$ depends on the variance of Lidar_linear^local and VIO_linear^local. Under the conditions of $M$, $N$, and 1, the smaller the data variance, the greater the trust weight.

Obtain the latest odometer information and input it into the Kalman filter for prediction. In Eq. (11), $P_i$ is the current robot_local_position odometer information observation value, $P_{i-1}$ is the previous time value, and during the testing process, the UAV is set to move at a constant speed, so $V_i$$=$$V_{i-1}$ the speed remains unchanged:

The simplified matrix equation yields the predicted pose result $\widehat{x_t}$, where $F$ is the hyperparameter, $\widehat{x_{t-1}}$ represents the predicted pose from the previous moment, and ${\omega }_t$ represents the noise generated during the UAV motion process.

Perform covariance calculation on the predicted pose result $\widehat{x_{t^{-}}}$ from the previous moment, and simplify the equation as shown in Eq. (12):

The covariance of the calculated pose estimation contains $p_t^{}$ the covariance of the prior estimation, $Q$ remains the covariance of the noise:

where, $Z_t$ is the measured value, $H$ is the matrix between the observed value and the measured value, $x_t$ is the observed value, and $V$ is the noise of the observed value. Apply Eq. (13) to calculate Eq. (14), where $Z_p$ is the position measurement value, $Z_v$ is the velocity measurement value, $P_t$ is the position observation value, $V_t$ is the velocity observation value, $\mathrm{\Delta }{P}_t$ is the observation position noise, and $\mathrm{\Delta }{V}_t$ is the observation velocity noise:

In this experiment, only the position is observed without considering the velocity. The observation value is equal to the measurement value, so matrix $H$ is [1, 0]. The observation velocity and noise are both 0, and only the pose measurement calculation is considered. Therefore, Eq. (14) can also be simplified into Eq. (15):

The calculated measurement value $Z_p$ is $Z_t$ in the basic equation (16), and $\widehat{x_{t^{-}}}$ is the predicted value from the previous moment. The accurate pose output $\widehat{x_t}$ is obtained by inputting the corrected estimation result:

The updated Kalman gain Eq. (17) and the updated validation estimation covariance Eq. (18) will yield $k_t$ and $P_t$, respectively, for the next Kalman filtering process. In Eq. (17), $R$ is the variance of the observed noise, which is not considered in this experiment. The value of $R$ is 0, and $I$ is the identity matrix:

The real-time processing of updated position information through Kalman filtering will make it more accurate, providing it to the local SLAM and global SLAM systems in the Cartographer algorithm to achieve higher positioning accuracy and robustness in mapping.

The structure of the improved Cartographer algorithm that integrates binocular VIO is shown in Fig. 3, which is divided into four parts: the VIO algorithm part, the multi-sensor fusion part, the global SLAM, and the local SLAM. Compared to the original Cartographer front-end, the improved algorithm after fusion is responsible for the input, storage, and operation of LiDAR data, and outputs odom data for Cartographer mapping. The improved algorithm solves the problem of inaccurate positioning caused by insufficient point cloud data in complex environments due to map matching using time stamps and the problem of inaccurate positioning accuracy caused by coordinate system drift caused by the rapid movement of the drone when integrating IMU data and LiDAR data for flight control. Combined with the optimized VIO tightly coupled algorithm and Kalman filtering algorithm, it can enhance robustness and adapt to environments with fewer feature points, improve the accuracy of position estimation and ensure the accuracy of the mapping.

Fig. 3Cartographer algorithm framework diagram

3.1. Design of UAV controller

The hardware platform of this paper is a self-built unmanned aerial vehicle, as shown in Fig. 4. The flight controller is PX4 flight control, using Cortex-M7 core and having a master-slave processor. It can communicate with onboard computers through its own UART serial port or USB interface. The embedded controller is Jetson Orin NX, the binocular camera is RealsenseT265, and the two-dimensional radar is Silan A2M12 LiDAR. The structure and algorithm framework of the entire UAV are shown in Fig. 5.

Fig. 5Hardware structure of quadrotor UAV

3.2. Site layout and experimental testing

The flight test environment is located inside the safety net site, with three rectangular obstacles placed around the UAV to provide more point cloud information and facilitate the accuracy of the map. The improved algorithm and original algorithm proposed in this paper are tested, and the effectiveness of the algorithm is verified through various data analysis and comparison. The size of the site is within the radar scanning radius, and the takeoff height of the UAV does not exceed the height of the obstacle. The actual test environment is shown in Fig. 6.

Fig. 6Real flight test of UAV in laboratory