Autonomous navigation method and system for fire rescue quadruped robot

Abstract

The invention belongs to the technical field of robot fire rescue navigation, and particularly relates to an autonomous navigation method and system for a fire rescue quadruped robot, and the method comprises the following six main steps: environment scanning and mapping, target identification and positioning, communication and instruction receiving, map conversion and repositioning, path planning and obstacle avoidance, and fire extinguishing and rescue. The first robot is responsible for entering a fire scene to conduct environment scanning and mapping, a three-dimensional point cloud map is obtained, and trapped persons and a fire source are accurately recognized and positioned through the depth camera. And then, the first robot sends the information to a fire-fighting command center and a second robot, and the command center issues a fire extinguishing instruction. And the second robot converts the three-dimensional point cloud map into a two-dimensional grid map and carries out repositioning. And then, the second robot performs path planning according to the fire source position information and finally arrives at the fire source to perform fire extinguishing work. Effective rescue of the quadruped robot on a fire scene is achieved, and rescue efficiency and safety are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot firefighting and rescue navigation, and in particular relates to an autonomous navigation method and system for a firefighting and rescue quadruped robot. Background Art

[0002] In the existing technology, quadruped robots face many challenges when performing firefighting and rescue tasks, especially in complex and dynamically changing fire scene environments. These challenges include but are not limited to environmental uncertainty, avoidance of dynamic obstacles, and accurate autonomous navigation without manual control, tracks, cables and GPS assistance. In order to solve these problems, a quadruped robot system that can effectively rescue in complex environments is needed. The system can achieve accurate mapping, positioning and path planning, and has the ability of real-time obstacle avoidance and fire fighting and rescue. To this end, the present invention proposes an autonomous navigation method and system for a firefighting and rescue quadruped robot. Summary of the invention

[0003] The purpose of the present invention is to provide an autonomous navigation method and system for a firefighting and rescue quadruped robot, which can achieve accurate mapping, positioning and path planning, and at the same time has the ability of real-time obstacle avoidance and fire fighting and rescue.

[0004] The technical solution adopted by the present invention is as follows:

[0005] An autonomous navigation method for a fire rescue quadruped robot comprises the following steps:

[0006] Step 1: Environmental scanning and mapping: Robot No. 1 enters the fire scene, uses its onboard laser radar to scan the environment, and uses SLAM technology to build a map to obtain a three-dimensional point cloud map of the fire scene and form fire information data;

[0007] Step 2: Target identification and positioning: Robot No. 1 uses its depth camera to accurately identify and locate the precise location of the trapped people and the fire source;

[0008] Step 3: Communication and command reception: Robot No. 1 sends fire information data to the fire command center and Robot No. 2, and the fire command center issues a fire extinguishing command to Robot No. 2;

[0009] Step 4: Map conversion and relocation: Robot No. 2 converts the 3D point cloud map in the fire information data into a 2D grid map suitable for path planning; and uses the relocation algorithm to determine its own position in the map;

[0010] Step 5: Path planning and obstacle avoidance: Robot No. 2 plans a path based on the fire source location information in the fire information data, and moves to the target location according to the planned path;

[0011] Step 6: Firefighting and rescue: After the No. 2 robot reaches the fire source, it carries out firefighting work.

[0012] Preferably, in step 1, SLAM technology is used for mapping, and the SLAM algorithm used is FAST-LIO. FAST-LIO receives the scan data and completes the mapping process after data accumulation, state estimation, feature extraction and dedistortion, point cloud registration, and map management.

[0013] Preferably, in step 1, in the specific process of obtaining the three-dimensional point cloud map of the fire scene, the scanned point cloud data in PointCloud2 format is converted into the Livox custom point cloud format CustomMsg and published to an independent topic, so that the industrial computer of robot No. 1 and the industrial computer of robot No. 2 can share this type of point cloud data.

[0014] Preferably, in step 2, the YOLOv8 target recognition algorithm is used to find the fire source and trapped persons, and the characteristics of the depth camera are used to obtain the x, y coordinates of the target and the distance information relative to the camera coordinate system, which are then transformed to the world coordinate system by TF and reported to the fire command center.

