Multi-sensor fusion robot mapping and navigation method and device

Through multi-sensor fusion and Bayesian estimation methods, combined with improved A* algorithm and path fusion algorithm, the problems of graph construction accuracy and path planning efficiency in robot navigation are solved, and a more efficient, accurate and adaptable navigation solution is achieved. .

CN118913298AActive Publication Date: 2024-11-08SHENZHEN KELIPOINT TECH CO LTD
View PDF 10 Cites 0 Cited by

Patent Information

Application Number
CN202410924842.X
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-11
Publication Date
2024-11-08
Estimated Expiration
2044-07-11

AI Technical Summary

Technical Problem

In the existing robot navigation system, the two-dimensional lidar mapping accuracy is low and cannot avoid obstacles below the scanning height. The RGB-D camera has poor effect in weak texture and low light environments, and the path planning efficiency is low. Traditional A* algorithm The node search efficiency is low, and the dynamic window method cannot achieve the global optimal path.

Method used

Using multi-sensor fusion method, combining two-dimensional lidar and RGB-D cameras, using Bayesian estimation to build a two-dimensional grid map, improve the A* algorithm for path planning, determine the optimal path through the path fusion algorithm, and update the environment in real time Model to adapt to dynamic obstacles.

Benefits of technology

It improves the accuracy of map construction and path planning efficiency, can effectively avoid obstacles, improve navigation accuracy and safety, adapt to complex and changing environments, and improve robot navigation performance.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure FDA0004937810310000021
    Figure FDA0004937810310000021
  • Figure HDA0004937810330000011
    Figure HDA0004937810330000011
  • Figure HDA0004937810330000021
    Figure HDA0004937810330000021
Patent Text Reader

Abstract

The invention provides a multi-sensor fusion robot mapping and navigation method and device, and relates to the technical field of robot navigation. A two-dimensional grid chart is established based on multi-sensor data fusion. Secondly, giving a starting point and a target point in the grid chart, and planning a path by adopting an improved A * algorithm; next, node information of the path planned by the A * algorithm is stored in a path set. And if the dynamic obstacle exists when moving along the path, taking the node closest to the dynamic obstacle as a sub-target point. Then judging whether the robot arrives at a sub-target point, and if the robot arrives at the sub-target point, continuing to carry out the next step; and if the sub-target point is not reached, local path planning is continued. And judging whether the robot reaches the target point. And if the target point is reached, robot navigation is ended. And if the node does not arrive at the target point, the node information is continuously stored in the path set.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of robot navigation technology, and in particular to a multi-sensor fusion robot mapping and navigation method and device with clear principle, strong robustness and fast convergence speed. Background Art

[0002] In recent years, autonomous mobile robots and autonomous driving have continued to be popular. Autonomous mobile robots have been implemented in more and more industries, making great contributions in improving efficiency, saving labor costs, and improving operational safety. As a result, the requirements for the accuracy of robot navigation are becoming higher and higher.

[0003] The current robot navigation still has the following problems: First, in terms of sensors, laser radar and camera are the mainstream sensors in the mapping and navigation system of mobile robots. The accuracy of two-dimensional laser radar is high, but laser radar can only scan environmental information at a single height, resulting in low mapping accuracy. If the two-dimensional grid map established by two-dimensional laser radar is used for navigation, the robot cannot avoid obstacles below the laser radar scanning height in global path planning. RGB-D cameras can obtain environmental information at different heights, but RGB-D cameras are less effective in environments with weak textures and light. Second, in terms of path planning methods, path planning can be divided into global path planning and local path planning. The Dijkstra algorithm is a global path planning algorithm based on traversal search, but the number of generated nodes is too large, resulting in low path planning efficiency. The A* algorithm is based on the Dijkstra algorithm and an additional assignment function to improve the efficiency of path planning. The traditional A* algorithm node search method is an 8-neighbor search. When a node is expanded, the child nodes in all directions will be expanded; when the mobile robot plans a path, invalid node searches will occur. The dynamic window method has good dynamic obstacle suppression capability, but cannot achieve the global optimal path. Local path planning requires real-time updating of the environment model, which requires the robot system to have high-speed computing capabilities. Therefore, this application proposes a multi-sensor fusion robot mapping and navigation method and device to at least partially solve the problems that may exist in the prior art. Summary of the invention

[0004] 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 robot mapping and navigation method and device, specifically fusing the data of lidar and RGB-D camera to improve the accuracy of mapping and path planning efficiency, and realize dynamic environment navigation.

