Robot autonomous navigation and obstacle avoidance system and method based on multi-sensor fusion

Abstract

The present invention relates to the field of robots, and in particular to a robot autonomous navigation and obstacle avoidance system based on multi-sensor fusion; the system comprises a data acquisition module, a regional path evaluation module, and an unknown object collision module; the data acquisition module is used to collect map data, monitor unknown obstacles, and collect body data, and obtain real-time motion data of the body when it is moving; the regional path evaluation module is used to calculate the risk rate of a scene area in different time periods according to the collected obstacle data, calculate the passing cost of the robot passing through each regional point according to the center point of the maximum range occupied by the body, and plan the minimum passing cost route for the robot to pass through each destination; the unknown object collision module is used to judge the movement state of the obstacle when an unknown obstacle appears in the working scene, correct the passing cost of the scene area point in the current working cycle according to the judgment result, and re-plan the path according to the current time point.

Description

Technical Field

[0001] The present invention relates to the field of robots, and in particular to a robot autonomous navigation and obstacle avoidance system and method based on multi-sensor fusion. Background Art

[0002] As one of the most important cutting-edge technologies today, robotics has been widely used in production and life in various fields. In daily production and life, autonomous navigation and obstacle avoidance have become an unavoidable technical problem for robots to integrate into the complex scenes of human environment.

[0003] However, today's robots mostly use real-time obstacle avoidance in the process of autonomous navigation and obstacle avoidance. In cyclically changing work scenarios, they do not analyze the possibility of obstacles appearing in the environment based on the location and time of obstacle appearance in the scene, and often cause robots to navigate to areas where obstacles frequently appear, resulting in complex avoidance behaviors every time they reach such areas. Summary of the invention

[0004] The object of the present invention is to provide a robot autonomous navigation and obstacle avoidance system and method based on multi-sensor fusion to solve the problems raised in the above background technology.

[0005] In order to solve the above technical problems, the present invention provides the following technical solutions: a robot autonomous navigation and obstacle avoidance method based on multi-sensor fusion, the method comprising the following steps:

[0006] Step S100: Use a laser radar to perform three-dimensional mapping of the robot's working scene area to obtain a scene map, and divide the scanned map of the robot's working scene area into different area points according to pixel points;

[0007] Step S200: monitor the existence information of unknown obstacles in the scene in real time, collect the regional position, time and movement trajectory of the obstacle in the scene, and calculate the obstacle avoidance risk rate of each regional point according to the monitored obstacle existence information;

[0008] Step S300: Collect the maximum movement size of the robot, obtain the center point of the robot's maximum area, determine the robot's activity range in the scene, and evaluate the robot's obstacle avoidance ability; the maximum movement size includes the maximum values ​​of the length, width, and height of the robot during movement;

[0009] Step S400: Collect the movement speed of the robot in different areas, predict the time required for the robot to travel to each area point, predict the time point of arrival at each area point according to the departure time, and calculate the risk cost of the path according to the risk rate of the predicted time point of arrival at the area point, and plan the minimum obstacle avoidance risk path according to the predicted risk cost of traveling to each area point;

[0010] Step S500: When an unknown obstacle is encountered on the way, determine whether the obstacle is a stationary or moving object; scan the obstacle, and update the obstacle avoidance risk level of the involved area points according to the obstacle characteristics;

[0011] Step S600: When the robot enters a regional point, if the deviation between the actual time and the predicted time is greater than the time deviation threshold, the path planning is re-performed according to the obstacle avoidance risk level of the regional point and the obstacle avoidance capability of the robot;

[0012] In the above technical solution, step S200 is divided into the following steps:

[0013] Step S201: When an unknown obstacle that does not exist when the map is built appears in the scene, the obstacle is monitored in real time by using a laser radar and an infrared ranging sensor to obtain the trajectory of the obstacle, collect the position of the obstacle at each time point, and calculate the time the obstacle exists at each area point;

[0014] Step S202: Divide the working time of the robot in one working cycle into time intervals of T, where represents the nth time period of regional point i;

[0015] Step S203: Monitoring The time that point i in the inner area is covered by obstacles Calculate The cumulative obstacle avoidance risk According to the formula:

[0016]

[0017] Where N is the number of robot working cycles collected;