[0015] Preferably, in step 3, robot No. 1 and robot No. 2 wirelessly communicate with the fire command center through a TCP module to receive instructions from the fire command center and transmit image streams and video streams to the fire command center.

[0016] Preferably, in step 4, when performing map conversion, the three-dimensional map is converted into a two-dimensional grid map using the map conversion function of the octomap function package, and the converted map is imported into move_base as a global cost map.

[0017] Preferably, in step 4, the relocalization algorithm uses the FAST_LIO_LOCALIZATION algorithm. The FAST_LIO_LOCALIZATION algorithm is used as an icp relocalization framework for real-time three-dimensional global positioning in a pre-built point cloud map. The relocalization algorithm fuses low-frequency global positioning and high-frequency odometer data provided by FAST-LIO, making the entire system computationally very efficient.

[0018] Preferably, in step 5, the path planning and obstacle avoidance technology includes:

[0019] The global path planner combines the global cost map and uses the Dijkstra algorithm to plan a global path, and the second robot will move along this path;

[0020] During the navigation process, the lidar works continuously and uses real-time scanning data to generate a local cost map. The local path planner combines the odometer information of the second robot and the local cost map, and uses the Trajectory Rollout algorithm to complete the planned path and real-time obstacle avoidance.

[0021] An autonomous navigation system for a firefighting and rescue quadruped robot, comprising a No. 1 robot, a No. 2 robot and a firefighting command center;

[0022] Robot No. 1: Equipped with laser radar, depth camera, IMU and odometer; used for mapping and finding trapped people and fire sources;

[0023] Robot No. 2: Also equipped with laser radar, depth camera, IMU and odometer; used to extinguish fires at the source of fires;

[0024] Fire Command Center: Used to receive fire information data and send instructions to Robot No. 1 and Robot No. 2 to complete the control of Robot No. 1 and Robot No. 2.

[0025] The technical effects achieved by the present invention are:

[0026] The present invention provides a method for solving the problem of insufficient autonomous navigation performance of quadruped robots in complex and dynamic environments in the prior art. The method includes six main steps: environmental scanning and mapping, target identification and positioning, communication and command reception, map conversion and repositioning, path planning and obstacle avoidance, and fire fighting and rescue. Robot No. 1 is responsible for entering the fire scene to scan and map the environment, using the FAST-LIO SLAM algorithm and laser radar to obtain a three-dimensional point cloud map, and using a depth camera to accurately identify and locate trapped people and fire sources. Subsequently, Robot No. 1 sends the information to the fire command center and Robot No. 2, and the command center issues a fire extinguishing order. Robot No. 2 converts the three-dimensional point cloud map into a two-dimensional grid map and repositions it using FAST-LIO-LOCALIZATION. Then, Robot No. 2 performs path planning based on the fire source location information, and uses the Dijkstra algorithm and Trajectory Rollout algorithm to achieve path planning and obstacle avoidance, and finally arrives at the fire source to extinguish the fire. The quadruped robot firefighting and rescue system of the present invention includes Robot No. 1 and Robot No. 2, both of which are equipped with a laser radar, a depth camera, an IMU, and an odometer. The advantage of the present invention is that it can effectively carry out rescue work at the fire scene without human control, tracks, cables and GPS assistance. Through the precise mapping and positioning of the No. 1 robot and the efficient firefighting and rescue of the No. 2 robot, the efficiency and safety of the fire scene rescue are greatly improved, and the effective rescue of the quadruped robot at the fire scene is realized, and the rescue efficiency and safety are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 is a flowchart of the steps of the autonomous navigation method of the present invention;

[0028] Figure 2 It is an algorithm flow chart of the autonomous navigation method in the present invention;

[0029] Figure 3 It is the overall framework diagram of the FAST-LIO algorithm in the present invention;

[0030] Figure 4 It is the overall framework diagram of the move_base algorithm in the present invention. DETAILED DESCRIPTION

[0031] In order to make the purpose and advantages of the present invention more clearly understood, the present invention is specifically described below in conjunction with embodiments. It should be understood that the following text is only used to describe one or several specific embodiments of the present invention, and does not strictly limit the scope of protection of the specific claims of the present invention.

[0032] like Figure 1-Figure 4 As shown, an autonomous navigation method for a fire rescue quadruped robot comprises the following steps:

[0033] Step 1: Environmental scanning and mapping: Robot No. 1 enters the fire scene, uses its onboard laser radar to scan the environment, and uses SLAM technology to build a map to obtain a three-dimensional point cloud map of the fire scene and form fire information data;

[0034] In the step 1, in the specific process of obtaining the three-dimensional point cloud map of the fire scene, the scanned point cloud data in PointCloud2 format is converted into the Livox custom point cloud format CustomMsg and published to an independent topic so that the industrial computer of robot No. 1 and the industrial computer of robot No. 2 can share this type of point cloud data.

[0035] In the actual implementation of the present invention, after entering the fire scene, the No. 1 robot starts the Livox MID360 laser radar to detect the surrounding environment, obtain the point cloud information of the surrounding obstacles, and output the high-precision scanning point cloud data in PointCloud2 format.

[0036] The SLAM algorithm adopted in the present invention is FAST-LIO, which is a computationally efficient and robust radar-inertial odometer mapping algorithm. It uses a tightly coupled iterative extended Kalman filter to fuse radar feature points with IMU data to achieve robust navigation in fast-moving, noisy or chaotic environments. It is very suitable for objects with bumpy and complex working conditions such as quadruped robots.

[0037] Since the FAST-LIO algorithm needs to receive the Livox radar data type as the custom point cloud format CustomMsg of Livox, for the sake of computational efficiency and ease of use, the code for converting PointCloud2 to CustomMsg point cloud format was written, and the point cloud data in CustomMsg format was published to an independent topic so that the industrial computers of robot No. 1 and robot No. 2 can share this type of point cloud data.

[0038] Step 2: Target identification and positioning: Robot No. 1 uses its depth camera to accurately identify and locate the precise location of the trapped people and the fire source;

[0039] Preferably, in step 2, the YOLOv8 target recognition algorithm is used to find the fire source and trapped persons, and the characteristics of the depth camera are used to obtain the x, y coordinates of the target and the distance information relative to the camera coordinate system, which are then transformed to the world coordinate system by TF and reported to the fire command center.

[0040] In the actual implementation of the present invention, the RealSens D456 depth camera of the No. 1 robot continues to work while building maps and navigating. While transmitting video streams to the command center, the YOLOv8 target recognition algorithm is used to find the fire source and trapped people. The present invention specifically trains the model for indoor fire scenes, so that YOLOv8 can accurately identify targets.

[0041] By utilizing the characteristics of the depth camera, Robot No. 1 can obtain the target's x, y coordinates and distance information relative to the camera coordinate system when it identifies the target. After being transformed to the world coordinate system through TF, it is reported to the command center.

[0042] Step 3: Communication and command reception: Robot No. 1 sends fire information data to the fire command center and Robot No. 2, and the fire command center issues a fire extinguishing command to Robot No. 2;

[0043] In step 3, robot No. 1 and robot No. 2 communicate wirelessly with the fire command center through the TCP module to receive instructions from the fire command center and transmit image streams and video streams to the fire command center.

[0044] Step 4: Map conversion and relocation: Robot No. 2 converts the 3D point cloud map in the fire information data into a 2D grid map suitable for path planning; and uses the relocation algorithm to determine its own position in the map;

[0045] Preferably, in step 4, when performing map conversion, the three-dimensional map is converted into a two-dimensional grid map using the map conversion function of the octomap function package, and the converted map is imported into move_base as a global cost map.

[0046] Preferably, in step 4, the relocalization algorithm uses the FAST_LIO_LOCALIZATION algorithm. The FAST_LIO_LOCALIZATION algorithm is used as an icp relocalization framework for real-time three-dimensional global positioning in a pre-built point cloud map. The relocalization algorithm fuses low-frequency global positioning and high-frequency odometer data provided by FAST-LIO, making the entire system computationally very efficient.

[0047] In the actual implementation of the present invention, after FAST-LIO receives the scan data, it goes through data accumulation, state estimation, feature extraction and dedistortion, point cloud registration, and map management.

[0048] Since the present invention is intended to carry out fire rescue indoors, a two-dimensional navigation algorithm move_base is used. It is necessary to convert the three-dimensional map (pcd format) built by FAST-LIO into a two-dimensional grid map (pgm format). The map conversion function of the octomap function package is used to convert the three-dimensional map into a two-dimensional grid map. The converted map can be imported into move_base as a global cost map.

