Fusion obstacle avoidance system and obstacle avoidance method for safety inspection robot

Abstract

The invention provides a fusion obstacle avoidance system and an obstacle avoidance method for a safety inspection robot, and relates to the technical field of robot obstacle avoidance. Extracting an RGB-D image through a depth camera and calculating a two-dimensional coordinate of an obstacle; converting the polar coordinate data into a Cartesian coordinate system and carrying out weighted average on the Cartesian coordinate system and the two-dimensional coordinates to generate comprehensive coordinates of the obstacle; determining the current position and the target position of the robot, calculating the total cost of paths, and selecting an optimal path; a risk area is generated by analyzing the movement condition of an obstacle, a plurality of candidate speeds are generated, the position of a robot in a future time period is predicted, it is ensured that the robot moves outside the risk area, and the candidate speed meeting the lowest total path cost is selected as the movement speed in the future time period; therefore, safe and effective path planning and obstacle avoidance are realized.

Description

Technical Field

[0001] The present invention relates to the technical field of robot obstacle avoidance, and in particular to a fusion obstacle avoidance system and an obstacle avoidance method for a safety inspection robot. Background Art

[0002] With the development of intelligent robot technology, more and more robots are widely used in the fields of industry, transportation, public safety, etc. However, in complex environments, robots face a wide variety of obstacles with irregular distribution, which poses a great challenge to the robot's autonomous navigation and obstacle avoidance. Traditional obstacle avoidance methods usually rely on single sensor data, which makes it difficult to accurately identify and locate obstacles, resulting in collisions and deviations from the predetermined path during the inspection process. In addition, for obstacles whose positions change in dynamic environments, the existing technology is also insufficient in the real-time and accuracy of identifying obstacles, which affects the safety and efficiency of robots when performing tasks.

[0003] In the prior art, publication number CN119336027A discloses a fusion obstacle avoidance system for a safety inspection robot, which obtains real-time mobile data from sensors set around the safety inspection robot, receives real-time mobile data from various sensors, fuses the real-time mobile data using a data fusion algorithm, builds a mobile map based on mobile environment information using an ORB-SLAM2 algorithm, and determines the real-time position information of the robot in the map; receives real-time position information, plans an inspection path for the real-time position information using a Dijkstra path planning algorithm, monitors obstacle information in the surrounding movement in real time, and when an obstacle is detected, calculates an obstacle avoidance path based on the initial planned path and the real-time position information.

[0004] The main problems with the above solutions are: the ORB-SLAM2 algorithm is mainly based on visual feature points, which will lead to incomplete three-dimensional map construction in low-texture environments and blurred obstacle boundary identification; the Dijkstra path planning algorithm plans the shortest path based on static maps and cannot respond to dynamic obstacles in real time. After detecting obstacles, the Dijkstra path planning algorithm needs to be re-called to calculate the path, which takes a long time and is not suitable for scenes with high-speed movement or dense dynamic obstacles. There may be a risk of collision due to planning delays.

[0005] The above information disclosed in this Background section is only for enhancement of understanding of the background of the present disclosure and therefore it may contain information that does not constitute the prior art that is already known to one of ordinary skill in the art. Summary of the invention

[0006] The object of the present invention is to provide a fusion obstacle avoidance system and obstacle avoidance method for a safety inspection robot to solve the problems raised in the above background technology.

[0007] To achieve the above object, the present invention provides the following technical solutions: A fusion obstacle avoidance system for a safety inspection robot, specifically comprising: The radar acquisition module is used to use the robot's location as the origin of the coordinate system, collect the distance and azimuth of obstacles within the robot's inspection range relative to the robot in real time based on the laser radar, and generate polar coordinate data of the obstacles; The depth acquisition module is used to collect RGB-D images of obstacles within the inspection range in real time through a depth camera, extract the depth value of each pixel, and generate two-dimensional coordinate data of the obstacle based on the camera's internal parameters, including focal length and optical center coordinates; The coordinate calculation module is used to convert the polar coordinate data into the Cartesian coordinate system, and to perform weighted average on the obstacle's two-dimensional coordinate data and the polar coordinate data in the Cartesian coordinate system to generate the obstacle's comprehensive coordinates; The cost calculation module is used to determine the current node position and target node position of the robot, calculate the heuristic function and path cost, generate the total path cost, and select the path with the minimum total path cost as the movement path of the robot; The motion optimization module is used to randomly generate candidate speeds of the robot within a feasible speed range during the robot's motion process. For each candidate speed, the new position of the robot in the future time period is predicted, and the area around the obstacle where the risk of collision may occur is set as the risk area. When the new position predicted by the candidate speed is outside the risk area, the total path cost corresponding to the candidate speed is calculated, and the path and speed corresponding to the lowest total path cost are selected as the robot's motion speed in the future time period.