[0018] Step S204: Calculate the cumulative obstacle avoidance risk of each area point in the robot working scene, and normalize the data to obtain the obstacle avoidance risk rate of each area point According to the formula:

[0019]

[0020] Among them, S max is the maximum value of the cumulative obstacle avoidance risk, S min is the minimum cumulative obstacle avoidance risk.

[0021] The existence information of obstacles at various locations in the work scene is collected and statistically analyzed through sensors, and then mapped to various area points in the scene map, so as to achieve early estimation and prediction of obstacles in the robot's work scene.

[0022] In the above technical solution, step S400 is divided into the following steps:

[0023] Step S401: According to the maximum movement size of the robot during the movement, the occupied area (a, b) and the travel height h of the robot during the movement are obtained, and according to the scanned map information, the range that the center point of the occupied area of ​​the robot can reach when working in the working scene is analyzed;

[0024] Among them, a is the maximum length of the robot when it is moving, and b is the maximum width of the robot when it is moving;

[0025] Step S402: When the center point of the robot's occupied area enters area point i, count the current area point in the current time period The cost of passing According to the format:

[0026]

[0027] in, is the risk rate of region ij. The formula calculates the cost of the center point of the robot's largest area passing through each area point of the scene by traversing the risk rate of the area points in the area with the largest area occupied by the robot;

[0028] Step S403: Plan a path with the minimum cost passing through all nodes based on time using a dynamic programming algorithm.

[0029] According to the risk rate in the scene and the size of the robot, the cost of the robot reaching each area in the scene is calculated. For robots of different sizes, adaptation analysis can be performed based on the scene map after detailed analysis, and the cost of different robots in different areas in the working scene can be obtained.

[0030] In the above technical solution, in step S500, obstacles are scanned by laser radar and infrared ranging to determine the movement type of the obstacles;

[0031] When the obstacle is a stationary object, the obstacle is scanned by the laser radar to obtain the area occupied by the obstacle in the map and the height of each part of the outer contour. The movable range of the center point of the robot's maximum area is analyzed according to the robot's maximum area.

[0032] In the current working cycle, the cost of passing through the range points that the robot cannot reach is set to infinity, and the forward route is replanned according to the current time point and the cost of passing through the map area points; the existence time of obstacles is monitored in real time, and the cost of passing through the map area points is calculated after the current working cycle ends;

[0033] For stationary obstacles, the position of the obstacle is directly monitored to temporarily correct the cost of the map in the current cycle, realizing the navigation and obstacle avoidance behavior in the current working cycle, and recording the presence information of the obstacle in the map area point information to enrich the map obstacle information sample and make the map analysis result more universal.

[0034] When the obstacle is a moving object, the obstacle is scanned by a lidar and an infrared ranging sensor to obtain the moving distance of the obstacle in a time interval of T0, measure the instantaneous speed of the obstacle, collect the instantaneous speeds at multiple discrete time points, and use the unscented Kalman filter method to recursively predict the position of the obstacle at future time points, so as to analyze the movement trajectory of the obstacle in the scene.

[0035] According to the predicted movement trajectory of the obstacle, the cost of the scene area points in each time period of the obstacle presence time is set to infinity in the current working cycle.

[0036] The shortest distance l between the outer contour of the obstacle and the three-dimensional structure composed of the maximum occupied range and height of the robot is monitored in real time. min When min l < l0, the robot moves to the area point with a small cost to avoid obstacles; where l0 is the safety distance threshold.

[0037] For moving obstacles, by collecting the spatial orientations of the obstacles at different time points, predicting the positions of the obstacles at different time points in the future, and then temporarily correcting the passing cost of each time period of the scene area points, the avoidance of dynamic obstacles is realized.

[0038] In the above technical solution, step S600 is divided into the following steps:

[0039] Step S601: When the center point of the maximum occupied range of the robot travels to area point i, record the current time point T. x ;

[0040] Step S602: Analyze the time period when reaching the current area point i in the planned path. Find the mid-time point T of the current time period. mid ;

[0041] Step S603: When <T 阈值 <, recalculate the passing cost planned path according to the current time point and the risk rate of the scene area points; where T 阈值 is the time deviation threshold between the actual time and the predicted time.

[0042] The arrival time of the scene area point is monitored in real time, the robot's route is corrected in real time, and accurate obstacle avoidance navigation is achieved based on the obstacle information in different time periods, thereby realizing the robot's obstacle avoidance prediction and navigation in a periodically changing working environment.

