Multi-sensor fusion mobile robot path planning method and device

Abstract

The invention provides a multi-sensor fusion mobile robot path planning method and device, and relates to the technical field of mobile robots. Through a path planning SLAM framework of automatic positioning and mapping of an indoor mobile robot based on multi-sensor information fusion, the problem that low obstacles in the environment cannot be perceived is solved. Information of the two-dimensional laser radar and information of the depth vision sensor are fused, the environment perception capacity of the robot and the obstacle detection capacity in a complex environment are improved, the defect that the detection precision is low due to the performance of a single sensor is overcome, the path planning efficiency and the reliability and accuracy of map construction are improved, local real-time obstacle avoidance is achieved, and the real-time obstacle avoidance effect is achieved. And the global optimum is kept.

Description

Technical Field

[0001] The present invention relates to the field of mobile robots, and in particular to a multi-sensor fusion mobile robot path planning method and device. Background Art

[0002] The mobile robot localization problem refers to determining the posture of a robot relative to its surroundings. It is considered an important function for mobile robots to complete autonomous tasks.

[0003] Navigation technology, as a key technology in the research and development of mobile robot technology, mainly includes simultaneous positioning, mapping and path planning.

[0004] SLAM needs to complete the robot's positioning and mapping tasks in the location environment, while the goal of path planning is to generate a collision-free path in a complex environment to ensure that the mobile robot can move freely. The key technology of SLAM is to construct the robot's posture and environment map in a complex environment. According to the position and map information, the unknown movement of the robot is estimated, and the map is incrementally constructed as the movement process progresses to achieve the robot's autonomous navigation and obstacle avoidance functions. Based on a single sensor for real-time positioning and mapping, the system's accuracy is insufficient and its robustness is poor. Therefore, the present application proposes a mobile robot path planning method and device with multi-sensor fusion to at least partially solve the problems that may exist in the prior art. Summary of the invention

[0005] In order to overcome the above problems or at least partially solve the above problems, an embodiment of the present invention provides a multi-sensor fusion mobile robot path planning method and device, which can improve the accuracy and robustness of the system.

[0006] The embodiment of the present invention is achieved as follows: The present application provides a mobile robot path planning method for implementing multi-sensor fusion, the method comprising: EKF-based data fusion: Using EKF, the depth camera’s measurement value is used as the system prediction value at the current moment, and the lidar’s measurement value is used as the system measurement value. The depth camera’s measurement data and the lidar’s data are mapped to determine the optimal estimate of data fusion and obtain the optimized covariance matrix. Map construction and loop detection: Utilize the covariance matrix data of data fusion and adopt the graph optimization-based algorithm to construct the map after the sensor data processing is completed. Use the ICP algorithm to match the current laser point cloud information with the sub-graph to obtain the posture transformation relationship and form a closed-loop constraint for inserting back-end optimization. Calculate the best candidate posture through optimization means, eliminate bias, and complete closed-loop detection. Path planning: Based on the map and candidate postures, ant colony algorithm and hybrid strategy are used to update pheromones, perform path planning, and obtain a feasible path optimal solution.

[0007] In some embodiments of the present invention, the step before the EKF data fusion further includes a data preprocessing step: using the data processing of the depth camera to convert the image data obtained by the depth camera into laser data to complete the data fusion between the sensors, specifically including: The obtained depth image is screened for effective areas to obtain the depth image to be processed. ; By depth image And the camera parameter model, the position coordinates of each pixel of the depth camera in the world coordinate system are obtained ; Spatial point cloud Project it into the corresponding laser scanning range and calculate the angle between the lines AO and DO ; Among them, the scanning result of the laser radar is , the laser beam is divided into N parts, the laser data is represented by [N], and the index value n of the point M projected into the array, where, , laser [N] is the distance from point D on the x-axis to the center of the camera light spot .

[0008] In some embodiments of the present invention, the EKF-based data fusion step includes: Initialize the current position and covariance error using the robot's initial position, including using the depth camera on the robot to collect data about the movement and using this value as the current moment system input To predict the pose of the next moment and calculate the predicted error covariance; According to the current posture predict ,in, is the Jacobian matrix of the state equation, is the covariance matrix of the Gaussian noise of the predicted state; Calculate Kalman gain , using multi-sensor measurements to correct previous system predictions, thereby performing data fusion; The Kalman gain method is used to calculate the optimal estimate of sensor data fusion and complete the update of the covariance matrix, where: is the transfer matrix, Represents the covariance matrix of the measurement Gaussian noise.

[0009] In some embodiments of the present invention, the data processing using the depth camera converts the image data obtained by the depth camera into laser data to complete the data fusion between sensors, and also includes: Correlate the data from the depth camera and the data from the lidar; The correlation between the data of the depth camera and the data of the lidar is determined based on the Mahalanobis distance between the observation information obtained by the sensor, wherein when the distance is less than the set threshold x2, the data correlation is high.

[0010] In some embodiments of the present invention, the mapping and loop closure detection steps include: Using the fused sensor data, a graph optimization-based algorithm is used to construct a map after the sensor data processing is completed; Use the ICP algorithm to match the current laser point cloud information with the sub-image, obtain the posture transformation relationship, and form a closed-loop constraint for insertion into the back-end optimization; The detection mechanism determines the similarity between the relative position of the mobile robot in the laser point cloud and the relative position of the carrier in the submap; Calculate the best candidate posture, eliminate bias, complete closed-loop detection, optimize and correct the robot posture between frames, and output the corrected posture trajectory and map.