[0008] Furthermore, the formula for generating the polar coordinate data of the obstacle is: ; ; in, It represents the horizontal coordinate of the obstacle collected by the LiDAR in the Cartesian coordinate system. It represents the vertical coordinate of the obstacle collected by the LiDAR in the Cartesian coordinate system. represents the distance between the obstacle and the robot, Indicates the azimuth.

[0009] Furthermore, the formula for generating the two-dimensional coordinate data of the obstacle is: ; ; in, Represents the horizontal coordinate of the obstacle in the two-dimensional space collected by the depth camera. Represents the vertical coordinate of the obstacle in the two-dimensional space collected by the depth camera. Represents the horizontal coordinate of the pixel in the RGB-D image, Represents the vertical coordinate of the pixel in the RGB-D image, represents the horizontal coordinate of the camera optical center, represents the ordinate of the camera optical center, Represents the depth value of the pixel. Indicates the focal length of the camera in the horizontal direction. Indicates the focal length of the camera in the vertical direction; Then the two-dimensional coordinate data of the obstacle is: ; in, Represents the two-dimensional coordinates of the obstacle.

[0010] Furthermore, the principle for generating the comprehensive coordinates of obstacles is: The polar coordinate data in the Cartesian coordinate system is: ; in, Represents polar coordinate data in Cartesian coordinate system; The formula for weighted averaging of the horizontal axis is: ; in, represents the horizontal axis after weighted average, Respectively represent the weight coefficients of the data collected by the laser radar and the depth camera, and ; The formula for weighted averaging of the ordinate is: ; in, represents the ordinate after weighted average; The comprehensive coordinates are: ; in, Represents the comprehensive coordinates of the obstacle.

[0011] Further, among them, Indicates the current time The total path cost from the corresponding node to the target node, Indicates the index of the current moment, which changes with time. Indicates the current time The path cost of the corresponding node, Indicates the current time The heuristic function for the corresponding node, Indicates the last moment The path cost of the corresponding node, Respectively represent the current time The horizontal and vertical coordinates of the corresponding nodes, Respectively represent the last moment The horizontal and vertical coordinates of the corresponding nodes, Respectively represent the horizontal and vertical coordinates of the target node.

[0012] Furthermore, the principle for generating the robot's motion path and motion speed in the future time period is based on: The formula for predicting the robot's new position in the future time period is based on: ; ; in, Indicates in the future The new position of the robot on the horizontal axis at the moment, represents the candidate speed, Indicates the robot's orientation angle, is a time variable, and , is the length of the future time period, Indicates in the future The new position of the robot on the vertical axis at the moment; Collect the boundary points, movement speed and movement direction of the obstacle in the current monitoring time period, and use the deep learning network to train the obstacle position prediction model with the boundary points, movement speed and movement direction of the obstacle at the previous moment as input and the boundary points of the obstacle at the next moment as labels; based on the boundary points, movement speed and movement direction of the obstacle at the current moment, obtain the boundary points of the obstacle at the future moment t, and determine the benchmark risk area at the future moment t based on the boundary points; based on the movement speed of the obstacle in the current monitoring time period and the candidate speed of the robot, generate an area adjustment index to adjust the radius of the benchmark risk area to generate the radius of the risk area, and determine the scope of the risk area based on the adjusted radius of the risk area and the center of the benchmark risk area; when the future When the new positions of the robot within the time period are all outside the risk area, the total path cost corresponding to the candidate speed is calculated based on the formula: ; ; in, represents the total path cost corresponding to the candidate speed, A heuristic function representing candidate velocity prediction positions; Calculate the total path cost of all candidate speed prediction positions and select the lowest one as the robot's movement speed at the next time t.

