Autonomous mapping and path planning method for mobile robot

Abstract

The invention discloses an autonomous mapping and path planning method for a mobile robot, and the method comprises the steps: carrying out the inspection of a target structure and environment through a robot carrying radar equipment, and fusing the data of a radar, a camera and an inertial unit through an extended Kalman filter (EKF). And performing global planning according to the improved ant colony algorithm, and performing local planning and obstacle avoidance by using a fuzzy control method to obtain an optimal path. According to the method, indexes such as cost (travel length) of alternative paths and information acquisition (an unknown area capable of being observed at a certain point) are evaluated by quantifying a boundary area and an optimal view angle under a current environment condition, and uncertain factors in an execution process are comprehensively considered. In this way, feasible path configuration can be determined, and a task scheme is adjusted in time according to changes of real-time obstacles and the like, so that the intelligent level of the robot is improved, and efficient, obstacle avoidance and accurate movement are achieved.

Description

Technical Field The present invention belongs to the technical field of path planning, and more specifically, to a method for autonomous mapping and path planning of a mobile robot. Background Art Autonomous mapping and path planning of mobile robots are important research directions in the field of intelligent robots, involving multiple technical levels. Autonomous mapping usually relies on sensors such as lidar, depth cameras, etc. By collecting environmental information, the robot can build and update the map of the surrounding environment in real time. The SLAM (Simultaneous Localization and Mapping) algorithm is the core technology to achieve this function, which enables the robot to simultaneously perform self-localization and map construction in an unknown environment. The SLAM algorithm combines filtering technology and graph optimization methods to improve the accuracy and real-time performance of the map. Path planning uses the constructed map to calculate the optimal route from the starting point to the target point. This process needs to consider the robot's motion model, environmental obstacles and their dynamic changes. In practical applications, these technologies are widely used in industrial automation, unmanned driving, service robots and other fields, improving the autonomy and intelligence of robots, enabling them to complete tasks safely and efficiently in complex and dynamic environments. With the continuous advancement of sensor technology, computing power and algorithms, the capabilities of mobile robots in autonomous mapping and path planning are also continuously improving, promoting the rapid development of intelligent robot technology.

[0003] Regarding the path planning methods for mobile robots, scholars at home and abroad have proposed many methods, including sampling optimization methods (Rapidly-exploring Random Tree algorithm), hybrid automatic planning technology (such as hybrid A*), and path planning strategies that consider dynamic environments.