[0011] In some embodiments of the present invention, the path planning step specifically includes: In the initial state, the probability of ants choosing a feasible path is equal, and the ants will determine the direction of the next node based on the pheromone content of the state transition point on the selected path; Set the state transition probability of b from point i to point g on the grid graph to , determine the pheromone content from point i to point g, where {i} represents the set of transfer nodes that b can choose at node i, and the heuristic function , pheromone heuristic factor a, expectation heuristic factor w; According to 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, a heuristic function is introduced: ,in , determined by the real-time environment, Represents the distance between the current node and the next node. Indicates the distance between the current node and the target point; Determine the pheromone concentration between the maximum and minimum points: ,in, is the optimal path length after a certain iteration, and C is the number of cycles per time; pass To update the pheromone, and adjust the pheromone on the entire path to obtain the optimal path, where , m is the pheromone increment of the bth ant between nodes i and g; The pheromone concentration increment of the bth ant between the two points is calculated by Update the strategy to ensure that each ant can find a feasible path, where is the pheromone volatility coefficient, is the pheromone increment between nodes i and g, This is the best solution found so far.

[0012] In some embodiments of the present invention, after the path planning step, a global path smoothing process is further included, specifically including: The global path is smoothed by at least cubic B-splines.

[0013] In some embodiments of the present invention, the steps after the global path smoothing process further include fusing the global and local path planning algorithms, specifically including: When the driving environment of the intelligent robot changes, the robot integrates obstacle information into the environmental map in real time through its own environmental sensors; The local path planning method is used to replan the driving path of the intelligent robot platform; The robot continues to drive according to the global optimal solution and calculates the evaluation function based on the global path planning information. , to ensure that the final local path planning is the global optimal path, where Represents the angle between the vector pointing to the target and the vector connecting the starting point and the current position. is the vertical distance between the static obstacle and the current trajectory towards the target, Indicates the vertical distance between the unknown obstacle and the currently pointed target track. is the evaluation function of the current velocity toward the target trajectory, u, v, w, is the weighting coefficient; Combined with the global planning path, the local moving target point is planned in real time, and the preview tracking method is used to track the target point according to the control parameters in the path planning process; the local optimal path is obtained; Determine whether the local moving target point is the final target point; if not, jump to the data preprocessing step; if yes, drive the robot to reach the target point.

[0014] A multi-sensor fusion mobile robot path planning device, comprising: The data fusion module uses EKF to take the measurement value of the depth camera as the system prediction value at the current moment, and the measurement value of the lidar as the system measurement value, maps the measurement data of the depth camera and the lidar data, determines the optimal estimate of data fusion, and obtains the optimized covariance matrix; The map building and loop detection module uses the covariance matrix data of data fusion and a graph optimization-based algorithm to build a map after the sensor data is processed. It also uses 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 that is inserted into the back-end optimization. The best candidate posture is calculated through optimization, paranoia is eliminated, and closed-loop detection is completed. The path planning module is used to update pheromones based on the map and candidate postures using an ant colony algorithm and a hybrid strategy to perform path planning and obtain a feasible optimal path solution.

[0015] An electronic device comprises a processor, a memory and a computer program stored in the memory and capable of running on the processor, wherein the computer program, when executed by the processor, implements the mobile robot path planning method for realizing multi-sensor fusion.

[0016] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the mobile robot path planning method for realizing multi-sensor fusion is implemented.

[0017] Compared with the prior art, the embodiments of the present invention have at least the following advantages or beneficial effects: By using EKF, the depth camera's measurement value is used as the system prediction value at the current moment, and the laser radar's measurement value is used as the system measurement value. The depth camera's measurement data and the laser radar data are mapped to determine the optimal estimate of data fusion and obtain the optimized covariance matrix; the covariance matrix data of data fusion is used to construct a map after the sensor data processing is completed by using a graph optimization-based algorithm, and the current laser point cloud information is matched with the subgraph using the ICP algorithm to obtain the posture transformation relationship and form a closed-loop constraint for inserting back-end optimization. The best candidate posture is calculated by optimization means to eliminate bias and complete closed-loop detection; based on the map and candidate posture, the ant colony algorithm and hybrid strategy are used to update pheromones, perform path planning, and obtain a feasible path optimal solution. Through the path planning SLAM framework of automatic positioning and mapping of indoor mobile robots based on multi-sensor information fusion, the problem of low obstacles in the environment that cannot be perceived is compensated. The information from the two-dimensional lidar and the depth vision sensor is integrated to improve the robot's environmental perception and obstacle detection capabilities in complex environments, avoid the shortcomings of low detection accuracy caused by the performance of a single sensor, improve path planning efficiency and the reliability and accuracy of map construction, achieve local real-time obstacle avoidance, and maintain global optimization. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0019] Figure 1 A flowchart of a multi-sensor fusion mobile robot path planning method provided by an embodiment of the present invention; Figure 2 A SLAM algorithm flow chart of a multi-sensor fusion mobile robot path planning method provided by one embodiment of the present invention; Figure 3 A schematic diagram of the principle of converting a depth image obtained by a depth camera into laser data in a multi-sensor fusion mobile robot path planning method provided by an embodiment of the present invention; Figure 4 A flowchart of a mapping process based on graph optimization of a mobile robot path planning method with multi-sensor fusion provided by an embodiment of the present invention; Figure 5 A smooth path planning flow chart based on a third-order B-spline curve for a multi-sensor fusion mobile robot path planning method provided by an embodiment of the present invention; Figure 6 A fusion algorithm flow chart of a multi-sensor fusion mobile robot path planning method provided by one embodiment of the present invention; Figure 7 A schematic diagram of the module structure of a multi-sensor fusion mobile robot path planning device provided by one embodiment of the present invention; Figure 8 A schematic diagram of the structure of a computer device provided in one embodiment of the present application. DETAILED DESCRIPTION

