Map Construction and Path Planning Method for a Mobile Robot Based on Multi-Sensor Information Fusion

Abstract

In order to solve the path planning problem of an intelligent vehicle in an unknown environment, this paper proposes a map construction and path planning method for mobile robots based on multi-sensor information fusion. Firstly, the extended Kalman filter (EKF) is used to fuse the ambient information of LiDAR and a depth camera. The pose and acceleration information of the robot is obtained through the pose sensor. The SLAM algorithm based on a fusion of LiDAR, a depth camera, and the inertial measurement unit was built. Secondly, the improved ant colony algorithm was used to carry out global path planning. Meanwhile, the dynamic window method was used to realize local planning and local obstacle avoidance. Finally, experiments were carried out on a robot platform to verify the reliability of the proposed method. The experiment results showed that the map constructed by multi-sensor information fusion was closer to the real environment, and the accuracy and robustness of SLAM were effectively improved. The turning angle of the path was smoothed using the improved ant colony algorithm, and the real-time obstacle avoidance was able to be realized using the dynamic window method. The efficiency of path planning was improved, and the automatic feedback control of the intelligent vehicle was able to be realized.

1. Introduction

With the rapid development of computer science, sensor technology, and other high-tech industries, mobile robot technology is developing rapidly. It has been applied to the fields of logistics, agriculture, and service, among others [1]. As the key technology of mobile robot technology research and development, navigation technology mainly includes simultaneous positioning, mapping, and path planning. SLAM needs to complete robot positioning and mapping tasks in the location environment, while path planning aims to generate collision-free paths in a complex environment to ensure the mobile robot can move [2,3].

The key technology of SLAM is to construct the robot’s pose and environment map in an unknown environment. According to the position and map information, the unknown movement of the vehicle is estimated, and the map is constructed incrementally along with the movement process to realize the autonomous navigation and obstacle avoidance functions of the vehicle. Depending on a single sensor for real-time positioning and mapping, the robustness of the system is poor. The method of multi-sensor fusion can improve the accuracy and robustness of the system. Yang et al., proposed a method that presents a dimension reduction preparation of a two-dimensional grid map through discussion, which involves a cloud camera coordinate relationship with the real space ground [4]. Zhong et al., proposed a binocular visual-inertial odometry method based on a semi-direct method. This method combines the inertial navigation data and the visual information of the binocular camera to construct a high-precision map, which reduces the calculation of measurement values and improves the work efficiency of the system [5]. Ye et al., used a graph-based optimization method to fuse LiDAR and IMU data. This method improves the accuracy of map construction and location [6]. Huang et al., adopted camera information and used inter-frame matching to constrain the data association between point clouds and improved the motion trajectory estimation method by iterating the closest points. However, the accuracy of constructing maps was poor [7]. Chen et al., installed the 2D LiDAR vertically on the robot and matched it with a depth camera for data collection to construct a high-precision map, but the positioning accuracy of this method is poor [8]. Multi-sensor fusion can provide better data support for mobile robot positioning mapping and make up for the defects of single-sensor detection.

Path planning is based on multi-source perception information. The feasible path to the destination is searched for with a certain update frequency, and an optimal target path is iterated from the initial point to the destination on the premise of ensuring safety. Among the path planning methods, the two-dimensional geometric search method is the most mature path planning method. In addition, there are some widely used algorithms, such as Dijkstra [9], A* [10], and Basic Theta* [11]. Xiong et al. proposed a large-scale adaptive ocean V-ACO path planner to improve the heuristic function of the ant colony algorithm to find the collision-free optimal trajectory. This method can find the optimal path with more sampling values [12]. Eduardo et al., proposed the dynamic obstacle dynamic window algorithm and the dynamic obstacle tree dynamic window algorithm to improve the safety of avoidance operations using two restricted occupancy grids [13]. Wang et al. proposed a hybrid path planning algorithm based on the A* algorithm and the dynamic window method. This method solves the problem of path planning in a complex multi-objective environment and improves the efficiency of autonomous navigation [14]. Cheng et al. proposed a hybrid path planning algorithm combining the A* algorithm and the dynamic window method. The resulting path is smoother, has global optimality, and improves dynamic obstacle avoidance and autonomous navigation capabilities [15]. The indoor environment is complex, and therefore the fusion algorithm has good robustness and real-time performance in solving the path planning problem, which is beneficial for improving the autonomous navigation ability of the vehicle.

