Positioning and navigation method and system for underground rescue equipment, and electronic equipment

Abstract

The invention provides an underground rescue equipment positioning and navigation method and system, and electronic equipment, and the method comprises the steps: collecting the surrounding image data of rescue equipment through a depth camera disposed on the rescue equipment, processing the collected data, and carrying out the passable channel judgment according to a preset judgment rule; and formulating an advancing route and an advancing rule based on the judgment result, in the advancing process of the rescue equipment, acquiring distance data between the rescue equipment and the channel in real time by utilizing a laser radar arranged on the rescue equipment, and meanwhile, acquiring inclination angle data of the rescue equipment by utilizing a strapdown inertial navigation module arranged on the rescue equipment. The trafficability of the rescue equipment in the channel is judged in real time according to the distance data, and whether the posture of the rescue equipment is within a safe range or not is judged according to the obtained inclination angle data. When complex terrains such as obstacles are encountered, real-time and efficient channel judgment is achieved, efficient obstacle breaking is conducted when the obstacles cannot avoid, and irregular and even dynamic obstacles cannot be avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and navigation, and in particular to a positioning and navigation method for underground rescue equipment and a system and electronic equipment thereof. Background Art

[0002] Various emergencies often occur around the world. In some special emergencies, due to unsafe factors, rescuers cannot go to the scene to implement rescue in person. In addition, in these accidents, rescuers must rescue the trapped people in a very short time (about 48 hours), otherwise the trapped people will hardly survive. For example, after a coal mine accident, due to the leakage of toxic gases and the risk of gas explosion, rescuers cannot enter the danger zone, and after 48 hours, the mortality rate of trapped people will increase sharply due to lack of air, lack of food, water and medical treatment. Normally mined coal mines are filled with various mechanical equipment, construction materials, etc. required for mining. When a gas explosion, roof collapse or other disaster accidents occur in the mine, the shock wave will cause great damage to the mine environment, causing the top support to collapse, coal blocks and construction materials to scatter, and mechanical equipment to overturn. These will form many obstacles of various shapes. In such an urgent and dangerous situation, the use of underground rescue equipment, such as rescue robots, can improve the rescue efficiency, reduce or avoid casualties of rescuers, and bring great help to the rescue work.

[0003] In the underground coal mine rescue work, the existing underground coal mine rescue robots have certain defects in practical application. First, some search and rescue robots need to be remotely controlled and coordinated by people to work. Due to the complex environment and geographical factors underground, there will be delays in data transmission, and the real-time information obtained by the operator is limited, and it is impossible to respond to emergencies in time; second, the existing search and rescue robots cannot provide accurate search and rescue information for the personnel on the well, resulting in the rescue personnel being unable to formulate efficient rescue plans, and it is difficult to meet the construction needs of smart mines. There are large-scale, multi-branched, and even particularly narrow and long geometric terrain structures in the underground coal mine environment. Its tunnels are narrow and deep, the distance is extremely long, the direction is rugged, and the location is complex. Because the ground is excavated along the mountain range, steep steps, muddy ground, and puddles of various sizes can be seen everywhere. Therefore, it is difficult for rescue equipment to carry out rescue work in the mine environment. Mine rescue equipment needs to combine environmental information, be able to avoid irregular or even dynamic obstacles, and successfully complete rescue tasks in the narrow and long mine tunnel structure with a relatively smooth movement path. Existing underground rescue equipment robots lack a simple, efficient, and highly intelligent underground positioning and navigation solution. When encountering complex terrain such as obstacles, they are unable to perform real-time and efficient channel judgment, and they are unable to efficiently break through obstacles when obstacles cannot be avoided. They are unable to avoid irregular or even dynamic obstacles.

[0004] Therefore, the prior art needs to be further developed. Summary of the invention

[0005] The purpose of the present invention is to overcome the above technical deficiencies and provide a method and system for positioning and navigating underground rescue equipment, as well as electronic equipment, so as to solve the problems existing in the prior art.

[0006] To achieve the above technical objectives, according to a first aspect of the present invention, the present invention provides a method for positioning and navigating underground rescue equipment, comprising: After entering the well, the rescue equipment moves at a first preset speed, and uses the depth camera provided on the rescue equipment to collect image data of the surroundings of the rescue equipment, and processes the collected data. The processed depth camera data is used to judge the passable channel according to preset judgment rules, and the travel route and travel rules are formulated based on the judgment results. When the rescue equipment moves according to the formulated travel route and travel rules, the laser radar provided on the rescue equipment is used to collect the distance data between the rescue equipment and the channel in real time. At the same time, the strapdown inertial navigation module provided on the rescue equipment is used to obtain the inclination angle data of the rescue equipment. The passability of the rescue equipment in the channel is judged in real time through the distance data, and whether the posture of the rescue equipment is within a safe range is judged through the obtained inclination angle data.

[0007] Specifically, the processing of the collected data includes: First, we obtain the set of depth arrays {Ad1, Ad2, …, Adn} and the corresponding angles and distances {d1, d2, …, dn} in the depth image data, where 125° of the laser radar corresponds to 0° of the depth camera's viewing angle. We count the points in the image within the range of d1±dσ and store the coordinates in array Ad1. We obtain the maximum and minimum values ​​of the horizontal coordinate and the maximum and minimum values ​​of the vertical coordinate in array Ad1. We establish a target search box M1 based on the maximum and minimum values ​​of the horizontal coordinate and the maximum and minimum values ​​of the vertical coordinate in Ad1. We count the points in the image within the range of d2±dσ and store the coordinates in array Ad2, …, and so on. The depth image is divided into a series of target search boxes Mn.

[0008] Specifically, the processing of the depth camera data to determine the passable channel according to the preset determination rules, and formulating the travel route and travel rules based on the determination results include: Calculate the distance difference of adjacent target search boxes respectively, and determine whether the maximum distance difference is greater than or equal to the first preset threshold value. If so, determine whether the width of the target search box with a larger distance between the two target search boxes that produce the maximum distance difference is greater than or equal to the second preset threshold value, and at the same time determine whether the height of the target search box is greater than or equal to the third preset threshold value. If so, the area indicated by the target search box is a passable channel, then calculate the deviation angle between the center of the target search box and the driving direction of the rescue equipment, and calculate the distance that the rescue equipment needs to move left or right based on the distance and the deviation angle. The rescue equipment moves the distance left or right so that the forward direction of the rescue equipment and the center of the target search box are located in the same vertical plane, and then the rescue equipment enters the passable channel along the intersection of the vertical plane and the horizon.