[0020] In order to make the objects, features and advantages of the present application more obvious and understandable, the present application is further described in detail below in conjunction with the accompanying drawings and specific implementation methods. Obviously, the described embodiments are part of the embodiments of the present application, rather than all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present application.

[0021] The core concept of the present invention is a path planning SLAM framework for automatic positioning and mapping of indoor mobile robots based on multi-sensor information fusion to make up for the problem that lower obstacles in the environment cannot be perceived. A multi-sensor information fusion method of laser radar, depth camera and IMU is adopted to obtain more comprehensive environmental information. The reliability and accuracy of map construction are guaranteed, and the robustness of mobile robots in SLAM mapping is effectively improved. In view of the problems that the traditional ant colony algorithm has a long search time and is prone to local stagnation, the heuristic function and pheromone update strategy are improved, and the turning angle of the path is smoothed by a 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 improves the accuracy and robustness of map construction. The improved path planning algorithm can achieve local real-time obstacle avoidance and maintain the global optimum.

[0022] Reference Figure 1 , shows a multi-sensor fusion mobile robot path planning method provided by an embodiment of the present application; the method comprises: S100, based on EKF data fusion: using EKF, taking the measurement value of the depth camera as the system prediction value at the current moment, and taking the measurement value of the laser radar as the system measurement value, mapping the measurement data of the depth camera and the laser radar data, determining the optimal estimate of data fusion, and obtaining an optimized covariance matrix; S200, Mapping and loop detection: Using the covariance matrix data of data fusion, the algorithm based on graph optimization is used to build the map after the sensor data processing is completed, and the ICP algorithm is used to match the current laser point cloud information with the sub-graph to obtain the posture transformation relationship and form a closed-loop constraint inserted into the back-end optimization. The best candidate posture is calculated through optimization means, paranoia is eliminated, and closed-loop detection is completed; S300, path planning: based on the map and candidate postures, ant colony algorithm and hybrid strategy are used to update pheromones, perform path planning, and obtain a feasible optimal path solution.

[0023] In an embodiment of the present application, a path planning SLAM framework for automatic positioning and mapping of indoor mobile robots based on multi-sensor information fusion is provided to make up for the problem that lower obstacles in the environment cannot be perceived. A multi-sensor information fusion method of laser radar, depth camera and IMU is used to obtain more comprehensive environmental information. The reliability and accuracy of map construction are guaranteed, and the robustness of mobile robots in SLAM mapping is effectively improved. The above SLAM algorithm, such as Figure 2 As shown in the figure, in order to solve the problem that the traditional ant colony algorithm has a long search time and is prone to local stagnation, the heuristic function and pheromone update strategy are improved, and the turning angle of the path is smoothed by using cubic B-spline curve, which has good stability and improves the efficiency of path planning. The map construction and path planning method based on multi-sensor fusion improves the accuracy and robustness of map construction. The improved path planning algorithm can achieve local real-time obstacle avoidance and maintain the global optimum.

[0024] It should be noted that path planning is based on multi-source perception information. It searches for feasible paths to the target at a certain update frequency, and iterates the optimal path from the initial point to the destination under the premise of ensuring safety. Among the path planning methods, the two-dimensional geometric search method is currently the most mature path planning method. In addition, there are some widely used algorithms, such as Dijkstra, A* and Basic Theta*. Filtering algorithms are usually used in sensor data fusion. In order to describe the mobile robot in order to describe the SLAM problem, the robot's motion in continuous time is decomposed into discrete moments. , the corresponding moment position Constitute the motion trajectory of the robot. For the nonlinear system in the actual motion process, the system's motion model and observation model can be expressed as .in represents the robot posture, represents the observed value of the system, is the system motion equation function, is the system observation equation function, represents the control value of the system, represents the system process noise, represents the system observation noise.

[0025] Next, the multi-sensor fusion mobile robot path planning method in this exemplary embodiment will be further described.

[0026] Before the step S100 based on EKF data fusion, a data preprocessing step is also included, which uses the data processing of the depth camera to convert the image data obtained by the depth camera into laser data to complete the data fusion between sensors, specifically including: The obtained depth image is screened for effective areas to obtain the depth image to be processed. ; By depth image And the camera parameter model, the position coordinates of each pixel of the depth camera in the world coordinate system are obtained ; The spatial point cloud Project it into the corresponding laser scanning range and calculate the angle between the lines AO and DO , where the scanning result of the laser radar is , the laser beam is divided into N parts, and the laser data is represented by [N]; the index value n of the point M projected into the array, where, , laser [N] is the distance from point D on the x-axis to the center of the camera light spot .

