A secure navigation method based on ROS2

Abstract

This invention discloses a safe navigation method based on ROS2. The method plans a global path from the robot's current position to a target point based on grid map information. A depth camera acquires depth image data and converts the depth image data into lidar data. The lidar data acquired by the lidar and the lidar data converted from the depth image data are fused, and a local cost map is constructed using the fused lidar data. A local optimal path is planned on the global path using a dynamic windowing method. Ultrasonic sensor data acquired by an ultrasonic sensor and lidar data acquired by a lidar are used to calculate the distance of the nearest obstacle from the robot, and the robot's movement speed is adjusted based on the distance. The robot navigates using the global path, the local optimal path, and the movement speed. The method uses a depth camera to achieve obstacle avoidance similar to that of a three-dimensional lidar, effectively reducing costs.

Description

Technical Field

[0001] The present invention relates to the field of robot navigation technology, and in particular to a safe navigation method based on ROS2. Background Art

[0002] With recent breakthroughs in artificial intelligence and the continuous development of service robots, research on mobile service robots has received unprecedented attention and development, driving explosive growth in the entire service robot industry. Mobile service robots can now be frequently seen in hospitals, airports, shopping malls, and other locations across my country. The biggest challenge for mobile service robots is safe and efficient navigation. When operating in crowded areas like supermarkets and hospitals, mobile robots need to have local obstacle avoidance and speed-variable capabilities to ensure their own safety and the safety of pedestrians. Current obstacle avoidance algorithms primarily use two-dimensional lidar to detect obstacles, but these algorithms fail when encountering obstacles that are above the lidar's scanning plane but below the robot's height. Furthermore, navigation algorithms without speed-variable capabilities can lead to risks due to excessive speed in crowded environments. Furthermore, current navigation frameworks are developed based on the ROS1 system. Due to limitations in the ROS Master node, the entire system ceases to function if the Master node fails. The ROS2 system utilizes distributed communication, which is more flexible and stable than the ROS1 system. Based on these characteristics, the safe navigation framework based on the ROS2 system is of great significance to mobile service robots.

[0003] Taking the Navigation2 navigation framework as an example, the general steps of robot navigation in the ROS2 system are briefly described:

[0004] First, locate the robot on the map and convert the sensor data into a cost map in the world coordinate system. Then, plan a global path and control the robot to reach the target point according to the path.

[0005] 2. Use the AMCL algorithm to locate the robot's position on the map;

[0006] 3. Convert the laser sensor data into a cost map in the world coordinate system. The cost map contains cost information and is used to calculate the optimal path during path planning.

[0007] 4. Load the global path planning plug-in to plan the global path;

[0008] 5. Load the local path planning plug-in to perform local trajectory planning;

[0009] 6. Issue speed instructions according to the planned trajectory to control the robot's movement and go to the target point.

[0010] Among them, the Navigation2 navigation framework uses lidar to identify obstacles. When encountering obstacles that are higher than the two-dimensional lidar scanning plane but lower than the height of the robot, the obstacle avoidance algorithm will fail; the Navigation2 navigation framework has no speed change function, which may cause the robot to encounter risks due to excessive speed in dense environments.

[0011] In summary, when faced with obstacles that are above the 2D LiDAR scanning plane but below the robot's height, existing navigation methods that rely solely on 2D LiDAR for obstacle avoidance will fail because the LiDAR cannot scan the obstacle. While using 3D LiDAR can solve this problem, it's prohibitively expensive. Summary of the Invention

[0012] The technical problem to be solved by the present invention is to provide a safe navigation method based on ROS2 with high security and low cost.

[0013] In order to solve the above problems, the present invention provides a secure navigation method based on ROS2, comprising the following steps:

[0014] S1. Place the robot in an unknown indoor environment;

[0015] S2, control the robot to move in an unknown indoor environment and build a grid map;

[0016] S3. placing the robot in a known indoor environment, loading the grid map, and locating the robot in the known indoor environment;

[0017] S4. Planning a global path from the robot's current position to the target point based on the grid map information;