[0013] Furthermore, the principle for generating the regional adjustment index to adjust the radius of the benchmark risk area is based on: Based on the movement speed of the obstacle in the current monitoring period, the historical speed standard deviation and historical speed mean of the obstacle are calculated. Based on the historical speed standard deviation, historical speed mean and candidate speed of the robot, the regional adjustment index reflecting the impact of speed on the radius of the risk area is generated. The formula is: ; ; ; in, represents the coefficient of variation of the historical speed of the obstacle, represents the standard deviation of the historical speed of the obstacle, represents the historical mean speed of the obstacle, represents the normalized robot candidate velocity, represents the maximum candidate speed, represents the regional adjustment index, They represent the coefficient of variation of the obstacle's historical speed and the weight coefficient of the normalized robot candidate speed, and ; Keep the center of the baseline risk area unchanged and adjust the radius of the baseline risk area based on the formula: ; in, represents the radius of the risk area, Indicates the radius of the baseline risk area.

[0014] The present invention also provides a fusion obstacle avoidance method for a safety inspection robot, the method is performed by the above-mentioned fusion obstacle avoidance system for a safety inspection robot, and the specific steps include: Step 1: With the robot's location as the origin of the coordinate system, the distance and azimuth of obstacles within the robot's inspection range relative to the robot are collected in real time based on the laser radar to generate the polar coordinate data of the obstacles; Step 2: Use a depth camera to collect RGB-D images of obstacles within the inspection range in real time, extract the depth value of each pixel, and generate three-dimensional coordinate data of the obstacles based on the camera's internal parameters, including focal length and optical center coordinates; Step 3: Convert the polar coordinate data to the Cartesian coordinate system, perform weighted average of the obstacle's three-dimensional coordinate data and the polar coordinate data in the Cartesian coordinate system, and generate the comprehensive coordinates of the obstacle; Step 4: Determine the current node position and target node position of the robot, calculate the heuristic function and path cost, generate the total path cost, and select the path with the smallest total path cost as the robot's motion path; Step 5: During the movement of the robot, randomly generate candidate speeds for the robot within the feasible speed range. For each candidate speed, predict the new position of the robot in the future time period. Set the area around the obstacle where the risk of collision may occur as the risk area. When the new position predicted by the candidate speed is outside the risk area, calculate the total path cost corresponding to the candidate speed, and select the path and speed corresponding to the lowest total path cost as the movement speed of the robot in the future time period.

[0015] Compared with the prior art, the present invention has the following beneficial effects: The present invention obtains the distance and azimuth of the obstacle according to the laser radar, generates the polar coordinate data of the obstacle, can effectively identify obstacles of different shapes and distances, extracts RGB-D images through the depth camera and calculates the three-dimensional coordinates of the obstacle, and converts the depth value into three-dimensional coordinates, so as to better understand the spatial position and shape of the obstacle and improve the accuracy of obstacle avoidance. The polar coordinate data is converted into a Cartesian coordinate system and weighted averaged to generate the comprehensive coordinates of the obstacle, which can minimize the deviation of each sensor and thus generate a more accurate obstacle position.

[0016] The present invention also determines the position of the benchmark risk area by predicting the movement of obstacles, and adjusts the radius of the benchmark risk area based on the movement speed of the obstacle and the candidate speed of the robot, thereby reflecting the impact of the obstacle and the movement speed of the robot on the collision risk and reducing the possibility of collision. For paths that do not fall within the risk area, the path cost is calculated and the heuristic function is used, which can efficiently evaluate the pros and cons of multiple possible paths, so as to quickly find the shortest or optimal path, and allow the robot to adjust the path according to the real-time environmental data, including the position of obstacles, dynamic changes, etc., so that the system can adapt to changes in the environment and improve the safety and reliability of inspections; Candidate speeds are generated within the feasible speed range, and the position in the future time period is predicted, so that the robot can flexibly select the speed according to environmental changes, which improves the applicability of the obstacle avoidance system in scenes with dense obstacles and frequent changes. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 This is a schematic diagram of the system modules of an embodiment of the present invention; Figure 2 The figure is a schematic diagram of the method flow of an embodiment of the present invention. DETAILED DESCRIPTION

[0018] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with specific embodiments.

[0019] It should be noted that, unless otherwise defined, the technical terms or scientific terms used in the present invention should be understood by people with ordinary skills in the field to which the present invention belongs. The words "first", "second" and similar words used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. "Include" or "comprise" and similar words mean that the elements or objects appearing before the word include the elements or objects listed after the word and their equivalents, without excluding other elements or objects. "Connect" or "connected" and similar words are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative positional relationships. When the absolute position of the described object changes, the relative positional relationship may also change accordingly.