[0027] The data processing using the depth camera converts the image data obtained by the depth camera into laser data to complete the data fusion between sensors, and also includes: The data from the depth camera and the data from the lidar are associated; the correlation between the data from the depth camera and the data from the lidar is determined based on the Mahalanobis distance between the observation information obtained by the sensor, wherein when the distance is less than the set threshold x2, the data correlation is high.

[0028] In a specific embodiment, Figure 3 As shown, the image data obtained by the depth camera is converted into laser data by using the data processing of the depth camera to complete the data fusion between sensors. First, the effective area of ​​the obtained depth image is screened to obtain the depth image to be processed. Then, by And the camera parameter model, the position coordinates of each pixel of the depth camera in the world coordinate system are obtained Next, the spatial point cloud Project it into the corresponding laser scanning range and calculate the angle between the lines AO and DO The scanning result of the laser radar is , the laser beam is divided into N parts, and the laser data is represented by [N]. The index value n of the point M projected into the array is Laser [N] is the distance from point D on the x-axis to the center of the camera light spot .

[0029] As described in step S100, based on EKF data fusion: using EKF, the measurement value of the depth camera is used as the system prediction value at the current moment, and the measurement value of the lidar is used as the system measurement value, the measurement data of the depth camera and the lidar data are mapped, the optimal estimate of the data fusion is determined, and the optimized covariance matrix is ​​obtained.

[0030] In one embodiment of the present invention, the specific process of “data fusion based on EKF” in step S100 may be further explained in combination with the following description.

[0031] When using a depth camera or lidar alone for map construction, certain errors will occur due to the defects of a single sensor. Therefore, this paper uses EKF (Kalman filter) to map the depth camera data and lidar data to improve the accuracy of the mapping. When using the extended Kalman filter for data fusion, the measurement accuracy of the lidar is higher than that of the depth camera, so the measurement value of the depth camera is used as the system prediction value at the current moment, and the measurement value of the lidar is used as the system measurement value to ensure the accuracy of data fusion.

[0032] First, initialize the current position and covariance error with the initial position of the robot. Use the depth camera on the robot to collect data about the motion. Use this value as the current moment system input To predict the next moment's posture and calculate the predicted error covariance. predict . is the Jacobian matrix of the state equation, is the covariance matrix of the Gaussian noise of the predicted state.

[0033] Before the depth camera and lidar data are fused, the data from the two sensors need to be associated to prevent mismatching of sensor data and the inability to build a complete map. The correlation between the data obtained by the two sensors is determined based on the Mahalanobis distance between the observation information obtained by the sensors. This shows that when the distance is less than the set threshold x2, the data correlation is high.

[0034] Calculate Kalman gain , using the measurements of multiple sensors to correct the previous system predictions, that is, using the position information obtained by the lidar to correct the positioning data obtained by the camera, so as to perform data fusion. At the same time, the Kalman gain method is used to calculate the optimal estimate of sensor data fusion and complete the update of the covariance matrix. is the transfer matrix, Represents the covariance matrix of the measurement Gaussian noise.

[0035] In an embodiment of the present invention, the specific process of “mapping and loop closure detection” in step S200 may be further explained in combination with the following description.

[0036] The map building and loop detection steps include: using the fused sensor data and adopting a graph optimization-based algorithm to build a map after the sensor data processing is completed; using the ICP algorithm to match the current laser point cloud information with the sub-map, obtain the posture transformation relationship, and form a closed-loop constraint for inserting the back-end optimization; the detection mechanism determines 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; calculating the best candidate posture, eliminating bias, completing the closed-loop detection, and optimizing and correcting the robot posture between frames, and outputting the corrected posture trajectory and map.

[0037] In a specific embodiment, Figure 4 As shown in the figure, the fused sensor data is used to construct a map after the sensor data processing is completed by using a graph optimization-based algorithm. The ICP algorithm is used to match the current laser point cloud information with the sub-graph to obtain the posture transformation relationship and form a closed-loop constraint for inserting 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 best candidate posture is calculated by optimization means, paranoia is eliminated, and closed-loop detection is completed.

[0038] Assume that the lidar feature point of the current frame is. The carrier position coordinate point obtained by EKF fusion is the center of the circle, and R is the radius of the posture distance detection. The point cloud of the target frame whose distance from the current posture is less than R is used as the point cloud to be detected. Then run the ICP algorithm on and, output the posture vector f, and perform loop detection. After obtaining f, the position of the detected posture in the historical posture can be calculated. Finally, the robot posture between and frames is optimized and corrected, and the corrected posture trajectory and map are output.