[0049] After completing the map building, in order to enable the No. 2 robot to find its current position when navigating, the robot needs to be positioned. In order to avoid cumulative errors and to avoid the need for the robot to have the same initial position each time it is powered on and the program is run, we used FAST_LIO_LOCALIZATION for repositioning. It is an icp repositioning framework that can perform real-time 3D global positioning in a pre-built point cloud map by integrating low-frequency global positioning and FAST-LIO's high-frequency odometer. After positioning, the robot's position on the grid map is consistent with reality, and the radar's real-time scanning data is also aligned with the grid map.

[0050] Step 5: Path planning and obstacle avoidance: Robot No. 2 plans a path based on the fire source location information in the fire information data, and moves to the target location according to the planned path;

[0051] The global path planner of move_base combines the global cost map and uses the Dijkstra algorithm to plan a global path, and robot No. 2 will move along this path;

[0052] During the navigation process, the lidar works continuously and uses real-time scanning data to generate a local cost map. The local path planner combines the odometer information of the second robot and the local cost map, and uses the Trajectory Rollout algorithm to complete the planned path and real-time obstacle avoidance.

[0053] In the actual implementation of the present invention, when the fire command center sends the target position and posture information to the second robot, the global path planner of move_base will combine the global cost map and use the Dijkstra algorithm to plan a global path, and the robot will move along the path.

[0054] During the navigation process, the lidar works continuously and uses real-time scanning data to generate a local cost map. The local path planner combines the robot's odometer information and the local cost map, uses the Trajectory Rollout algorithm, plans the path in real time, and realizes real-time obstacle avoidance.

[0055] The present invention combines the specific situation of the quadruped robot with the needs of actual scenarios and working conditions, and modifies appropriate robot parameters such as the robot's external dimensions, expansion radius, obstacle coefficient, obstacle detection range, peak velocity acceleration, and obstacle avoidance speed, so that the robot can avoid falling into a "pseudo-death" situation as much as possible, and also makes the obstacle avoidance and navigation effects better.

[0056] Step 6: Firefighting and rescue: After the No. 2 robot reaches the fire source, it carries out firefighting work.

[0057] The command center dispatched firefighters to rescue the trapped people and issued rescue instructions to the robot. The robot autonomously navigated to the target point in the world coordinate system and carried out fire-fighting actions.

[0058] In the present invention, SLAM (Simultaneous Localization and Mapping) technology is one of the key technologies to achieve autonomous navigation of robots, which enables robots to perform real-time positioning and map construction in unknown environments. As an efficient radar-inertial odometer SLAM algorithm, FAST-LIO fuses radar feature points with IMU data through a tightly coupled iterative extended Kalman filter to achieve robust navigation in fast-moving, noisy or chaotic environments. In addition, path planning and obstacle avoidance technology are also indispensable parts of quadruped robot firefighting and rescue. The Dijkstra algorithm and TrajectoryRollout algorithm are used for global path planning and real-time obstacle avoidance, respectively, to ensure that the robot can reach the target location safely and effectively.

[0059] The background technology of the present invention focuses on improving the autonomous navigation capability of quadruped robots in complex fire environments to achieve more efficient and safe fire rescue tasks. By combining advanced SLAM algorithms, path planning and obstacle avoidance technologies, the present invention aims to provide an innovative solution to overcome the limitations of the prior art.

[0060] An autonomous navigation system for a firefighting and rescue quadruped robot, comprising a No. 1 robot, a No. 2 robot and a firefighting command center;

[0061] Robot No. 1: Equipped with laser radar, depth camera, IMU and odometer; used for mapping and finding trapped people and fire sources;

[0062] Robot No. 2: Also equipped with laser radar, depth camera, IMU and odometer; used to extinguish fires at the source of fires;

[0063] Fire Command Center: Used to receive fire information data and send instructions to Robot No. 1 and Robot No. 2 to complete the control of Robot No. 1 and Robot No. 2.