[0018] S5. Acquire depth image data through a depth camera and convert the depth image data into lidar data;

[0019] S6. Fusing the lidar data acquired by the lidar and the lidar data converted from the depth image data, constructing a local cost map using the fused lidar data, and planning a local optimal path on the global path using a dynamic window method;

[0020] S7, using the ultrasonic sensor data obtained by the ultrasonic sensor and the lidar data obtained by the lidar to calculate the distance between the nearest obstacle and the robot, and adjusting the robot's movement speed according to the distance;

[0021] S8. Perform robot navigation using the global path, the local optimal path, and the moving speed.

[0022] As a further improvement of the present invention, step S2 includes:

[0023] S21. Control the robot to move in an unknown indoor environment through the handle, and establish a grid map for the unknown indoor environment through the mapping function package and the lidar carried by the robot.

[0024] As a further improvement of the present invention, step S2 further includes: setting virtual walls on the grid map; the steps include:

[0025] S22. Set the starting point and ending point of the obstacle, and obtain the starting point coordinates (x1, y1) and the ending point coordinates (x2, y2);

[0026] S23. Define the parameter m = x2 - x1, and calculate the straight line equation by the following formula:

[0027]

[0028] b = y1 - k * x1 (2)

[0029] y = k * x + b (3);

[0030] S24. Judge the size of x1 and x2; if x1 < x2, then let x = x1 + 1, substitute x into the straight line equation, solve for y, and store the obtained coordinates (x, y) in the point set; if x is still less than x2, then let x = x + 1 again, and solve for the new coordinates (x, y) and store them in the point set, and iterate repeatedly in this way until the obtained x > x2; if x1 > x2, then let x = x2 + 1, substitute x into the straight line equation, solve for y, and store the obtained coordinates (x, y) in the point set; if x is still less than x1, then let x = x + 1 again, and solve for the new coordinates (x, y) and store them in the point set, and iterate repeatedly in this way until the obtained x > x1; finally, obtain a set of dense points on the straight line equation, and expand the volume of the points in the point set to obtain a virtual wall.

[0031] As a further improvement of the present invention, step S5 includes:

[0032] S51. Obtain depth image data through the depth camera;

[0033] S52. Given a depth image point, convert the coordinates of the given point to the depth camera coordinate system;

[0034] S53. Map the depth data of the given depth image point into the laser data slot according to the following formula:

[0035]

[0036] Where laser[n] is the radar laser data converted from a given depth image point; z and x are the distances of the point from the optical center of the camera on the z and x axes after being projected onto the same plane as the depth camera, respectively;

[0037] S54, horizontally scan the depth image points at the same height from left to right, and repeat steps S52 and S53 until all the depth image points at the height have been scanned, to obtain the radar data dep_range sequence;

[0038] S55, setting the height range of the longitudinal scan;

[0039] S56, repeating step S54 in the height range from top to bottom until all depth image points in the range are scanned. The value in the new dep_range sequence obtained by each execution of step S54 is compared with the value in the dep_range sequence obtained in the previous loop, and the smaller value between the two is taken to generate a new dep_range sequence;

[0040] S57. After executing step S56, the final dep_range sequence is obtained as the lidar data converted into depth image data.

[0041] As a further improvement of the present invention, step S7 includes:

[0042] S71, receiving ultrasonic sensor data acquired by the ultrasonic sensor and lidar data acquired by the lidar;

[0043] S72. Calculate the minimum value of the laser radar data obtained by the laser radar and compare it with the ultrasonic sensor data. The minimum value between the two is the distance dist between the nearest obstacle and the robot. min ;

[0044] S73, receiving a robot speed signal issued by a local path planner;

[0045] S74, based on the distance dist between the nearest obstacle and the robot min Calculate speed adjustment parameters;

[0046] S75. Adjust the robot movement speed according to the linear speed and speed adjustment parameters in the robot speed signal.

[0047] As a further improvement of the present invention, in step S74, the distance dist from the nearest obstacle to the robot is calculated. min Calculate the speed adjustment parameters, the formula is as follows:

[0048]

[0049] Among them, λ is the speed adjustment parameter;

[0050] In step S75, the robot movement speed is adjusted according to the linear speed and speed adjustment parameter in the robot speed signal. The formula is as follows:

[0051] vel new =λ*vel.linear

[0052] Among them, vel new is the adjusted moving speed; vel.linear is the robot linear velocity released by the local path planner.

[0053] As a further improvement of the present invention, the following steps are also included:

[0054] S9. Obtain the distance to the nearest obstacle in the robot's forward direction through ultrasonic sensor data. When the distance reaches a dangerous distance, control the robot to stop suddenly and adjust its posture to find a feasible forward direction.

[0055] As a further improvement of the present invention, step S9 includes:

[0056] S91, receiving ultrasonic sensor data;

[0057] S92, setting the distance to the nearest obstacle in the robot's forward direction to be equal to the data obtained by the ultrasonic sensor;

[0058] S93, receiving a robot speed signal issued by a local path planner;

[0059] S94. When the distance to the nearest obstacle in the robot's forward direction is less than the safety distance, the linear speed in the robot's speed control signal is set to 0;

[0060] S95. When the distance to the nearest obstacle in the robot's forward direction is less than the safe distance, adjust the angular velocity of the robot's speed control signal. The formula is as follows:

[0061]

[0062] Among them, vel.angular.z is the angular velocity of the robot speed control signal; vel.angular.z new is the adjusted angular velocity;

[0063] S96. Repeat steps S91-S95 until the distance to the nearest obstacle in the robot's forward direction is greater than the safety distance.

[0064] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of any one of the above methods when executing the computer program.

[0065] The present invention also provides a computer-readable storage medium having a computer program stored thereon, which implements the steps of any one of the above methods when executed by a processor.

[0066] Beneficial effects of the present invention:

[0067] The ROS2-based safe navigation method of the present invention achieves an obstacle avoidance effect similar to that of a three-dimensional lidar through a depth camera, effectively reducing costs.

[0068] At the same time, the virtual wall, speed adjustment and emergency stop obstacle avoidance functions of the present invention not only ensure the safety of the robot itself during navigation, but also ensure the safety of dynamic obstacles such as pedestrians in the working environment.

[0069] The above description is only an overview of the technical solution of the present invention. In order to more clearly understand the technical means of the present invention, it can be implemented in accordance with the contents of the specification. In order to make the above and other purposes, features and advantages of the present invention more obvious and easy to understand, the following preferred embodiments are specifically cited and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 Flowchart of a secure navigation method based on ROS2 in a preferred embodiment of the present invention;

[0071] Figure 2 A schematic diagram of setting a virtual wall in a preferred embodiment of the present invention;

[0072] Figure 3 It is a schematic diagram of converting depth image data into lidar data in a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0073] The present invention will be further described below with reference to the accompanying drawings and specific embodiments so that those skilled in the art can better understand the present invention and implement it. However, the embodiments are not intended to limit the present invention.

[0074] like Figure 1 As shown, a secure navigation method based on ROS2 in a preferred embodiment of the present invention includes the following steps:

[0075] Step S1: placing the robot in an unknown indoor environment;

[0076] Step S2: Control the robot to move in an unknown indoor environment and create a grid map; specifically, the following steps are performed:

[0077] Step S21: Control the robot to move in an unknown indoor environment through the handle, and create a grid map for the unknown indoor environment through the mapping function package and the laser radar carried by the robot.

[0078] Refer to Figure 2 , in some embodiments, step S2 further includes: setting a virtual wall on the grid map; the steps include:

[0079] Step S22: Set the starting point and ending point of the obstacle to obtain the starting point coordinates (x1, y1) and the ending point coordinates (x2, y2); specifically, first click on the clicked point in rviz to set the starting point of the obstacle, and the volume size of the starting point can be set by changing the size of point.scale. Then click on the clicked point again to set the ending point of the obstacle.

[0080] Step S23: Define the parameter m = x2 - x1, and calculate the straight-line equation from the following formula:

[0081]

[0082] b = y1 - k * x1 (2)

[0083] y = k * x + b (3);

[0084] Step S24: Determine the size relationship between x1 and x2; if x1 < x2, then set x = x1 + 1, substitute x into the straight-line equation to solve for y, and store the obtained coordinates (x, y) in the point set; if x is still less than x2, then set x = x + 1 again and solve for the new coordinates (x, y) and store them in the point set. Repeat this iteration until the obtained x > x2; if x1 > x2, then set x = x2 + 1, substitute x into the straight-line equation to solve for y, and store the obtained coordinates (x, y) in the point set; if x is still less than x1, then set x = x + 1 again and solve for the new coordinates (x, y) and store them in the point set. Repeat this iteration until the obtained x > x1; finally, obtain a set of dense points on the straight-line equation, and expand the volume of the points in the point set to obtain a virtual wall.

[0085] Step S3: Place the robot in a known indoor environment, load the grid map, and locate the position of the robot in the known indoor environment; wherein, the above-mentioned unknown indoor environment becomes a known indoor environment after steps S1 and S2.

[0086] Step S4: Plan a global path from the current position of the robot to the target point according to the grid map information;

[0087] Step S5: Obtain depth image data through a depth camera and convert the depth image data into lidar data; Refer to Figure 3 . Step S5 specifically includes:

[0088] Step S51: Acquire depth image data through a depth camera;

[0089] Step S52: Given a depth image point, convert the coordinates of the given point into the depth camera coordinate system;

[0090] Step S53: Map the depth data of a given depth image point into the laser data slot according to the following formula:

[0091]

[0092] Where laser[n] is the radar laser data converted from a given depth image point; z and x are the distances of the point from the optical center of the camera on the z and x axes after being projected onto the same plane as the depth camera, respectively;

[0093] Step S54: Scan the depth image points at the same height horizontally from left to right, and repeat steps S52 and S53 until all the depth image points at the height have been scanned, to obtain the radar data dep_range sequence;

[0094] Step S55: Setting the height range of the longitudinal scan to prevent the ground from being mistaken for an obstacle;

[0095] Step S56: Repeat step S54 in the height range from top to bottom until all depth image points in the range are scanned. The value in the new dep_range sequence obtained by each execution of step S54 is compared with the value in the dep_range sequence obtained in the previous loop, and the smaller value between the two is taken to generate a new dep_range sequence;

[0096] Step S57: After executing step S56, the final dep_range sequence is obtained as the lidar data converted from the depth image data.

[0097] Step S6: Fusing the lidar data acquired by the lidar and the lidar data converted from the depth image data, constructing a local cost map using the fused lidar data, and planning a local optimal path on the global path using a dynamic window method;

[0098] Step S7, using the ultrasonic sensor data obtained by the ultrasonic sensor and the lidar data obtained by the lidar to calculate the distance between the nearest obstacle and the robot, and adjusting the robot's movement speed according to the distance; specifically including:

[0099] Step S71: receiving ultrasonic sensor data acquired by the ultrasonic sensor and lidar data acquired by the lidar;

[0100] Step S72: Calculate the minimum value of the laser radar data obtained by the laser radar and compare it with the ultrasonic sensor data. The minimum value between the two is taken as the distance dist from the nearest obstacle to the robot. min ; The formula is as follows:

[0101] dist min =min(min(laser.ranges),ultrasanic.range)(5)

[0102] Among them, laser.ranges is the laser radar data sequence, and ultrasonic.range is the ultrasonic sensor data;

[0103] Step S73: receiving the robot speed signal issued by the local path planner;

[0104] Step S74: Dist based on the distance between the nearest obstacle and the robot min Calculate the speed adjustment parameter; optionally, the formula is as follows:

[0105]

[0106] Among them, λ is the speed adjustment parameter;

[0107] Step S75: Adjust the robot's moving speed according to the linear speed and speed adjustment parameter in the robot's speed signal; specifically, the formula is as follows:

[0108] vel new =λ*vel.linear (7)