[0004] The Chinese patent publication number is "CN 103295080 A", and the patent name is "Three-dimensional path planning method based on elevation map and ant colony foraging". The path planning method based on elevation map and ant colony foraging is proposed. By combining artificial potential field to diffuse pheromones, the path planning time is reduced, and the blindness and randomness in the ant search process are reduced. However, the acceleration and deceleration effect of the artificial potential field may interfere with the pheromone propagation mechanism in the ant colony algorithm, and the ant colony algorithm itself has a certain degree of randomness. Combining with the artificial potential field may introduce more uncertainty. Summary of the invention The purpose of the present invention is to solve the defects in the prior art and to propose a method for autonomous mapping and path planning of a mobile robot. The technical solution of the present invention to solve the technical problem is: 1. A method for autonomous mapping and path planning of a mobile robot, characterized in that the method specifically comprises the following steps: Step 1: Continuously collect environmental information through lidar, depth camera and gesture sensor, and pre-process the data; Step 2: Use the extended Kalman filter algorithm (EKF) to fuse the data of the lidar and depth camera measurement units, and use the dynamic information provided by the attitude sensor to realize the fusion application of the SLAM algorithm to build a global map; Step 3: Use feature matching algorithm to match the current laser point cloud information with the sub-image to form a closed-loop constraint that is inserted into the back-end optimization, thereby realizing loop closure detection; Step 4: Use the fusion algorithm to perform path planning, use the improved ant colony algorithm for global path planning, and combine the fuzzy control algorithm for local path planning and obstacle avoidance. 2. The method for autonomous mapping and path planning of a mobile robot according to claim 1, characterized in that the data fusion method of the laser radar and the depth camera measurement unit in step 2 is: converting the depth image captured by the depth camera into laser data, and initializing the covariance error between the current position and the initial position of the robot. Correlating the data of the two sensors, calculating the Kalman gain, and using the measurement values ​​of multiple sensors to correct the previous system prediction. 3. The method for autonomous mapping and path planning of a mobile robot according to claim 1 is characterized in that the method for establishing a global map in step 2 is: based on sensor data, frame matching is performed through a graph optimization algorithm, and map estimation is performed using the optimized posture and sensor data. 4. The method for autonomous mapping and path planning of a mobile robot according to claim 1, characterized in that the loop closure detection method in step 3 is: using a feature matching algorithm to match the current laser point cloud information with the sub-graph, obtaining the posture transformation relationship and forming a closed-loop constraint. Then detecting the similarity of the relative position of the mobile robot in the laser point cloud and the carrier in the sub-graph. Calculating the optimal candidate posture, optimizing and eliminating deviations, and completing the loop closure detection. 5. The mobile robot autonomous mapping and path planning method according to claim 1, characterized in that the distance heuristic function of the improved ant colony algorithm in step 4 is: in, represents the corrected distance between the current grid j and the target grid T, d(j,T) represents the Euclidean distance between two points, MAX and MIN represent the maximum and minimum distances to be transmitted respectively; Ges is the size of the grid side length, allow represents the set of transferable grids when the ant is in the grid; τ distance coefficient; ε distance correction parameter. The beneficial effects of the present invention are as follows: by fusing laser radar, depth camera and posture information, the accuracy and robustness of map construction are enhanced. The heuristic function and pheromone strategy are improved to improve the efficiency of path planning. The fusion algorithm realizes local real-time obstacle avoidance and maintains global optimization, improves the efficiency of path planning, is suitable for robot automatic feedback control, and has excellent robustness and accuracy. BRIEF DESCRIPTION OF THE DRAWINGS The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. They are used to explain the present invention together with the embodiments of the present invention and do not constitute a limitation of the present invention. Figure 1 This is a schematic diagram of converting the depth camera to radar data of the present invention; Figure 2 is a flow chart of the fusion algorithm of the present invention; DETAILED DESCRIPTION In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below. It should be understood that the details described herein are only used to explain the present invention and are not used to limit the present invention. When a robot is driving in a dynamic environment, it needs to sense and respond to environmental changes in real time. Through the multi-sensor fusion of LiDAR, depth camera and IMU, environmental information such as obstacle location and movement trend can be continuously obtained to improve driving safety and efficiency. Autonomous mapping and path planning of mobile robots use real-time data to update path planning, ensuring that the robot can respond quickly in changing scenarios, avoid collisions and choose the optimal path. like Figure 1 , Figure 2 As shown, a method for autonomous mapping and path planning of a mobile robot, the method specifically includes: Step 1: Reference Figure 1 , continuously acquire environmental data through LiDAR, depth camera and attitude sensor, and calibrate the sensor (to ensure accurate calibration of LiDAR, depth camera and attitude sensor in time and space), so that the data output by the sensor is consistent in the same plane and time. In the data preprocessing stage, the LiDAR point cloud and camera information are processed, and the IMU is pre-integrated to obtain the relative attitude transformation matrix of the vehicle, providing accurate transformation basis for radar point cloud frame matching. Specifically, the steps for converting the depth image acquired by the depth camera into laser data are as follows: First, key regions are extracted from the depth image for detailed analysis in order to identify and focus on processing pixels rich in depth information. Subsequently, the extracted depth image is combined with the camera's parameter model to perform reverse projection to deduce the precise coordinates of each pixel in the three-dimensional world coordinate system. Finally, these coordinate points are converted into spatial point cloud data and mapped into the scanning area of ​​the laser radar. The angle between the point cloud data and a certain line (such as AO and DO) is calculated and recorded as angle AOD. The calculation formula of the angle is as follows: angle AOD =cos -1 ((AO·DO) / (|AO|*|DO|)) Among them, "|AO|" and "|DO|" represent the lengths of vectors AO and DO respectively. Step 2: First, convert the depth image obtained by the depth camera into laser data, use the depth camera's measurement value as the system prediction value at the current moment, and use the radar measurement value as the system measurement value to ensure the accuracy of data fusion, initialize the current position and the covariance error with the robot's initial position, then associate the data of the two sensors, and finally calculate the Kalman gain, and use the multi-sensor measurement value to correct the previous system prediction. Specifically, the EKF data fusion method is as follows: First, by initializing the covariance error between the current position and the initial position of the robot, the depth camera is used to collect motion data as the system input at the current moment, the posture at the next moment is predicted and the prediction error covariance is calculated. Next, the data correlation is evaluated based on the Mahalanobis distance of the sensor observation information. When the distance is less than the set threshold, the data correlation is high. Finally, the Kalman gain is calculated, and the system prediction is corrected using multi-sensor measurement values. For example, the position information of the lidar is used to correct the camera's positioning data for data fusion, and the covariance matrix is ​​updated. Specifically, the map is constructed as follows: First, feature points are extracted from the environmental data obtained from the sensor and a graph is constructed. The nodes represent the robot position and sensor observations, and the edges represent motion and measurement constraints. Next, the graph is optimized using a graph optimization algorithm to minimize the error while smoothing the robot trajectory. Finally, an environmental map is constructed based on the optimized results. As the robot moves and new data arrives, the robot status and map are continuously updated. Step 3: The real-time acquired laser scanning data is precisely aligned with the previously constructed map subset using the Iterative Closest Point (ICP) algorithm to form a closed-loop constraint that is inserted into the back-end optimization, thereby achieving loop closure detection for subsequent global map adjustment. Specifically, the loop closure detection method is to use the iterative closest point (ICP) algorithm to accurately locate the alignment between the current lidar point cloud and the local map, thereby deriving the position and direction changes between the two, and constructing a closed-loop constraint that can be integrated into the global optimization framework. The core of the closed-loop identification mechanism is to evaluate the geometric consistency of the mobile platform with the corresponding fragments in the registered submap in the real-time point cloud coordinate system. With the help of the optimization algorithm, the most likely relative posture is screened out and the accumulated deviation is corrected, thereby achieving accurate optimization and correction of the closed loop. The optimization methods mentioned in the above search process are: 1) Let the instantaneous feature set observed by the radar be marked as P l The platform geometric center synthesized by the extended Kalman filter (EKF) defines a sphere whose radius is denoted by Specially used for posture proximity evaluation. When the Euclidean distance between the recognized scene point cloud and the current estimated posture falls within When they are within the specified range, these point clouds are considered as potential pairing candidates and are marked as P k . 2) The iterative closest point (ICP) algorithm is deployed to handle P l and the selected P k , and a vector representing the relative posture is generated. If the effective calculation of this vector is completed, it means that the closed-loop phenomenon has been captured. Once the vector is determined, the exact index K of this closed-loop event in the past posture sequence can be calculated. τ . 3) K τ-1 With K τ The system posture at that moment has undergone a round of fine-tuning and optimization to ensure the accuracy of the posture transition between consecutive frames. Ultimately, this series of operations results in a corrected position trajectory and a more accurate map representation. Step 4: Use the improved ant colony algorithm for global path planning, and combine it with the fuzzy control algorithm for local path planning and obstacle avoidance until the route distance between the robot and the target position is significantly less than the initial distance length to obtain the optimal path. Specifically, Figure 2 As shown in the figure, the specific steps of the fusion algorithm are: first, the surrounding environment data is collected with the help of the on-board sensor device to establish the current coordinates of the vehicle, and a raster map reflecting the environment layout is constructed to lay the foundation for path planning. Secondly, an optimized ant colony algorithm is used to carefully plan the global path, from which an optimal solution path with comprehensive efficiency and safety is selected to guide the vehicle's entire journey from the starting point to the end point. Furthermore, in the face of sudden obstacles perceived in real time, the updated information is quickly integrated into the existing grid map. Based on the fuzzy control algorithm, the system instantly adjusts the action strategy to achieve accurate local obstacle avoidance. In addition, to ensure that it is always in the optimal driving state, we will continuously update the local motion goals based on the global path guidance, and use tracking technology to ensure that it can respond to environmental changes in a timely manner and effectively avoid obstacles, so as to continuously approach the destination. Finally, the algorithm checks whether the current local target point is equal to the final destination. If not, the entire process loops back to the initial stage and continues to perform environmental perception and path optimization tasks; otherwise, it means that the robot has successfully arrived at the predetermined location and the algorithm runs to a successful end. Specifically, the specific steps of the improved ant colony algorithm mentioned above are: 1) Initialization, define the speed of ant k moving from the current grid i to the grid j as: Among them, k is the number of ants, allow is the set of transferable grids when the ants are in the grid, is the pheromone between i and j, is the heuristic information about j. 2) Use the smoothness and length of the path as optimization objectives to improve the heuristic information. The heuristic information about j is: in, is the smoothness of the path, is the length of the path. 4) To increase the distance between the candidate node and the endpoint grid, the improved distance heuristic function is: in, represents the corrected distance between the current grid j and the target grid T, d(j,T) represents the Euclidean distance between two points, MAX and MIN represent the maximum and minimum distances to be transmitted respectively; Ges is the size of the grid side length, allow represents the set of transferable grids when the ant is in the grid; τ distance coefficient; ε distance correction parameter. 5) After each round of iteration, pheromone enhancement and attenuation are performed to make the path smoother. The maximum and minimum values ​​of the pheromone concentration between two points are: Among them, D s is the optimal path length after a particular iteration, and c is the number of cycles per iteration. Specifically, the specific steps of the above fuzzy control algorithm are: the obstacle position and the movement of the robot are respectively corresponded to the input and output of the fuzzy controller in the robot reasoning system, and the control instructions of the robot are determined according to the input variables, such as "adjusting the direction angle", "forward or backward speed", etc. According to the relationship between input variables and output variables, fuzzy control rules are established. For example, if the "distance to the obstacle" is "far", then the "speed of forward or backward movement" should be "accelerated". The Mamdani inference rule library is used to infer the input variables and calculate the results of each rule. The fuzzy control results are then converted into clear control signals. During the operation of the robot, the data is updated in real time to recalculate the control instructions to achieve dynamic obstacle avoidance and path adjustment.