[0009] Specifically, judging the passability of the rescue equipment in the passage in real time through the distance data includes: The 0 degree of the laser radar always points in the opposite direction of the rescue equipment. Through the values ​​of 90 degrees and 270 degrees of the laser radar, its own position DL relative to the left wall and the position DR relative to the right wall are obtained. When the area of ​​the target search box is less than or equal to the fourth preset threshold and greater than or equal to the fifth preset threshold, the rescue equipment enters the channel and moves at the second preset speed, moves at the second preset speed, and determines whether the changes in DL and DR are greater than or equal to the sixth preset threshold. If both are greater than the sixth preset threshold, the rescue equipment moves at the first preset speed.

[0010] Specifically, judging whether the posture of the rescue equipment is within a safe range by obtaining the tilt angle data includes: If the inclination angle value of the rescue equipment is greater than or equal to the preset safety angle value, the rescue equipment improves its forward posture and moves forward at a second preset speed until the inclination angle value of the rescue equipment is less than the preset safety angle, and determines whether it has reached the rescue target point. If it has reached the rescue target point, navigation ends.

[0011] Specifically, the method further includes: If the width of the target search box with the larger distance between the two target search boxes that produce the maximum distance difference is smaller than the width of the rescue equipment or the height of the target search box is smaller than the height of the rescue equipment, the deviation angle between the center of the target search box and the driving direction of the rescue equipment is calculated to obtain the distance that the rescue equipment needs to move left or right. The rescue equipment moves left or right by this distance so that the forward direction of the rescue equipment and the center of the target search box are in the same vertical plane. Then, the decision module sends out excavation and crushing information, and the rescue equipment performs excavation and crushing actions and moves along the intersection of the vertical plane and the ground plane.

[0012] Specifically, the method also includes: collecting a current frame of infrared thermal imaging image in the forward direction of the rescue equipment according to an infrared thermal imager arranged on the rescue equipment and parsing to obtain the current frame of infrared thermal imaging data; if there is a pixel point whose grayscale value is greater than or equal to the seventh preset threshold in the current frame of infrared thermal imaging data, the pixel point is determined as a target point; a target range is determined according to the target point, and the target range needs to include all target points in the current frame of infrared thermal imaging image data; a target search box is established according to the target range; the rescue equipment approaches the target indicated by the target search box and measures the real-time distance between the rescue equipment and the target in real time; when the distance between the rescue equipment and the target is less than or equal to the eighth preset distance, it is determined whether the area of ​​the target search box is greater than or equal to the ninth preset threshold and whether the total grayscale value of each target point is greater than or equal to the ninth preset threshold; if the area of ​​the target search box is greater than or equal to the ninth preset threshold or the total grayscale value of each target point is greater than or equal to the tenth preset threshold, it is determined that there is a target image defined as a survivor in the current frame of infrared thermal imaging image, and the decision unit sends a survival signal to the rescue personnel.

[0013] Specifically, the target search box needs to include the target range and the background of a preset range around the target, calculate the target's mass points and establish a search box with the target's boundary, and expand the search box to a preset multiple of the search box established with the target's boundary based on the target's center of mass.

[0014] According to a second aspect of the present invention, there is provided a positioning and navigation system for underground rescue equipment, comprising: The acquisition unit is used to move at a first preset speed after the rescue equipment enters the well, and use a depth camera arranged on the rescue equipment to collect surrounding image data of the rescue equipment, or use a laser radar arranged on the rescue equipment to collect real-time distance data between the rescue equipment and the channel, or use a strapdown inertial navigation module arranged on the rescue equipment to obtain the inclination angle data of the rescue equipment.

[0015] The processing unit is used to process the data collected by the collection unit, use the processed depth camera data to judge the passable channel according to the preset judgment rules, formulate the travel route and travel rules based on the judgment results, judge the passability of the rescue equipment in the channel in real time through the distance data, and judge whether the posture of the rescue equipment is within the safe range through the obtained inclination angle data.

[0016] The execution unit is used to control the rescue equipment to move according to the route and travel rules formulated by the processing unit.

[0017] According to a third aspect of the present invention, there is provided an electronic device, comprising: a memory; and a processor, wherein the memory stores computer-readable instructions, and when the computer-readable instructions are executed by the processor, the method for positioning and navigating underground rescue equipment according to any one of claims 1 to 8 is implemented.

[0018] Beneficial effects: 1. The present invention is innovative in collecting the surrounding environment data of the rescue equipment by collecting the laser radar and depth camera installed on the rescue equipment, processing the collected data, using the processed depth camera data to judge the passable channel according to the preset judgment rules, screening out the passable channel, and selecting the corresponding speed to enter according to the width data of the passable channel, by first obtaining the set of depth arrays {Ad1, Ad2, ..., Adn} and corresponding angles and distances {d1, d2, ..., dn} in the depth image data, wherein 125° of the laser radar corresponds to 0° of the depth camera viewing angle, counting the points in the image within the range of d1±dσ, storing the coordinates in the array Ad1, and obtaining the maximum and minimum values ​​of the horizontal coordinates and the maximum and minimum values ​​of the vertical coordinates in the array Ad1, and establishing the target search box M1 according to the maximum and minimum values ​​of the horizontal coordinates and the maximum and minimum values ​​of the vertical coordinates in Ad1, The points in the image whose distance is within the range of d2±dσ are counted, and the coordinates are stored in the array Ad2, ..., and so on, so that the depth image is divided into a series of target search frames Mn. The distance differences of adjacent target search frames are calculated respectively, and it is determined whether the maximum distance difference is greater than or equal to the first preset threshold. If so, it is determined whether the width of the target search frame with a larger distance between the two target search frames that produce the maximum distance difference is greater than or equal to the second preset threshold, and at the same time, it is determined whether the height of the target search frame is greater than or equal to the third preset threshold. If so, the area indicated by the target search frame is a passable channel, then the deviation angle between the center of the target search frame and the driving direction of the rescue equipment is calculated, and the distance that the rescue equipment needs to move left or right is calculated based on the distance and the deviation angle. After the rescue equipment moves left or right by this distance, the forward direction of the rescue equipment and the center of the target search frame are located in the same vertical plane, and then the rescue equipment enters the passable channel. Through clever design, the extremely complex channel judgment problem is simplified without the need for complex algorithm modeling, which greatly improves the intelligence level of the present invention and realizes simple, efficient and highly intelligent positioning and navigation of underground rescue equipment. When encountering complex terrain such as obstacles, real-time and efficient channel judgment can be performed, and irregular and even dynamic obstacles can be avoided, which greatly expands the application scenarios of the present invention.