[0020] Example: See also Figure 1 , the present invention provides a technical solution: A fusion obstacle avoidance system for a safety inspection robot, specifically comprising: The radar acquisition module is used to use the robot's location as the origin of the coordinate system, collect the distance and azimuth of obstacles within the robot's inspection range relative to the robot in real time based on the laser radar, and generate polar coordinate data of the obstacles; In this embodiment, the formula for generating the polar coordinate data of the obstacle is: ; ; in, It represents the horizontal coordinate of the obstacle collected by the LiDAR in the Cartesian coordinate system. It represents the vertical coordinate of the obstacle collected by the LiDAR in the Cartesian coordinate system. represents the distance between the obstacle and the robot, Indicates the azimuth.

[0021] Set the robot's position as the origin of the plane coordinate system, with the direction the robot is facing as the positive Axis, select the horizontal direction with Axis vertical as Axis, build a coordinate system; the data collected by the laser radar is only reflected on the x-axis and y-axis, reflecting the distribution of obstacles on the plane. The real-time collection of the laser radar is to collect data at the current moment. The laser radar scans point by point when collecting data, which can reflect the distance and azimuth of the obstacle in real time, and judge the distance and azimuth of the obstacle's outline and the robot.

[0022] The depth acquisition module is used to collect RGB-D images of obstacles within the inspection range in real time through a depth camera, extract the depth value of each pixel, and generate two-dimensional coordinate data of the obstacle based on the camera's internal parameters, including focal length and optical center coordinates; In this embodiment, the formula for generating the two-dimensional coordinate data of the obstacle is: ; ; in, Represents the horizontal coordinate of the obstacle in the two-dimensional space collected by the depth camera. Represents the vertical coordinate of the obstacle in the two-dimensional space collected by the depth camera. Represents the horizontal coordinate of the pixel in the RGB-D image, Represents the vertical coordinate of the pixel in the RGB-D image, represents the horizontal coordinate of the camera optical center, represents the ordinate of the camera optical center, Represents the depth value of the pixel. Indicates the focal length of the camera in the horizontal direction. Indicates the focal length of the camera in the vertical direction; RGB-D images represent images that contain both red, green, and blue channel values ​​and pixel depth values. The column where the leftmost pixel of the RGB-D image is located is used as the Axis, the row of the bottom pixel is Axis, establish plane rectangular coordinate system, depth value Represents the actual distance from the pixel to the depth camera. The optical center coordinates represent the coordinates of the camera's optical center in RGB-D, which is usually located at the center of the image. That is, the focal length of the camera in the horizontal and vertical directions, which reflects the imaging capability of the camera lens. The formula for generating the two-dimensional coordinates of obstacles is based on the pinhole camera model, which describes how points in three-dimensional space are projected onto the two-dimensional image plane. The core idea is to convert pixel coordinates into actual physical coordinates through the camera's intrinsic parameters, which include the optical center coordinates and focal length. The depth camera not only provides pixel coordinates, but also provides the depth value of each pixel. The depth value represents the actual distance from the pixel to the camera. By subtracting the optical center coordinates, it reflects the offset of the pixel relative to the optical center. Based on the principle of similar triangles, , sorted out , similarly, . Then the two-dimensional coordinate data of the obstacle is: ; in, Represents the two-dimensional coordinates of the obstacle.

[0023] A coordinate generation module is used to convert polar coordinate data and distance data into Cartesian coordinate system respectively, and to perform weighted average on the obstacle's three-dimensional coordinate data, polar coordinate data in Cartesian coordinate system and distance data in Cartesian coordinate system to generate comprehensive coordinates of the obstacle; In this embodiment, the principle for generating the comprehensive coordinates of the obstacle is: The polar coordinate data in the Cartesian coordinate system is: ; in, Represents polar coordinate data in Cartesian coordinate system; Converting polar coordinate data to Cartesian coordinate system can accurately represent the boundary of obstacles; The formula for weighted averaging of the horizontal axis is: ; in, represents the horizontal axis after weighted average, Respectively represent the weight coefficients of the data collected by the laser radar and the depth camera, and ; The formula for weighted averaging of the ordinate is: ; in, represents the ordinate after weighted average; The comprehensive coordinates are: ; in, Represents the comprehensive coordinates of the obstacle.

[0024] The data collected by the LiDAR reflects the outline of the obstacle, and the depth camera reflects the surface details of the obstacle. The LiDAR is suitable for medium and long-distance obstacle avoidance and global path planning. The noise of the depth camera increases significantly at long distances. The data collected by the LiDAR is more reliable within the main working range of the inspection robot. The data collected by the LiDAR directly reflects the position and size of the obstacle, and has a more direct impact on obstacle avoidance. Therefore, the data collected by the LiDAR has a higher weight coefficient and is taken as , .