[0043] In order to better implement the above technical solution, a robot autonomous navigation and obstacle avoidance system based on multi-sensor fusion is proposed, the system includes a data acquisition module, a regional path evaluation module, and an unknown object obstacle avoidance module;

[0044] The data acquisition module is used to collect map data, monitor and scan unknown obstacles, and is also used to collect robot body data to obtain real-time motion data of the body when it is moving; the regional path evaluation module is used to calculate the risk rate of the scene area in different time periods based on the collected scene obstacle data, and then calculate the passing cost of each regional point of the robot's path based on the center point of the maximum area occupied by the robot body, and then plan the minimum passing cost route for the robot to each destination; the unknown object collision module is used to determine whether the obstacle is stationary or in motion when an unknown obstacle appears in the working scene, and correct the passing cost of the scene area point in the current working cycle according to the judgment result, and re-plan the path according to the current time point;

[0045] The output end of the data acquisition module is connected to the input end of the regional path evaluation module and the unknown object obstacle avoidance module; the regional path evaluation module is bidirectionally connected to the unknown object obstacle avoidance module.

[0046] In the above technical solution, the data acquisition module includes an environment monitoring unit and a body data acquisition unit;

[0047] The environment monitoring unit uses a laser radar to scan the robot's working scene to establish a map, and obtains information about the existence of obstacles in the environment by real-time monitoring of each area in the scene, and collects the regional location, existence time and movement trajectory of the obstacles; the body data acquisition unit is used to obtain the robot's own data and the maximum movement size of the robot when it is moving, and analyze the maximum area and center point of the robot when it is moving;

[0048] The output ends of the environment monitoring unit and the body data acquisition unit are connected to the input ends of the regional path evaluation module and the unknown object obstacle avoidance module.

[0049] In the above technical solution, the regional path assessment module includes a regional risk analysis unit and a path passing cost analysis unit;

[0050] The regional risk analysis unit analyzes the risk rate of each regional point in the robot's work scene according to the existence information of obstacles in the robot's historical work cycle; the path passing cost analysis unit calculates the passing cost of the robot passing each regional point in the work scene according to the collected current robot body data and the risk rate of each regional point in the work scene and the time point when the robot is predicted to arrive at each regional point;

[0051] The output end of the regional risk analysis unit is connected to the input end of the path through the cost analysis unit; the path is bidirectionally connected to the unknown object obstacle avoidance module through the cost analysis unit.

[0052] In the above technical solution, the unknown object obstacle avoidance module includes a stationary object obstacle avoidance unit and a moving object obstacle avoidance module; wherein the moving object obstacle avoidance module includes an object trajectory prediction unit and a trajectory obstacle avoidance unit;

[0053] The stationary object obstacle avoidance unit corrects the passing cost of the scene area points in the current working cycle according to the position of the stationary obstacle to plan the obstacle avoidance route; the object trajectory prediction unit obtains the motion data of the moving obstacle according to the position of the moving obstacle at different time points collected by the infrared ranging sensor, and then predicts the movement trajectory of the obstacle; the trajectory obstacle avoidance unit corrects the passing cost of the scene area points corresponding to the predicted obstacle position at each time point according to the predicted trajectory of the moving obstacle to plan the obstacle avoidance route;

[0054] The stationary object obstacle avoidance unit and the input end of the moving object obstacle avoidance module are connected to the output end of the data acquisition module; the stationary object obstacle avoidance unit and the object trajectory prediction unit are bidirectionally connected to the regional path evaluation module; the output end of the object trajectory prediction unit is connected to the input end of the trajectory obstacle avoidance unit.

[0055] Compared with the prior art, the beneficial effects achieved by the present invention are: for scenes with periodic changes, such as roads for commuting during rush hours in the morning and evening, areas around office buildings in office areas, and other scenes with periodic obstacles, the obstacle avoidance risk assessment can be performed on the scene according to the time and position of the obstacle appearing in the scene, and the passing cost of the robot when traveling in each area of ​​the scene can be assessed according to the structure of the robot, and then the possibility of the robot encountering obstacles and needing to avoid obstacles when working is estimated, and finally a path with the least possibility of needing to avoid obstacles is selected according to the working time of the robot. This avoids the robot from repeatedly encountering obstacles in the same area when traveling in the scene, laying a solid foundation for the robot to integrate into human society, avoid peak hours, and work efficiently. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] The accompanying drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation of the present invention. In the accompanying drawings:

[0057] Figure 1 It is a structural schematic diagram of a robot autonomous navigation and obstacle avoidance system based on multi-sensor fusion of the present invention;

[0058] Figure 2 It is a structural connection diagram of the robot autonomous navigation and obstacle avoidance system based on multi-sensor fusion of the present invention. DETAILED DESCRIPTION

[0059] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0060] See also Figure 1-2 The present invention provides a technical solution: a robot autonomous navigation and obstacle avoidance method based on multi-sensor fusion, the method comprising the following steps:

[0061] Step S100: Use a laser radar to perform three-dimensional mapping of the robot's working scene area to obtain a scene map, and divide the scanned map of the robot's working scene area into different area points according to pixel points;

[0062] Step S200: monitor the existence information of unknown obstacles in the scene in real time, collect the regional position, time and movement trajectory of the obstacle in the scene, and calculate the obstacle avoidance risk rate of each regional point according to the monitored obstacle existence information;

[0063] Step S300: Collect the maximum movement size of the robot, obtain the center point of the robot's maximum area, determine the robot's activity range in the scene, and evaluate the robot's obstacle avoidance ability; the maximum movement size includes the maximum values ​​of the length, width, and height of the robot during movement;

[0064] Step S400: Collect the movement speed of the robot in different areas, predict the time required for the robot to travel to each area point, predict the time point of arrival at each area point according to the departure time, and calculate the risk cost of the path according to the risk rate of the predicted time point of arrival at the area point, and plan the minimum obstacle avoidance risk path according to the predicted risk cost of traveling to each area point;

[0065] Step S500: When an unknown obstacle is encountered on the way, determine whether the obstacle is a stationary or moving object; scan the obstacle, and update the obstacle avoidance risk level of the involved area points according to the obstacle characteristics;

[0066] Step S600: When the robot enters a regional point, if the deviation between the actual time and the predicted time is greater than the time deviation threshold, the path planning is re-performed according to the obstacle avoidance risk level of the regional point and the obstacle avoidance capability of the robot;

[0067] In the above technical solution, step S200 is divided into the following steps:

[0068] Step S201: When an unknown obstacle that does not exist when the map is built appears in the scene, the obstacle is monitored in real time by using a laser radar and an infrared ranging sensor to obtain the trajectory of the obstacle, collect the position of the obstacle at each time point, and calculate the time the obstacle exists at each area point;

[0069] Step S202: Divide the working time of the robot in one working cycle into time intervals of T, where represents the nth time period of regional point i;

[0070] Step S203: Monitoring The time that point i in the inner area is covered by obstacles Calculate The cumulative obstacle avoidance risk According to the formula:

[0071]

[0072] Where N is the number of robot working cycles collected;

[0073] Step S204: Calculate the cumulative obstacle avoidance risk of each area point in the robot working scene, and normalize the data to obtain the obstacle avoidance risk rate of each area point According to the formula:

[0074]

[0075] Among them, S max is the maximum value of the cumulative obstacle avoidance risk, S min is the minimum cumulative obstacle avoidance risk.

[0076] The existence information of obstacles at various locations in the working scene is collected and statistically analyzed through sensors, and then mapped to various regional points in the scene map, so as to achieve early estimation and prediction of obstacles in the robot's working scene;

[0077] In actual use, after the robot's working environment is determined, a lidar scan is performed on the scene to obtain a scanned map of the scene, and the map is scaled to an appropriate size to ensure that the structural features of the obstacles in the scene can be reflected in the scene map. The scaled map is divided according to pixel points, and the risk rate of each area point is calculated using the calculation method of the above technical solution.

[0078] In the above technical solution, step S400 is divided into the following steps:

[0079] Step S401: According to the maximum movement size of the robot during the movement, the occupied area (a, b) and the travel height h of the robot during the movement are obtained, and according to the scanned map information, the range that the center point of the occupied area of ​​the robot can reach when working in the working scene is analyzed;

[0080] Among them, a is the maximum length of the robot when it is moving, and b is the maximum width of the robot when it is moving;