[0019] 2. The present invention uses a laser radar to determine whether the passable passage is successfully passed. If it is successfully passed, the rescue equipment continues to move at a first preset speed; during the movement of the rescue equipment underground, the strapdown inertial navigation module provided on the rescue equipment is used to obtain the inclination angle data of the rescue equipment and determine whether it is greater than or equal to the preset safety angle value. If not, the rescue equipment moves at a second preset speed. The present invention always points the 0 degree of the laser radar to the opposite direction of the rescue equipment. Through the values ​​of 90 degrees and 270 degrees of the laser radar, when the rescue equipment enters the passage, its position DL relative to the left wall and its position DR relative to the right wall are obtained. When the area of ​​the target search box is less than or equal to the fourth preset threshold and greater than or equal to the fifth preset threshold, the rescue equipment moves at the second preset speed, and determines whether the changes in DL and DR are both greater than or equal to the sixth preset threshold. If both are greater than the sixth preset threshold, the rescue equipment moves at the first preset speed. By setting the fourth preset threshold and the fifth preset threshold, the automatic judgment of whether the rescue equipment has passed the channel in the channel is cleverly achieved, without the need for complex algorithm modeling, and the underground rescue equipment can judge whether to slow down in the channel in real time according to the road conditions ahead. By setting the sixth preset threshold, the rescue equipment can cleverly judge whether to pass the channel autonomously. If it passes the channel, the speed of the rescue equipment is restored to the original first preset speed, further simplifying the complex problem and further improving the intelligence and usability of the present invention.

[0020] 3. In the present invention, when the inclination angle value of the rescue equipment is greater than or equal to the preset safety angle value, the rescue equipment improves its forward posture and moves forward at a second preset speed until the inclination angle value of the rescue equipment is less than the preset safety angle, and determines whether it has reached the rescue target point. This enables the rescue equipment to autonomously determine whether the inclination angle exceeds the preset safety angle and slow down when passing through a slope. When the inclination angle of the rescue equipment is greater than or equal to the preset safety angle, the forward posture is automatically adjusted, such as by lowering the height of the front end of the robot and raising the height of the rear end of the robot through a mechanical lifting device provided on the robot to prevent the robot from tipping over. This further improves the safety of the present invention and improves the rescue reliability and availability.

[0021] 4. In the technical solution of the present invention, if the width of the target search box with the larger distance between the two target search boxes that produce the maximum distance difference is smaller than the width of the rescue equipment or the height of the target search box is smaller than the height of the rescue equipment, the deflection angle between the center of the target search box and the driving direction of the rescue equipment is calculated to obtain the distance that the rescue equipment needs to move left or right. After the rescue equipment moves the distance left or right so that the forward direction of the rescue equipment and the center of the target search box are located in the same vertical plane, the decision module sends out excavation and crushing information, and the rescue equipment performs the excavation and crushing action and moves forward. When it is determined that the size of the target channel does not meet the size of the rescue equipment and cannot pass through, the rescue equipment is still controlled to move toward the channel along the intersection of the vertical plane of the center of the channel and the ground plane and perform the crushing and excavation action, and the obstacle crushing and excavation is performed from the weakest or least part of the obstacle, which greatly saves the rescue time. The complex problem of being unable to perform efficient channel judgment when encountering complex terrain such as obstacles and performing efficient obstacle removal when obstacles cannot be avoided is further simplified through a simple and efficient algorithm, which greatly improves the rescue effect and intelligence, and further improves the usability of the present invention.

[0022] 5. The present invention collects the infrared thermal imaging image of the current frame of the rescue equipment in the forward direction according to the infrared thermal imager arranged on the rescue equipment and analyzes it to obtain the infrared thermal imaging data of the current frame. If there is a pixel point with a gray value greater than or equal to the seventh preset threshold value in the infrared thermal imaging data of the current frame, the pixel point is determined as the target point, and the target range is determined according to the target point. The target range needs to include all target points in the infrared thermal imaging image data of the current frame. A target search box is established according to the target range. The rescue equipment approaches the target indicated by the target search box and measures the real-time distance between the rescue equipment and the target in real time. When the distance between the rescue equipment and the target is less than or equal to the eighth preset distance, it is determined whether the area of ​​the target search box is greater than or equal to the ninth preset distance. The threshold is determined and whether the total gray value of each target point is greater than or equal to the ninth preset threshold. If the area of ​​the target search box is greater than or equal to the ninth preset threshold or the total gray value of each target point is greater than or equal to the tenth preset threshold, it is determined that there is a target image defined as a survivor in the current frame of infrared thermal imaging image, and the decision unit sends a survival signal to the rescue personnel. The extremely complex problem of determining whether there are survivors is simplified through clever design, without the need for complex algorithm modeling, which greatly improves the usability and reliability of the analysis results of the present invention. While ensuring the reliability of the analysis results, the system architecture is greatly simplified, the implementation difficulty of the present invention is greatly reduced, the rescue effect of the present invention is further improved, and the application scenarios of the present invention are further expanded. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 is a flow chart of a method for positioning and navigating underground rescue equipment provided in a specific embodiment of the present invention; Figure 2 is a schematic diagram of a method for positioning and navigating underground rescue equipment provided in a specific embodiment of the present invention; Figure 3 It is a principle block diagram of the underground rescue equipment positioning and navigation system provided in a specific embodiment of the present invention. DETAILED DESCRIPTION

[0024] In order to enable those skilled in the art to better understand the technical solution of the present invention, the technical solution of the present invention is clearly and completely described below in conjunction with the accompanying drawings of the present invention. Based on the embodiments in this application, other similar embodiments obtained by ordinary technicians in this field without making creative work should all fall within the scope of protection of this application. In addition, the directional words mentioned in the following embodiments, such as "up", "down", "left", "right", etc., are only reference to the directions of the accompanying drawings. Therefore, the directional words used are used to illustrate rather than limit the invention.

[0025] The present invention will be further described below in conjunction with the accompanying drawings and preferred embodiments.

[0026] See also Figure 1 The present invention provides a method for positioning and navigating underground rescue equipment, comprising: S100: After entering the well, the rescue equipment moves at a first preset speed, and uses a depth camera provided on the rescue equipment to collect surrounding image data of the rescue equipment.

[0027] It should be noted that, in the technical solution of the present invention, firstly, an initial map of the tunnel is established with the help of the underground tunnel drawing, a virtual prototype of the rescue equipment and a virtual simulation platform are established, and after the switch receives the data sent by the relay machine, it forwards it to the host computer, and the host computer solves the data to obtain the operation status of the rescue equipment and the tunnel environment information, and sets the initial starting point and rescue target point of the rescue equipment; During the rescue process, the rescue equipment uses the environment perception module and strapdown inertial navigation module to perceive the environment and equipment status of the rescue equipment, obtain the current location information, attitude information, navigation information, speed information, and obtain the coordinates of the current location. During the process of moving, a coordinate point is recorded every short period of time, and the connection is the own movement trajectory, which is matched with the initial map to realize the positioning of the rescue equipment. The initial map model and target point are corrected based on the relevant information.