[0109] Among them, vel new is the adjusted moving speed; vel.linear is the robot linear velocity released by the local path planner.

[0110] Step S8: Using the global path, the local optimal path and the moving speed to perform robot navigation.

[0111] In some embodiments, the following steps are further included:

[0112] Step S9: Obtain the distance to the nearest obstacle in the robot's forward direction through ultrasonic sensor data. When the distance reaches a dangerous distance, the robot is controlled to stop suddenly and adjust its posture to find a feasible forward direction. Further, step S9 specifically includes:

[0113] Step S91, receiving ultrasonic sensor data;

[0114] Step S92: Set the distance to the nearest obstacle in the robot's forward direction to be equal to the data obtained by the ultrasonic sensor; the formula is as follows:

[0115] dist front =ultrasonic.range (8)

[0116] Among them, dist front is the distance to the nearest obstacle in the robot's forward direction; ultrasonic.rang is the data obtained by the ultrasonic sensor;

[0117] Step S93: receiving the robot speed signal issued by the local path planner;

[0118] Step S94: When the distance to the nearest obstacle in the robot's forward direction is less than the safety distance, the linear velocity in the robot's velocity control signal is set to 0; the formula is as follows:

[0119] vel.linear new =0,dist front <dist safe (9)

[0120] Among them, vel.linear new is the linear velocity in the robot speed control signal; dist safe For a safe distance.

[0121] Step S95: When the distance to the nearest obstacle in the robot's forward direction is less than the safety distance, adjust the angular velocity of the robot's speed control signal. The formula is as follows:

[0122]

[0123] Among them, vel.angular.z is the angular velocity of the robot speed control signal; vel.angular.z new is the adjusted angular velocity;

[0124] Step S96: Repeat steps S91-S95 until the distance to the nearest obstacle in the robot's forward direction is greater than the safety distance.

[0125] The ROS2-based safe navigation method of the present invention achieves an obstacle avoidance effect similar to that of a three-dimensional lidar through a depth camera, effectively reducing costs.

[0126] At the same time, the virtual wall, speed adjustment and emergency stop obstacle avoidance functions of the present invention not only ensure the safety of the robot itself during navigation, but also ensure the safety of dynamic obstacles such as pedestrians in the working environment.

[0127] A preferred embodiment of the present invention further discloses an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the method described in the above embodiment when executing the program.

[0128] A preferred embodiment of the present invention further discloses a computer-readable storage medium having a computer program stored thereon. When the program is executed by a processor, the steps of the method described in the above embodiment are implemented.

[0129] The above embodiments are merely preferred embodiments for the purpose of fully illustrating the present invention, and the scope of protection of the present invention is not limited thereto. Equivalent substitutions or modifications made by those skilled in the art based on the present invention are all within the scope of protection of the present invention. The scope of protection of the present invention shall be subject to the claims.

Claims

1. A safe navigation method based on ROS2, characterized in that: The following steps are involved: S1. Place the robot in an unknown indoor environment; S2, control the robot to move in an unknown indoor environment and build a grid map; S3. placing the robot in a known indoor environment, loading the grid map, and locating the robot in the known indoor environment; S4. Planning a global path from the robot's current position to the target point based on the grid map information; S5. Acquire depth image data through a depth camera and convert the depth image data into lidar data; S6. Fusing the lidar data acquired by the lidar and the lidar data converted from the depth image data, constructing a local cost map using the fused lidar data, and planning a local optimal path on the global path using a dynamic window method; S7, using the ultrasonic sensor data obtained by the ultrasonic sensor and the lidar data obtained by the lidar to calculate the distance between the nearest obstacle and the robot, and adjusting the robot's movement speed according to the distance; S8. Perform robot navigation using the global path, the local optimal path, and the moving speed; Wherein step S7 comprises: S71, receiving ultrasonic sensor data acquired by the ultrasonic sensor and lidar data acquired by the lidar; S72. Calculate the minimum value of the laser radar data obtained by the laser radar and compare it with the ultrasonic sensor data. The minimum value between the two is taken as the distance between the nearest obstacle and the robot. ; S73, receiving a robot speed signal issued by a local path planner; S74, based on the distance between the nearest obstacle and the robot Calculate the speed adjustment parameter; in step S74, according to the distance between the nearest obstacle and the robot Calculate the speed adjustment parameters, the formula is as follows: in, is the speed adjustment parameter; S75. Adjust the robot's moving speed according to the linear speed and speed adjustment parameter in the robot's speed signal. In step S75, adjust the robot's moving speed according to the linear speed and speed adjustment parameter in the robot's speed signal. The formula is as follows: in, is the adjusted moving speed; The robot linear velocity published by the local path planner.