[0025] The cost calculation module is used to determine the current node position and target node position of the robot, calculate the heuristic function and path cost, generate the total path cost, and select the path with the minimum total path cost as the movement path of the robot; In this embodiment, the formula for generating the total path cost is: ; ; ; in, Indicates the current time The total path cost from the corresponding node to the target node, Represents the index of the current moment, which changes with time, and the time interval between adjacent moments is , Indicates the current time The path cost of the corresponding node, Indicates the current time The heuristic function for the corresponding node, Indicates the last moment The path cost of the corresponding node, Respectively represent the current time The horizontal and vertical coordinates of the corresponding nodes, Respectively represent the last moment The horizontal and vertical coordinates of the corresponding nodes, Respectively represent the horizontal and vertical coordinates of the target node

[0026] The total path cost reflects the comprehensive cost of the robot moving from the current node to the target node. It is composed of the path cost and the heuristic function. The current node is the time The corresponding node, time The corresponding node is the time The parent node of the corresponding node, the path cost represents the actual accumulated cost before the current node, which is mainly reflected in the cost between the current node and its parent node. The heuristic function reflects the estimated cost from the current node to the target node, which is used to guide the search direction. By minimizing the total path cost, the system gives priority to the path with the lowest total path cost. The path cost is proportional to the distance between the current node and its parent node, and the size of the heuristic function is proportional to the distance between the current node and the target node.

[0027] The motion optimization module is used to randomly generate candidate speeds of the robot within a feasible speed range during the robot's motion process. For each candidate speed, the new position of the robot in the future time period is predicted, and the area around the obstacle where the risk of collision may occur is set as the risk area. When the new position predicted by the candidate speed is outside the risk area, the total path cost corresponding to the candidate speed is calculated, and the path and speed corresponding to the lowest total path cost are selected as the robot's motion speed in the future time period.

[0028] In this embodiment, the principle for generating the robot's motion path and motion speed in the future time period is based on: The formula for predicting the robot's new position in the future time period is based on: ; ; in, Indicates in the future The new position of the robot on the horizontal axis at the moment, represents the candidate speed, Indicates the robot's orientation angle, is a time variable, and , is the length of the future time period, Indicates in the future The new position of the robot on the vertical axis at the moment; When predicting the new position of the robot in the future, the current time The location is taken as the initial location, after After that, the robot moves a distance of , then Multiply the cosine value and sine value of the included angle respectively to obtain the displacement components corresponding to the robot in the horizontal and vertical directions as the new positions of the robot in the horizontal and vertical directions.

[0029] During the movement of the robot, the obstacle may be in motion or stationary state. For a stationary obstacle, its candidate speed is constant at 0, and its risk area is the obstacle itself. The boundary points, movement speed and movement direction of the obstacle in the current monitoring time period are collected. Through the deep learning network, the boundary points, movement speed and movement direction of the obstacle at the previous moment are used as input, and the boundary points of the obstacle at the next moment are used as labels to train the obstacle position prediction model. Based on the boundary points, movement speed and movement direction of the obstacle at the current moment, the boundary points of the obstacle at the future moment t are obtained, and the baseline risk area at the future moment t is determined based on the boundary points. Based on the movement speed of the obstacle in the current monitoring time period and the candidate speed of the robot, the area adjustment index is generated to adjust the baseline risk area radius to generate the risk area radius, and the risk area position is determined based on the adjusted risk area radius. The current monitoring time period means starting from the current time and covering the forward The entire time range, calculate each The average data within the range is used to reflect This is the data of time length.

[0030] when the future When the new positions of the robot within the time period are all outside the risk area, the total path cost corresponding to the candidate speed is calculated based on the formula: ; ; in, represents the total path cost corresponding to the candidate speed, A heuristic function representing candidate velocity prediction positions; The purpose of the motion optimization module is to allow the robot to avoid obstacles while reaching the target position at the lowest cost. is a time variable, and each prediction of the future The robot motion at the moment, starting from the initial node, for each candidate speed of the robot, after After that, a displacement will be generated to reach the new coordinates; based on the movement of obstacles in the past time period, an obstacle motion prediction model is constructed to predict the possible movement range of obstacles at time t in the future, so as to prevent the robot from moving into the possible movement range of the obstacle. The possible movement range of the obstacle is set as the risk area. Only when the new position predicted by the candidate speed is outside the risk area, it is considered that obstacle avoidance is achieved and there will be no collision risk. For each candidate speed that will not cause a collision risk, the corresponding total path cost is calculated, and the candidate speed corresponding to the path with the lowest total path cost is selected as the movement speed of the robot at time t.

