A navigation method and device for an unmanned fire-fighting robot based on 4D millimeter-wave radar

Abstract

The present application provides a navigation method and device for an unmanned fire-fighting robot based on a 4D millimeter-wave radar, which solves the technical problem that the existing front-line fire environment cannot accurately capture effective data for positioning and navigation, and thus cannot accurately extinguish fires. It includes obtaining the robot's initial posture angle, local map and optimal path, navigating the robot according to the optimal path; obtaining a feature point cloud, preprocessing the feature point cloud and the local map, obtaining a preprocessed feature point cloud set and a preprocessed local map, performing point cloud matching based on a variant ICP algorithm to obtain posture change parameters, superimposing them with the robot's initial posture angle to form an incremental odometer, obtaining a preliminary estimate of the robot's real-time motion state, performing corrections to obtain high-precision robot positioning and motion state, and repeating this process until reaching the fire source and spraying water to extinguish the fire. The present application can be widely used in the technical field of robot navigation.

Description

Technical Field

[0001] The present application belongs to the technical field of robot navigation, and more specifically, to a navigation method and device for an unmanned fire-fighting robot based on a 4D millimeter-wave radar. Background Art

[0002] With the acceleration of urbanization and the increase in building density, fire prevention and emergency response face unprecedented challenges. Traditional firefighting methods mainly rely on ground personnel and fixed cameras, which are inefficient and difficult to achieve comprehensive coverage. The increasing frequency and losses of fires have put forward higher requirements for fire management. In a complex fire environment, there is a large amount of thick smoke, toxic gas, and oxygen-deficient atmosphere. These environmental factors near the fire source always affect the safety and physiological health of firefighters. Therefore, mobile robots that assist front-line firefighting tasks have become a development hotspot in the industry.

[0003] Navigation technology is the key technology for developing fire-fighting robots that can break into fires. At present, most of the fire-fighting robots used at home and abroad are used in the periphery of fires. The method of achieving fire-fighting is to locate the fire by intelligent perception, and then use manual remote control or marking to carry out fire-fighting tasks in collaboration with humans and machines. Especially in fires with thick smoke and high temperatures, traditional visual sensors and lidars cannot capture effective data to establish sufficient environmental features for self-positioning and autonomous navigation, and need to rely on manual judgment to carry out fire-fighting work. Therefore, the intelligence level of fire-fighting robots and the efficiency of fire-fighting tasks in special environments with high concentrations of smoke, water mist and other media and low visibility are important indicators that reflect high-level and efficient fire-fighting task processing. Summary of the invention

[0004] The purpose of the embodiments of the present application is to provide a navigation method and device for an unmanned fire-fighting robot based on 4D millimeter-wave radar, so as to solve the technical problem in the prior art that effective data cannot be accurately captured for positioning and navigation in a front-line fire environment, resulting in the inability to accurately extinguish fires.

[0005] To achieve the above-mentioned purpose, a first aspect of an embodiment of the present application provides a navigation method for an unmanned fire-fighting robot based on a 4D millimeter-wave radar, comprising the following steps: obtaining an initial attitude angle, a local map and an optimal path of the robot, and navigating the robot according to the optimal path;

[0006] Acquire feature point cloud, preprocess feature point cloud and local map, obtain preprocessed feature point cloud set and preprocessed local map, perform point cloud matching based on variant ICP algorithm to obtain posture change parameters, superimpose them with the initial posture angle of the robot to form an incremental odometer, obtain a preliminary estimate of the real-time motion state of the robot, perform corrections to obtain high-precision robot positioning and motion state, and repeat this process until reaching the fire source and spraying water to extinguish the fire;

[0007] Preprocessing includes point cloud clustering and upsampling. Point cloud clustering introduces the reflection intensity information of the 4D millimeter-wave radar waveform and divides it using nearest neighbor search and threshold.

[0008] Preferably, the division is implemented in the following way: performing nearest neighbor search on each point cloud to obtain multiple point clouds, detecting the reflection intensity information of the multiple point clouds, and if it exceeds a threshold, marking it as a sensitive point cloud cluster, otherwise it is a common point cloud cluster.

[0009] Preferably, upsampling means that point clouds in sensitive point cloud clusters and common point cloud clusters are selected by using boundary conditions for linear interpolation to achieve enhancement of point cloud data;

[0010] The boundary condition means that the three-dimensional coordinates of the point cloud are not extreme values.

[0011] Preferably, the basic formula for point cloud matching based on the variant ICP algorithm is:

[0012] ;

[0013] In the formula, is the pose transformation relationship from the preprocessed feature point cloud of the current radar frame to the preprocessed local map point cloud. is the rotation matrix from the preprocessed feature point cloud of the current radar frame to the preprocessed local map point cloud, which means the transformation relationship between the two point cloud sets from the initial position to the matching overlap. is the translation amount from the feature point cloud after preprocessing of the current radar frame to the local map point cloud after preprocessing, that is, it represents the position transformation relationship between the two point cloud sets from the initial position to the matching overlap. is the number of point clouds in the preprocessed local map, is the i-th feature point cloud in the preprocessed feature point cloud set, is the point cloud in the i-th preprocessed local map, and the pose change parameter refers to and .

[0014] Preferably, the radar frame and IMU data obtained after the 4D millimeter-wave radar and IMU inertial sensor are jointly calibrated are time-synchronized to obtain high-frequency IMU data, and the posture transformation is calculated by integration to obtain the initial posture angle of the robot.

[0015] Preferably, a deep learning network is used to identify features acquired by a binocular depth camera and a thermal imaging camera to obtain fire source location information;

[0016] Mark the fire source location information on the local map, perform beyond-line-of-sight path planning, and obtain the optimal path;

[0017] The implementation method of beyond-visual-range path planning is as follows: the traveling salesman problem is solved for the relative positions of the robot and multiple fire sources, and the firefighting task strategies corresponding to the multiple fire source targets are constructed based on the weights of the multiple fire source target locations, so as to realize the global planning function of multiple firefighting tasks and obtain the optimal path.

[0018] Preferably, the point clouds obtained by 4D millimeter wave radar and laser radar scanning are fused to obtain a local map;