[0039] In an embodiment of the present invention, the specific process of “path planning” in step S300 may be further described in combination with the following description, including: In the initial state, the probability of ants choosing a feasible path is equal, and the ants will determine the direction of the next node based on the pheromone content of the state transition point on the selected path; Set the state transition probability of b from point i to point g on the grid graph to , determine the pheromone content from point i to point g, where {i} represents the set of transfer nodes that b can choose at node i, and the heuristic function , pheromone heuristic factor a, expectation heuristic factor w; According to 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, a heuristic function is introduced: ,in , determined by the real-time environment, Represents the distance between the current node and the next node. Indicates the distance between the current node and the target point; Determine the pheromone concentration between the maximum and minimum points: ,in, is the optimal path length after a certain iteration, and C is the number of cycles per time; pass To update the pheromone, and adjust the pheromone on the entire path to obtain the optimal path, where , m is the pheromone increment of the bth ant between nodes i and g; The pheromone concentration increment of the bth ant between the two points is calculated by Update the strategy to ensure that each ant can find a feasible path, where is the pheromone volatility coefficient, is the pheromone increment between nodes i and g, This is the best solution found so far.

[0040] In a specific embodiment of the present invention, the specific process of "path planning" can be further described in combination with the following description: an improved heuristic function is used to provide an initial selection in an ant colony algorithm to guide the direction, and a hybrid strategy is used to update pheromones, thereby improving the slow convergence speed and local optimization problems.

[0041] (1) In the initial state, the probability of ants choosing a feasible path is equal. Ants will determine the direction of the next node based on the pheromone content of the state transition point on the selected path. Set the state transition probability of b from point i to point g on the grid graph to , {i} represents the set of transfer nodes that b can choose at node i. Thus, the pheromone content from point i to point g is determined. , the heuristic function , pheromone heuristic factor a, expectation heuristic factor w.

[0042] (2) Based on 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, a heuristic function is introduced: .in , determined by the real-time environment; Indicates the distance between the current node and the next node; It indicates the distance between the current node and the target point, which increases the selection and guidance effect of the ant colony on the next target point and helps to reduce the search time of the ant colony.

[0043] (3) In order to speed up the ants’ search time, reduce the probability of choosing a route, and avoid pheromone concentration being too low or accumulating after volatilization, determine the pheromone concentration between the maximum and minimum points: . is the optimal path length after a certain iteration, and C is the number of cycles. After each cycle, the pheromone information on the path will change, and the pheromone on the entire path will be adjusted to obtain the optimal path. To update. , m is the pheromone increment of the bth ant between nodes i and g.

[0044] In order to prevent the ant colony from using the pheromone on the local optimal path for path planning, the pheromone concentration increment between the two points of the bth ant is calculated by Update the strategy to ensure that each ant can find a feasible path. is the pheromone volatility coefficient, is the pheromone increment between nodes i and g, This is the best solution found so far.

[0045] In an embodiment of the present invention, after the above-mentioned path planning step S300, global path smoothing processing is also included, specifically including: smoothing the global path through at least a cubic B-spline curve.

[0046] like Figure 5 The smoothing process of the third-order B-spline curve shown in FIG. 4 smoothes the global path by using the cubic B-spline curve to ensure that the robot passes smoothly and safely at the turn and satisfies the geometric constraint characteristics of the vehicle body.

[0047] In one embodiment of the present invention, the steps after the global path smoothing process further include the fusion of global and local path planning algorithms, specifically including: When the driving environment of the intelligent robot changes, the robot integrates obstacle information into the environmental map in real time through its own environmental sensors; The local path planning method is used to replan the driving path of the intelligent robot platform; The robot continues to drive according to the global optimal solution and calculates the evaluation function based on the global path planning information. , to ensure that the final local path planning is the global optimal path, where Represents the angle between the vector pointing to the target and the vector connecting the starting point and the current position. is the vertical distance between the static obstacle and the current trajectory towards the target, Indicates the vertical distance between the unknown obstacle and the currently pointed target track. is the evaluation function of the current velocity toward the target trajectory, u, v, w, is the weighting coefficient; Combined with the global planning path, the local moving target point is planned in real time, and the preview tracking method is used to track the target point according to the control parameters in the path planning process; the local optimal path is obtained; Determine whether the local moving target point is the final target point; if not, jump to the data preprocessing step; if yes, drive the robot to reach the target point.

[0048] In a specific embodiment of the present invention, the global and local path planning algorithms are integrated. Global path planning can plan the global optimal solution of the route, but it requires accurate information about the environment in advance. When the driving environment of the intelligent robot changes, for example, when an unknown obstacle appears, the robot integrates the obstacle information into the environmental map in real time through its own environmental sensors. Then, the local path planning method is used to replan the driving path of the intelligent robot platform. This is to avoid obstacles and complete local obstacle avoidance. After completing local obstacle avoidance, the robot continues to drive according to the global optimal solution. The evaluation function is calculated in combination with the global path planning information. , to ensure that the final local path planning is the global optimal path. Represents the angle between the vector pointing to the target and the vector connecting the starting point and the current position. is the vertical distance between the static obstacle and the current trajectory towards the target, Indicates the vertical distance between the unknown obstacle and the currently pointed target track. is the evaluation function of the current velocity toward the target trajectory, u, v, w, is the weighting coefficient.

[0049] like Figure 6As shown in the figure, the camera sensor is used to obtain environmental information, realize the robot's own positioning, and establish a raster map. Based on the improved ant colony algorithm, the robot's global path planning is completed to obtain the optimal path. According to the new obstacle information obtained by the sensor in real time, this information is integrated into the grid map, and the dynamic window method is used to complete the local obstacle avoidance work. Combined with the global planning path, the local moving target point is planned in real time, and the preview tracking method is used to track the target point 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 so, the robot reaches the target point, and then the algorithm ends.