[0005] The embodiment of the present invention is achieved as follows:

[0006] The present application provides a multi-sensor fusion robot mapping and navigation method, the method comprising:

[0007] Based on the two-dimensional lidar and RGB-D camera, a two-dimensional grid map is constructed using the Bayesian estimation method;

[0008] Locate the starting point and the target point according to the two-dimensional grid map, use the improved A* algorithm to perform path planning, and obtain path node information;

[0009] According to the path node information, a fusion algorithm is used to perform path fusion, determine the optimal path, and save the path node information of the optimal path;

[0010] According to the optimal path, determine the path node closest to the obstacle as the sub-target point, and determine whether the sub-target point is reachable;

[0011] If the sub-target point is unreachable, return to the step of determining the starting point and the target point according to the two-dimensional grid map, perform path planning using the improved A* algorithm, and obtain the path node information; otherwise, determine whether the target point is reachable;

[0012] If the target point is unreachable, the process returns to the step of performing path fusion based on the path node information, using a fusion algorithm to determine the optimal path, and saving the path node information of the optimal path; otherwise, the target point is determined to be reachable, and a two-dimensional grid map is loaded for navigation.

[0013] In some embodiments of the present invention, the two-dimensional grid map is constructed based on the two-dimensional laser radar and the RGB-D camera using the Bayesian estimation method, including:

[0014] Acquire first data through the two-dimensional laser radar, and acquire second data through the RGB-D camera;

[0015] Use the prior distribution function to obtain the prior probability;

[0016] According to the prior probability, the Bayesian estimation function is used to fuse the first data and the second data to obtain the updated probability estimate.

[0017] A two-dimensional grid graph is constructed based on the probability estimates.

[0018] In some embodiments of the present invention, the step of locating a starting point and a target point according to the two-dimensional grid map, performing path planning using an improved A* algorithm, and obtaining path node information includes:

[0019] Locating a starting point and a target point according to the two-dimensional grid map, and determining a search direction containing 8 search nodes for path planning using an improved A* algorithm according to the starting point position and the target point position;

[0020] According to the search direction, at least one invalid search node opposite to the search direction is deleted to obtain an improved A* algorithm;

[0021] The improved A* algorithm is used for path planning to obtain path node information.

[0022] In some embodiments of the present invention, deleting at least one invalid search node opposite to the search direction according to the search direction to obtain an improved A* algorithm includes:

[0023] Use a straight line to connect the starting point and the target point to determine the eight search directions of the traditional algorithm and the search nodes corresponding to each direction;

[0024] Determine the positive direction of the X-axis, and use the angle calculation formula to calculate the ρ angle between each search node and the straight line, and obtain 8 sets of ρ angle values ​​corresponding to each search node.

[0025] Among them, the calculation formula is: In the formula, x t is the current X-axis coordinate value, y t is the current Y-axis coordinate value, x s is the X-axis coordinate value of the next search node, y s is the Y-axis coordinate value of the next search node, and the values ​​of λ and θ vary with x t ,y t , x s ,y s Change according to preset rules;

[0026] The 8 groups of ρ angle values ​​are compared, the maximum value among them is used as an invalid search node, and the invalid search node is deleted to obtain an improved A* algorithm.

[0027] In some embodiments of the present invention, performing path fusion using a fusion algorithm based on the path node information to determine the optimal path, and saving the path node information of the optimal path includes:

[0028] Acquire dynamic data of dynamic objects invading the scene through the two-dimensional laser radar and the RGB-D camera;

[0029] According to the path node information and the dynamic data, a DWA fusion algorithm of global path planning and local path planning is used to perform path planning, determine the optimal path, and save the path node information of the optimal path.

[0030] In some embodiments of the present invention, determining the path node closest to the obstacle as the sub-target point based on the optimal path, and judging whether the sub-target point is reachable includes:

[0031] According to the optimal path, in the two-dimensional grid map, the obstacle position is determined and represented by a black grid, the obstacle-free area is represented by a white grid, and the two-dimensional grid map is updated and iterated;

[0032] According to the starting point position and the two-dimensional grid map after the update iteration, the black grid closest to the starting point position is determined as the sub-target point, and it is determined whether the sub-target point is reachable.

[0033] In some embodiments of the present invention, the obstacle position is determined in a two-dimensional grid map according to the optimal path, and is represented by a black grid, and the obstacle-free area is represented by a white grid, and the two-dimensional grid map is updated and iterated, including:

[0034] Determine a first set of data of a two-dimensional grid sub-image acquired by a laser radar, and a second set of data acquired by an RGB-D camera, wherein the first set of data includes M first obstacles, and the second set of data includes N second obstacles, where M and N are integers greater than or equal to 0;

[0035] The first set of data and the second set of data are fused, the first obstacle and the second obstacle are represented by black grids in the two-dimensional grid map, and the obstacle-free area is represented by white grids, and the two-dimensional grid map is updated and iterated.

[0036] A rapid positioning device for a mobile robot in a large indoor scene, the device comprising:

[0037] A multi-sensor fusion robot mapping and navigation device, the device is used to implement a multi-sensor fusion robot mapping and navigation method; the device comprises:

[0038] The 2D mapping module is used to construct a 2D grid map based on 2D LiDAR and RGB-D camera using the Bayesian estimation method.

[0039] A path planning module is used to locate the starting point and the target point according to the two-dimensional grid map, perform path planning using an improved A* algorithm, and obtain path node information;

[0040] A fusion processing module, used to perform path fusion using a fusion algorithm based on the path node information, determine the optimal path, and save the path node information of the optimal path;

[0041] The obstacle analysis module is used to determine the path node closest to the obstacle as the sub-target point based on the optimal path, and to determine whether the sub-target point is reachable;

[0042] The path selection module is used to call the path planning module if the sub-target point is unreachable; otherwise, the path navigation module is called;

[0043] The path navigation module is used to call the fusion processing module if the target point is unreachable; otherwise, it determines that the target point is reachable and loads the two-dimensional grid map for navigation.

[0044] 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 implements the above-mentioned multi-sensor fusion robot mapping and navigation method when executed by the processor.

[0045] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the robot mapping and navigation method using multi-sensor fusion is implemented.

[0046] Compared with the prior art, the embodiments of the present invention have at least the following advantages or beneficial effects:

[0047] Based on two-dimensional laser radar and RGB-D camera, a two-dimensional grid map is constructed by using Bayesian estimation method; according to the two-dimensional grid map, the starting point and target point are identified, and the path planning is performed using the improved A* algorithm to obtain the path node information; according to the path node information, the path fusion is performed using the fusion algorithm to determine the optimal path, and the path node information of the optimal path is saved; according to the optimal path, the path node closest to the obstacle is determined as the sub-target point, and it is judged whether the sub-target point is reachable; if the sub-target point is not reachable, the step of identifying the starting point and target point according to the two-dimensional grid map, performing path planning using the improved A* algorithm, and obtaining the path node information is executed; otherwise, it is judged whether the target point is reachable; if the target point is not reachable, the step of performing path fusion using the fusion algorithm according to the path node information, determining the optimal path, and saving the path node information of the optimal path is executed; otherwise, it is determined that the target point is reachable, and the two-dimensional grid map is loaded for navigation. By comprehensively utilizing the advantages of different sensors and optimizing the algorithm, the shortcomings of the existing technology in terms of sensor accuracy and path planning efficiency are effectively solved, and a more efficient, accurate and adaptable navigation solution is provided. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. 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.

[0049] Figure 1 A flowchart of the steps of a multi-sensor fusion robot mapping and navigation method provided by one embodiment of the present invention;

[0050] Figure 2 A schematic diagram of an A* algorithm based on node search in a multi-sensor fusion robot mapping and navigation method provided by an embodiment of the present invention;

[0051] Figure 3 for Figure 2 Schematic diagram of the improved A* algorithm;

[0052] Figure 4 A two-dimensional grid diagram comparing the laser radar and camera before and after fusion of a multi-sensor fusion robot mapping and navigation method provided by an embodiment of the present invention;

[0053] Figure 5 A schematic diagram of the module structure of a multi-sensor fusion robot mapping and navigation device provided by one embodiment of the present invention;

[0054] Figure 6 A schematic diagram of the structure of a computer device provided in one embodiment of the present application. DETAILED DESCRIPTION

[0055] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings here can be arranged and designed in various different configurations.

[0056] In conjunction with the accompanying drawings, some embodiments of the present application are described in detail below. In the absence of conflict, the following embodiments and features in the embodiments can be combined with each other.

[0057] Please refer to Figure 1 , an embodiment of the present invention provides a flowchart of a multi-sensor fusion robot mapping and navigation method, the method comprising:

[0058] Step S100: constructing a two-dimensional grid map using a Bayesian estimation method based on a two-dimensional laser radar and an RGB-D camera;