[0019] The feature point cloud is obtained by using the nearest neighbor search and setting the distance penalty factor.

[0020] Preferably, the correction refers to correcting the accumulated error between the positioning of the local map in the incremental odometer and the preliminary estimation of the real-time motion state of the robot.

[0021] A second aspect of the embodiment of the present application provides an unmanned fire-fighting robot navigation device based on a 4D millimeter-wave radar, comprising: a control mechanism, an environment sensing mechanism, a remote monitoring mechanism, a global positioning system receiver, and a hydraulic transmission mechanism;

[0022] The control mechanism controls the environmental perception mechanism, the remote monitoring mechanism, the global positioning system receiver and the hydraulic transmission mechanism to obtain the initial posture angle, the local map and the optimal path of the robot, and navigates the robot according to the optimal path; obtains the feature point cloud, preprocesses the feature point cloud and the local map, obtains the preprocessed feature point cloud set and the preprocessed local map, performs point cloud matching based on the variant ICP algorithm to obtain the posture change parameters, and superimposes them with the initial posture angle of the robot to form an incremental odometer, obtains a preliminary estimate of the real-time motion state of the robot, performs corrections to obtain high-precision robot positioning and motion state, and repeats this process until the fire source is reached and water is sprayed to extinguish the fire.

[0023] Preferably, the remote monitoring mechanism and the global positioning system receiver are combined to obtain the optimal path.

[0024] The beneficial effects of the present application are as follows: the present application first obtains the initial attitude angle of the robot when it is just started, then navigates the robot according to the optimal path, obtains the feature point cloud, and performs preprocessing to obtain a preprocessed feature point cloud set. The preprocessing here includes point cloud clustering and upsampling. The present application introduces the reflection intensity information of the 4D millimeter-wave radar waveform on the basis of the original point cloud clustering. Compared with the traditional 3D millimeter-wave radar, the reflection intensity information of the 4D millimeter-wave radar waveform introduced in the present application can obtain the point cloud position information in the pitch direction, so that the dimension of the information obtained is wider and the amount of point cloud output is larger. The use of the present application for point cloud clustering guarantees that tens of thousands of point clouds can be reached per second, and its angular resolution can reach the sub-degree level, which significantly improves the accuracy of point cloud matching.

[0025] On the other hand, the wavelength of the 4D millimeter-wave radar introduced in this application can pass through solid particles such as thick smoke and water mist. In the front-line fire environment where vision is obstructed, the point cloud is more stable and can provide richer fire environment information. Not only that, using the results of point cloud matching based on the variant ICP algorithm and the incremental odometer, it is possible to reduce the cumulative error of traditional navigation methods by only using the Doppler velocity information unique to the four-dimensional point cloud in the 4D millimeter-wave radar, and significantly reduce the dependence on the point cloud loop detection algorithm. In summary, the present application can capture more effective information, output a larger amount of point clouds, and have higher recognition accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0027] Figure 1 A flowchart of a navigation method for an unmanned fire-fighting robot based on 4D millimeter-wave radar provided in one embodiment of the present application;

[0028] Figure 2 A flowchart of beyond-line-of-sight path planning provided in an embodiment of the present application;

[0029] Figure 3 A schematic diagram of an unmanned fire-fighting robot navigation device based on 4D millimeter-wave radar provided in one embodiment of the present application;

[0030] Figure 4 A schematic diagram of a hydraulic transmission mechanism provided in one embodiment of the present application;

[0031] Figure 5 A flowchart of the real-time motion state of a robot provided in one embodiment of the present application;

[0032] Figure 6 A schematic diagram of robot positioning provided by an embodiment of the present application;

[0033] Figure 7 A diagram showing the mileage calculation and mapping effect of a correct path provided in an embodiment of the present application;

[0034] Figure 8 A diagram showing the mileage calculation and mapping effect of the navigation method of the present application equipped with 4D millimeter wave radar data provided in an embodiment of the present application;

[0035] Fig. 9 This is a diagram of the mileage calculation and mapping effect of inputting 4D millimeter-wave radar data into a traditional SLAM algorithm provided in an embodiment of the present application. DETAILED DESCRIPTION

[0036] In order to make the technical problems, technical solutions and beneficial effects to be solved by the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0037] The first aspect of the embodiment of the present application provides an unmanned fire-fighting robot navigation method based on 4D millimeter wave radar:

[0038] See also Figure 1 , is a flow chart of a navigation method for an unmanned fire-fighting robot based on a 4D millimeter-wave radar provided in an embodiment of the present application, comprising the following steps:

[0039] S1: Obtain the robot's initial attitude angle, local map and optimal path, and navigate the robot according to the optimal path.

[0040] Acquire radar frames and IMU data, synchronize the radar frames and IMU data to obtain high-frequency IMU data, integrate them to obtain the robot's posture transformation, and superimpose them to obtain the robot's initial posture angle; fuse the point clouds obtained by 4D millimeter-wave radar and lidar scanning to obtain a local map; use thermal imaging cameras and binocular depth cameras to identify the environment near the fire source to obtain the fire source location information, mark it on the local map for beyond-visual-range path planning, and obtain the optimal path.

[0041] The 4D millimeter-wave radar and the IMU inertial sensor are jointly calibrated to confirm the transformation relationship between the two coordinate systems. In the initial state, the 4D millimeter-wave radar captures the radar frame, and the IMU inertial sensor obtains the IMU data. Time synchronization is performed to confirm the high-frequency IMU data corresponding to the start time of the radar key frame, and then the high-frequency IMU data is integrated to solve the posture transformation, which is superimposed on the posture estimation result of the most recent frame of the 4D millimeter-wave radar as the initial posture angle of the 4D millimeter-wave radar coordinate system. It is worth noting that the 4D millimeter-wave radar coordinate system coincides with the robot's coordinate system, so the initial posture angle of the 4D millimeter-wave radar coordinate system is obtained, that is, the initial posture angle of the robot when it is just started.