Claims

1. A method for autonomous mapping and path planning of a mobile robot. It is characterized in that The method specifically comprises the following steps: Step 1: Continuously collect environmental information through lidar, depth camera and gesture sensor, and pre-process the data; Step 2: Use the extended Kalman filter algorithm (EKF) to fuse the data of the lidar and depth camera measurement units, and use the dynamic information provided by the attitude sensor to realize the fusion application of the SLAM algorithm to build a global map; Step 3: Use feature matching algorithm to match the current laser point cloud information with the sub-image to form a closed-loop constraint that is inserted into the back-end optimization, thereby realizing loop closure detection; Step 4: Use the fusion algorithm to perform path planning, use the improved ant colony algorithm for global path planning, and combine the fuzzy control algorithm for local path planning and obstacle avoidance.

2. The mobile robot autonomous mapping and path planning method according to claim 1, It is characterized in that The data fusion method of the laser radar and depth camera measurement unit in step 2 is: convert the depth image captured by the depth camera into laser data, and initialize the covariance error between the current position and the initial position of the robot. Correlate the data of the two sensors, calculate the Kalman gain, and use the measurement values ​​of multiple sensors to correct the previous system prediction.

3. The mobile robot autonomous mapping and path planning method according to claim 1, It is characterized in that The method for establishing a global map in step 2 is as follows: based on the sensor data, inter-frame matching is performed through a graph optimization algorithm, and map estimation is performed using the optimized posture and sensor data.

4. The mobile robot autonomous mapping and path planning method according to claim 1, It is characterized in that The loop closure detection method in step 3 is: use the ICP algorithm to match the current laser point cloud information with the sub-graph, obtain the posture transformation relationship and form a closed-loop constraint. Then detect the similarity of the relative position of the mobile robot in the laser point cloud and the carrier in the sub-graph. Calculate the optimal candidate posture, optimize and eliminate deviations, and complete the loop closure detection.

5. The mobile robot autonomous mapping and path planning method according to claim 1, It is characterized in that The distance heuristic function of the improved ant colony algorithm in step 4 is: in, represents the corrected distance between the current grid j and the target grid T, d(j,T) represents the Euclidean distance between two points, MAX and MIN represent the maximum and minimum distances to be transmitted respectively; Ges is the size of the grid side length, allow represents the set of transferable grids when the ant is in the grid; τ distance coefficient; ε distance correction parameter.

Images