In the aspect of sensor data fusion, the filtering algorithm is usually used for data fusion [16,17,18,19]. On the basis of the Sage-Husa adaptive filtering algorithm, through the Cholesky decomposition of the posterior covariance estimation matrix existing in the extended Kalman filter SLAM algorithm, Cheng Lu et al. greatly improved the operating efficiency and estimation accuracy of the system [20]. Li Yan et al., proposed a multi-sensor fusion mapping method. The EKF is used to fuse LiDAR and depth camera data information, and the generated map is fused twice through Bayesian estimation. This method makes full use of the redundant information of the sensor to the target and improves the robustness of SLAM [21].

In order to solve the path planning problem of intelligent vehicles in unknown environments, we designed a method of map construction and path planning for mobile robots based on multi-sensor information fusion. The main innovations are as follows: (1) In the face of an unknown environment, a real-time positioning and mapping method based on the fusion of LiDAR, depth camera, and inertial measurement unit is proposed. (2) A hybrid path planning method is proposed on the basis of the combination of the improved ant colony algorithm and the dynamic window method. Using this method, the path turning angle can be optimized, the collision with obstacles can be avoided, the efficiency of path planning is improved, and the automatic feedback mechanism of a mobile robot is realized.

2. SLAM Based on Multi-Sensor Fusion

In the field of robot navigation, the problem of SLAM can be simply described as follows: A robot moving in an unknown environment can estimate its own pose according to the pose and map information and can build an incremental map in the process of moving to achieve autonomous obstacle avoidance and navigation [22]. In order to describe the SLAM problem, the motion of the vehicle in a continuous period of time is decomposed into discrete moments t = 1, 2, ∙∙∙, k, and the corresponding moment positions x₁, x₂, ∙∙∙, x_k constitute the motion trajectory of the vehicle. For the nonlinear system in the actual motion process, the motion model and observation model of the system can be expressed as follows:

$$\{\begin{array}{l}{x}_k=f\left(x_{k-1}, u_k,w_k\right)\\ z_k=h\left(x_k,v_k\right)\end{array}$$

(1)

where x_k represents robot pose, z_k represents the observed value of the system, f (∙) is the system motion equation function, h (∙) is the system observation equation function, u_k represents the control value of the system, w_k represents system process noise, and v_k stands for system observation noise.

2.1. Description of Algorithm

On the basis of 2D LiDAR and depth vision sensors, this paper proposes a SLAM framework for autonomous vehicle localization and mapping based on multi-sensor information fusion to make up for the problem that low obstacles in the environment cannot be perceived. The algorithm realizes the information fusion of 2D LiDAR and depth vision sensor, improves the vehicle’s environmental perception ability, increases the vehicle’s ability to detect obstacles in a complex environment, avoids the disadvantage of low detection accuracy due to the performance of single sensor, and improves the SLAM accuracy and stability of a mobile robot. The overall framework of the algorithm is shown in Figure 1.

The overall process of the algorithm is mainly divided into the following four parts:

• Sensor calibration: The camera, LiDAR, and IMU are calibrated in time and space, and therefore the data information output by the sensor is on the same plane and time.
• Data preprocessing: Data processing of laser point cloud and camera information. The IMU is pre-integrated to obtain the relative pose transformation matrix of the vehicle, which provides the transformation basis for the LiDAR point cloud frame matching.
• EKF data fusion: The extended Kalman filter is used to fuse the laser detection data and the visual detection data to generate a local map.
• Map construction and loopback detection: The real-time pose of the robot is used as the center of the circle, R is the radius to delineate the detection circle, and the laser point cloud ICP algorithm is used to perform loopback detection on the robot trajectory.

2.2. Data Preprocessing

The data processing of the depth camera is used to convert the image data obtained by the depth camera into laser data in order to complete the data fusion between sensors. The principle of converting the depth image acquired by the depth camera into laser data is shown in Figure 2. The conversion steps are as follows:

• The obtained depth image is screened for the effective area to obtain the depth image (u, v) to be processed.
• Through the depth image (u, v) and the camera parameter model, the position coordinates M (x, y, z) of each pixel of the depth camera in the world coordinate system are obtained.
• Project the spatial point cloud (x, y, z) into the corresponding laser scanning range, and calculate the angle AOD between the straight lines AO and DO; the calculation formula is shown in (2):

  $$\theta =arctant\left(\frac{x}{z}\right)$$

  (2)

The scanning of the laser is [$\epsilon$, $\zeta$], the laser beam is divided into N parts, and the laser data are represented by laser [N]. The index value n of the point M projected into the array is

$$n=\frac{\theta -\epsilon }{\left(\zeta -\epsilon \right)/N}=\frac{N\left(\theta -\epsilon \right)}{\zeta -\epsilon }$$

(3)

The value of laser [N] is the distance r from point D on the x-axis to the center of the camera’s light spot.

$$laser \left[N\right]=\mathrm{r}=\mathrm{OD}=\sqrt{z^2+x^2}$$

(4)

2.3. Data Fusion Based on EKF

When a depth camera or LiDAR is used alone for map construction, there will be a certain error due to a single sensor defect. Therefore, this paper used the EKF to fuse the depth camera data and LiDAR data for mapping to improve the accuracy of the mapping. When using the extended Kalman filter for data fusion, the measurement accuracy of LiDAR is higher than that of the depth camera, and thus the measurement value of the depth camera is taken as the system prediction value at the current moment, and the measurement value of LiDAR is taken as the system measurement value, so as to ensure the accuracy of data fusion. The process of EKF data fusion is as follows:

• Forecast section. First, initialize the current position and covariance error with the initial position of the robot. Use the depth camera on the vehicle to collect data on vehicle motion. This value is used as the current moment system input u_k to predict the pose of the next moment, and the predicted error covariance is calculated. Predict x_{k|k − 1} according to the current pose:

  $$x_{k|k-1}=f\left(x_{k-1|k-1},u_k\right)$$

  (5)

  $$P_{k|k-1}=A_k{P}_{k-1|k-1}{A}_k^T+Q_k$$

  (6)

  where A_k is the Jacobian matrix of the state equation $A_k=\frac{\partial f}{\partial x}{|}_{x_{k|k-1},u_k}$, and Q_k represents the covariance matrix of the Gaussian noise of the predicted state.
• Data association. Before the data fusion of the depth camera and the LiDAR, the data of the two sensors need to be correlated to prevent the mis-matching of the sensor data and the inability to build a complete map. According to the Mahalanobis distance between the observation information obtained by the sensors, the correlation of the data obtained by the two sensors is judged. It indicates that the data correlation is high when the distance is less than the set threshold x².

  $$\begin{gathered} D_{ij}^2={\tau }_t^T{P}_{k|k-1}{\tau }_t<x_{d,\alpha }^2 \\ {\tau }_t=Z_l-Z_c \end{gathered}$$

  (7)

  where d = rank(${\tau }_{ij}$), α is the reliability,${\tau }_t$ is the measured position error, Z_l is the measured value of the LiDAR, and Z_c is the measured value of the depth camera.
• Update section. First, calculate the Kalman gain K_k, and use the measured values of the multi-sensors to correct the previous system predictions, that is, to use the position information obtained by the LiDAR to correct the positioning data obtained by the camera for data fusion. At the same time, the optimal estimation value of sensor data fusion is calculated by Kalman gain, and the update of the covariance matrix is completed.

  $$K_k=P_{k|k-1}{H}_k^T{\left[H_k{P}_{k|k-1}{H}_k^T+R_k\right]}^{-1}$$

  (8)

  $$x_{k|k}=x_{k|k-1}+K_k\left(z_k-h\left(x_{k|k-1}\right)\right)$$

  (9)

  $$P_{k|k}=\left(I-K_k{H}_k\right)P_{k|k-1}$$

  (10)

  where H_k is the transformation matrix, and R_k represents the covariance matrix of the Gaussian noise of the measurement.

2.4. Map Construction and Loop Closure Detection

Using the fused sensor data, the algorithm based on graph optimization is used to construct the map after the sensor data processing is completed. The SLAM algorithm framework based on graph optimization is shown in Figure 3.

Loopback detection. Use the ICP algorithm to match the current laser point cloud information with the sub-map, obtain the pose transformation relationship, and form a closed-loop constraint inserted into the back-end optimization. The closed-loop detection process is to judge the similarity between the relative position of the mobile robot in the laser point cloud and the relative position of the carrier in the sub-map through the detection mechanism. The optimal candidate pose is calculated by optimization means, and the paranoia is eliminated to complete the loop closure detection.

Assume that the LiDAR feature point of the current frame is P_i. The carrier position coordinate point obtained by EKF fusion is the center of the circle, and R is the radius for pose distance detection. The detected point cloud of the target frame whose distance from the current pose is less than R is taken as the point cloud P_j to be detected. The detected point cloud of the target frame whose distance from the current pose is less than R is taken as the point cloud P_j to be detected. Then the ICP algorithm is run on P_i and P_j to output pose vector f, and the loopback is considered to be detected. After obtaining f, the position k_t of the detected pose in the historical pose can be calculated. Finally, optimize and correct the vehicle pose between the k_{t −1} and k_t frames, and output the corrected pose trajectory and map.

3. Path Planning for Mobile Robots

3.1. Global Path Planning Based on Improved Ant Colony Algorithm

This paper proposes two improvements for traditional algorithms in terms of slow convergence speed and local optimization problems. (1) Use an improved heuristic function to provide an initial selection of the ant colony to guide the direction. (2) Use a hybrid strategy to update the pheromone.

• Initial state: In the initial state, the probability of the ants choosing a feasible path is equal, and the ant will determine the direction of the next node according to the pheromone content of the state transition point on the selection path. Setting the state transition probability of ant b from point I to point g on the grid graph as $p_{ig}^{\left(b\right)}\left(t\right)$ , the pheromone content T_ig(t) from point i to point g, heuristic function N_ig(t), pheromone heuristic factor a, and expectation the heuristic factor w are determined. Then the state transition probability formula is as follows:

  $$p_{\mathrm{ig}}^{\left(\mathrm{b}\right)}\left(\mathrm{t}\right)=\{\begin{array}{l}{T}_{ig}^a·{ N}_{ig}^w\left(\mathrm{t}\right)/{\displaystyle {\displaystyle \sum }_{g\in \mathit{all}\left(i\right)}}{T}_{ig}^a·{ N}_{\mathrm{ig}}^w\left(\mathrm{t}\right)\\ 0, \mathit{otherwise}\end{array}$$

  (11)

  where all(i) represents the set of transfer nodes that ant b can select at node i, and N_ig(t) is inversely proportional to the distance l_ig between nodes i to g.
• Improved heuristic function: On the basis of the square of the sum of the distance from the current node to the next node and the distance from the next node to the target in [23], the heuristic function is introduced to speed up the search speed of the ant colony, which is

  $$N_{ig}\left(t\right)=1/{\left(\sigma ·l_{ig}+\left(1-\sigma \right)·l_{iE}\right)}^2$$

  (12)

  where σ ∈ [0, 1], determined by the real-time environment; l_ig represents the distance between the current node and the next node; and l_iE represents the distance between the current node and the target point, and increases the ant colony’s selection and guidance effect on the next target point, which helps to reduce the search time of the ant colony.
• Pheromone update: In order to speed up the search time of ants, to reduce the probability of choosing the route that has been taken, and to avoid the pheromone concentration being too low or accumulating after volatilization, it is necessary to determine the maximum value T_max and the minimum value T_min of the pheromone concentration between two points. In this paper, the maximum and minimum values of pheromone were determined according to the volatilization degree and optimal path of the pheromone, which is

  $$\{\begin{array}{c}{T}_{max}=\left[1/2\left(1-\lambda \right)\right]\times \left[1/D_s\right]+1/D_s\\ T_{min}=T_{max}/c \end{array}$$

  (13)

  where D_s is the optimal path length after a certain iteration, and c is the number of cycles each time.

After the ants finish each cycle, the pheromone information on the path changes. In order to obtain the optimal path, the pheromone on the entire path needs to be adjusted. The update conditions are as shown in Equation (14). At the same time, in order to prevent the ant colony from using the pheromone on the local optimal path for path planning, the pheromone concentration increment of the b-th ant between the two points adopts the update strategy shown in (16) to ensure that each ant can find a feasible path.

$$T_{ig}\left(t+1\right)=\left(1-\lambda \right)·T_{ig}\left(t\right)+\Delta T_{ig}\left(t\right)$$

(14)

$$\Delta T_{\mathit{ig}}\left(\mathrm{t}\right)={\displaystyle \sum }_{\mathrm{b}=1}^{\mathrm{m}}\Delta T_{\mathit{ig}}^b\left(\mathrm{t}\right)$$

(15)

$$\Delta T_{\mathit{ig}}^b=\{\begin{array}{l}{T}_{\mathit{min} }\Delta T_{\mathit{ig}}^b+\left(1-\mathsf{\lambda }\right)T_{\mathit{ig}}^{b-1}{<T}_{\mathit{min}}\\ {1/r}_s{ T}_{\mathit{min}}<\Delta T_{\mathit{ig}}^b+\left(1-\mathsf{\lambda }\right)T_{\mathit{ig}}^{b-1}{<T}_{\mathit{max}} \\ T_{\mathit{max}}\Delta T_{\mathit{ig}}^b+\left(1-\mathsf{\lambda }\right)T_{\mathit{ig}}^{b-1}{>T}_{\mathit{max}}\end{array}$$

(16)

where λ represents the pheromone volatilization coefficient, λ ∈ [0, 1]; ΔT_ig(t) represents the pheromone increment between node i and g; m represents the pheromone increment of the b-th ant between nodes i and g, m ∈ [1, 50]; and r_s is the optimal solution found thus far.

3.2. Path Smoothing Based on Cubic B-Spline Curve

The path planned by the traditional ant colony algorithm forms a certain peak at the turning point, which increases the turning angle of the path to a certain extent. Therefore, the global path is smoothed by the cubic B-spline curve to ensure the smooth and safe passage of the smart car at the turning point, as well as to satisfy the geometric constraint characteristics of the smart car. The smoothing process of third-order B-spline curve is shown in Figure 4.

3.3. Local Path Planning

Global path planning can plan the global optimal solution of the route, but it needs to know the accurate information of the environment in advance. The intelligent robot platform integrates the obstacle information into the environment map in real time through its own environmental sensors, when the driving environment of the intelligent robot changes, such as when an unknown obstacle appears. Then, the driving path of intelligent robot platform is replanned by the local path planning method. This is to avoid obstacles and complete local obstacle avoidance. After completing the local obstacle avoidance, the intelligent robot platform continues to drive according to the global optimal solution.

The dynamic window algorithm can realize trial path planning and real-time obstacle avoidance. In the running process of the smart car, there are no groups of velocity spaces (v_t, ω_t), and the feasible trajectory of the robot can be simulated within the window region time t. At the same time, the feasible trajectory is evaluated according to some metrics, so as to select the speed corresponding to the optimal trajectory to the mobile platform. In view of the existence of several feasible simulated motion trajectories in the velocity space, this paper calculated the evaluation function combined with the global path planning information to ensure that the final local path planning is the global optimal path. The designed evaluation function is as follows:

$$\begin{array}{c}G\left(v,\omega \right)=\mu ·goal\left(v,\omega \right)+\beta ·d\left(v,\omega \right) \\ +d\left(v_m,{\omega }_m\right)+\lambda v_e\left(v,\omega \right)\end{array}$$

(17)

where goal(v, ω) represents the angle between the vector pointing to the goal and the vector connecting the starting point and the current position; d(v, ω) represents the vertical distance between the static obstacle and the current trajectory pointing to the target; d(v_m, ω_m) represents the vertical distance between the unknown obstacle and the current pointing target trajectory; v_e(v, ω) represents the evaluation function of the current speed pointing to the target trajectory; and μ, β, ϕ, and λ are expressed as weighting coefficients.

3.4. Path Planning Combined with Improved Ant Colony Algorithm and Dynamic Window Algorithm

The flow chart of the fusion algorithm in this paper is shown in Figure 5:

• Use on-board sensors to obtain environmental information to realize the vehicle’s own positioning and build a raster map.
• Use the improved ant colony algorithm to complete the global path planning of the intelligent vehicle and obtain the optimal path.
• According to the new obstacle information obtained by the vehicle sensor in real time, integrate the information into the grid map, and use the dynamic window method to complete the local obstacle avoidance.
• Combine the global planning path, plan the local moving target points in real time, and use the preview tracking method to track the target points according to the control parameters in the path planning process to achieve real-time obstacle avoidance and obtain the local optimal path.
• Determine whether the local moving target point is the final target point; if not, jump to Step 1. If yes, the intelligent vehicle reaches the target point, and the algorithm ends.

4. Analysis and Verification

4.1. ROS-Based Experimental Platform

In this paper, the algorithm was verified and analyzed on the ROS-based intelligent real vehicle platform (DrivingPo). The four in-wheel motors were used in the platform as the drive system, and the platform was equipped with 2D-LiDAR (FS-D10), which was installed horizontally at the front and rear ends of the platform. The depth camera (Kinect V2), Inertial Navigation, Microcomputer, and other equipment were equipped on the platform. The algorithm running environment was Ubuntu18.04, using an AMD Ryzen3 2200G processor (Zhongheng Dayou Automobile Technology Co., Ltd. Beijing, China.) and 4 GB running memory. The ROS intelligent vehicle platform and the system framework diagram are shown in Figure 6.

4.2. Map Building Experiment

The building diagram is shown in Figure 7. By adding cartons as obstacles, the basic experiment environment in the laboratory was built, as is shown in Figure 7a. The height of obstacle A was lower than the installation height of the vehicle-mounted LiDAR. In practical applications, since the information obtained by the two-dimensional LiDAR is affected by the installation location, the depth camera will be affected by the ambient light, and thus there will be obstacles that are missed and the environmental information will be missing, resulting in low accuracy of the constructed environmental map information. Therefore, a map construction experiment was carried out in a simulated environment to verify the reliability of the method proposed in this paper. Figure 7b maps the environment using only LiDAR. It can be seen from the figure that due to the influence of the installation position of the LiDAR, the obstacle A cannot be detected, and therefore the ground information obtained by scanning was incomplete, resulting in low mapping accuracy. Figure 7c maps the environment using only the depth camera. Figure 7d maps the environment after data fusion. When a depth camera is used for map construction, more environmental information can be obtained than LiDAR. However, the detection accuracy of the depth camera is easily affected by ambient light, resulting in poor target detection accuracy and blurred map construction.

Through the method in this paper, the detection data of the depth camera and the LiDAR were fused to construct a map; the map after data fusion is shown in Figure 7d. Through the comparison, it can be found that after the fusion of the data of the two sensors, the limitation of the environment detection by a single sensor was able to be effectively compensated for. The interference of the complex environment can be overcome through the complementarity between the sensors, so that the constructed map information is more complete and accurate, and the environment map is closer to the real environment.

4.3. Path Planning Experiment

4.3.1. Simulation Experiment Verification and Analysis

In order to verify the effectiveness and robustness of the fusion algorithm in this paper, the proposed algorithm was simulated and verified in the MATLAB environment. The improved ant colony algorithm in this paper was compared with the basic ant colony algorithm, RRT algorithm, and the algorithm in reference [24]. The simulation results are shown in Figure 8.

As can be seen from Figure 8, all four algorithms were able to find drivable paths during the process of running the search. The path planning curve of the RRT path planning algorithm is shown in Figure 8c, and the path length was 34.9690 m. It can be seen from the figure that compared with the traditional RRT path planning algorithm, the ant colony algorithm was more efficient in exploring path planning with short and straight paths, as shown in Figure 8a,c. The path planned by the ant colony algorithm will form a certain peak at the turning point, which increases the turning angle of the intelligent robot to a certain extent, affecting the passing efficiency of the intelligent robot. In this paper, by comparing the polynomial fitting curve and the cubic B-spline curve optimization path experiment, we found that the polynomial method only deals with the peak of the path. However, the cubic B-spline curve flattened the overall path, smoothed the turning angle of the path, and accelerated the convergence iteration speed, as shown in Figure 9. Figure 8d is the iterative convergence trend diagram of the three ant colony algorithms. It can be seen from the figure that the ant colony algorithm proposed in [20] had a faster convergence speed, and the optimal path length obtained in the 13th iteration was 29.1848 m. In the 11th iteration of the improved ant colony algorithm proposed in this paper, the shortest path was 28.0416 m. In the 11th iteration of the improved ant colony algorithm proposed in this paper, the shortest path was 28.0416 m. The algorithm had less fluctuation in the initial stage, which improved the convergence speed. Therefore, the improved ant colony algorithm in this paper had better iterative convergence, which increased the smooth and safe passage of the intelligent robot.

The starting point S, the target point T, and the unknown obstacle were set in the raster map, and the fusion algorithm in this paper was used to complete the simulation experiment of different positions of the two unknown obstacles. The experimental results are shown in the Figure 10.

As can be seen from Figure 10, the improved ant colony algorithm first set the global optimal path, while the dynamic window algorithm detected the position of obstacles according to the on-board sensors and set local target points on the global path. If an unknown obstacle is detected, the dynamic window method will set local target points combined with the evaluation function of global path information and will track the local target points until the target point is reached. When avoiding local obstacles, the dynamic window method will ensure that the intelligent robot will not collide with the obstacles, and the direction of real-time tracking of local target points will be consistent with the direction of the target points. During the operation of the intelligent robot, the control parameters will be output in real time according to the number of control nodes, which is convenient to realize the automatic feedback control of the intelligent robot. When encountering unknown obstacles, the intelligent robot starts to slow down for local obstacle avoidance. At the same time, the intelligent robot outputs control parameters in real time according to the number of control nodes to change the intelligent robot pose angle change. When the local obstacle avoidance is finished, the intelligent robot adjusts the running speed of the intelligent robot according to the real-time output control parameters and gradually reaches a stable running speed. The intelligent robot controls the parameter feedback quantity, as shown in Figure 11. Therefore, the local path algorithm integrating global path planning can set local target points according to the optimal global path and can track the local target points according to the control parameters fed back in real time during the path planning process. It shortens the time of path planning and improves the efficiency of path planning. At the same time, the global optimality is always maintained, achieving the purpose of designing the fusion algorithm in this paper.

4.3.2. Real Vehicle Test Verification and Analysis

In the real vehicle test, the basic indoor environment was simulated by adding a card-board box as an obstacle. The ROS smart car with sensors was used to build an environment map, and the real-time operation status of the intelligent robot was observed in the Rviz visualization window. According to the constructed real-time environment information, the path planning algorithm proposed in this paper was verified and analyzed, as shown in Figure 12.

As shown in Figure 13a, both the traditional ant colony algorithm and the improved ant colony algorithm were able to avoid obstacles and reach the target position in the grid graph formed in the real closed environment in a safe and stable manner. However, by improving the heuristic function of the traditional ant colony algorithm and the hybrid strategy, the shortest path effect in the global path planning was found to be better, and the smooth and safe passing speed of the intelligent robot was accelerated. The trajectory and error parameters of the intelligent robot are shown in Figure 14. It can be seen from the figure that when the intelligent robot travelled to the range of 1.6 to 2 m, the error between the actual path and the planned path was about 4 cm at most due to the influence of low obstacles, indicating that the improved ant colony algorithm is more accurate. In order to verify the robustness and accuracy of the algorithm in this paper, the global path planning of the intelligent robot was completed according to the sensors carried by the intelligent robot to detect the driving environment, as shown in Figure 13a. Then, position obstacles were set on the global path, as shown in Figure 12b. When there was a positional obstacle, the intelligent robot adopted the dynamic window method to complete the local obstacle avoidance during the driving process, and always travelled along the target direction of the global optimal path, which improved the efficiency of path planning and the driving stability of the robot, as shown in Figure 13b.

5. Conclusions

• When a single sensor is used for map construction in an unknown environment, there are problems such as large mapping errors and poor robustness. The method of multi-sensor information fusion of LiDAR, depth camera, and IMU is used to obtain more comprehensive environmental information. Thereby, the reliability and accuracy of map construction are improved, and the robustness of the mobile robots in SLAM mapping is effectively improved.
• Aiming at the problem that the traditional ant colony algorithm has long searching time and is easy to fall into local stagnation, the heuristic function and pheromone updating strategy are improved, and the turning angle of the path is smoothed by using the cubic B-spline curve, which has good stability and improves the efficiency of path planning.
• The method of map construction and path planning based on multi-sensor fusion was verified and analyzed. The accuracy and robustness of the map construction was improved by the fusing of multi-sensor information. The local real-time obstacle avoidance can be realized by using the improved path planning algorithm, and the global optimal can be maintained. The efficiency of path planning was improved, and the automatic feedback control of an intelligent vehicle can be realized with good robustness and accuracy.