[0042] Specifically, after the robot starts the automatic navigation function, the 4D millimeter-wave radar starts scanning to obtain the initial posture at the first radar key frame, and then integrates the high-frequency IMU data to solve the robot's posture transformation in the radar key frame, and superimposes it on the posture estimation result of the most recent frame of the 4D millimeter-wave radar as the robot's initial posture angle. Among them, at the first radar key frame, the initial posture angle and the robot's posture transformation obtained from the point cloud of the frame are superimposed to obtain the robot's posture estimation result. For each subsequent radar key frame, the posture estimation result is achieved by superimposing the robot's posture transformation calculated from the point cloud of the frame on the basis of the posture estimation result of the previous frame.

[0043] Among them, the radar key frame is selected from multiple adjacent radar frames, and the screening condition is: the difference in point cloud distribution between the radar key frame and other radar frames exceeds a certain degree, and the specific indicator for distinguishing the difference is the distance and rotation angle of the radar frame point cloud. In this embodiment, the threshold for judging the radar key frame is as follows: if the distance of the radar frame exceeds 0.2m, and if the rotation angle exceeds 10 degrees, then the radar frame is judged to be a radar key frame. There is no restriction on the setting of the threshold here, and it can be set according to the actual situation.

[0044] Thermal imaging cameras and binocular depth cameras are used to identify the environment near the fire source, obtain the fire source location information, mark it on the local map for beyond-line-of-sight path planning, and obtain the optimal path.

[0045] Specifically, the binocular depth camera and the thermal imaging camera are jointly calibrated, and then the thermal imaging camera and the binocular depth camera respectively identify the environment near the fire source at the same time. Among them, the binocular depth camera is sensitive to the high-intensity light environment characteristics near the fire source, and is used to obtain high-intensity light characteristics. The thermal imaging camera is sensitive to the high-temperature environment characteristics near the fire source and is used to obtain high-temperature characteristics. The features obtained by the two cameras are input into the deep learning network for identification to obtain the fire source location information. It is worth noting that the present application can be used in various scenarios. In a non-first-line, high-visibility fire environment, the high-intensity light features obtained by the binocular depth camera can constitute a visual odometer as a calculation factor to improve the calculation accuracy of the main odometer.

[0046] Furthermore, the point cloud obtained by the 4D millimeter wave radar and the laser radar scanning is fused to obtain a local map, and the fire source location information is marked in the local map. Then the host computer starts to navigate the robot according to the fire source target in the local map.

[0047] First, determine the relative position of the fire source target and the robot, perform beyond-visual-range path planning, and obtain the location information of a series of point sets in the local map. These point sets constitute the route to the fire source location, that is, the optimal path, and then perform robot navigation.

[0048] Specifically, the global positioning system receiver receives the robot's positioning information in real time, the global navigation satellite system receiver gives the location information of multiple fire sources in real time, the global navigation satellite system receiver transmits the location information of multiple fire sources to the upper computer, the global positioning system receiver transmits the robot's positioning information to the lower computer, the lower computer transmits it to the upper computer, the upper computer uses the robot's positioning information and the location information of multiple fire sources for beyond-line-of-sight path planning to obtain the optimal path.

[0049] The process of BVR path planning is as follows: Figure 2 As shown in the figure, first, the location information of multiple fire sources is jointly calibrated with the positioning information of the robot to obtain the relative position of the robot and the multiple fire sources. Then, according to the relative position of the robot and the multiple fire sources, the corresponding target navigation points in the global map, that is, the corresponding points of the multiple fire sources in the global map, are proposed, and the proposed target navigation points are input into the global path planning. At the same time, the global map is constantly matched with the local map, and the local map continuously optimizes the local path planning in real time to update the global path planning in real time.

[0050] On the other hand, the host computer solves the traveling salesman problem for the relative positions of the robot and multiple fire sources, and combines the weights of the multiple fire source target locations to finally form a firefighting task strategy corresponding to the multiple fire source targets, which is combined with the global path planning to obtain the over-the-horizon path planning for multiple firefighting tasks. Among them, the weights of the multiple fire source target locations are not limited here and can be set according to actual conditions.

[0051] Finally, the beyond-visual-range path planning for multiple firefighting tasks establishes a primary path based on the input firefighting task strategy and the target navigation point in the global map. The primary path is optimized in real time with the local path planning and is gradually corrected into a path that can autonomously avoid dynamically updated obstacles in the local map, that is, the optimal path to reach the target navigation point.

[0052] It is worth noting that the global map referred to in this application is a map that can reflect the overall layout of the environment information of the current robot. Environmental information is information that can reflect the current environment of the robot. Environmental information is collected by the robot through its own sensors. In this application, sensors refer to 4D millimeter wave radars, lidars, IMU inertial sensors, thermal imaging cameras, binocular depth cameras, etc. This application does not limit this.

[0053] S2: Obtain feature point cloud.

[0054] After obtaining the initial attitude angle of the initial state radar frame, the robot is navigated according to the optimal path. The current radar frame is used as the input of the original data, and the radar odometer as the subject calculates the change value of the robot's translation and rotation within the key frame time by matching the current radar frame to the point cloud of the local map.

[0055] Specifically, the current radar frame is matched with the local map point-to-surface point cloud, that is, the current radar frame point cloud is traversed, and the feature point cloud is obtained by using the nearest neighbor search and setting the distance penalty factor. The current radar frame refers to the most recent radar key frame at the current time. The feature point cloud obtained in this way carries environmental feature information, which can significantly improve the perception accuracy and ensure the accuracy of subsequent positioning.

[0056] In an optional embodiment, let the set of point clouds of the current radar frame be X, and the number of point clouds be ,Right now ; The point cloud set in the local map is Y, and the number of point clouds is ,Right now .

[0057] Each current radar frame point cloud will be traversed Convert to the coordinate system of the local map and use the nearest neighbor search to confirm each current radar frame point cloud The 5 closest point clouds in the local map coordinate system , and fit a cloud of 5 points based on their position coordinates The nearest plane ,Right now .

[0058] Judging 5 point clouds Arrival of fitting plane Whether the distance exceeds the threshold of 0.8m, if it exceeds the threshold, the 5 point clouds are abandoned The fitting plane , start traversing the current radar frame point cloud collection The next point cloud in ; On the contrary, calculate the point cloud of the current radar frame arrive Distance , the specific formula is:

[0059] ;

[0060] In the formula, Represents the current radar frame point cloud To plane The distance Point cloud for the current radar frame The horizontal axis of Point cloud for the current radar frame The vertical coordinate of Point cloud for the current radar frame The vertical coordinate of , , They are , , The slopes on the three coordinate axes, For plane The intercept at the origin (0,0).

[0061] Furthermore, set the distance penalty factor To determine the current radar frame point cloud Whether the surface feature point cloud can participate in the subsequent point cloud matching, The specific formula is:

[0062] ;

[0063] like >0.24, the point cloud of the current radar frame traversed this time As the surface feature point cloud participating in the subsequent point cloud matching, otherwise, start traversing the current radar frame point cloud collection Next point cloud .

[0064] Repeat the above steps until the current radar frame point cloud set is traversed. , thereby screening out several current radar frame point clouds with obvious surface features, and using them as feature point clouds for subsequent calculation of the robot's inter-frame pose changes.

[0065] S3: Preprocess the feature point cloud and the local map to obtain a preprocessed feature point cloud set and a preprocessed local map.

[0066] Before point cloud matching, the above feature point clouds and local maps need to be clustered to obtain sensitive point cloud clusters and common point cloud clusters, and linear interpolation is performed to obtain the preprocessed feature point cloud set and the preprocessed local map.

[0067] (1) Point cloud clustering:

[0068] This application utilizes an improved point cloud clustering method to perform clustering processing on feature point clouds and local maps, and obtains sensitive point cloud clusters and common point cloud clusters in the feature point clouds and local maps.

[0069] Specifically, this application introduces the reflection intensity information of the waveform of 4D millimeter-wave radar point cloud on the basis of the traditional point cloud clustering method, and uses the nearest neighbor search and threshold for division. Among them, the reflection intensity information reflects the degree of absorption of millimeter waves by the point cloud characterizing the robot's surrounding environment, and for materials with high conductivity such as metals, the reflectivity of millimeter waves is extremely high and close to 1. Compared with the traditional point cloud clustering method, the improved point cloud clustering method has better imaging performance. The division process is: perform nearest neighbor search on each point cloud to obtain multiple point clouds, detect the reflection intensity information of multiple point clouds, and if it exceeds the threshold, mark it as a sensitive point cloud cluster, otherwise it is a normal point cloud cluster.

[0070] In an optional embodiment, taking a feature point cloud as an example, firstly, a feature point cloud set is Traverse, for each feature point cloud , in the feature point cloud collection A nearest neighbor search with a radius of 1m is performed to obtain 25 feature point clouds. If the number of feature point clouds after the search is less than 25, the feature point cloud set is traversed. The next feature point cloud in It is worth noting that there is no limit on the number of search feature point clouds and the radius of the nearest neighbor search, which can be set according to actual conditions.

[0071] In this embodiment, if the feature point cloud When the number of searches reaches 25, the Rcs values ​​(reflection intensity information) of these 25 feature point clouds are detected. If the Rcs values ​​of 16 feature point clouds out of the 25 feature point clouds exceed the threshold T, the point cloud clustering result is marked as a sensitive point cloud cluster. , otherwise the point cloud clustering result is defined as a common point cloud cluster The shape accuracy of the sensitive point cloud cluster is higher, and the surface features and imaging are more obvious. It is worth noting that the threshold T set in this embodiment is 10dBsm. There is no requirement for the threshold here and it can be set according to actual conditions.

[0072] During the traversal process, mark the feature point clouds that have been clustered. , to ensure that the other feature point clouds in the feature point cloud set are ...skip the nearest neighbor search step during the traversal .

[0073] It is worth noting that the method of point cloud clustering for local maps is the same as that for feature point clouds, which is based on two main factors: point cloud density and reflection intensity. In the process of traversing the point cloud of the local map, the number of point clouds searched by the nearest neighbor is 35. That is, if 35 point clouds are searched within a space with a radius of 1m centered on the traversed point cloud, a point cloud cluster is formed and classified based on the reflection intensity information to obtain ordinary point cloud clusters and sensitive point cloud clusters in the local map.

[0074] (2) Upsampling:

[0075] After point cloud clustering, upsampling with different precision is performed according to the different marking results of each point cloud cluster. That is, the point clouds in sensitive point cloud clusters and common point cloud clusters are selected by boundary conditions for linear interpolation to enhance the point cloud data.

[0076] In an optional embodiment, taking the common point cloud cluster and the sensitive point cloud cluster in the feature point cloud as an example, the common point cloud cluster The upsampling process is as follows: traverse the common point cloud cluster Confirm the spatial boundary conditions of the point cloud set. The specific formula is:

[0077] ;

[0078] ;

[0079] According to the above boundary conditions, for ordinary point cloud clusters Perform linear interpolation, the normal point cloud to be added Add to feature point cloud collection In the above example, we get the preprocessed feature point cloud set , whose three-dimensional coordinates satisfy , , , where , , They refer to the common point cloud in the radar coordinate system. The horizontal coordinate, vertical coordinate and vertical coordinate of . And the interpolation step size is set as follows: , , .

[0080] Sensitive point cloud clusters The upsampling process is as follows: traverse the sensitive point cloud clusters , for each sensitive point cloud In the sensitive point cloud cluster Perform the nearest neighbor search for three points and fit the distance between the three points , , The nearest plane , judgment plane Parallelism with the z-axis of the local map coordinate system. The unit normal vector The vector product with the unit vector parallel to the z-axis is less than 0.15, then In the plane, 25 points are randomly sampled, and in the opposite case, 15 points are randomly sampled. Inside and four fitting plane points , , , Under the boundary conditions of the coordinates, the specific formula is:

[0081] ;

[0082] ;

[0083] According to the above boundary conditions, the sensitive point cloud cluster Perform linear interpolation, that is, sensitive point cloud Randomly add to the feature point cloud collection In the above example, we get the preprocessed feature point cloud set , and the three-dimensional coordinates satisfy , , , where , , Respectively refer to sensitive point clouds The horizontal, vertical and vertical coordinates of .