[0050] The present invention relates to a path planning SLAM framework for automatic positioning and mapping of an indoor mobile robot based on multi-sensor information fusion, so as to make up for the problem that lower obstacles in the environment cannot be perceived. A multi-sensor information fusion method of laser radar, depth camera and IMU is adopted to obtain more comprehensive environmental information. The reliability and accuracy of map construction are guaranteed, and the robustness of the mobile robot in SLAM mapping is effectively improved. In view of the problems that the traditional ant colony algorithm has a long search time and is prone to local stagnation, the heuristic function and pheromone update strategy are improved, and the turning angle of the path is smoothed by a 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 improves the accuracy and robustness of map construction. The improved path planning algorithm can achieve local real-time obstacle avoidance and maintain the global optimum.

[0051] As for the device embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0052] like Figure 7 As shown, a multi-sensor fusion mobile robot path planning device provided by an embodiment of the present application is shown, comprising: The data fusion module 100 uses EKF to take the measurement value of the depth camera as the system prediction value at the current moment, and the measurement value of the laser radar as the system measurement value, maps the measurement data of the depth camera and the laser radar data, determines the optimal estimate of the data fusion, and obtains the optimized covariance matrix; The map building and loop detection module 200 uses the covariance matrix data of data fusion and a graph optimization-based algorithm to build a map after the sensor data processing is completed, and uses the ICP algorithm to match the current laser point cloud information with the sub-graph to obtain the posture transformation relationship and form a closed-loop constraint inserted into the back-end optimization, and calculates the best candidate posture through optimization means, eliminates bias, and completes the closed-loop detection; The path planning module 300 is used to update pheromones based on the map and candidate postures using an ant colony algorithm and a hybrid strategy to perform path planning and obtain a feasible optimal path solution.

[0053] Reference Figure 8 , showing a computer device of a multi-sensor fusion mobile robot path planning method of the present invention, which may specifically include the following: The computer device 12 is in the form of a general-purpose computing device, and the components of the computer device 12 may include but are not limited to: one or more processors or processing units 16, a system memory 28, and a bus 18 connecting different system components (including the system memory 28 and the processing unit 16).

[0054] The bus 18 represents one or more of several types of bus 18 structures, including a memory bus 18 or memory controller, a peripheral bus 18, an accelerated graphics port, a processor, or a local bus 18 using any of a variety of bus 18 architectures. These architectures include, by way of example, but are not limited to, an Industry Standard Architecture (ISA) bus 18, a Micro Channel Architecture (MAC) bus 18, an Enhanced ISA bus 18, an Audio Video Electronics Standards Association (VESA) local bus 18, and a Peripheral Component Interconnect (PCI) bus 18.

[0055] The computer device 12 typically includes a variety of computer system readable media. These media can be any available media that can be accessed by the computer device 12, including volatile and non-volatile media, removable and non-removable media.

[0056] System memory 28 may include computer system readable media in the form of volatile memory, such as random access memory (RAM) 30 and / or cache memory 32. Computer device 12 may further include other removable / non-removable, volatile / non-volatile computer system storage media. By way of example only, storage system 34 may be used to read and write to non-removable, non-volatile magnetic media (commonly referred to as a "hard drive"). Although Figure 8 Not shown, a disk drive for reading and writing to a removable non-volatile disk (such as a "floppy disk"), and an optical disk drive for reading and writing to a removable non-volatile optical disk (such as a CD-ROM, DVD-ROM or other optical medium) may be provided. In these cases, each drive may be connected to the bus 18 via one or more data medium interfaces. The memory may include at least one program product having a set (e.g., at least one) of program modules 42, which are configured to perform the functions of various embodiments of the present invention.

[0057] A program / utility 40 having a set (at least one) of program modules 42 may be stored in, for example, a memory, such program modules 42 including, but not limited to, an operating system, one or more application programs, other program modules 42, and program data, each of which or some combination may include an implementation of a network environment. The program modules 42 generally perform the functions and / or methods of the embodiments described herein.

[0058] The computer device 12 may also communicate with one or more external devices 14 (e.g., keyboards, pointing devices, displays 24, cameras, etc.), one or more devices that enable medical personnel to interact with the computer device 12, and / or any device that enables the computer device 12 to communicate with one or more other computing devices (e.g., network cards, modems, etc.). Such communication may be performed via an input / output (I / O) interface 22. In addition, the computer device 12 may also communicate with one or more networks (e.g., local area networks (LANs)), wide area networks (WANs), and / or public networks (e.g., the Internet) via a network adapter 20. As shown, the network adapter 20 communicates with other modules of the computer device 12 via a bus 18. It should be understood that although Figure 8 Not shown, other hardware and / or software modules may be used in conjunction with the computer device 12, including but not limited to: microcode, device drivers, redundant processing units 16, external disk drive arrays, RAID systems, tape drives, and data backup storage systems 34, etc.

[0059] The processing unit 16 executes various functional applications and data processing by running programs stored in the system memory 28, such as implementing the mobile robot path planning method with multi-sensor fusion provided in an embodiment of the present invention.