[0059] Step S200: Locate the starting point and the target point according to the two-dimensional grid map, perform path planning using the improved A* algorithm, and obtain path node information;

[0060] Step S300: performing path fusion using a fusion algorithm based on the path node information to determine an optimal path, and saving the path node information of the optimal path;

[0061] Step S400: According to the optimal path, determine the path node closest to the obstacle as the sub-target point, and determine whether the sub-target point is reachable;

[0062] Step S500: if the sub-target point is unreachable, then return to the step of determining the starting point and the target point according to the two-dimensional grid map, performing path planning using the improved A* algorithm, and obtaining the path node information; otherwise, determine whether the target point is reachable;

[0063] Step S600: If the target point is unreachable, return to the step of performing path fusion based on the path node information, using a fusion algorithm to determine the optimal path, and saving the path node information of the optimal path; otherwise, determine that the target point is reachable, and load a two-dimensional grid map for navigation.

[0064] In this embodiment, by combining a two-dimensional laser radar and an RGB-D camera, the high precision of the laser radar and the ability of the RGB-D camera to obtain environmental information at different heights are utilized to improve the accuracy and comprehensiveness of mapping; by improving the A* algorithm, it is possible to solve the problem that the traditional A* algorithm is inefficient in node search by optimizing the search strategy or reducing invalid node search, especially when expanding subnodes in all directions. The improved A* algorithm and path fusion technology improve the efficiency and quality of path planning, reduce invalid searches, optimize path selection, and update the environmental model in real time and dynamically adjust the path so that the robot can better adapt to complex and changing environments; the path fusion algorithm is used to determine the optimal path, and then determine the strategy for smoothing the path or avoiding specific obstacles, thereby ensuring the effectiveness and safety of the path; by optimizing path planning and real-time environmental adaptation, the navigation efficiency and safety of the robot are significantly improved; by updating the environmental model in real time and dynamically adjusting the path planning, the method can adapt to environmental changes, especially in the presence of dynamic obstacles, thereby improving the navigation performance of the robot.

[0065] It should be noted that the above-mentioned RGB-D camera is a camera that combines RGB (red, green, blue) color images and depth information. The RGB image captures the color information of the scene, while the depth information provides the distance of objects in the scene from the camera. This type of camera usually uses infrared or other technologies to measure depth to create a scene in three-dimensional space. RGB-D cameras have a wide range of applications in computer vision, robotics, augmented reality and other fields, and can be used for tasks such as object recognition, scene reconstruction, and posture estimation.

[0066] As an example, based on a mobile robot system, the fusion process of the two algorithms is as follows: Figure 1As shown. First, a two-dimensional grid map is established based on multi-sensor data fusion. Secondly, the starting point and target point are given in the grid map, and the improved A* algorithm is used to plan the path. Next, the node information of the path planned by the A* algorithm is saved in the path set. If there is a dynamic obstacle when moving along the path, the node closest to the dynamic obstacle is used as the sub-target point. Then determine whether the robot has reached the sub-target point. If it has reached the sub-target point, it will proceed to the next step; if it has not reached the sub-target point, it will continue to carry out local path planning. Determine whether the robot has reached the target point. If it reaches the target point, the robot navigation ends. If it does not reach the target point, continue to save the node information in the path set.

[0067] In one embodiment of the present invention, a multi-sensor fusion robot mapping and navigation method is provided, wherein the specific process of "building a two-dimensional grid map based on a two-dimensional laser radar and an RGB-D camera using a Bayesian estimation method" as described in step S100 above includes the following sub-steps:

[0068] Acquire first data through the two-dimensional laser radar, and acquire second data through the RGB-D camera;

[0069] Use the prior distribution function to obtain the prior probability;

[0070] Bayesian estimation is a data fusion algorithm based on conditional probability statistics and posterior probability statistics. Bayesian estimation is based on posterior probability, but before obtaining the posterior probability, the prior probability needs to be defined. The prior distribution formula includes the observation vector Z and the estimated state vector X. Assume that the state information at time k is x k , the current observation vector is:

[0071] Zk={z1,...,zk}

[0072] The following prior distribution function is used as the likelihood estimation function of the lidar model observation, where: In the formula, P(x k |Z k-1 ) is the prior distribution function of the current state, P(Z k |Z k-1 ) is the estimate of the current state by the previous state;