[0031] In this embodiment, the principle for generating the regional adjustment index to adjust the radius of the benchmark risk area is: Based on the movement speed of the obstacle in the current monitoring period, the historical speed standard deviation and historical speed mean of the obstacle are calculated. Based on the historical speed standard deviation, historical speed mean and candidate speed of the robot, the regional adjustment index reflecting the impact of speed on the radius of the risk area is generated. The formula is: ; ; ; in, represents the coefficient of variation of the historical speed of the obstacle, represents the standard deviation of the historical speed of the obstacle, represents the historical mean speed of the obstacle, represents the normalized robot candidate velocity, represents the maximum candidate speed, represents the regional adjustment index, They represent the coefficient of variation of the obstacle's historical speed and the weight coefficient of the normalized robot candidate speed, and ; The historical speed of obstacles indicates the forward coverage within the current monitoring period. , each Speed ​​within the range, The speed in the range is The average value of the velocity at each time t within the range.

[0032] Keep the center of the baseline risk area unchanged and adjust the radius of the baseline risk area based on the formula: ; in, represents the radius of the risk area, represents the radius of the baseline risk area; The risk area of ​​the obstacle can be regarded as a circle with the obstacle as the center. After determining the position of the benchmark risk area according to the boundary point, the radius of the benchmark risk area is adjusted by the movement speed of the obstacle and the candidate speed of the robot. The coefficient of variation of the obstacle's historical speed is generated based on the standard deviation of the obstacle's historical speed and the mean of the historical speed, which reflects the fluctuation of the obstacle's historical speed. The higher the coefficient of variation, the more drastic the fluctuation of the obstacle's historical speed, and the risk area needs to be expanded to avoid the collision risk caused by the obstacle's excessive speed. The normalized robot candidate speed reflects the size of the robot's candidate speed relative to the maximum candidate speed. The closer it is to the maximum candidate speed, the greater the possible collision risk, and the corresponding risk area needs to be expanded. The benchmark risk area is adjusted at the same time according to the coefficient of variation of the obstacle's historical speed and the normalized robot candidate speed. Both have an important impact on the risk area, so The larger the area adjustment index is, the greater the adjustment ratio of the baseline risk area radius is. The area adjustment index is proportional to the coefficient of variation of the obstacle's historical speed and the normalized robot candidate speed. The range of the risk area is determined based on the adjusted risk area radius and the center of the baseline risk area.

[0033] See also Figure 2 The present invention also provides a fusion obstacle avoidance method for a safety inspection robot, the method is executed by the above-mentioned fusion obstacle avoidance system for a safety inspection robot, and the specific steps include: Step 1: Taking the robot's location as the origin of the coordinate system, the laser radar is used to obtain the distance and azimuth of the obstacle within the robot's inspection range relative to the robot at the time of acquisition, and the polar coordinate data of the obstacle is generated; Step 2: At the same acquisition time, the RGB-D image of the obstacle within the inspection range is obtained through the depth camera, the depth value of each pixel is extracted, and the three-dimensional coordinate data of the obstacle is generated based on the intrinsic parameters of the camera, wherein the intrinsic parameters of the camera include the focal length and the coordinates of the optical center; Step 3: At the same collection time, the distance between the obstacle within the inspection range and the safety inspection robot is measured based on the ultrasonic sensor; Step 4: Convert the polar coordinate data and distance data to the Cartesian coordinate system respectively, and perform weighted average on the obstacle's three-dimensional coordinate data, the polar coordinate data in the Cartesian coordinate system, and the distance data in the Cartesian coordinate system to generate the comprehensive coordinates of the obstacle; Step 5: Determine the current node position and target node position of the robot, calculate the heuristic function and path cost, generate the total path cost, and select the path with the smallest total path cost as the robot's motion path; Step 6: During the movement of the robot, randomly generate candidate speeds for the robot within the feasible speed range. For each candidate speed, predict the new position of the robot in the future time period. Set the minimum distance at which the robot will not collide with obstacles as the safety distance. When the new position predicted by the candidate speed is outside the safety distance, calculate the total path cost corresponding to the candidate speed, and select the path and speed corresponding to the lowest total path cost as the movement speed of the robot in the future time period.