[0060] That is, when the processing unit 16 executes the program, the following is achieved: using EKF, taking the measurement value of the depth camera as the system prediction value at the current moment, and taking the measurement value of the laser radar as the system measurement value, mapping the measurement data of the depth camera and the laser radar data, determining the optimal estimate of data fusion, and obtaining an optimized covariance matrix; using the covariance matrix data of data fusion, using a graph optimization-based algorithm to construct a map after sensor data processing is completed, and using the ICP algorithm to match the current laser point cloud information with the subgraph to obtain a posture transformation relationship, and forming a closed-loop constraint for inserting back-end optimization, calculating the best candidate posture through optimization means, eliminating bias, and completing closed-loop detection; based on the map and candidate postures, using the ant colony algorithm and hybrid strategy to update pheromones, perform path planning, and obtain a feasible path optimal solution.

[0061] In an embodiment of the present invention, the present invention further provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the multi-sensor fusion mobile robot path planning method provided in all embodiments of the present application: That is, when the program is executed by the processor, it is implemented as follows: using EKF, taking the measurement value of the depth camera as the system prediction value at the current moment, and taking the measurement value of the laser radar as the system measurement value, mapping the measurement data of the depth camera and the laser radar data, determining the optimal estimate of data fusion, and obtaining an optimized covariance matrix; using the covariance matrix data of data fusion, using a graph optimization-based algorithm to construct a map after the sensor data processing is completed, and using the ICP algorithm to match the current laser point cloud information with the subgraph to obtain a posture transformation relationship, and forming a closed-loop constraint for inserting back-end optimization, calculating the best candidate posture through optimization means, eliminating bias, and completing closed-loop detection; based on the map and candidate postures, using the ant colony algorithm and the hybrid strategy to update pheromones, performing path planning, and obtaining a feasible path optimal solution.

[0062] Any combination of one or more computer-readable media may be used. A computer-readable medium may be a computer signal medium or a computer-readable storage medium. A computer-readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared or semiconductor system, device or device, or any combination thereof. More specific examples (a non-exhaustive list) of computer-readable storage media include: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPOM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination thereof. In this document, a computer-readable storage medium may be any tangible medium containing or storing a program that may be used by or in conjunction with an instruction execution system, device or device.

[0063] Computer-readable signal media may include a data signal propagated in baseband or as part of a carrier wave, which carries a computer-readable program code. Such propagated data signals may take a variety of forms, including, but not limited to, electromagnetic signals, optical signals, or any suitable combination of the above. Computer-readable signal media may also be any computer-readable medium other than a computer-readable storage medium, which may send, propagate, or transmit a program for use by or in conjunction with an instruction execution system, apparatus, or device.

[0064] The computer program code for performing the operation of the present invention can be written in one or more programming languages ​​or a combination thereof, including object-oriented programming languages, such as Java, Smalltalk, C++, and conventional procedural programming languages, such as "C" language or similar programming languages. The program code can be executed entirely on the medical staff computer, partially on the medical staff computer, as an independent software package, partially on the medical staff computer, partially on the remote computer, or entirely on the remote computer or server. In the case of a remote computer, the remote computer can be connected to the medical staff computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or can be connected to an external computer (for example, using an Internet service provider to connect through the Internet). The various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same and similar parts between the various embodiments can be referred to each other.

[0065] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the embodiments of the present application.

[0066] Finally, it should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or terminal device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or terminal device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or terminal device including the elements.

[0067] The above is a detailed introduction to the multi-sensor fusion mobile robot path planning method and device provided by the present application. Specific examples are used in this article to illustrate the principles and implementation methods of the present application. The description of the above embodiments is only used to help understand the method of the present application and its core idea; at the same time, for general technical personnel in this field, according to the idea of ​​the present application, there will be changes in the specific implementation method and application scope. In summary, the content of this specification should not be understood as a limitation on the present application.

Claims

1. A multi-sensor fusion mobile robot path planning method, characterized in that: include: EKF-based data fusion: Using EKF, the depth camera’s measurement value is used as the system prediction value at the current moment, and the lidar’s measurement value is used as the system measurement value. The depth camera’s measurement data and the lidar’s data are mapped to determine the optimal estimate of data fusion and obtain the optimized covariance matrix. Map construction and loop detection: Utilize the covariance matrix data of data fusion and adopt the graph optimization-based algorithm to construct the map after the sensor data processing is completed. Use the ICP algorithm to match the current laser point cloud information with the sub-graph to obtain the posture transformation relationship and form a closed-loop constraint for inserting back-end optimization. Calculate the best candidate posture through optimization means, eliminate bias, and complete closed-loop detection. Path planning: Based on the map and candidate postures, ant colony algorithm and hybrid strategy are used to update pheromones, perform path planning, and obtain a feasible path optimal solution.

2. The method according to claim 1, characterized in that The steps before the EKF-based data fusion also include a data preprocessing step: using the data processing of the depth camera to convert the image data obtained by the depth camera into laser data to complete the data fusion between sensors, specifically including: The obtained depth image is screened for effective areas to obtain the depth image to be processed. ; By depth image And the camera parameter model, the position coordinates of each pixel of the depth camera in the world coordinate system are obtained ; Spatial point cloud Project it into the corresponding laser scanning range and calculate the angle between the lines AO and DO ; Among them, the scanning result of the laser radar is , the laser beam is divided into N parts, the laser data is represented by [N], and the index value n of the point M projected into the array, where, , laser [N] is the distance from point D on the x-axis to the center of the camera light spot .