[0084] It is worth noting that the upsampling processing of ordinary point cloud clusters and sensitive point cloud clusters in the local map is similar to that of the feature point cloud, and both use linear interpolation under their boundary conditions to increase the number of point clouds, that is, linear interpolation of the three-dimensional coordinates x, y, and z of the point cloud is performed to generate the corresponding point cloud. In the process of linear interpolation, the coordinate system for upsampling the local map is the local map coordinate system, and the processing method for sensitive point cloud clusters is different from that of the feature point cloud set. Specifically, the upsampling processing method for sensitive point cloud clusters in the feature point cloud is random sampling, while the processing method for sensitive point cloud clusters in the local map is the same as that for ordinary point cloud clusters, both of which are linear interpolation under boundary conditions, where the step size of linear interpolation is: , , .

[0085] On the other hand, the robot's 3D local map point cloud is composed of the distance between the point cloud in the radar key frame set and the robot body after judgment and screening, and the radar key frame set is obtained by the nearest neighbor search of the current radar frame; the obstacle position generated in the robot's 3D local map point cloud is determined by the center position of the point cloud cluster in the local map after preprocessing.

[0086] S4: The preprocessed feature point cloud set and the preprocessed local map are matched with each other based on the variant ICP algorithm to obtain the posture change parameters, which are superimposed with the robot's initial posture angle to form an incremental odometer to obtain a preliminary estimate of the robot's real-time motion state.

[0087] Based on the variant ICP algorithm, the preprocessed feature point cloud collection The three-dimensional point cloud is matched with the preprocessed local map to obtain the preliminary estimated posture change parameters of the robot before and after the current radar frame, which are superimposed on the posture estimation result. In this embodiment, it refers to the initial posture angle, forming an incremental odometer for preliminarily estimating the real-time motion state of the robot.

[0088] The above preprocessing converts the sparse point clouds with irregular shapes in the feature point clouds and local maps into dense point clouds with obvious surface features.

[0089] The preprocessed feature point cloud collection With the preprocessed local map Substitute the following formula:

[0090] ;

[0091] In the formula, is the pose transformation relationship from the feature point cloud preprocessed by the current radar frame to the local map point cloud, and the pose transformation relationship of the point cloud detected by the radar. is the rotation matrix from the preprocessed feature point cloud of the current radar frame to the preprocessed local map point cloud, which means the transformation relationship between the two point cloud sets from the initial position to the matching overlap. is the translation amount from the feature point cloud after preprocessing of the current radar frame to the local map point cloud after preprocessing, that is, it represents the position transformation relationship between the two point cloud sets from the initial position to the matching overlap. is the number of point clouds in the preprocessed local map, is the number of feature point clouds in the preprocessed feature point cloud set, is the i-th feature point cloud in the preprocessed feature point cloud set, is the point cloud in the i-th preprocessed local map.

[0092] According to the minimum value of the above formula, the preprocessed feature point cloud set can be estimated And the local map after preprocessing The matching pose transformation, wherein the variant ICP algorithm in this application adopts the Gauss-Newton iteration method, and optimizes the error after 60 iterations. When the error is less than 0.01, it can be determined that the feature point cloud after preprocessing of the current radar frame is successfully matched with the preprocessed local map.

[0093] After the match is successful, the calculated and As the preliminary estimated posture change parameter of the robot before and after the current radar frame, this preliminary estimated posture change parameter is further superimposed on the posture estimation result of the robot's previous radar frame, forming an incremental odometer for preliminarily estimating the real-time motion state of the robot.

[0094] S5: Correct the preliminarily estimated real-time motion state of the robot to obtain high-precision robot positioning and motion state estimation, and then navigate in a loop until the fire source is reached and water is sprayed to extinguish the fire.

[0095] The point cloud Doppler velocity information provided by the 4D millimeter-wave radar is fused with the IMU inertial sensor velocity information to correct the preliminary estimated real-time motion state of the robot.

[0096] First, traverse all sensitive point cloud clusters , screen out point cloud clusters with more than 125 in number, and then combine them with the speed information obtained by the IMU inertial sensor and the Doppler speed information obtained by the 4D millimeter-wave radar to solve them, find out the point cloud clusters with low speed and obvious surface features whose point cloud relative to the robot radial speed is less than 0.03m / s, and mark them as special point cloud clusters.

[0097] Then, using the Doppler velocity information of the center of the special point cloud cluster, the radial velocity of the robot itself relative to the center of the special point cloud cluster in the current radar frame can be estimated. , where the radial velocity The direction of velocity is , , , The position coordinates of the center of this special point cloud cluster are used, and the timestamp in the IMU data is pre-integrated with the acceleration data matching the current radar frame and superimposed on the previous velocity estimation result to obtain the robot's own velocity in the radar key frame, recorded as , according to the following formula: , the radial velocity of the robot relative to the center of a special point cloud cluster can be calculated , is the relative position vector between the robot and the center of the special point cloud cluster, and the speed error is , thus obtaining the IMU velocity error based on the 4D millimeter-wave radar Doppler velocity, and comparing this error with the radial velocity of the robot relative to the special point cloud cluster corresponding to the original current radar frame Perform weighted average calculation to obtain the fusion relative radial velocity It is worth noting that the robot moves in real time, so the result of the pre-integration calculation of the timestamp in the IMU data and the acceleration data matching the current radar frame will be used as the previous velocity estimation result in the next cycle.

[0098] Secondly, the relative radial velocity is integrated According to the formula , the corrected robot body speed can be obtained by reverse deduction After the speed is integrated, it is used as the IMU integration factor and fused with the radar odometer factor to obtain the robot odometer result with high precision between frames.

[0099] Furthermore, the relative radial velocity After integral calculation in the local map coordinate system, the high-precision relative position of the robot and the center of this special point cloud cluster at the end of the radar key frame can be obtained. This positioning result is fused with the calculation result of the three-dimensional point cloud for point cloud matching, which can reduce the cumulative error between the local map positioning and the preliminary estimation of the robot's real-time motion state in the incremental odometer, thereby achieving high-precision robot positioning and motion state estimation by fusing Doppler velocity information and point cloud matching. Finally, by fusing the four-dimensional information of the 4D millimeter-wave radar and the IMU data, the combined navigation odometer was successfully fitted, and the high-intensity light features obtained by the binocular depth camera can be used as a visual odometer factor for reference comparison results of odometer calculation in non-special environments.