[0028] The controller module pre-processes obstacles in the map through the environment perception module, and the controller independently establishes the latest lane environment model and visualization map. The execution module implements the global path planning strategy through the fusion algorithm to obtain the optimal path from the current position to the rescue target point; The rescue equipment moves along the optimal path and detects the surrounding environment information of the path in real time through infrared sensors, binocular vision systems and lidar. When an unknown obstacle is sensed, the environmental perception module is activated to identify the type, shape and size of the obstacle, and autonomously determine whether the rescue equipment is blocked by the obstacle and whether it can avoid it. If it is blocked, it will enter local path planning, read the local map information provided by the environmental perception module in real time, and use the fusion algorithm to implement local dynamic path planning for the rescue equipment, planning a local collision-free path to avoid new obstacles and moving obstacles; if it is blocked and cannot be avoided, the controller will send instructions, and the rescue equipment will perform excavation and crushing actions to break the obstacles and pass; if it is not blocked, it will also enter local path planning, and the intelligent algorithm will detect collisions and move towards the target point avoiding obstacles.

[0029] The equipment position information provided by the local path planning thread is read in real time, and the equipment is controlled to follow the local path. After avoiding or passing obstacles, the equipment is merged into the global optimal path until it reaches the rescue target point to search and rescue the trapped persons.

[0030] The technical solution of the present invention can use the depth camera arranged on the rescue equipment to collect the surrounding image data of the rescue equipment, use the laser radar arranged on the rescue equipment to collect the distance data between the rescue equipment and the channel in real time, and use the strapdown inertial navigation module arranged on the rescue equipment to obtain the inclination angle data of the rescue equipment and send it to the processing unit.

[0031] Before step S100, it includes presetting a first preset speed, a second preset speed, a first preset threshold, a second preset threshold, a third preset threshold, a fourth preset threshold, a fifth preset threshold, a sixth preset threshold, a seventh preset threshold, an eighth preset threshold, a ninth preset threshold, a tenth preset threshold and a preset safety angle, wherein the first preset speed is greater than the second preset speed, so as to realize hierarchical control of the moving speed of the rescue device, the second preset threshold is greater than the width of the rescue equipment, the third preset threshold is greater than the height of the rescue equipment, so as to ensure that the rescue equipment can pass through the passable passage, the fifth preset threshold is less than the fourth preset threshold, the fourth preset threshold is 1.5 times the product of the second preset threshold and the third preset threshold, and the fifth preset threshold is the product of the second preset threshold and the third preset threshold, so as to realize that when the area of ​​the passable passage is small enough for the rescue equipment to pass through, the moving speed of the rescue equipment is reduced to prevent the collapse of the passable passage caused by the rescue equipment moving too fast, thereby greatly improving the application scenario and reliability of the present invention, preferably, the first preset threshold is 1 meter. Setting the first preset threshold to 1 meter is the result of a large number of experiments conducted by the technicians of the present invention. Since the difference between the distance from the end of the passable channel to the rescue equipment and the distance from the obstacle to the rescue equipment is usually greater than 1 meter, setting the first preset threshold to 1 meter can be a good analysis of whether a passable channel is found. The extremely complex channel judgment problem is simplified through clever design without the need for complex algorithm modeling, which greatly improves the intelligence level of the present invention, realizes simple, efficient and highly intelligent positioning and navigation of underground rescue equipment, and can perform real-time and efficient channel judgment when encountering complex terrain such as obstacles, thereby avoiding irregular and even dynamic obstacles, and greatly expanding the application scenarios of the present invention.

[0032] S200, processing the collected data, using the processed depth camera data to determine the passable channel according to preset determination rules, and formulating the travel route and travel rules based on the determination results.

[0033] It should be noted that the processing of the collected data includes: First, we obtain the set of depth arrays {Ad1, Ad2, …, Adn} and the corresponding angles and distances {d1, d2, …, dn} in the depth image data, where 125° of the laser radar corresponds to 0° of the depth camera's viewing angle. We count the points in the image within the range of d1±dσ and store the coordinates in array Ad1. We obtain the maximum and minimum values ​​of the horizontal coordinate and the maximum and minimum values ​​of the vertical coordinate in array Ad1. We establish a target search box M1 based on the maximum and minimum values ​​of the horizontal coordinate and the maximum and minimum values ​​of the vertical coordinate in Ad1. We count the points in the image within the range of d2±dσ and store the coordinates in array Ad2, …, and so on. The depth image is divided into a series of target search boxes Mn.

[0034] The method of using the processed depth camera data to determine the passable channel according to the preset determination rules, and formulating the travel route and travel rules based on the determination results includes: Calculate the distance difference of adjacent target search boxes respectively, and determine whether the maximum distance difference is greater than or equal to the first preset threshold. If so, determine whether the width of the target search box with a larger distance between the two target search boxes that produce the maximum distance difference is greater than or equal to the second preset threshold, and at the same time determine whether the height of the target search box is greater than or equal to the third preset threshold. If so, the area indicated by the target search box is a passable channel, then calculate the deviation angle between the center of the target search box and the driving direction of the rescue equipment, and calculate the distance that the rescue equipment needs to move left or right based on the distance and the deviation angle. The rescue equipment moves the distance left or right so that the forward direction of the rescue equipment and the center of the target search box are in the same vertical plane, and then the rescue equipment enters the passable channel.

[0035] Specifically, if the width of the target search box with a larger distance between the two target search boxes that produce the maximum distance difference is smaller than the width of the rescue equipment or the height of the target search box is smaller than the height of the rescue equipment, the deviation angle between the center of the target search box and the travel direction of the rescue equipment is calculated to obtain the distance that the rescue equipment needs to move left or right. After the rescue equipment moves this distance left or right so that the forward direction of the rescue equipment and the center of the target search box are in the same vertical plane, the decision module sends out excavation and crushing information, and the rescue equipment performs the excavation and crushing action and moves forward.

[0036] It can be understood that when it is determined that the size of the target channel does not meet the size of the rescue equipment and cannot pass through, the rescue equipment is still controlled to move toward the channel along the intersection of the vertical plane of the center of the channel and the ground plane and perform crushing and excavation actions, and the obstacle is crushed and excavated from the weakest or least part of the obstacle, which greatly saves rescue time. Through simple and efficient algorithms, the complex problems of being unable to perform efficient channel judgment when encountering complex terrains such as obstacles and performing efficient obstacle removal when obstacles cannot be avoided are further simplified, which greatly improves the rescue effect and intelligence level, and further improves the usability of the present invention.

[0037] S300, when the rescue equipment moves according to the established route and travel rules, the laser radar provided on the rescue equipment is used to collect the distance data between the rescue equipment and the channel in real time, and the strapdown inertial navigation module provided on the rescue equipment is used to obtain the tilt angle data of the rescue equipment.