[0081] Step S402: When the center point of the robot's occupied area enters area point i, count the current area point in the current time period The cost of passing According to the format:

[0082]

[0083] in, is the risk rate of region ij. The formula calculates the cost of the center point of the robot's largest area passing through each area point of the scene by traversing the risk rate of the area points in the area with the largest area occupied by the robot;

[0084] Step S403: Plan a path with the minimum cost passing through all nodes based on time using a dynamic programming algorithm.

[0085] According to the risk rate in the scene and the size of the robot, the cost of the robot reaching each area in the scene is calculated. For robots of different sizes, adaptation analysis can be performed based on the scene map after detailed analysis, and the cost of different robots in different areas in the working scene can be obtained.

[0086] In the specific implementation, after calculating the regional point passing cost of the robot according to the robot's body size, the scene's passing cost altitude map can be drawn with the passing cost of each area of ​​the scene as the map altitude, and then the route map with the lowest average altitude when the robot is moving can be analyzed.

[0087] In the above technical solution, in step S500, obstacles are scanned by laser radar and infrared ranging to determine the movement type of the obstacles;

[0088] When the obstacle is a stationary object, scan the obstacle with a lidar to obtain the occupied area range in the corresponding map of the obstacle and the height of each part of the outer contour. Analyze the movable range of the center point of the maximum occupied area of the robot according to the maximum occupied area of the robot.

[0089] In the current working cycle, set the passing cost of the area points that the robot cannot reach to infinity. Re-plan the forward route according to the current time point and the passing cost of the map area points; Monitor the existence time of the obstacle in real time, and calculate the passing cost of the map area points after the end of the current working cycle.

[0090] For stationary obstacles, temporarily correct the passing cost of the current cycle's map by directly monitoring the position of the obstacle, implement the navigation and obstacle avoidance behavior of the current working cycle, and record the existence information of the obstacle in the map area point information to enrich the map obstacle information sample and make the map analysis result more universal.

[0091] When the obstacle is a moving object, scan the obstacle with a lidar and an infrared ranging sensor to obtain the moving distance of the obstacle in a time interval of T0, measure the instantaneous speed of the obstacle, collect the instantaneous speeds at multiple discrete time points, and use the unscented Kalman filter method to recursively predict the position of the obstacle at future time points, so as to analyze the movement trajectory of the obstacle in the scene.

[0092] According to the predicted movement trajectory of the obstacle, set the passing cost of the scene area points in each time period of the obstacle existence time to infinity in the current working cycle.

[0093] Monitor the shortest distance l between the outer contour of the obstacle and the three-dimensional structure composed of the maximum occupied area and height of the robot in real time min , when the shortest distance l min < l0, the robot moves to the area point with a small passing cost to avoid obstacles; where l0 is the safety distance threshold.

[0094] For moving obstacles, collect the spatial orientations of the obstacles at different time points, predict the positions of the obstacles at different time points in the future, and then temporarily correct the passing cost of each time period of the scene area points, so as to realize the avoidance of dynamic obstacles.

[0095] In specific implementation, when the robot encounters an obstacle, it can detect the outer contour of the obstacle, obtain the range of the obstacle mapped on the ground, and collect the lowest height of the obstacle. When the height is lower than the maximum movement height of the robot, it means that the obstacle will collide with the moving robot. At this time, the existence information of the obstacle can be collected; when the lowest height of the obstacle is higher than the maximum movement height of the robot, it means that the obstacle will not affect the movement of the robot. At this time, there is no need to correct the passing cost of the map area.

[0096] When the robot takes the obstacle trajectory to avoid obstacles, if there are two obstacle avoidance directions with the same passing cost, the direction with the smaller passing cost to reach the destination is selected.

[0097] In the above technical solution, step S600 is divided into the following steps:

[0098] Step S601: When the center point of the robot's maximum area reaches area point i, record the current time point T x ;

[0099] Step S602: Analyze the time period of the planned path to reach the current area point i Find the midpoint time point T of the current time period mid ;

[0100] Step S603: When <T 阈值 When , the path through the cost planning is recalculated according to the risk rate of the current time point and the scene area point; where T 阈值 It is the time deviation threshold between the actual time and the predicted time.

[0101] The arrival time of the scene area point is monitored in real time, the robot's route is corrected in real time, and accurate obstacle avoidance navigation is achieved based on the obstacle information in different time periods, thereby realizing the robot's obstacle avoidance prediction and navigation in a periodically changing working environment.