[0073] According to the prior probability, the first data and the second data are fused using the Bayesian estimation function to obtain a fused and updated probability estimate, and a two-dimensional grid map is constructed according to the probability estimate; specifically, the Bayesian estimation function is used to Fusion of 2D LiDAR and RGB-D camera (depth camera) data, in the formula, is the prior probability of grid cell occupation when data is not fused; is the prior probability that the grid cell is not occupied when the data is not fused; is the conditional probability of the sensor returning the grid unit; Po is the probability estimate after fusion update.

[0074] In one embodiment of the present invention, the step S200 of "locating the starting point and the target point according to the two-dimensional grid map, performing path planning using the improved A* algorithm, and obtaining path node information" includes:

[0075] Locating a starting point and a target point according to the two-dimensional grid map, and determining a search direction containing 8 search nodes for path planning using an improved A* algorithm according to the starting point position and the target point position;

[0076] According to the search direction, at least one invalid search node opposite to the search direction is deleted to obtain an improved A* algorithm, which specifically includes the following sub-steps: using a straight line to connect the starting point and the target point to determine 8 search directions of the traditional algorithm and the search nodes corresponding to each direction;

[0077] Determine the positive direction of the X-axis, and use the angle calculation formula to calculate the ρ angle between each search node and the straight line, and obtain 8 sets of ρ angle values ​​corresponding to each search node.

[0078] Among them, the calculation formula is: In the formula, x t is the current X-axis coordinate value, y t is the current Y-axis coordinate value, x s is the X-axis coordinate value of the next search node, y s is the Y-axis coordinate value of the next search node, and the values ​​of λ and θ vary with x t ,y t , x s ,y s Change according to preset rules;

[0079] Compare the 8 groups of ρ angle values, take the maximum value as an invalid search node, and delete the invalid search node to obtain an improved A* algorithm;

[0080] The improved A* algorithm is used for path planning to obtain path node information.

[0081] As an example, the node search method of the traditional A* algorithm is 8-neighbor search, such as Figure 2 As shown. When the node is expanded, the sub-nodes in 8 directions will be expanded, and 8 ρ values ​​can be obtained. In the path planning process, invalid nodes will appear, which will increase the invalid search time. By comparing the size of the ρ value, the largest value is deleted. In view of the above problem, the present invention proposes an improved neighborhood node search algorithm, in which, as shown in Figure 3As shown in the figure, S is the starting point, T is the target point, the circle position node represents the valid search node, and the cross position node represents the deleted search node. By deleting an invalid node search, the improved A* algorithm uses the direction judgment function to transform the 8-neighbor search into a 7-neighbor search, and deletes the search node in the opposite direction of the target point, thereby improving the efficiency of the node search. Use a straight line to connect the starting point and the target point, and calculate the angle ρ of the straight line and the positive direction of the X axis at the same time. The direction judgment formula is defined as In the formula, x t is the current X-axis coordinate value, y t is the current Y-axis coordinate value, x s is the X-axis coordinate value of the next search node, y s is the Y-axis coordinate value of the next search node, and the values ​​of λ and θ vary with x t ,y t , x s ,y s Change according to the change rules in Table 1 below.

[0082] Table 1

[0083] <![CDATA[ Xt -x s ]]> <![CDATA[y t -and s ]]> λ θ >0 >0 0° 1 >0 <0 180° -1 <0 <0 180° 1 <0 >0 360° -1

[0084] In one embodiment of the present invention, as described in step S300 above, "locating the starting point and the target point according to the two-dimensional grid map, using the improved A* algorithm to perform path planning, and obtaining path node information" includes:

[0085] Acquire dynamic data of dynamic objects invading the scene through the two-dimensional laser radar and the RGB-D camera;

[0086] According to the path node information and the dynamic data, a DWA fusion algorithm of global path planning and local path planning is used to perform path planning, determine the optimal path, and save the path node information of the optimal path.

[0087] As an example, in order to deal with the intrusion of dynamic objects when the robot navigates in a complex hospital environment, a fusion method of global path planning and local path planning is used for path planning. The evaluation function of the DWA algorithm is:

[0088] G(v,w)=σ(α·head(v,w)+β·dist(v,w)+γ·vel(v,w))

[0089] Set the evaluation criteria for multiple groups of paths to select the optimal path. In the formula, v is the linear velocity of the robot, w is the angular velocity of the robot, head(v,w) is the azimuth evaluation function, which calculates the angle between the robot's current direction of motion and the target point, dist(v,w) is the distance between the robot and the nearest obstacle when it is running on the current trajectory, vel(v,w) represents the current speed of the robot, σ is the smoothing coefficient, and α, β, γ are weighting coefficients.