3. The method according to claim 1, characterized in that The EKF-based data fusion step includes: Initialize the current position and covariance error using the robot's initial position, including using the depth camera on the robot to collect data about the movement and using this value as the current moment system input To predict the pose of the next moment and calculate the predicted error covariance; According to the current posture predict ,in, is the Jacobian matrix of the state equation, is the covariance matrix of the Gaussian noise of the predicted state; Calculate Kalman gain , using multi-sensor measurements to correct previous system predictions, thereby performing data fusion; The Kalman gain method is used to calculate the optimal estimate of sensor data fusion and complete the update of the covariance matrix, where: is the transfer matrix, Represents the covariance matrix of the measurement Gaussian noise.

4. The method according to claim 2, characterized in that: The data processing using the depth camera converts the image data obtained by the depth camera into laser data to complete the data fusion between sensors, and also includes: Correlate the data from the depth camera and the data from the lidar; The correlation between the data of the depth camera and the data of the lidar is determined based on the Mahalanobis distance between the observation information obtained by the sensor, wherein when the distance is less than the set threshold x2, the data correlation is high.

5. The method according to claim 1, characterized in that The mapping and loop closure detection steps include: Using the fused sensor data, a graph optimization-based algorithm is used to construct a map after the sensor data processing is completed; Use the ICP algorithm to match the current laser point cloud information with the sub-image, obtain the posture transformation relationship, and form a closed-loop constraint for insertion into the back-end optimization; The detection mechanism determines the similarity between the relative position of the mobile robot in the laser point cloud and the relative position of the carrier in the submap; Calculate the best candidate posture, eliminate bias, complete closed-loop detection, optimize and correct the robot posture between frames, and output the corrected posture trajectory and map.

6. The method according to claim 1, characterized in that The path planning step specifically includes: In the initial state, the probability of ants choosing a feasible path is equal, and the ants will determine the direction of the next node based on the pheromone content of the state transition point on the selected path; Set the state transition probability of b from point i to point g on the grid graph to , determine the pheromone content from point i to point g, where {i} represents the set of transfer nodes that b can choose at node i, and the heuristic function , pheromone heuristic factor a, expectation heuristic factor w; According to 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, a heuristic function is introduced: ,in , determined by the real-time environment, Represents the distance between the current node and the next node. Indicates the distance between the current node and the target point; Determine the pheromone concentration between the maximum and minimum points: ,in, is the optimal path length after a certain iteration, and C is the number of cycles per time; pass To update the pheromone, and adjust the pheromone on the entire path to obtain the optimal path, where , m is the pheromone increment of the bth ant between nodes i and g; The increment of the pheromone concentration of the bth ant between the two points is calculated by Update the strategy to ensure that each ant can find a feasible path, where is the pheromone volatility coefficient, is the pheromone increment between nodes i and g, This is the best solution found so far.

7. The method according to claim 1, characterized in that After the path planning step, a global path smoothing process is also included, which specifically includes: The global path is smoothed by at least cubic B-splines.

8. The method according to claim 7, characterized in that The steps after the global path smoothing process also include the fusion of global and local path planning algorithms, specifically including: When the driving environment of the intelligent robot changes, the robot integrates obstacle information into the environmental map in real time through its own environmental sensors; The local path planning method is used to replan the driving path of the intelligent robot platform; The robot continues to drive according to the global optimal solution and calculates the evaluation function based on the global path planning information. , to ensure that the final local path planning is the global optimal path, where Represents the angle between the vector pointing to the target and the vector connecting the starting point and the current position. is the vertical distance between the static obstacle and the current trajectory towards the target, Indicates the vertical distance between the unknown obstacle and the currently pointed target track. is the evaluation function of the current velocity toward the target trajectory, u, v, w, is the weighting coefficient; Combined with the global planning path, the local moving target point is planned in real time, and the preview tracking method is used to track the target point according to the control parameters in the path planning process; the local optimal path is obtained; Determine whether the local moving target point is the final target point; if not, jump to the data preprocessing step; if yes, drive the robot to reach the target point.

9. A multi-sensor fusion mobile robot path planning device, characterized in that: include: The data fusion module uses EKF to take the measurement value of the depth camera as the system prediction value at the current moment, and the measurement value of the lidar as the system measurement value, maps the measurement data of the depth camera and the lidar data, determines the optimal estimate of data fusion, and obtains the optimized covariance matrix; The map building and loop detection module uses the covariance matrix data of data fusion and a graph optimization-based algorithm to build a map after the sensor data is processed. It also uses 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 that is inserted into the back-end optimization. The best candidate posture is calculated through optimization, paranoia is eliminated, and closed-loop detection is completed. The path planning module is used to update pheromones based on the map and candidate postures using an ant colony algorithm and a hybrid strategy to perform path planning and obtain a feasible optimal path solution.

10. An electronic device, characterized in that: The method comprises a processor, a memory, and a computer program stored in the memory and capable of running on the processor, wherein the computer program implements the method according to any one of claims 1 to 8 when executed by the processor.

Images