[0100] Finally, loop detection is required between each node of the robot's motion state calculated by the combined navigation odometer to reduce the cumulative error of this incremental odometer solution. After calculating the robot's posture, the upper computer sends a control signal matching the optimal path to the lower computer, which drives the robot's hydraulic transmission mechanism, thereby realizing the function of autonomous navigation and water spraying after reaching the fire source.

[0101] Specific Example 1: Motion estimation of a robot firefighting

[0102] See also Figure 5 , the robot's pose estimation uses the current radar frame as the input of raw data. The main radar odometer factor 62 calculates the translation and rotation changes within the robot radar key frame time by matching the radar key frame to the point cloud of the local map. The IMU integration factor 63 corrects the solution result of the radar odometer factor by weighted average to improve the accuracy of the solution pose. The IMU integration factor 62 and the radar odometer factor 63 constitute the robot's main odometer in the dense smoke and fog environment 71. The odometer is divided into the preliminary state estimation at the front end and the iterative optimization of the pose at the back end. The front end uses the IMU integration factor 62 to pre-integrate and calculate the initial attitude angle. The back end uses the radar odometer to obtain the pose transformation obtained by scanning to the local map point cloud matching and the weighted average of the calculation result of the IMU pre-integration to form an incremental odometer for preliminarily estimating the real-time motion state of the robot. Then, the nearest neighbor search is performed on the current radar frame. The speed information obtained by multiple IMU inertial sensors and the Doppler speed information obtained by the 4D millimeter-wave radar are used to screen out special point cloud clusters J2 with obvious surface features and low speed and near-stationary. Figure 6 shown.

[0103] Subsequently, the robot radial velocity V2 from the robot to the special point cloud cluster J2 is calculated by radar J4, and then the weighted average fusion of the velocity information based on the local map coordinate system is performed with the robot body velocity V1 calculated by IMU J3 integration. The fused velocity can be integrated to obtain the high-precision relative position of the robot body J1 to the special point cloud cluster J2 at the end of the radar key frame. This result can be fused with the result of point cloud matching to reduce the cumulative error between the local map positioning and the preliminary estimation of the real-time motion state of the robot in the incremental odometer, and finally a combined navigation odometer integrating the four-dimensional information of the 4D millimeter-wave radar and the IMU data is fitted. The visual odometer 61 is used as a reference comparison result of the odometer calculation in a non-special environment.

[0104] Finally, the robot motion state node is calculated A loop check 81 is required to reduce the cumulative error of this incremental odometer solution. Figure 3After calculating the robot's posture, the upper computer 11 sends a control signal matching the optimal path to the lower computer 12, and the lower computer 12 drives the hydraulic transmission mechanism 51 of the robot, thereby realizing the function of autonomous navigation and water spraying to extinguish the fire after reaching the fire source.

[0105] Specific embodiment 2: actual measurement experiment

[0106] The experiment was set up as a linear displacement in a 15m long corridor. The noetic version of ROS was used as the communication framework, and the rviz visualization tool was used to display the processing results of the odometer and mapping algorithm. Figure 7 Mileage calculation and mapping effect diagram for the correct path, Figure 8 This is the mileage calculation and mapping effect diagram of the navigation method used in this application, using 4D millimeter wave radar data. Fig. 9 Mileage calculation and mapping effect diagram for traditional SLAM point cloud matching algorithm input for 4D millimeter wave radar data.

[0107] Combination Figure 7-8 It can be seen that the path shape solved by the navigation method of the present application is smooth and ensures a certain degree of continuity and stability. On the other hand, it is consistent with the correct solution path. Figure 7 The shapes are consistent, and the final position of the robot is also similar. Fig. 9 In the example, the stability of the solution result of the SLAM algorithm based on point cloud matching of the traditional laser radar input 4D millimeter wave radar data is extremely poor. Compared with the correct solution path, it can be seen that its mileage drift is serious, and the terminal position of the robot is completely different, with a huge error. It can be seen that the navigation method of this application has stronger stability, smaller error and higher accuracy.

[0108] Moreover, the present application ensures the stability of robot positioning and the accuracy of robot posture estimation during navigation when the original point cloud is sparse and the shape accuracy is poor.

[0109] The following table shows the quantitative analysis results of this application compared with the correct path:

[0110]

[0111] Combining the above table with Figure 8It can be seen that the positioning error of the navigation method of this application with the correct path is at the centimeter level when the original point cloud is sparse and the shape accuracy is poor. This feature can ensure that this application can still combine the characteristics of 4D millimeter-wave radar penetrating tiny particles such as smoke in the fire scene and the navigation method of this application to solve the robot positioning information and cumulative mileage results with higher accuracy, lower error and stronger stability in the front-line fire scene environment filled with media such as thick smoke and water mist, and when the information of the laser radar point cloud data is seriously distorted and it is difficult to perform robot positioning and mileage calculation.

[0112] The second aspect of the embodiment of the present application provides an unmanned fire-fighting robot navigation device based on 4D millimeter wave radar:

[0113] See also Figure 3 , which is a schematic diagram of an unmanned fire-fighting robot navigation device based on 4D millimeter-wave radar provided in one embodiment of the present application, including a control mechanism, an environmental sensing structure, a remote monitoring mechanism, a global positioning system receiver and a hydraulic transmission mechanism.

[0114] The control mechanism controls the environment perception mechanism, remote monitoring mechanism, global positioning system receiver and hydraulic transmission mechanism to obtain the robot's initial posture angle, local map and optimal path, and navigate the robot according to the optimal path; obtain the feature point cloud, pre-process the feature point cloud and local map, obtain the pre-processed feature point cloud set and the pre-processed local map, perform point cloud matching based on the variant ICP algorithm to obtain the posture change parameters, and superimpose them with the robot's initial posture angle to form an incremental odometer, obtain a preliminary estimate of the robot's real-time motion state, perform corrections to obtain high-precision robot positioning and motion state, and loop navigation until reaching the fire source and spraying water to extinguish the fire. Among them, the remote monitoring mechanism and the global positioning system receiver jointly obtain the optimal path.