[0064] The above is only a preferred embodiment of the present invention. It should be noted that, for those skilled in the art, several improvements and modifications can be made without departing from the principles of the present invention, and these improvements and modifications should also be considered as the protection scope of the present invention. The structures, devices and operating methods not specifically described and explained in the present invention shall be implemented according to the conventional means in the art unless otherwise specified and limited.

Claims

1. An autonomous navigation method for a fire rescue quadruped robot, characterized in that: The following steps are involved: Step 1: Environmental scanning and mapping: Robot No. 1 enters the fire scene, uses its onboard laser radar to scan the environment, and uses SLAM technology to build a map to obtain a three-dimensional point cloud map of the fire scene and form fire information data; Step 2: Target recognition and positioning: Robot No. 1 uses its depth camera to identify and locate the trapped people and the fire source; Step 3: Communication and command reception: Robot No. 1 sends fire information data to the fire command center and Robot No. 2, and the fire command center issues a fire extinguishing command to Robot No. 2; Step 4: Map conversion and relocation: Robot No. 2 converts the 3D point cloud map in the fire information data into a 2D grid map suitable for path planning; and uses the relocation algorithm to determine its own position in the map; Step 5: Path planning and obstacle avoidance: Robot No. 2 plans a path based on the fire source location information in the fire information data, and moves to the target location according to the planned path; Step 6: Fire extinguishing and rescue: After the No. 2 robot arrives at the fire source, it carries out fire extinguishing work.

2. The autonomous navigation method for a fire rescue quadruped robot according to claim 1, characterized in that: In step 1, SLAM technology is used for mapping, and the SLAM algorithm used is FAST-LIO. FAST-LIO receives the scan data and completes the mapping process after data accumulation, state estimation, feature extraction and dedistortion, point cloud registration, and map management.

3. The autonomous navigation method for a firefighting and rescue quadruped robot according to claim 2, characterized in that: In the step 1, in the specific process of obtaining the three-dimensional point cloud map of the fire scene, the scanned point cloud data in PointCloud2 format is converted into the Livox custom point cloud format CustomMsg and published to an independent topic.

4. The autonomous navigation method for a firefighting and rescue quadruped robot according to claim 1, characterized in that: In step 2, the YOLOv8 target recognition algorithm is used to find the fire source and trapped persons, and the characteristics of the depth camera are used to obtain the x, y coordinates of the target and the distance information relative to the camera coordinate system. After being transformed to the world coordinate system by TF, it is reported to the fire command center.

5. The autonomous navigation method for a firefighting and rescue quadruped robot according to claim 1, characterized in that: In step 3, robot No. 1 and robot No. 2 communicate wirelessly with the fire command center through the TCP module to receive instructions from the fire command center and transmit image streams and video streams to the fire command center.

6. The autonomous navigation method for a firefighting and rescue quadruped robot according to claim 1, characterized in that: In step 4, when performing map conversion, the map conversion function of the octomap function package is used to convert the three-dimensional map into a two-dimensional grid map, and the converted map is imported into move_base as a global cost map.

7. The autonomous navigation method for a firefighting and rescue quadruped robot according to claim 1, characterized in that: In step 4, the relocalization algorithm uses the FAST_LIO_LOCALIZATION algorithm, which is used as an icp relocalization framework for real-time three-dimensional global positioning in a pre-built point cloud map.

8. The autonomous navigation method for a firefighting and rescue quadruped robot according to claim 1, characterized in that: In step 5, the path planning and obstacle avoidance technology includes: The global path planner combines the global cost map and uses the Dijkstra algorithm to plan a global path, and the second robot will move along this path; During the navigation process, the lidar works continuously and uses real-time scanning data to generate a local cost map. The local path planner combines the odometer information of the second robot and the local cost map, and uses the Trajectory Rollout algorithm to complete the planned path and real-time obstacle avoidance.

9. An autonomous navigation system for a fire rescue quadruped robot, characterized in that: Including Robot No. 1, Robot No. 2 and Fire Command Center; Robot No. 1: Equipped with laser radar, depth camera, IMU and odometer; used for mapping and finding trapped people and fire sources; Robot No. 2: Also equipped with laser radar, depth camera, IMU and odometer; used to extinguish fires at the source of fires; Fire Command Center: Used to receive fire information data and send instructions to Robot No. 1 and Robot No. 2 to complete the control of Robot No. 1 and Robot No. 2.

Images