[0038] S400: judging the passability of the rescue equipment in the passage in real time through the distance data, and judging whether the posture of the rescue equipment is within a safe range through the obtained tilt angle data.

[0039] It should be noted here that the 0 degree of the laser radar always points to the opposite direction of the rescue equipment. Through the values ​​of 90 degrees and 270 degrees of the laser radar, when the rescue equipment enters the channel, it obtains its own position DL relative to the left wall and its position DR relative to the right wall. When the area of ​​the target search box is less than or equal to the fourth preset threshold and greater than or equal to the fifth preset threshold, the rescue equipment moves at the second preset speed, and determines whether the changes in DL and DR are both greater than or equal to the sixth preset threshold. If both are greater than the sixth preset threshold, the rescue equipment moves at the first preset speed. It can be understood here that when the changes in DL and DR are greater than or equal to the sixth preset threshold, it proves that DL and DR are large enough. The rescue equipment restores the first preset speed at this time, which will not cause the collapse of the passable channel caused by the excessive movement speed, and it can also be judged that the passable channel has been passed. The rescue equipment can cleverly determine whether to pass through the channel autonomously. If it passes through the channel, the speed of the rescue equipment is restored to the original first preset speed, which further simplifies the complex problem and further improves the intelligence and usability of the present invention.

[0040] Specifically, judging whether the posture of the rescue equipment is within a safe range through the obtained tilt angle data includes: if the tilt angle value of the rescue equipment is greater than or equal to the preset safety angle value, the rescue equipment improves the forward posture and moves forward at a second preset speed until the tilt angle value of the rescue equipment is less than the preset safety angle, and judging whether the rescue target point has been reached. If the rescue target point has been reached, the navigation ends.

[0041] It can be understood that this setting realizes the autonomous judgment of whether the inclination angle exceeds the preset safety angle and slows down when the rescue equipment passes through a slope. When the inclination angle of the rescue equipment is greater than or equal to the preset safety angle, the forward posture is automatically adjusted, such as by lowering the height of the front end of the robot and raising the height of the rear end of the robot through a mechanical lifting device provided on the robot to prevent the robot from tipping over, further improving the safety of the present invention and improving the rescue reliability and availability.

[0042] Specifically, during the movement of the rescue equipment, the infrared thermal imager installed on the rescue equipment collects the infrared thermal imaging image of the current frame in the forward direction of the rescue equipment and parses the infrared thermal imaging data of the current frame. If there is a pixel point with a grayscale value greater than or equal to the seventh preset threshold value in the infrared thermal imaging data of the current frame, the pixel point is determined as the target point, and the target range is determined according to the target point. The target range needs to include all target points in the infrared thermal imaging image data of the current frame. A target search box is established according to the target range. The rescue equipment approaches the target indicated by the target search box and measures the real-time distance between the rescue equipment and the target in real time. When the distance between the rescue equipment and the target is less than or equal to the eighth preset distance, the surface of the target search box is determined. Whether the product is greater than or equal to the ninth preset threshold and whether the total grayscale value of each target point is greater than or equal to the ninth preset threshold; if the area of ​​the target search box is greater than or equal to the ninth preset threshold or the total grayscale value of each target point is greater than or equal to the tenth preset threshold, it is determined that there is a target image defined as a survivor in the current frame of infrared thermal imaging image, and the decision unit sends a survival signal to the rescue personnel. It can be understood here that when the rescue equipment is a certain distance away from the survivor, according to the roughly consistent human physiological structure, the area of ​​the target search box should be within a limited range, and the total grayscale value of each target point should be greater than a certain range. Since the situation where the infrared target is non-human is excluded, the reliability of the rescue analysis results and the rescue effect are further improved.

[0043] Specifically, the target search frame needs to include the target range and the background of the preset range around the target, calculate the mass points of the target and establish the search frame with the boundary of the target, and expand the search frame to a preset multiple of the search frame established with the boundary of the target with the center of mass of the target as the base point. This is used to well separate the target from the surrounding environment and make the target search frame well include the target, prevent inaccurate analysis results caused by incomplete extraction of the target due to environmental factors such as obstacles, and improve the reliability of the analysis results.

[0044] It can be understood that the above-mentioned setting simplifies the extremely complex problem of determining whether there are survivors through clever design, without the need for complex algorithm modeling, which greatly improves the usability of the present invention and the reliability of the analysis results. While ensuring the reliability of the analysis results, it greatly simplifies the system architecture, greatly reduces the difficulty of implementing the present invention, further improves the rescue effect of the present invention, and further expands the application scenarios of the present invention.

[0045] Specifically, the calculation of the target centroid coordinates includes: According to the mean and standard deviation of the grayscale values ​​of the original image, a piecewise linear change is made to the grayscale value of the original image to obtain the grayscale value of each point of the 8-bit single-channel image of the current frame image, and the target center of mass coordinates are calculated according to the grayscale value of each point of the 8-bit single-channel image of the current frame image.

[0046] The formula for the piecewise linear change is: Wherein, μ and σ are the mean and standard deviation of the grayscale values ​​of the original image, respectively; x is the grayscale value of each point in the original image; y is the grayscale value of each point in the 8-bit single-channel image obtained after linear transformation; and y is a rounding operation.

[0047] The centroid positioning calculation formula is: Wherein, M and N represent the width and height of the target search box, (Mmin, Nmin), and (Xmax, Ymax) represent the coordinates of the upper left corner and lower right corner of the target search box, Iij represents the gray value of the pixel point in the rectangular area, i represents the row position of the pixel point, j represents the column position of the pixel point, and (Xc, Yc) represents the centroid coordinates. The area of ​​the target search box is the product of the height and width of the target search box.

[0048] Specifically, the system composition of the robot of the present invention is as follows: the present invention installs the Ubuntu system on the Raspberry Pi embedded system, and then transplants the ROS robot operating system (Robot Operating System) to independently develop a universal platform capable of technology integration and application. The SLAM algorithm is transplanted into ROS to realize functions such as autonomous obstacle avoidance navigation, environmental information collection, and unknown environment map construction. At the same time, the speed of the robot motor is controlled, and the data information measured by the sensor is processed to realize the actual control of the robot, and finally a feature map is constructed.

[0049] To realize the autonomous motion control of the robot, it is necessary to obtain and track its current position and posture, and determine the angle of the robot's pre-travel route. The sensor is used to collect the robot's current position, posture, and movement information in real time to obtain the position and posture. The position deviation of the robot from the predefined planned path is calculated based on this. This work uses a pure path tracking algorithm to track the robot's pre-travel route. The Beidou positioning system is used to locate the robot's path. Based on RTK, centimeter-level positioning accuracy can be obtained, which can meet the needs of the robot's automatic navigation. The lateral deviation of the robot can be known through the distance formula between a point and a straight line, and the current heading of the robot can be measured using the Beidou receiver.