[0115] Specifically, the control mechanism includes an upper computer 11 and a lower computer 12, and the upper computer 11 is electrically connected to the lower computer 12. The robot is navigated by utilizing the cooperation of the upper computer 11 and the lower computer 12.

[0116] The environment perception structure includes a 4D millimeter wave radar 21, a laser radar 22, an IMU inertial sensor 23, a thermal imaging camera 24, and a binocular depth camera 25. The binocular depth camera 25 and the thermal imaging camera 24 are used to obtain the environment around the robot. The thermal imaging camera 24 is mainly used to obtain the location information of the fire source. The 4D millimeter wave radar 21 and the laser radar 22 are used to obtain the distance and azimuth between the obstacles in the environment and the robot. The IMU inertial sensor 23 is used to obtain the robot's motion acceleration and angular acceleration and other motion parameters in a short period of time.

[0117] The remote monitoring mechanism 31 is a global navigation satellite system receiver, which can receive and display the robot's position information in the remote human-machine interaction system, so that the remote human-machine interaction system can grasp the dynamics of the robot at any time. In addition, it can receive the position information of multiple fire sources at a long distance sent by the remote human-machine interaction system, which can be used as the target point of the robot's beyond-visual-range path planning.

[0118] See also Figure 4 The hydraulic transmission mechanism 51 includes an engine 511, which is connected to a hydraulic pump 512 to drive the hydraulic pump 512 to move. The hydraulic pump 512 is sequentially connected to and drives the track wheel 513, the oil cylinder 514 and the fire extinguishing nozzle 515 to move, wherein the fire extinguishing nozzle 515 is connected to a water tank 516 to spray water to extinguish the fire;

[0119] The 4D millimeter wave radar 21 , the laser radar 22 , the IMU inertial sensor 23 , the thermal imaging camera 24 , the binocular depth camera 25 and the remote monitoring mechanism 31 are all electrically connected to the host computer 11 , and the global positioning system receiver 41 and the hydraulic transmission mechanism 51 are electrically connected to the slave computer 12 .

[0120] Specifically, the communication interface between the lower computer 12 and the IMU inertial sensor 23 and the upper computer 11 is in UART format, the 4D millimeter wave radar 21 and the laser radar 22 communicate with the upper computer 11 through an Ethernet cable, the thermal imaging camera 24 and the binocular depth camera 25 are connected to the upper computer 11 through a USB interface, the remote monitoring mechanism 31 communicates with the upper computer 11 through a wireless network, the communication interface between the global positioning system receiver 41 and the lower computer 12 is in UART format, and the hydraulic transmission mechanism 51 is connected to the lower computer 12 through a CAN bus.

[0121] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.

[0122] The embodiments described above are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, a person skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. Such modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application, and should all be included in the protection scope of the present application.

Claims

1. A navigation method for an unmanned fire-fighting robot based on 4D millimeter-wave radar, characterized in that: The following steps are involved: Obtaining the robot's initial attitude angle, local map, and optimal path, and navigating the robot according to the optimal path; Acquire a feature point cloud, preprocess the feature point cloud and the local map to obtain a preprocessed feature point cloud set and a preprocessed local map, perform point cloud matching based on a variant ICP algorithm to obtain a posture change parameter, superimpose the parameter with the initial posture angle of the robot to form an incremental odometer, obtain a preliminary estimate of the real-time motion state of the robot, perform corrections to obtain high-precision robot positioning and motion state, and repeat this process until reaching the fire source and spraying water to extinguish the fire; The preprocessing includes point cloud clustering and upsampling, wherein the point cloud clustering introduces the reflection intensity information of the 4D millimeter wave radar waveform and divides it by using the nearest neighbor search and threshold; The point cloud Doppler velocity information provided by the 4D millimeter wave radar is fused with the IMU inertial sensor velocity information to correct the accumulated error between the positioning of the local map in the incremental odometer and the real-time motion state of the robot preliminarily estimated; Acquire the feature point cloud by using the nearest neighbor search and setting the distance penalty factor; Using the Doppler velocity information of the center of the special point cloud cluster, the radial velocity of the robot itself relative to the center of the special point cloud cluster in the current radar frame can be estimated. , where the radial velocity The direction of velocity is , , , The position coordinates of the center of this special point cloud cluster are used, and the timestamp in the IMU data is pre-integrated with the acceleration data matching the current radar frame and superimposed on the previous velocity estimation result to obtain the robot's own velocity in the radar key frame, recorded as , according to the following formula: , the radial velocity of the robot relative to the center of a special point cloud cluster can be calculated , is the relative position vector between the robot and the center of the special point cloud cluster, and the speed error is , thus obtaining the IMU velocity error based on the 4D millimeter-wave radar Doppler velocity, and comparing this error with the radial velocity of the robot relative to the special point cloud cluster corresponding to the original current radar frame Perform weighted average calculation to obtain the fusion relative radial velocity ; It is worth noting that the robot moves in real time, so the result of the pre-integration calculation of the timestamp in the IMU data and the acceleration data matching the current radar frame will be used as the previous velocity estimation result in the next cycle; Secondly, the relative radial velocity is integrated According to the formula , the corrected robot body speed can be obtained by reverse deduction After the speed is integrated, it is used as the IMU integration factor and fused with the radar odometer factor to obtain the robot odometer result with high precision between frames. Furthermore, the relative radial velocity After integral calculation in the local map coordinate system, the high-precision relative position of the robot and the center of this special point cloud cluster at the end of the radar key frame can be obtained. This positioning result is fused with the calculation result of the three-dimensional point cloud for point cloud matching, which can reduce the cumulative error between the local map positioning in the incremental odometer and the preliminary estimation of the real-time motion state of the robot, thereby realizing high-precision robot positioning and motion state estimation by integrating Doppler velocity information and point cloud matching.