[0034] The above formulas are all dimensionless and numerical calculations. The formula is a formula for the most recent real situation obtained by collecting a large amount of data and performing software simulation. The preset parameters in the formula are set by technicians in this field according to actual conditions.

[0035] The above embodiments may be implemented in whole or in part by software, hardware, firmware or any other combination thereof. When implemented by software, the above embodiments may be implemented in whole or in part in the form of a computer program product. Those skilled in the art may appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein may be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed by hardware or software methods depends on the specific application and design constraints of the technical solution.

[0036] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, and may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0037] The above description is only a specific implementation manner of the present application, but the protection scope of the present application is not limited thereto. Any technician familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application.

Claims

1. A fusion obstacle avoidance system for a safety inspection robot, characterized in that: Specifically include: The radar acquisition module is used to use the robot's location as the origin of the coordinate system, collect the distance and azimuth of obstacles within the robot's inspection range relative to the robot in real time based on the laser radar, and generate polar coordinate data of the obstacles; The depth acquisition module is used to collect RGB-D images of obstacles within the inspection range in real time through a depth camera, extract the depth value of each pixel, and generate two-dimensional coordinate data of the obstacle based on the camera's internal parameters, including focal length and optical center coordinates; The coordinate calculation module is used to convert the polar coordinate data into the Cartesian coordinate system, and to perform weighted average on the obstacle's two-dimensional coordinate data and the polar coordinate data in the Cartesian coordinate system to generate the obstacle's comprehensive coordinates; The cost calculation module is used to determine the current node position and target node position of the robot, calculate the heuristic function and path cost, generate the total path cost, and select the path with the minimum total path cost as the movement path of the robot; The motion optimization module is used to randomly generate candidate speeds of the robot within a feasible speed range during the robot's motion process. For each candidate speed, the new position of the robot in the future time period is predicted, and the area around the obstacle where the risk of collision may occur is set as the risk area. When the new position predicted by the candidate speed is outside the risk area, the total path cost corresponding to the candidate speed is calculated, and the path and speed corresponding to the lowest total path cost are selected as the robot's motion speed in the future time period.

2. The fusion obstacle avoidance system for a safety inspection robot according to claim 1, characterized in that: The formula for generating the polar coordinate data of obstacles in the radar acquisition module is: ； ； in, Represents the horizontal coordinate of the obstacle collected by the LiDAR in the Cartesian coordinate system. It represents the vertical coordinate of the obstacle collected by the LiDAR in the Cartesian coordinate system. represents the distance between the obstacle and the robot, Indicates the azimuth.

3. The fusion obstacle avoidance system for a safety inspection robot according to claim 1, characterized in that: The formula for generating the two-dimensional coordinate data of obstacles in the depth acquisition module is: ； ； in, Represents the horizontal coordinate of the obstacle in the two-dimensional space collected by the depth camera. Represents the vertical coordinate of the obstacle in the two-dimensional space collected by the depth camera. Represents the horizontal coordinate of the pixel in the RGB-D image, Represents the vertical coordinate of the pixel in the RGB-D image, represents the horizontal coordinate of the camera optical center, represents the ordinate of the camera optical center, Represents the depth value of the pixel. Indicates the focal length of the camera in the horizontal direction. Indicates the focal length of the camera in the vertical direction; Then the two-dimensional coordinate data of the obstacle is: ； in, Represents the two-dimensional coordinates of the obstacle.

4. The fusion obstacle avoidance system for a safety inspection robot according to claim 1, characterized in that: The principle on which the comprehensive coordinates of obstacles are generated in the coordinate calculation module is: The polar coordinate data in the Cartesian coordinate system is: ； in, Represents polar coordinate data in Cartesian coordinate system; The formula for weighted averaging of the horizontal axis is: ； in, represents the horizontal axis after weighted average, Respectively represent the weight coefficients of the data collected by the laser radar and the depth camera, and ; The formula for weighted averaging of the ordinate is: ； in, represents the ordinate after weighted average; The comprehensive coordinates are: ； in, Represents the comprehensive coordinates of the obstacle.