[0050] The robot of the present invention has an environment perception system, which mainly includes an omnidirectional visual sensor, inertial navigation, and odometer (photoelectric encoder) independently developed and designed. The omnidirectional visual sensing technology, designed like a "fly's eye", can obtain 360-degree environmental images of different depths of the outside world, and splice them to establish a three-dimensional scene. At the same time, the pitch angle of the camera can be adjusted to establish a three-dimensional scene at different height levels. The inertial navigation module integrates a high-precision gyroscope, accelerometer, and geomagnetic field sensor, and adopts a high-performance microprocessor and advanced dynamic solution and Kalman dynamic filtering algorithm to quickly solve the current real-time motion posture of the module. The use of advanced digital filtering technology can effectively reduce measurement noise and improve measurement accuracy. The odometer (photoelectric encoder) can measure the speed of the motor, so that the robot's motion state and position can be measured in real time and accurately, providing necessary data for the robot's positioning and path tracking.

[0051] See also Figure 2 The present invention proposes a specific embodiment, which provides a low-voltage power distribution system intelligent supervision method based on an intelligent gateway, the method comprising: P0, Start P1. After entering the well, the rescue equipment moves at a first preset speed, and uses a depth camera provided on the rescue equipment to collect surrounding image data of the rescue equipment.

[0052] The technical solution of the present invention can use a depth camera arranged on the rescue equipment to collect surrounding image data of the rescue equipment, or use a laser radar arranged on the rescue equipment to collect distance data between the rescue equipment and the channel in real time, or use a strapdown inertial navigation module arranged on the rescue equipment to obtain the inclination angle data of the rescue equipment and send it to the processing unit.

[0053] Before step P1, the process includes presetting a first preset speed, a second preset speed, a first preset threshold, a second preset threshold, a third preset threshold, a fourth preset threshold, a fifth preset threshold, a sixth preset threshold, a seventh preset threshold, an eighth preset threshold, a ninth preset threshold, a tenth preset threshold and a preset safety angle, wherein the first preset speed is greater than the second preset speed, so as to realize hierarchical control of the moving speed of the rescue device, the second preset threshold is greater than the width of the rescue equipment, the third preset threshold is greater than the height of the rescue equipment, so as to ensure that the rescue equipment can pass through the passable passage, the fifth preset threshold is less than the fourth preset threshold, the fourth preset threshold is 1.5 times the product of the second preset threshold and the third preset threshold, and the fifth preset threshold is the product of the second preset threshold and the third preset threshold, so as to realize that when the area of ​​the passable passage is small enough for the rescue equipment to pass through, the moving speed of the rescue equipment is reduced to prevent the collapse of the passable passage caused by the rescue equipment moving too fast, thereby greatly improving the application scenario and reliability of the present invention, preferably, the first preset threshold is 1 meter. Setting the first preset threshold to 1 meter is the result of a large number of experiments conducted by the technicians of the present invention. Since the difference between the distance from the end of the passable channel to the rescue equipment and the distance from the obstacle to the rescue equipment is usually greater than 1 meter, setting the first preset threshold to 1 meter can be a good analysis of whether a passable channel is found. The extremely complex channel judgment problem is simplified through clever design without the need for complex algorithm modeling, which greatly improves the intelligence level of the present invention, realizes simple, efficient and highly intelligent positioning and navigation of underground rescue equipment, and can perform real-time and efficient channel judgment when encountering complex terrain such as obstacles, thereby avoiding irregular and even dynamic obstacles, and greatly expanding the application scenarios of the present invention.

[0054] P2. Process the collected data and divide the depth image into a series of target search boxes Mn.

[0055] It should be noted that the processing of the collected data includes: First, we obtain the set of depth arrays {Ad1, Ad2, …, Adn} and the corresponding angles and distances {d1, d2, …, dn} in the depth image data, where 125° of the laser radar corresponds to 0° of the depth camera's viewing angle. We count the points in the image within the range of d1±dσ and store the coordinates in array Ad1. We obtain the maximum and minimum values ​​of the horizontal coordinate and the maximum and minimum values ​​of the vertical coordinate in array Ad1. We establish a target search box M1 based on the maximum and minimum values ​​of the horizontal coordinate and the maximum and minimum values ​​of the vertical coordinate in Ad1. We count the points in the image within the range of d2±dσ and store the coordinates in array Ad2, …, and so on. The depth image is divided into a series of target search boxes Mn.

[0056] The method of using the processed depth camera data to determine the passable channel according to the preset determination rules, and formulating the travel route and travel rules based on the determination results includes: Calculate the distance difference of adjacent target search boxes respectively, and determine whether the maximum distance difference is greater than or equal to the first preset threshold. If so, determine whether the width of the target search box with a larger distance between the two target search boxes that produce the maximum distance difference is greater than or equal to the second preset threshold, and at the same time determine whether the height of the target search box is greater than or equal to the third preset threshold. If so, the area indicated by the target search box is a passable channel, then calculate the deviation angle between the center of the target search box and the driving direction of the rescue equipment, and calculate the distance that the rescue equipment needs to move left or right based on the distance and the deviation angle. The rescue equipment moves the distance left or right so that the forward direction of the rescue equipment and the center of the target search box are in the same vertical plane, and then the rescue equipment enters the passable channel.

[0057] P3. Calculate the distance differences between adjacent target search boxes respectively.

[0058] P4. Determine whether the maximum distance difference is greater than or equal to the first preset threshold. If yes, execute step P5; if no, return to step P1.

[0059] P5. Determine whether the width of the target search box with the larger distance between the two target search boxes that generate the maximum distance difference is greater than or equal to the second preset threshold. If so, execute step P6; if not, execute step P14.

[0060] P6. Determine whether the height of the target search box is greater than or equal to a third preset threshold. If yes, execute step P7; if no, execute step P14.

[0061] P7. If the area indicated by the target search box is a passable passage, calculate the deviation angle between the center of the target search box and the direction of travel of the rescue equipment, and calculate the distance that the rescue equipment needs to move left or right based on the distance and the deviation angle. After the rescue equipment moves left or right by this distance so that the forward direction of the rescue equipment and the center of the target search box are in the same vertical plane, the rescue equipment enters the passable passage along the intersection of the vertical plane and the ground plane.

[0062] P8. After the rescue equipment enters the passage, it obtains its position DL relative to the left wall and its position DR relative to the right wall.

[0063] P9. Determine whether the area of ​​the target search box is greater than or equal to the fifth preset threshold. If yes, execute step P10; if no, execute step P14.