[0102] In order to better implement the above technical solution, a robot autonomous navigation and obstacle avoidance system based on multi-sensor fusion is proposed, the system includes a data acquisition module, a regional path evaluation module, and an unknown object obstacle avoidance module;

[0103] The data acquisition module is used to collect map data, monitor and scan unknown obstacles, and is also used to collect robot body data to obtain real-time motion data of the body when it is moving; the regional path evaluation module is used to calculate the risk rate of the scene area in different time periods based on the collected scene obstacle data, and then calculate the passing cost of each regional point of the robot's path based on the center point of the maximum area occupied by the robot body, and then plan the minimum passing cost route for the robot to each destination; the unknown object collision module is used to determine whether the obstacle is stationary or in motion when an unknown obstacle appears in the working scene, and correct the passing cost of the scene area point in the current working cycle according to the judgment result, and re-plan the path according to the current time point;

[0104] The output end of the data acquisition module is connected to the input end of the regional path evaluation module and the unknown object obstacle avoidance module; the regional path evaluation module is bidirectionally connected to the unknown object obstacle avoidance module.

[0105] In the above technical solution, the data acquisition module includes an environment monitoring unit and a body data acquisition unit;

[0106] The environment monitoring unit uses a laser radar to scan the robot's working scene to establish a map, and obtains information about the existence of obstacles in the environment by real-time monitoring of each area in the scene, and collects the regional location, existence time and movement trajectory of the obstacles; the body data acquisition unit is used to obtain the robot's own data and the maximum movement size of the robot when it is moving, and analyze the maximum area and center point of the robot when it is moving;

[0107] The output ends of the environment monitoring unit and the body data acquisition unit are connected to the input ends of the regional path evaluation module and the unknown object obstacle avoidance module.

[0108] In the above technical solution, the regional path assessment module includes a regional risk analysis unit and a path passing cost analysis unit;

[0109] The regional risk analysis unit analyzes the risk rate of each regional point in the robot's work scene according to the existence information of obstacles in the robot's historical work cycle; the path passing cost analysis unit calculates the passing cost of the robot passing each regional point in the work scene according to the collected current robot body data and the risk rate of each regional point in the work scene and the time point when the robot is predicted to arrive at each regional point;

[0110] The output end of the regional risk analysis unit is connected to the input end of the path through the cost analysis unit; the path is bidirectionally connected to the unknown object obstacle avoidance module through the cost analysis unit.

[0111] In the above technical solution, the unknown object obstacle avoidance module includes a stationary object obstacle avoidance unit and a moving object obstacle avoidance module; wherein the moving object obstacle avoidance module includes an object trajectory prediction unit and a trajectory obstacle avoidance unit;

[0112] The stationary object obstacle avoidance unit corrects the passing cost of the scene area points in the current working cycle according to the position of the stationary obstacle to plan the obstacle avoidance route; the object trajectory prediction unit obtains the motion data of the moving obstacle according to the position of the moving obstacle at different time points collected by the infrared ranging sensor, and then predicts the movement trajectory of the obstacle; the trajectory obstacle avoidance unit corrects the passing cost of the scene area points corresponding to the predicted obstacle position at each time point according to the predicted trajectory of the moving obstacle to plan the obstacle avoidance route;

[0113] The stationary object obstacle avoidance unit and the input end of the moving object obstacle avoidance module are connected to the output end of the data acquisition module; the stationary object obstacle avoidance unit and the object trajectory prediction unit are bidirectionally connected to the regional path evaluation module; the output end of the object trajectory prediction unit is connected to the input end of the trajectory obstacle avoidance unit.

[0114] 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 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 device.