5. The fusion obstacle avoidance system for a safety inspection robot according to claim 4, characterized in that: The formula for generating the total path cost in the cost calculation module is: ； ； ； in, Indicates the current time The total path cost from the corresponding node to the target node, Indicates the index of the current moment, which changes with time. Indicates the current time The path cost of the corresponding node, Indicates the current time The heuristic function corresponding to the node, Indicates the last moment The path cost of the corresponding node, Respectively represent the current time The horizontal and vertical coordinates of the corresponding nodes, Respectively represent the last moment The horizontal and vertical coordinates of the corresponding nodes, Respectively represent the horizontal and vertical coordinates of the target node.

6. The fusion obstacle avoidance system for a safety inspection robot according to claim 5, characterized in that: The principle for generating the motion path and motion speed of the robot in the future time period in the motion optimization module is based on: The formula for predicting the robot's new position in the future time period is based on: ； ； in, Indicates in the future The new position of the robot on the horizontal axis at the moment, represents the candidate speed, Indicates the robot's orientation angle, is a time variable, and , is the length of the future time period, Indicates in the future The new position of the robot on the vertical axis at the moment; Collect the boundary points, movement speed and movement direction of the obstacle at each moment in the current monitoring time period, and use the deep learning network to train the obstacle position prediction model with the boundary points, movement speed and movement direction of the obstacle at the previous moment as input and the boundary points of the obstacle at the next moment as labels; based on the boundary points, movement speed and movement direction of the obstacle at the current moment, obtain the boundary points of the obstacle at the future moment t, and determine the benchmark risk area at the future moment t based on the boundary points; based on the movement speed of the obstacle in the current monitoring time period and the candidate speed of the robot, generate an area adjustment index to adjust the radius of the benchmark risk area to generate the radius of the risk area, and determine the scope of the risk area based on the adjusted radius of the risk area and the center of the benchmark risk area; when the future When the new positions of the robot within the time period are all outside the risk area, the total path cost corresponding to the candidate speed is calculated based on the formula: ； ； in, represents the total path cost corresponding to the candidate speed, A heuristic function representing candidate velocity prediction positions; Calculate the total path cost of all candidate speed prediction positions and select the lowest one as the robot's movement speed at time t.

7. The fusion obstacle avoidance system of a safety inspection robot according to claim 6, characterized in that: The principle for generating the regional adjustment index to adjust the radius of the benchmark risk area is based on: Based on the movement speed of the obstacle in the current monitoring period, the historical speed standard deviation and historical speed mean of the obstacle are calculated. Based on the historical speed standard deviation, historical speed mean and candidate speed of the robot, the regional adjustment index reflecting the impact of speed on the radius of the risk area is generated. The formula is: ； ； ； in, represents the coefficient of variation of the historical speed of the obstacle, represents the standard deviation of the historical speed of the obstacle, represents the historical mean speed of the obstacle, represents the normalized robot candidate velocity, represents the maximum candidate speed, represents the regional adjustment index, They represent the coefficient of variation of the obstacle's historical speed and the weight coefficient of the normalized robot candidate speed, and ; Keep the center of the baseline risk area unchanged and adjust the radius of the baseline risk area based on the formula: ； in, represents the radius of the risk area, Indicates the radius of the baseline risk area.

8. A fusion obstacle avoidance method for a safety inspection robot, characterized in that: The method is performed by the fusion obstacle avoidance system for a safety inspection robot according to any one of claims 1 to 7, and the specific steps include: Step 1: Taking the robot's location as the origin of the coordinate system, the laser radar collects the distance and azimuth of obstacles within the robot's inspection range relative to the robot in real time to generate the polar coordinate data of the obstacles; Step 2: Use a depth camera to collect RGB-D images of obstacles within the inspection range in real time, extract the depth value of each pixel, and generate three-dimensional coordinate data of the obstacles based on the camera's internal parameters, including focal length and optical center coordinates; Step 3: Convert the polar coordinate data to the Cartesian coordinate system, perform weighted average of the obstacle's three-dimensional coordinate data and the polar coordinate data in the Cartesian coordinate system, and generate the comprehensive coordinates of the obstacle; Step 4: Determine the current node position and target node position of the robot, calculate the heuristic function and path cost, generate the total path cost, and select the path with the smallest total path cost as the robot's motion path; Step 5: During the movement of the robot, randomly generate candidate speeds for the robot within the feasible speed range. For each candidate speed, predict the new position of the robot in the future time period. Set the area around the obstacle where the risk of collision may occur as the risk area. When the new position predicted by the candidate speed is outside the risk area, calculate the total path cost corresponding to the candidate speed, and select the path and speed corresponding to the lowest total path cost as the movement speed of the robot in the future time period.

Images