[0064] P10. Determine whether the area of ​​the target search box is less than or equal to a fourth preset threshold. If yes, execute step P11; if no, execute step P13.

[0065] P11. The rescue equipment moves at the second preset speed.

[0066] P12: Determine whether the changes in DL and DR are both greater than or equal to a sixth preset threshold. If yes, execute step P13; if no, return to step P11.

[0067] P13. The rescue equipment moves at a first preset speed and executes step P15.

[0068] P14. Use the laser radar and depth camera to calculate the deviation angle between the center of the target search box and the direction of travel of the rescue equipment, and calculate the distance that the rescue equipment needs to move left or right. After the rescue equipment moves this distance left or right so that the forward direction of the rescue equipment and the center of the target search box are in the same vertical plane, the decision module sends out excavation and crushing information, and the rescue equipment executes the excavation and crushing action and moves along the intersection of the vertical plane and the ground plane.

[0069] Specifically, if the width of the target search box with a larger distance between the two target search boxes that produce the maximum distance difference is smaller than the width of the rescue equipment or the height of the target search box is smaller than the height of the rescue equipment, the deviation angle between the center of the target search box and the travel direction of the rescue equipment is calculated to obtain the distance that the rescue equipment needs to move left or right. After the rescue equipment moves this distance left or right so that the forward direction of the rescue equipment and the center of the target search box are in the same vertical plane, the decision module sends out excavation and crushing information, and the rescue equipment performs the excavation and crushing action and moves forward.

[0070] It can be understood that when it is determined that the size of the target channel does not meet the size of the rescue equipment and cannot pass through, the rescue equipment is still controlled to move toward the channel along the intersection of the vertical plane of the center of the channel and the ground plane and perform crushing and excavation actions, and the obstacle is crushed and excavated from the weakest or least part of the obstacle, which greatly saves rescue time. Through simple and efficient algorithms, the complex problems of being unable to perform efficient channel judgment when encountering complex terrains such as obstacles and performing efficient obstacle removal when obstacles cannot be avoided are further simplified, which greatly improves the rescue effect and intelligence level, and further improves the usability of the present invention.

[0071] P15. Is it determined whether the tilt angle value of the rescue equipment is greater than or equal to the preset safety angle value? If so, execute step P16; if not, execute step P18.

[0072] P16. The rescue equipment improves its forward posture and moves forward at a second preset speed.

[0073] P17. Is it determined whether the tilt angle value of the rescue equipment is greater than or equal to the preset safety angle value? If so, return to step P16; if not, execute step P18.

[0074] P18. The rescue equipment moves at the first preset speed and determines whether it has reached the rescue target point or found survivors using the infrared thermal imager. If yes, execute step P19; if no, return to step P17.

[0075] P19. Send corresponding signals to the staff.

[0076] P20, end.

[0077] See also Figure 3 The present invention provides another embodiment, which provides a positioning and navigation system for underground rescue equipment, and the positioning and navigation system for underground rescue equipment includes: The acquisition unit 1 is used for moving at a first preset speed after the rescue equipment enters the well, and using a depth camera arranged on the rescue equipment to acquire surrounding image data of the rescue equipment, or using a laser radar arranged on the rescue equipment to acquire real-time distance data between the rescue equipment and the channel, or using a strapdown inertial navigation module arranged on the rescue equipment to obtain inclination angle data of the rescue equipment.

[0078] The processing unit 2 is used to process the data collected by the collection unit 1, use the processed depth camera data to judge the passable channel according to the preset judgment rules, formulate the travel route and travel rules based on the judgment results, judge the passability of the rescue equipment in the channel in real time through the distance data, and judge whether the posture of the rescue equipment is within the safe range through the obtained inclination angle data.

[0079] The execution unit 3 is used to control the rescue equipment to move according to the route and movement rules set by the processing unit 2.

[0080] In a preferred embodiment, the present application further provides an electronic device, the electronic device comprising: A memory; and a processor, wherein the memory stores computer-readable instructions, and when the computer-readable instructions are executed by the processor, the method for positioning and navigating underground rescue equipment is implemented. The computer device can be broadly a server, a terminal, or any other electronic device with necessary computing and / or processing capabilities. In one embodiment, the computer device may include a processor, a memory, a network interface, a communication interface, etc. connected via a system bus. The processor of the computer device can be used to provide necessary computing, processing and / or control capabilities. The memory of the computer device may include a non-volatile storage medium and an internal memory. An operating system, a computer program, etc. may be stored in or on the non-volatile storage medium. The internal memory can provide an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The network interface and the communication interface of the computer device can be used to connect and communicate with external devices via a network. When the computer program is executed by the processor, the steps of the method of the present invention are performed.

[0081] The present invention may be implemented as a computer-readable storage medium having a computer program stored thereon, which causes the steps of the method of an embodiment of the present invention to be executed when executed by a processor. In one embodiment, the computer program is distributed on a plurality of computer devices or processors coupled to a network so that the computer program is stored, accessed and executed in a distributed manner by one or more computer devices or processors. A single method step / operation, or two or more method steps / operations, may be performed by a single computer device or processor or by two or more computer devices or processors. One or more method steps / operations may be performed by one or more computer devices or processors, and one or more other method steps / operations may be performed by one or more other computer devices or processors. One or more computer devices or processors may perform a single method step / operation, or perform two or more method steps / operations.

[0082] It will be appreciated by a person skilled in the art that the method steps of the present invention may be performed by instructing related hardware such as a computer device or a processor through a computer program, and the computer program may be stored in a non-temporary computer-readable storage medium, which causes the steps of the present invention to be performed when the computer program is executed. Depending on the circumstances, any reference to memory, storage, database, or other media herein may include non-volatile and / or volatile memory. Examples of non-volatile memory include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), flash memory, magnetic tape, floppy disk, magneto-optical data storage device, optical data storage device, hard disk, solid state disk, etc. Examples of volatile memory include random access memory (RAM), external cache memory, etc.

[0083] The various technical features described above can be combined arbitrarily. Although all possible combinations of these technical features are not described, any combination of these technical features should be considered to be covered by this specification as long as there is no contradiction in such combination.

[0084] The specific implementation of the present invention described above does not constitute a limitation on the protection scope of the present invention. Any other corresponding changes and modifications made based on the technical concept of the present invention should be included in the protection scope of the claims of the present invention.

Claims

1. A method for positioning and navigating underground rescue equipment, characterized in that: After entering the well, the rescue equipment moves at a first preset speed, and uses the depth camera provided on the rescue equipment to collect image data of the surroundings of the rescue equipment, and processes the collected data. The processed depth camera data is used to judge the passable channel according to preset judgment rules, and the travel route and travel rules are formulated based on the judgment results. When the rescue equipment moves according to the formulated travel route and travel rules, the laser radar provided on the rescue equipment is used to collect the distance data between the rescue equipment and the channel in real time. At the same time, the strapdown inertial navigation module provided on the rescue equipment is used to obtain the inclination angle data of the rescue equipment. The passability of the rescue equipment in the channel is judged in real time through the distance data, and whether the posture of the rescue equipment is within a safe range is judged through the obtained inclination angle data.