[0115] Finally, it should be noted that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art can still modify the technical solutions described in the aforementioned embodiments or replace some of the technical features therein by equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A robot autonomous navigation and obstacle avoidance method based on multi-sensor fusion, characterized in that: The method comprises the following steps: Step S100: Use a laser radar to perform three-dimensional mapping of the robot's working scene area to obtain a scene map, and divide the scanned map of the robot's working scene area into different area points according to pixel points; Step S200: monitor the existence information of unknown obstacles in the scene in real time, collect the regional position, time and movement trajectory of the obstacle in the scene, and calculate the obstacle avoidance risk rate of each regional point according to the monitored obstacle existence information; Step S300: Collect the maximum movement size of the robot, obtain the center point of the robot's maximum area, determine the robot's activity range in the scene, and evaluate the robot's obstacle avoidance ability; the maximum movement size includes the maximum values ​​of the length, width, and height of the robot during movement; Step S400: Collect the movement speed of the robot in different areas, predict the time required for the robot to travel to each area point, predict the time point of arrival at each area point according to the departure time, and calculate the risk cost of the path according to the risk rate of the predicted time point of arrival at the area point, and plan the minimum obstacle avoidance risk path according to the predicted risk cost of traveling to each area point; Step S500: When an unknown obstacle is encountered on the way, determine whether the obstacle is a stationary or moving object; scan the obstacle, and update the obstacle avoidance risk level of the involved area points according to the obstacle characteristics; Step S600: When the robot enters a regional point, if the deviation between the actual time and the predicted time is greater than the time deviation threshold, the path planning is re-performed according to the obstacle avoidance risk level of the regional point and the obstacle avoidance capability of the robot; The step S200 is divided into the following steps: Step S201: When an unknown obstacle that does not exist when the map is built appears in the scene, the obstacle is monitored in real time by using a laser radar and an infrared ranging sensor to obtain the trajectory of the obstacle, collect the position of the obstacle at each time point, and calculate the time the obstacle exists at each area point; Step S202: Divide the working time of the robot in one working cycle into time intervals of T, where represents the nth time period of regional point i; Step S203: Monitoring The time that point i in the inner area is covered by obstacles Calculate The cumulative obstacle avoidance risk According to the formula: Where N is the number of robot working cycles collected; Step S204: Calculate the cumulative obstacle avoidance risk of each area point in the robot working scene, and normalize the data to obtain the obstacle avoidance risk rate of each area point According to the formula: Among them, S max is the maximum value of the cumulative obstacle avoidance risk, S min is the minimum cumulative obstacle avoidance risk; The step S400 is divided into the following steps: Step S401: According to the maximum movement size of the robot during the movement, the occupied area (a, b) and the travel height h of the robot during the movement are obtained, and according to the scanned map information, the range that the center point of the occupied area of ​​the robot can reach when working in the working scene is analyzed; Among them, a is the maximum length of the robot when it is moving, and b is the maximum width of the robot when it is moving; Step S402: When the center point of the robot's occupied area enters area point i, count the current area point in the current time period The cost of passing According to the format: in, is the risk rate of region ij. The formula calculates the cost of the center point of the robot's largest area passing through each area point of the scene by traversing the risk rate of the area points in the area with the largest area occupied by the robot; Step S403: Plan a path with the minimum cost passing through all nodes based on time using a dynamic programming algorithm.

2. The robot autonomous navigation and obstacle avoidance method based on multi-sensor fusion according to claim 1, characterized in that: In step S500, obstacles are scanned by laser radar and infrared ranging to determine the movement type of the obstacles; When the obstacle is a stationary object, the obstacle is scanned by the laser radar to obtain the area occupied by the obstacle in the map and the height of each part of the outer contour. The movable range of the center point of the robot's maximum area is analyzed according to the robot's maximum area. In the current working cycle, the cost of passing through the range points that the robot cannot reach is set to infinity, and the forward route is replanned according to the current time point and the cost of passing through the map area points; the existence time of obstacles is monitored in real time, and the cost of passing through the map area points is calculated after the current working cycle ends; When the obstacle is a moving object, the obstacle is scanned by laser radar and infrared ranging sensor to obtain the obstacle's moving distance in the time interval T0, measure the instantaneous speed of the obstacle, collect the instantaneous speed at multiple discrete time points, and use the unscented Kalman filter method to recursively predict the position of the obstacle at future time points, so as to analyze the movement trajectory of the obstacle in the scene; According to the predicted obstacle trajectory, the scene area point passing cost in each time period of the obstacle existence time is set to infinity in the current working cycle; Real-time monitor the shortest distance l between the outer contour of the obstacle and the three-dimensional structure composed of the maximum occupied range and height of the robot min , when the shortest distance l min < l0, the robot moves to the area point with a small cost to avoid obstacles; where l0 is the safety distance threshold.