[0090] In one embodiment of the present invention, as described in step S400 above, “determining the path node closest to the obstacle as the sub-target point according to the optimal path, and determining whether the sub-target point is reachable” includes:

[0091] According to the optimal path, in the two-dimensional grid map, the obstacle position is determined and represented by a black grid, the obstacle-free area is represented by a white grid, and the two-dimensional grid map is updated and iterated;

[0092] According to the starting point position and the two-dimensional grid map after the update iteration, the black grid closest to the starting point position is determined as the sub-target point, and it is determined whether the sub-target point is reachable.

[0093] As an example, according to the optimal path, in a two-dimensional grid map, the obstacle position is determined and represented by a black grid, the obstacle-free area is represented by a white grid, and the two-dimensional grid map is updated and iterated, including the following sub-steps:

[0094] Determine a first set of data of a two-dimensional grid sub-image acquired by a laser radar, and a second set of data acquired by an RGB-D camera, wherein the first set of data includes M first obstacles, and the second set of data includes N second obstacles, where M and N are integers greater than or equal to 0;

[0095] The first set of data and the second set of data are fused, the first obstacle and the second obstacle are represented by black grids in the two-dimensional grid map, and the obstacle-free area is represented by white grids, and the two-dimensional grid map is updated and iterated.

[0096] In a specific embodiment, Figure 4 As shown in the figure, the actual experimental environment in a hospital includes obstacles 1, 2, 3, 4, and 5, among which obstacles 1 and 3 are both lower than the scanning height of the laser radar. The detected obstacles are represented by black grids, and the obstacle-free areas are represented by white grids. Figure 4As shown in the figure, group a is a two-dimensional grid map based on LiDAR, with only static obstacles 2, 4, and 5 on the map. Since the heights of obstacles 1 and 3 are lower than the scanning height of LiDAR, LiDAR cannot detect obstacles 1 and 3. Group b is a two-dimensional grid map based on the fusion of LiDAR and RGB-D camera data. This map contains all static obstacles in the experimental environment, thereby improving the accuracy of the two-dimensional grid map.

[0097] In the present application, first, the mobile robot loads a two-dimensional grid map created by data fusion. Secondly, the improved A* algorithm is used to perform global path planning for the robot. Finally, the mobile robot achieves navigation. The present invention relates to a mobile robot mapping and navigation method with multi-sensor fusion. An improved 7-neighborhood search A* algorithm is proposed to improve the efficiency of path planning. A fusion method of laser radar and RGB-D camera data is adopted to improve the accuracy of the two-dimensional grid map. Based on the fusion algorithm of the improved A* algorithm and the DWA algorithm, the mobile robot can navigate in a dynamic environment, and the terminal positioning error can be competent for the mobile robot navigation task in complex hospital scenes.

[0098] 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.

[0099] In one embodiment of the present invention, a multi-sensor fusion robot mapping and navigation device is also disclosed, and the device is used to implement the multi-sensor fusion robot mapping and navigation method; the device includes:

[0100] A two-dimensional mapping module 100 is used to construct a two-dimensional grid map based on a two-dimensional laser radar and an RGB-D camera using a Bayesian estimation method;

[0101] A path planning module 200 is used to locate the starting point and the target point according to the two-dimensional grid map, perform path planning using an improved A* algorithm, and obtain path node information;

[0102] A fusion processing module 300 is used to perform path fusion using a fusion algorithm based on the path node information, determine the optimal path, and save the path node information of the optimal path;

[0103] The obstacle analysis module 400 is used to determine the path node closest to the obstacle as the sub-target point according to the optimal path, and determine whether the sub-target point is reachable;

[0104] The path selection module 500 is used to call the path planning module if the sub-target point is unreachable; otherwise, call the path navigation module;

[0105] The path navigation module 600 is used to call the fusion processing module if the target point is unreachable; otherwise, determine that the target point is reachable and load a two-dimensional grid map for navigation.

[0106] The purpose of the present invention is to simplify the eight-neighbor node search method of the A* algorithm to seven by using the direction judgment formula, adopt the Bayesian estimation method to fuse the data of the two-dimensional laser radar and the RGB-D camera to improve the mapping accuracy, combine the improved A* algorithm and the DWA algorithm, build a mobile robot ROS system, and realize dynamic environment navigation.

[0107] Reference Figure 6 , showing a computer device of a multi-sensor fusion robot mapping and navigation method of the present invention, which may specifically include the following:

[0108] 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).

[0109] 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.

[0110] 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.

[0111] The 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. The computer device 12 may further include other removable / non-removable, volatile / non-volatile computer system storage media. By way of example only, the 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 6Not 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.

[0112] 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.

[0113] 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. Furthermore, 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 6 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.

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

[0115] That is, when the processing unit 16 executes the above program, it is achieved as follows: based on the two-dimensional laser radar and the RGB-D camera, a two-dimensional grid map is constructed using the Bayesian estimation method; the starting point and the target point are identified according to the two-dimensional grid map, and the path planning is performed using the improved A* algorithm to obtain the path node information; according to the path node information, the path fusion is performed using the fusion algorithm to determine the optimal path, and the path node information of the optimal path is saved; according to the optimal path, the path node closest to the obstacle is determined as the sub-target point, and it is determined whether the sub-target point is reachable; if the sub-target point is not reachable, the step of returning to the step of identifying the starting point and the target point according to the two-dimensional grid map, performing path planning using the improved A* algorithm, and obtaining the path node information is executed; otherwise, it is determined whether the target point is reachable; if the target point is not reachable, the step of returning to the step of performing path fusion using the fusion algorithm according to the path node information, determining the optimal path, and saving the path node information of the optimal path is executed; otherwise, it is determined that the target point is reachable, and the two-dimensional grid map is loaded for navigation.

[0116] 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 a multi-sensor fusion robot mapping and navigation method as provided in all embodiments of the present application:

[0117] That is, when the program is executed by the processor, it is implemented as follows: based on the two-dimensional laser radar and the RGB-D camera, a two-dimensional grid map is constructed using the Bayesian estimation method; based on the two-dimensional grid map, the starting point and the target point are identified, and the path planning is performed using the improved A* algorithm to obtain the path node information; based on the path node information, the path fusion is performed using the fusion algorithm to determine the optimal path, and the path node information of the optimal path is saved; based on the optimal path, the path node closest to the obstacle is determined as the sub-target point, and it is determined whether the sub-target point is reachable; if the sub-target point is not reachable, the step of identifying the starting point and the target point based on the two-dimensional grid map, performing path planning using the improved A* algorithm, and obtaining the path node information is returned to execute; otherwise, it is determined whether the target point is reachable; if the target point is not reachable, the step of performing path fusion based on the path node information using the fusion algorithm to determine the optimal path, and saving the path node information of the optimal path is executed; otherwise, it is determined that the target point is reachable, and the two-dimensional grid map is loaded for navigation.

[0118] Any combination of one or more computer-readable media may be used. A computer-readable medium may be a computer-readable 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.

[0119] 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.

[0120] 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.

[0121] 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.

[0122] 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.

[0123] The above is a detailed introduction to a multi-sensor fusion robot mapping and navigation method and device provided by the present application. This article uses specific examples 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 robot mapping and navigation method based on multi-sensor fusion, characterized in that: The method comprises: Based on the two-dimensional lidar and RGB-D camera, a two-dimensional grid map is constructed using the Bayesian estimation method; Locate the starting point and the target point according to the two-dimensional grid map, use the improved A* algorithm to perform path planning, and obtain path node information; According to the path node information, a fusion algorithm is used to perform path fusion, determine the optimal path, and save the path node information of the optimal path; According to the optimal path, determine the path node closest to the obstacle as the sub-target point, and determine whether the sub-target point is reachable; If the sub-target point is unreachable, return to the step of determining the starting point and the target point according to the two-dimensional grid map, perform path planning using the improved A* algorithm, and obtain the path node information; otherwise, determine whether the target point is reachable; If the target point is unreachable, the process returns to the step of performing path fusion based on the path node information, using a fusion algorithm to determine the optimal path, and saving the path node information of the optimal path; otherwise, the target point is determined to be reachable, and a two-dimensional grid map is loaded for navigation.

2. The method according to claim 1, characterized in that The two-dimensional grid map is constructed based on the two-dimensional laser radar and the RGB-D camera using the Bayesian estimation method, including: Acquire first data through the two-dimensional laser radar, and acquire second data through the RGB-D camera; Use the prior distribution function to obtain the prior probability; According to the prior probability, the Bayesian estimation function is used to fuse the first data and the second data to obtain the updated probability estimate. A two-dimensional grid graph is constructed based on the probability estimates.