2. The underground rescue equipment positioning and navigation method according to claim 1, characterized in that: The processing of the collected data comprises: First, we obtain the set of depth arrays {Ad1, Ad2, …, Adn} and the corresponding angles and distances {d1, d2, …, dn} in the depth image data, where 125° of the laser radar corresponds to 0° of the depth camera's viewing angle. We count the points in the image within the range of d1±dσ and store the coordinates in array Ad1. We obtain the maximum and minimum values ​​of the horizontal coordinate and the maximum and minimum values ​​of the vertical coordinate in array Ad1. We establish a target search box M1 based on the maximum and minimum values ​​of the horizontal coordinate and the maximum and minimum values ​​of the vertical coordinate in Ad1. We count the points in the image within the range of d2±dσ and store the coordinates in array Ad2, …, and so on. The depth image is divided into a series of target search boxes Mn.

3. The underground rescue equipment positioning and navigation method according to claim 2, characterized in that: The method of using the processed depth camera data to determine the passable channel according to the preset determination rules, and formulating the travel route and travel rules based on the determination results includes: Calculate the distance difference of adjacent target search boxes respectively, and determine whether the maximum distance difference is greater than or equal to the first preset threshold. If so, determine whether the width of the target search box with a larger distance between the two target search boxes that produce the maximum distance difference is greater than or equal to the second preset threshold, and at the same time determine whether the height of the target search box is greater than or equal to the third preset threshold. If so, the area indicated by the target search box is a passable channel, then calculate the deviation angle between the center of the target search box and the driving direction of the rescue equipment, and calculate the distance that the rescue equipment needs to move left or right based on the distance and the deviation angle. The rescue equipment moves the distance left or right so that the forward direction of the rescue equipment and the center of the target search box are located in the same vertical plane, and then the rescue equipment enters the passable channel along the intersection of the vertical plane and the ground plane.

4. The underground rescue equipment positioning and navigation method according to claim 3, characterized in that: The real-time determination of the passability of the rescue equipment in the passage through the distance data includes: The 0 degree of the laser radar always points in the opposite direction of the rescue equipment. Through the values ​​of 90 degrees and 270 degrees of the laser radar, when the rescue equipment enters the channel, it obtains its own position DL relative to the left wall and the position DR relative to the right wall. When the area of ​​the target search box is less than or equal to the fourth preset threshold and greater than or equal to the fifth preset threshold, the rescue equipment moves at the second preset speed and determines whether the changes in DL and DR are both greater than or equal to the sixth preset threshold. If both are greater than the sixth preset threshold, the rescue equipment moves at the first preset speed.

5. The underground rescue equipment positioning and navigation method according to claim 3, characterized in that: The step of judging whether the posture of the rescue equipment is within a safe range by using the obtained tilt angle data includes: If the inclination angle value of the rescue equipment is greater than or equal to the preset safety angle value, the rescue equipment improves its forward posture and moves forward at a second preset speed until the inclination angle value of the rescue equipment is less than the preset safety angle, and determines whether it has reached the rescue target point. If it has reached the rescue target point, navigation ends.

6. The underground rescue equipment positioning and navigation method according to claim 4, characterized in that: The method further comprises: If the width of the target search box with the larger distance between the two target search boxes that produce the maximum distance difference is smaller than the width of the rescue equipment or the height of the target search box is smaller than the height of the rescue equipment, the deviation angle between the center of the target search box and the driving direction of the rescue equipment is calculated to obtain the distance that the rescue equipment needs to move left or right. The rescue equipment moves left or right by this distance so that the forward direction of the rescue equipment and the center of the target search box are in the same vertical plane. Then, the decision module sends out excavation and crushing information, and the rescue equipment performs excavation and crushing actions and moves along the intersection of the vertical plane and the ground plane.

7. The underground rescue equipment positioning and navigation method according to claim 3, characterized in that: The method further comprises: According to the infrared thermal imager set on the rescue equipment, the current frame infrared thermal imaging image of the rescue equipment's forward direction is collected and analyzed to obtain the current frame infrared thermal imaging data. If there is a pixel point with a grayscale value greater than or equal to the seventh preset threshold in the current frame infrared thermal imaging data, the pixel point is determined as a target point, and a target range is determined according to the target point. The target range needs to include all target points in the current frame infrared thermal imaging image data. A target search box is established according to the target range. The rescue equipment approaches the target indicated by the target search box and measures the real-time distance between the rescue equipment and the target in real time. When the distance between the rescue equipment and the target is less than or equal to the eighth preset distance, it is determined whether the area of ​​the target search box is greater than or equal to the ninth preset threshold and whether the total grayscale value of each target point is greater than or equal to the ninth preset threshold. If the area of ​​the target search box is greater than or equal to the ninth preset threshold or the total grayscale value of each target point is greater than or equal to the tenth preset threshold, it is determined that there is a target image defined as a survivor in the current frame infrared thermal imaging image, and the decision unit sends a survival signal to the rescue personnel.

8. The underground rescue equipment positioning and navigation method according to claim 7, characterized in that: The target search frame needs to include the target range and the background of a preset range around the target, calculate the target's mass points and establish a search frame based on the target's boundary, and use the target's center of mass as the base point to expand the search frame to a preset multiple of the search frame established based on the target's boundary.

9. A positioning and navigation system for underground rescue equipment, characterized in that: include: The acquisition unit is used to move at a first preset speed after the rescue equipment enters the well, collect the surrounding image data of the rescue equipment by using a depth camera provided on the rescue equipment, or collect the distance data between the rescue equipment and the channel in real time by using a laser radar provided on the rescue equipment, or obtain the inclination angle data of the rescue equipment by using a strapdown inertial navigation module provided on the rescue equipment; A processing unit is used to process the data collected by the collection unit, use the processed depth camera data to determine the passable channel according to the preset judgment rules, formulate the travel route and travel rules based on the judgment result, judge the passability of the rescue equipment in the channel in real time through the distance data, and judge whether the posture of the rescue equipment is within the safe range through the obtained tilt angle data; The execution unit is used to control the rescue equipment to move according to the route and travel rules formulated by the processing unit.

10. An electronic device, characterized in that: include: Memory; and a processor, wherein the memory stores computer-readable instructions, and when the computer-readable instructions are executed by the processor, the underground rescue equipment positioning and navigation method according to any one of claims 1 to 8 is implemented.

Images