3. The robot autonomous navigation and obstacle avoidance method based on multi-sensor fusion according to claim 1, characterized in that: Step S600 is divided into the following steps: Step S601: When the center point of the robot's maximum area reaches area point i, record the current time point T x ; Step S602: Analyze the time period of the planned path to reach the current area point i Find the midpoint time point T of the current time period mid ; Step S603: When When , the path through the cost planning is recalculated according to the risk rate of the current time point and the scene area point; where T 阈值 It is the time deviation threshold between the actual time and the predicted time.

4. A robot autonomous navigation and obstacle avoidance system based on multi-sensor fusion using a robot autonomous navigation and obstacle avoidance method based on multi-sensor fusion as claimed in any one of claims 1 to 3, characterized in that: The system includes a data acquisition module, a regional path assessment module, and an unknown object obstacle avoidance module; The data acquisition module is used to collect map data, monitor and scan unknown obstacles, and is also used to collect robot body data to obtain real-time motion data of the body when it is moving; the regional path evaluation module is used to calculate the risk rate of the scene area in different time periods based on the collected scene obstacle data, and then calculate the passing cost of each regional point of the robot's path based on the center point of the maximum area occupied by the robot body, and then plan the minimum passing cost route for the robot to each destination; the unknown object collision module is used to determine whether the obstacle is stationary or in motion when an unknown obstacle appears in the working scene, and correct the passing cost of the scene area point in the current working cycle according to the judgment result, and re-plan the path according to the current time point; The output end of the data acquisition module is connected to the input end of the regional path evaluation module and the unknown object obstacle avoidance module; the regional path evaluation module is bidirectionally connected to the unknown object obstacle avoidance module.

5. The robot autonomous navigation and obstacle avoidance system based on multi-sensor fusion according to claim 4, characterized in that: The data acquisition module includes an environment monitoring unit and a body data acquisition unit; The environment monitoring unit uses a laser radar to scan the robot's working scene to establish a map, and obtains information about the existence of obstacles in the environment by real-time monitoring of each area in the scene, and collects the regional location, existence time and movement trajectory of the obstacles; the body data acquisition unit is used to obtain the robot's own data and the maximum movement size of the robot when it is moving, and analyze the maximum area and center point of the robot when it is moving; The output ends of the environment monitoring unit and the body data acquisition unit are connected to the input ends of the regional path evaluation module and the unknown object obstacle avoidance module.

6. The robot autonomous navigation and obstacle avoidance system based on multi-sensor fusion according to claim 4, characterized in that: The regional path assessment module includes a regional risk analysis unit and a path passing cost analysis unit; The regional risk analysis unit analyzes the risk rate of each regional point in the robot's work scene according to the existence information of obstacles in the robot's historical work cycle; the path passing cost analysis unit calculates the passing cost of the robot passing each regional point in the work scene according to the collected current robot body data and the risk rate of each regional point in the work scene and the time point when the robot is predicted to arrive at each regional point; The output end of the regional risk analysis unit is connected to the input end of the path through the cost analysis unit; the path is bidirectionally connected to the unknown object obstacle avoidance module through the cost analysis unit.

7. The robot autonomous navigation and obstacle avoidance system based on multi-sensor fusion according to claim 4, characterized in that: The unknown object obstacle avoidance module includes a stationary object obstacle avoidance unit and a moving object obstacle avoidance module; The moving object obstacle avoidance module includes an object trajectory prediction unit and a trajectory obstacle avoidance unit; The stationary object obstacle avoidance unit corrects the passing cost of the scene area points in the current working cycle according to the position of the stationary obstacle to plan the obstacle avoidance route; the object trajectory prediction unit obtains the motion data of the moving obstacle according to the position of the moving obstacle at different time points collected by the infrared ranging sensor, and then predicts the movement trajectory of the obstacle; the trajectory obstacle avoidance unit corrects the passing cost of the scene area points corresponding to the predicted obstacle position at each time point according to the predicted trajectory of the moving obstacle to plan the obstacle avoidance route; The stationary object obstacle avoidance unit and the input end of the moving object obstacle avoidance module are connected to the output end of the data acquisition module; the stationary object obstacle avoidance unit and the object trajectory prediction unit are bidirectionally connected to the regional path evaluation module; the output end of the object trajectory prediction unit is connected to the input end of the trajectory obstacle avoidance unit.

Images