3. The method according to claim 1, characterized in that The method of locating the starting point and the target point according to the two-dimensional grid map and performing path planning using the improved A* algorithm to obtain path node information includes: Locating a starting point and a target point according to the two-dimensional grid map, and determining a search direction containing 8 search nodes for path planning using an improved A* algorithm according to the starting point position and the target point position; According to the search direction, at least one invalid search node opposite to the search direction is deleted to obtain an improved A* algorithm; The improved A* algorithm is used for path planning to obtain path node information.

4. The method according to claim 3, characterized in that The step of deleting at least one invalid search node opposite to the search direction according to the search direction to obtain an improved A* algorithm comprises: Use a straight line to connect the starting point and the target point to determine the eight search directions of the traditional algorithm and the search nodes corresponding to each direction; Determine the positive direction of the X-axis, and use the angle calculation formula to calculate the ρ angle between each search node and the straight line, and obtain 8 sets of ρ angle values ​​corresponding to each search node. Among them, the calculation formula is: In the formula, x t is the current X-axis coordinate value, y t is the current Y-axis coordinate value, x s is the X-axis coordinate value of the next search node, y s is the Y-axis coordinate value of the next search node, and the values ​​of λ and θ vary with x t ,y t , x s ,y s Change according to preset rules; The 8 groups of ρ angle values ​​are compared, the maximum value among them is used as an invalid search node, and the invalid search node is deleted to obtain an improved A* algorithm.

5. The method according to claim 1, characterized in that The step of performing path fusion using a fusion algorithm based on the path node information to determine the optimal path and save the path node information of the optimal path includes: Acquire dynamic data of dynamic objects invading the scene through the two-dimensional laser radar and the RGB-D camera; According to the path node information and the dynamic data, a DWA fusion algorithm of global path planning and local path planning is used to perform path planning, determine the optimal path, and save the path node information of the optimal path.

6. The method according to claim 1, characterized in that The step of determining the path node closest to the obstacle as the sub-target point based on the optimal path, and judging whether the sub-target point is reachable, includes: According to the optimal path, in the two-dimensional grid map, the obstacle position is determined and represented by a black grid, the obstacle-free area is represented by a white grid, and the two-dimensional grid map is updated and iterated; According to the starting point position and the two-dimensional grid map after the update iteration, the black grid closest to the starting point position is determined as the sub-target point, and it is determined whether the sub-target point is reachable.

7. The method according to claim 6, characterized in that According to the optimal path, in the two-dimensional grid map, the obstacle position is determined and represented by a black grid, the obstacle-free area is represented by a white grid, and the two-dimensional grid map is updated and iterated, including: Determine a first set of data of a two-dimensional grid sub-image acquired by a laser radar, and a second set of data acquired by an RGB-D camera, wherein the first set of data includes M first obstacles, and the second set of data includes N second obstacles, where M and N are integers greater than or equal to 0; The first set of data and the second set of data are fused, the first obstacle and the second obstacle are represented by black grids in the two-dimensional grid map, and the obstacle-free area is represented by white grids, and the two-dimensional grid map is updated and iterated.

8. A multi-sensor fusion robot mapping and navigation device, characterized in that: The device is used to implement the method described in any one of claims 1 to 7; the device comprises: The 2D mapping module is used to construct a 2D grid map based on 2D LiDAR and RGB-D camera using the Bayesian estimation method. A path planning module is used to locate the starting point and the target point according to the two-dimensional grid map, perform path planning using an improved A* algorithm, and obtain path node information; A fusion processing module, used to perform path fusion using a fusion algorithm based on the path node information, determine the optimal path, and save the path node information of the optimal path; The obstacle analysis module is used to determine the path node closest to the obstacle as the sub-target point based on the optimal path, and to determine whether the sub-target point is reachable; The path selection module is used to call the path planning module if the sub-target point is unreachable; otherwise, the path navigation module is called; The path navigation module is used to call the fusion processing module if the target point is unreachable; otherwise, it determines that the target point is reachable and loads the two-dimensional grid map for navigation.

9. 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 7 when executed by the processor.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Citation Information

Patent Citations

  • Laser navigation system applicable to intelligent inspection robot of transformer substation

    CN105698807A

  • Navigation method and device, wheeled robot and storage medium

    CN115200601A

  • Real-time dynamic intelligent path planning method and system based on multi-sensor information fusion

    CN116678394A

  • Mixed path planning method and device based on improved adaptive window algorithm

    CN116878527A

  • Fusion algorithm-based autonomous navigation method for electric power meter inspection robot

    CN117570993A