2. The ROS2-based secure navigation method according to claim 1, wherein: Step S2 includes: S21. Use the handle to control the robot to move in an unknown indoor environment, and use the mapping function package and the robot's laser radar to create a grid map of the unknown indoor environment.

3. The ROS2-based secure navigation method according to claim 1, wherein: Step S2 also includes: setting a virtual wall on the grid map; the steps include: S22, set the starting point and end point of the obstacle, and obtain the coordinates of the starting point ( , ) and the end point coordinates ( , ); S23. Define parameters , calculate the equation of the line using the following formula: (1) (2) (3)； S24. Judgment and The size of , then let ,Will Substituting into the equation of the line, we get , the obtained coordinates ( , ) is stored in the point set; if Still less than , then let , and solve to get the new coordinates ( , ) is stored in the point set, and it is iterated repeatedly until the obtained ;like , then let ,Will Substituting into the equation of the line, we get , the obtained coordinates ( , ) is stored in the point set; if Still less than , then let , and solve to get the new coordinates ( , ) is stored in the point set, and it is iterated repeatedly until the obtained Finally, a set of dense points on the equation of the line is obtained, and the volume of the points in the point set is expanded to obtain a virtual wall.

4. The ROS2-based secure navigation method according to claim 1, wherein: Step S5 includes: S51, obtaining depth image data through a depth camera; S52. Given a depth image point, convert the coordinates of the given point into the depth camera coordinate system; S53, mapping the depth data of a given depth image point into the laser data slot according to the following formula: (4) in, Radar laser data converted for a given depth image point; and are the distances on the z-axis and x-axis from the optical center of the camera after the point is projected onto the same plane as the depth camera; S54, horizontally scan the depth image points at the same height from left to right, and repeat steps S52 and S53 until all the depth image points at the height have been scanned, to obtain the radar data dep_range sequence; S55, setting the height range of the longitudinal scan; S56, repeating step S54 in the height range from top to bottom until all depth image points in the range are scanned. The value in the new dep_range sequence obtained by each execution of step S54 is compared with the value in the dep_range sequence obtained in the previous loop, and the smaller value between the two is taken to generate a new dep_range sequence; S57. After executing step S56, the final dep_range sequence is obtained as the lidar data converted into depth image data.

5. The ROS2-based secure navigation method according to claim 1, wherein: The following steps are also included: S9. Obtain the distance to the nearest obstacle in the robot's forward direction through ultrasonic sensor data. When the distance reaches a dangerous distance, control the robot to stop suddenly and adjust its posture to find a feasible forward direction.

6. The ROS2-based secure navigation method according to claim 5, wherein: Step S9 includes: S91, receiving ultrasonic sensor data; S92, setting the distance to the nearest obstacle in the robot's forward direction to be equal to the data obtained by the ultrasonic sensor; S93, receiving a robot speed signal issued by a local path planner; S94. When the distance to the nearest obstacle in the robot's forward direction is less than the safety distance, the linear speed in the robot's speed control signal is set to 0; S95. When the distance to the nearest obstacle in the robot's forward direction is less than the safe distance, adjust the angular velocity of the robot's speed control signal. The formula is as follows: (10)； in, The angular velocity of the robot speed control signal; is the adjusted angular velocity; S96. Repeat steps S91-S95 until the distance to the nearest obstacle in the robot's forward direction is greater than the safety distance.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 6 are implemented.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images