2. The unmanned fire-fighting robot navigation method based on 4D millimeter-wave radar according to claim 1 is characterized in that: The division is implemented in the following way: a nearest neighbor search is performed on each point cloud to obtain multiple point clouds, and reflection intensity information of the multiple point clouds is detected. If the reflection intensity information exceeds a threshold, it is marked as a sensitive point cloud cluster, otherwise it is a common point cloud cluster.

3. The unmanned fire-fighting robot navigation method based on 4D millimeter-wave radar according to claim 2 is characterized in that: The upsampling refers to selecting point clouds in sensitive point cloud clusters and common point cloud clusters for linear interpolation using boundary conditions to enhance point cloud data; The boundary condition means that the three-dimensional coordinates of the point cloud are not extreme values.

4. The unmanned fire-fighting robot navigation method based on 4D millimeter-wave radar according to claim 3 is characterized in that: The basic formula of point cloud matching based on the variant ICP algorithm is: ； In the formula, is the pose transformation relationship from the preprocessed feature point cloud of the current radar frame to the preprocessed local map point cloud. is the rotation matrix from the preprocessed feature point cloud of the current radar frame to the preprocessed local map point cloud, which means the transformation relationship between the two point cloud sets from the initial position to the matching overlap. is the translation amount from the feature point cloud after preprocessing of the current radar frame to the local map point cloud after preprocessing, that is, it represents the position transformation relationship between the two point cloud sets from the initial position to the matching overlap. is the number of point clouds in the preprocessed local map, is the i-th feature point cloud in the preprocessed feature point cloud set, is the point cloud in the i-th preprocessed local map, and the posture change parameter refers to the and stated .

5. The unmanned fire-fighting robot navigation method based on 4D millimeter-wave radar according to claim 4 is characterized in that: The radar frame and IMU data obtained after the 4D millimeter-wave radar and IMU inertial sensor are jointly calibrated are time-synchronized to obtain high-frequency IMU data, and the posture transformation is calculated by integration to obtain the initial posture angle of the robot.

6. The unmanned fire-fighting robot navigation method based on 4D millimeter-wave radar according to claim 1 is characterized in that: The deep learning network is used to identify the features obtained by the binocular depth camera and the thermal imaging camera to obtain the fire source location information; Marking the fire source location information into the local map, performing beyond-visual-range path planning, and obtaining the optimal path; The implementation method of the beyond-visual-range path planning is as follows: the traveling salesman problem is solved for the relative positions of the robot and multiple fire sources, and the firefighting task strategies corresponding to the multiple fire source targets are constructed in combination with the weights of the multiple fire source target locations to realize the global planning function of multiple firefighting tasks.

7. The unmanned fire-fighting robot navigation method based on 4D millimeter-wave radar according to claim 6 is characterized in that: The point clouds obtained by 4D millimeter wave radar and laser radar scanning are fused to obtain the local map.

8. An unmanned fire-fighting robot navigation device based on 4D millimeter-wave radar, characterized in that: include: Control mechanism, environmental sensing mechanism, remote monitoring mechanism, global positioning system receiver and hydraulic transmission mechanism; The control mechanism controls the environment perception mechanism, the remote monitoring mechanism, the global positioning system receiver and the hydraulic transmission mechanism to obtain the initial posture angle, the local map and the optimal path of the robot, and navigates the robot according to the optimal path; obtains the feature point cloud, pre-processes the feature point cloud and the local map, obtains the pre-processed feature point cloud set and the pre-processed local map, performs point cloud matching based on the variant ICP algorithm to obtain the posture change parameters, superimposes them with the initial posture angle of the robot to form an incremental odometer, obtains a preliminary estimate of the real-time motion state of the robot, performs corrections to obtain high-precision robot positioning and motion state, and repeats this process until the fire source is reached and water is sprayed to extinguish the fire; The preprocessing includes point cloud clustering and upsampling, wherein the point cloud clustering introduces the reflection intensity information of the 4D millimeter wave radar waveform and divides it by using the nearest neighbor search and threshold; The point cloud Doppler velocity information provided by the 4D millimeter wave radar is fused with the IMU inertial sensor velocity information to correct the accumulated error between the positioning of the local map in the incremental odometer and the real-time motion state of the robot preliminarily estimated; Acquire the feature point cloud by using the nearest neighbor search and setting the distance penalty factor; Using the Doppler velocity information of the center of the special point cloud cluster, the radial velocity of the robot itself relative to the center of the special point cloud cluster in the current radar frame can be estimated. , where the radial velocity The direction of velocity is , , , The position coordinates of the center of this special point cloud cluster are used, and the timestamp in the IMU data is pre-integrated with the acceleration data matching the current radar frame and superimposed on the previous velocity estimation result to obtain the robot's own velocity in the radar key frame, recorded as , according to the following formula: , the radial velocity of the robot relative to the center of a special point cloud cluster can be calculated , is the relative position vector between the robot and the center of the special point cloud cluster, and the speed error is , thus obtaining the IMU velocity error based on the 4D millimeter-wave radar Doppler velocity, and comparing this error with the radial velocity of the robot relative to the special point cloud cluster corresponding to the original current radar frame Perform weighted average calculation to obtain the fusion relative radial velocity ; It is worth noting that the robot moves in real time, so the result of the pre-integration calculation of the timestamp in the IMU data and the acceleration data matching the current radar frame will be used as the previous velocity estimation result in the next cycle; Secondly, the relative radial velocity is integrated According to the formula , the corrected robot body speed can be obtained by reverse deduction After the speed is integrated, it is used as the IMU integration factor and fused with the radar odometer factor to obtain the robot odometer result with high precision between frames. Furthermore, the relative radial velocity After integral calculation in the local map coordinate system, the high-precision relative position of the robot and the center of this special point cloud cluster at the end of the radar key frame can be obtained. This positioning result is fused with the calculation result of the three-dimensional point cloud for point cloud matching, which can reduce the cumulative error between the local map positioning in the incremental odometer and the preliminary estimation of the real-time motion state of the robot, thereby realizing high-precision robot positioning and motion state estimation by integrating Doppler velocity information and point cloud matching.

9. The unmanned fire-fighting robot navigation device based on 4D millimeter-wave radar according to claim 8, characterized in that: The remote monitoring mechanism and the global positioning system receiver jointly obtain the optimal path.

Images