Quadruped robot navigation system in semi-dynamic environment based on hierarchical positioning strategy

Abstract

The invention belongs to the technical field of robots, and discloses a quadruped robot navigation system in a semi-dynamic environment based on a hierarchical positioning strategy, comprising a data acquisition module for generating first point cloud data based on first original point cloud data, a first map construction module for constructing a first map based on the first point cloud data and first inertial measurement unit data, and a second map construction module for constructing a second map based on the second point cloud data and second inertial measurement unit data. The second map building module is used for eliminating the semi-dynamic object from the first map to generate a second map, the navigation control module is used for planning a driving route of the quadruped robot based on the second map, and the repositioning module is used for generating second point cloud data based on second original point cloud data when the semi-dynamic object is detected to move. The position coordinates of the quadruped robot are determined again, the obstacle sensing module classifies the meeting state based on the first operation data and the second operation data, and navigation control is conducted according to the classification result. According to the technical scheme, the positioning precision and stability of the robot can be improved, and the robot is guided to rapidly avoid obstacles.

Description

Technical Field

[0001] The invention belongs to the technical field of robots, and in particular relates to a quadruped robot navigation system in a semi-dynamic environment based on a hierarchical positioning strategy. Background Art

[0002] In hazardous chemical parks and workshops, the normal operation of equipment must be based on a full understanding of the overall production situation, otherwise any error in any link may lead to catastrophic consequences. In order to ensure the safety of the environment and personnel, as well as the normal progress of production, a dedicated person is required to be responsible for inspection and monitoring. However, the traditional manual inspection method has the disadvantages of low efficiency, high cost, poor safety, and many false alarms. In recent years, with the simultaneous advancement of smart factories, intelligent production, and informatization, inspection management through patrol robots has become an inevitable choice.

[0003] Similar prior art includes a Chinese patent application with publication number CN118192532A, which discloses a visual navigation method and device for a trackless inspection robot for a production workshop. The environmental perception module obtains environmental image data, the landmark line detection module extracts landmark line features from the environmental image data, determines the drivable area of ​​the inspection robot, the obstacle detection module detects obstacles from the environmental image data, and the real-time obstacle avoidance module plans local path information within the drivable area of ​​the inspection robot according to the position of the obstacle, so that the inspection robot can complete the inspection task. This method obtains the position of the robot and the obstacle based on the image, and plans the robot's driving route based on the landmark line. The accuracy of route planning and positioning is not high. There is also a Chinese patent application with publication number CN112577491A, which discloses a robot path planning method based on an improved artificial potential field method. Step 1: Initialize the logistics workshop information to determine the robot's current position, target position, and the positions of static and dynamic obstacles; Step 2: Perform global path planning based on the A* algorithm to obtain an initial path, and set each turning point in the initial path as a sub-target point; Step 3: Set the boundaries of obstacles and logistics workshop environments to a repulsive level, the sub-target point to an attractive level, and set the target point to a potential energy minimum point; Step 4: The robot moves in the process of driving. When encountering an obstacle, calculate the gravitational force and repulsive force on the robot's current position, calculate the magnitude of the resultant force, and guide the robot to the next place according to the resultant force; Step 5: optimize the repulsive field function of the artificial potential field method, and introduce the robot collision coefficient into the artificial potential field method; Step 6: after the robot's sensor detects an unknown dynamic obstacle around, it uses the improved artificial potential field method to avoid obstacles locally based on the principle of repulsion between the obstacles; Step 7: after avoiding the obstacle, the force guides the robot back to the initial path; Step 8: if it has reached the final target point, end the algorithm loop, otherwise, jump to step 4. This method reflects all the gravitational and repulsive forces on the robot in the potential field, and the processing time is long.

[0004] Therefore, it is an urgent problem to provide a quadruped robot navigation system in a semi-dynamic environment based on a hierarchical positioning strategy to improve the robot's positioning accuracy and stability and speed up obstacle avoidance processing. Summary of the invention

[0005] In response to the above-mentioned technical problems, the present invention provides a quadruped robot navigation system in a semi-dynamic environment based on a hierarchical positioning strategy.

[0006] The present invention provides a quadruped robot navigation system in a semi-dynamic environment based on a hierarchical positioning strategy, the system comprising: a data acquisition module, a first map construction module, a second map construction module, a navigation control module, a repositioning module and an obstacle perception module;

[0007] A data acquisition module, which acquires first original point cloud data and first inertial measurement unit data in the target area, performs first preprocessing on the first original point cloud data, and acquires first point cloud data;

[0008] A first map building module, based on the first point cloud data and the first inertial measurement unit data, uses a graph-optimized SLAM algorithm to build a first map of the target area;

[0009] A second map construction module, which uses a target detection algorithm to detect and select the semi-dynamic objects in the first map with a rectangular frame, determines the coordinate positions of the semi-dynamic objects, and removes the semi-dynamic objects from the first map based on the coordinate positions to generate a second map;

[0010] a navigation control module, which plans a first driving route of the quadruped robot based on the second map, and controls the quadruped robot to drive along the first driving route;

[0011] The repositioning module performs obstacle detection during the driving process of the quadruped robot. When a moving semi-dynamic object is detected, the second original point cloud data and the second inertial measurement unit data collected when the moving semi-dynamic object is detected are obtained, the second original point cloud data are subjected to a second preprocessing to obtain the second point cloud data, and then the position coordinates of the quadruped robot are determined by using a graph-optimized SLAM algorithm based on the second map, the second point cloud data and the second inertial measurement unit data;

[0012] The obstacle perception module obtains the first operation data of the mobile semi-dynamic object and the second operation data of the quadruped robot, classifies the encounter state of the mobile semi-dynamic object and the quadruped robot based on the first operation data and the second operation data, and performs navigation control according to the classification result.

[0013] Specifically, the first point cloud data includes third point cloud data and fourth point cloud data, and the first preprocessing includes:

[0014] Step 11, obtaining a first coordinate point of a data acquisition device when acquiring first original point cloud data, and a first acquisition area corresponding to the first original point cloud data, dividing the first acquisition area, defining an area whose distance from the first coordinate point is less than or equal to a first preset distance as a first area, and defining an area whose distance from the acquisition point coordinate point is greater than the first preset distance as a second area;

[0015] Step 12, extract any area, if any area is the first area, proceed to step 13, if any area is the second area, proceed to step 14;

[0016] Step 13, the first area is evenly divided into N1 first grids according to the first preset value corresponding to the specific parameter, any first grid is extracted, and it is determined whether the number of laser measurement points in any first grid is less than or equal to 1. If so, any first grid is not processed. If not, any first grid is processed according to the preset rule so that the number of laser measurement points in any first grid is 1. Step 13 is repeated, and after all first grids are traversed, the third point cloud data is obtained;

[0017] Step 14, divide the second area evenly into N2 second grids according to the second preset value corresponding to the specific parameter, extract any second grid, and determine whether the number of laser measurement points in any second grid is less than or equal to 1. If so, do not process any second grid. If not, process any second grid according to the preset rule so that the number of laser measurement points in any second grid is 1. Repeat step 14, and after traversing all second grids, obtain the fourth point cloud data, wherein the second preset value is K1 times the first preset value, and K1 is a positive number greater than 1.

[0018] Specifically, the preset rules are:

[0019] Calculate the midpoint coordinates of multiple laser measurement points in the grid and use the midpoint coordinates as the only laser measurement point in the grid; or,

[0020] A laser measurement point located at the center among the multiple laser measurement points is used as the only laser measurement point in a grid, wherein the grid is any first grid or any second grid.

[0021] Specifically, the second preprocessing includes:

[0022] Deleting the point cloud data corresponding to the semi-dynamic object from the second original point cloud data to obtain fifth point cloud data;

[0023] A second coordinate point of the data acquisition device when acquiring the second original point cloud data is acquired, and all laser measurement points in the fifth point cloud data whose distance from the second coordinate point is greater than a third preset value are defined as second point cloud data.

[0024] Specifically, the encounter status includes four types, and the classification method is as follows:

[0025] Step 21, taking the first driving route as the center line, setting the areas on both sides of the center line at a second preset distance from the center line as identification areas, wherein the identification areas are within the collection range of the data collection device;

[0026] Step 22, determine whether the moving semi-dynamic object is located in the identification area, if so, proceed to step 23, if not, proceed to step 25;

[0027] Step 23, calculating a first angle between the moving direction of the quadruped robot and the moving direction of the semi-dynamic object, and determining whether the first angle is within a first preset range, if so, setting the encounter state to the first type, if not, proceeding to step 24;

[0028] Step 24, determining whether the first angle is within a second preset range, if so, setting the encounter state to the second type, if not, proceeding to step 25;

[0029] Step 25: Obtain a second driving route of the mobile semi-dynamic object, and determine whether the second driving route has an intersection with the first driving route. If so, set the encounter state to the third type; if not, set the encounter state to the fourth type.

[0030] Specifically, when the encounter state is the first type, navigation control of the quadruped robot includes:

[0031] Step 231, determining whether the first speed of the moving semi-dynamic object is greater than or equal to the second speed of the quadruped robot, if so, controlling the quadruped robot to travel along the first travel route at the second speed, if not, proceeding to step 232;

[0032] Step 232, determining whether the first speed is greater than or equal to a first preset speed of the quadruped robot, if so, controlling the quadruped robot to travel along the first travel route at the first speed, if not, proceeding to step 233;

[0033] Step 233: generate a new driving route for the quadruped robot, and control the quadruped robot to drive along the new driving route.

[0034] Specifically, when the encounter state is of the second type, the navigation control of the quadruped robot includes: generating a new driving route for the quadruped robot, and controlling the quadruped robot to drive along the new driving route.

[0035] Specifically, when the encounter state is the third type, the navigation control of the quadruped robot includes:

[0036] Step 251, defining the intersection point as the meeting point, calculating the first time for the mobile semi-dynamic object to travel to the meeting point, and calculating the second time for the quadruped robot to travel to the meeting point;

[0037] Step 252, calculating a first difference between the first time and the second time, and determining whether the first difference is greater than or equal to a preset time threshold, if so, controlling the quadruped robot to travel along the first driving route, if not, proceeding to step 253;

[0038] Step 253: obtaining a second preset speed of the quadruped robot, and calculating a third time for the quadruped robot to travel to the meeting point along the first travel route at the second preset speed, wherein the second preset speed is the first preset speed or the third preset speed;

[0039] Step 254, calculating a second difference between the first time and the third time, and determining whether the second difference is greater than or equal to a preset time threshold, if so, controlling the quadruped robot to travel along the first travel route at a second preset speed, if not, proceeding to step 255;

[0040] Step 255: generate a new driving route for the quadruped robot, and control the quadruped robot to drive along the new driving route.

[0041] Specifically, when the encounter state is of the fourth type, the navigation control of the quadruped robot includes: controlling the quadruped robot to travel along the first driving route.

[0042] Specifically, in step 25, obtaining the second driving route of the mobile semi-dynamic object includes: obtaining historical operation data of the mobile semi-dynamic object within a first preset time, and generating the second driving route based on the historical operation data and the first operation data.

[0043] Compared with the prior art, the beneficial effects of the present invention are at least as follows:

[0044] 1. When constructing a map, the collected point cloud data is first preprocessed to generate the first point cloud data, and then the first map is constructed based on the first point cloud data and the first inertial measurement unit data. The point cloud is then layered to eliminate interference from semi-dynamic objects, and a second map containing only static features is constructed. The first driving route of the quadruped robot is planned based on the second map to improve the stability and robustness of route planning.

[0045] 2. When a moving semi-dynamic object is detected, the collected point cloud data is subjected to a second preprocessing to remove the point cloud corresponding to the semi-dynamic object, and the quadruped robot is positioned using static features, thereby improving the positioning accuracy and stability.

[0046] 3. During the navigation process, the real-time status of the mobile semi-dynamic objects in the environment is obtained, and the encounter status of the mobile semi-dynamic objects and the quadruped robot is classified based on the operating data of the mobile semi-dynamic objects and the operating data of the quadruped robot. Navigation control is performed according to the classification results to speed up data processing while ensuring that the quadruped robot safely bypasses the mobile semi-dynamic objects. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, 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 embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0048] Figure 1 It is a modular schematic diagram of the quadruped robot navigation system in a semi-dynamic environment based on the hierarchical positioning strategy of the present invention. DETAILED DESCRIPTION

[0049] In order to make the purpose, technical scheme and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. Obviously, the specific embodiments described herein are only used to explain the present invention and are part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work belong to the scope of protection of the present invention.

[0050] It should be noted that if there are descriptions involving "first", "second", etc. in the embodiments of the present invention, the descriptions of "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In addition, the technical solutions between the various embodiments can be combined with each other, but they must be based on the ability of ordinary technicians in the field to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0051] In this invention, objects are divided into three categories: static, dynamic, and semi-dynamic. Static objects are objects whose positions do not change during the mapping and positioning process, such as houses and trees in the environment; dynamic objects are objects that have a high probability of moving in the scene, such as people, birds, dogs, etc.; semi-dynamic objects are objects that remain stationary most of the time, but may move when they come into contact with dynamic objects, such as books, umbrellas, cars, etc. The classification of static objects, dynamic objects, and semi-dynamic objects has been completed in advance.

[0052] Figure 1 FIG. 1 is a schematic diagram showing a structure of an embodiment of a quadruped robot navigation system in a semi-dynamic environment based on a hierarchical positioning strategy provided by the present invention. Figure 1 As shown, the system includes: a data acquisition module 10, a first map construction module 20, a second map construction module 30, a navigation control module 40, a repositioning module 50 and an obstacle perception module 60.

[0053] The data acquisition module 10 acquires first original point cloud data and first inertial measurement unit data in the target area, performs first preprocessing on the first original point cloud data, and acquires first point cloud data.

[0054] The first map construction module 20 constructs a first map of the target area using a graph-optimized SLAM algorithm based on the first point cloud data and the first inertial measurement unit data.

[0055] The second map construction module 30 uses a target detection algorithm to detect and select the semi-dynamic objects in the first map with a rectangular frame, determines the coordinate positions of the semi-dynamic objects, and removes the semi-dynamic objects from the first map based on the coordinate positions to generate a second map.

[0056] The navigation control module 40 plans a first driving route of the quadruped robot based on the second map, and controls the quadruped robot to travel along the first driving route.

[0057] The repositioning module 50 performs obstacle detection during the driving process of the quadruped robot. When a moving semi-dynamic object is detected, the second original point cloud data and the second inertial measurement unit data collected when the moving semi-dynamic object is detected are obtained, and the second original point cloud data are subjected to a second preprocessing to obtain the second point cloud data. Subsequently, based on the second map, the second point cloud data and the second inertial measurement unit data, a graph-optimized SLAM algorithm is used to determine the position coordinates of the quadruped robot.

[0058] The obstacle sensing module 60 obtains the first operating data of the mobile semi-dynamic object and the second operating data of the quadruped robot, classifies the encounter state of the mobile semi-dynamic object and the quadruped robot based on the first operating data and the second operating data, and performs navigation control according to the classification result.

[0059] The first map is a map including static objects and semi-dynamic objects, and the second map is a map including only static objects.

[0060] Generally speaking, when a semi-dynamic object is not in a working state, it is located at a position that does not interfere with the robot. The first driving route is generated based on the second map that only contains static features. The first driving route has a high utilization rate. When the semi-dynamic object affects the robot's driving, a temporary driving path is generated in real time according to the actual situation, which helps to improve the stability, robustness and flexibility of route planning.

[0061] Preferably, the first operating data and the second operating data include at least current position, driving speed, angular velocity, acceleration, etc.

[0062] Specifically, the first point cloud data includes third point cloud data and fourth point cloud data, and the first preprocessing includes:

[0063] Step 11, obtain the first coordinate point of the data acquisition device when collecting the first original point cloud data, and the first acquisition area corresponding to the first original point cloud data, divide the first acquisition area, define the area whose distance from the first coordinate point is less than or equal to the first preset distance as the first area, and define the area whose distance from the acquisition point coordinate is greater than the first preset distance as the second area.

[0064] Step 12: extract any area. If any area is the first area, proceed to step 13; if any area is the second area, proceed to step 14.

[0065] Step 13, divide the first area evenly into N1 first grids according to the first preset value corresponding to the specific parameter, extract any first grid, and determine whether the number of laser measurement points in any first grid is less than or equal to 1. If so, do not process any first grid. If not, process any first grid according to the preset rule so that the number of laser measurement points in any first grid is 1. Repeat step 13, and after traversing all the first grids, obtain the third point cloud data.

[0066] Step 14, divide the second area evenly into N2 second grids according to the second preset value corresponding to the specific parameter, extract any second grid, and determine whether the number of laser measurement points in any second grid is less than or equal to 1. If so, do not process any second grid. If not, process any second grid according to the preset rule so that the number of laser measurement points in any second grid is 1. Repeat step 14, and after traversing all second grids, obtain the fourth point cloud data, wherein the second preset value is K1 times the first preset value, and K1 is a positive number greater than 1.

[0067] The first preset distance, the first preset value, the second preset value and K1 are set according to the experience of those skilled in the art or according to actual application scenarios, and the embodiments of the present application are not limited thereto.

[0068] Preferably, the first grid and the second grid are three-dimensional grids, and the specific parameter is volume or side length. N1 is a positive integer determined according to the first preset value and the size of the first area, and N2 is a positive integer determined according to the second preset value and the size of the second area.

[0069] Preferably, the above-mentioned data acquisition device is a laser radar.

[0070] The laser radar point cloud of each object in the collection area is obtained through the data collection device. The closer the distance to the data collection device is, the greater the point cloud density is, and the farther the distance to the data collection device is, the smaller the point cloud density is. In general, the point cloud data collected by the data collection device is generally huge in quantity. In order to increase the processing speed of the point cloud data, it is necessary to prune the point cloud and reduce the number of point clouds.

[0071] Based on the first preset distance, the first acquisition area is divided into a first area close to the data acquisition device and a second area far from the data acquisition device, and then the first area and the second area are gridded respectively, and the laser measurement points in each grid are reduced according to the preset rules. The value of the specific parameter corresponding to the first grid in the first area is less than the value of the specific parameter corresponding to the second grid in the second area, based on which the point cloud density in the first area after the reduction process can be made greater than the point cloud density in the second area.

[0072] According to the technical solution of the present invention, while ensuring the accuracy and precision of map generation, the amount of data processing can be reduced and the data processing speed can be improved.

[0073] Specifically, the preset rules are:

[0074] Calculate the midpoint coordinates of multiple laser measurement points in the grid and use the midpoint coordinates as the only laser measurement point in the grid; or,

[0075] A laser measurement point located at the center among the multiple laser measurement points is used as the only laser measurement point in a grid, wherein the grid is any first grid or any second grid.

[0076] For example, there are 3 laser measurement points in the grid, and the midpoint coordinates of the 3 laser measurement points are (x1, y1, z1), (x2, y2, z2), and (x3, y3, z3) are the coordinates of the above three laser measurement points respectively.

[0077] Exemplarily, there are three laser measurement points a1, a2 and a3 in the grid, wherein a2 is a laser measurement point close to / located at the center, and a2 is taken as the only laser measurement point in the grid.

[0078] Specifically, the second preprocessing includes:

[0079] Deleting the point cloud data corresponding to the semi-dynamic object from the second original point cloud data to obtain fifth point cloud data;

[0080] A second coordinate point of the data acquisition device when acquiring the second original point cloud data is acquired, and all laser measurement points in the fifth point cloud data whose distance from the second coordinate point is greater than a third preset value are defined as second point cloud data.

[0081] The third preset value is set according to the experience of those skilled in the art or according to the actual application scenario, and the embodiment of the present application does not limit this. The third preset value and the second preset value may be equal or unequal.

[0082] When a moving semi-dynamic object is detected, the point cloud data corresponding to the semi-dynamic object is deleted from the second original point cloud data, and the quadruped robot is positioned based on static features to improve the positioning accuracy and stability of the quadruped robot.

[0083] Exemplarily, for two objects A and B within the collection range of a data acquisition device (located at point P), A is close to the data acquisition device and B is far away from the data acquisition device, and two laser measurement points (P1, P2) located on both sides of A and two laser measurement points (P3, P4) located on both sides of B are collected respectively. The angle ∠P1PP2 formed by P, P1, and P2 is greater than the angle ∠P3PP4 formed by P, P3, and P4, that is, the jitter error caused by the long-distance point cloud is smaller than the jitter error caused by the short-distance point cloud. Therefore, the positioning accuracy of the quadruped robot can be improved by positioning the long-distance point cloud data.

[0084] According to the technical solution of the present invention, the robot is positioned according to general conditions during normal driving. When the quadruped robot is accurately positioned based on the precise positioning opportunity, the robot's workload and data processing volume can be reduced while ensuring accurate obstacle avoidance. In addition, long-distance static features are used for positioning to improve positioning accuracy and stability.

[0085] As a preferred technical solution of the present invention, all laser measurement points in the fifth point cloud data whose distance from the second coordinate point is less than or equal to a third preset value are defined as sixth point cloud data, and then the position coordinates of the quadruped robot are determined by a graph-optimized SLAM algorithm based on the second map, the second point cloud data and the second inertial measurement unit data.

[0086] Specifically, the encounter status includes four types, and the classification method is as follows:

[0087] Step 21: Taking the first driving route as the center line, setting the areas on both sides of the center line at a second preset distance from the center line as identification areas, wherein the identification areas are within the collection range of the data collection device.

[0088] Step 22, determine whether the moving semi-dynamic object is located in the identification area, if so, proceed to step 23, if not, proceed to step 25.

[0089] Step 23, calculate the first angle between the moving direction of the quadruped robot and the moving direction of the semi-dynamic object, and determine whether the first angle is within a first preset range. If so, set the encounter state to the first type; if not, proceed to step 24.

[0090] Step 24, determining whether the first angle is within a second preset range, if so, setting the encounter state to the second type, if not, proceeding to step 25.

[0091] Step 25: Obtain a second driving route of the mobile semi-dynamic object, and determine whether the second driving route has an intersection with the first driving route. If so, set the encounter state to the third type; if not, set the encounter state to the fourth type.

[0092] The second preset distance, the first preset range, and the second preset range are set according to the experience of those skilled in the art or according to actual application scenarios, and the embodiments of the present application do not limit this.

[0093] Preferably, the second preset distance is set according to the maximum radius of the quadruped robot, and the second preset distance is K2 times the maximum radius, where K2 is a positive number between 1 and 2. The starting end of the above-mentioned identification area is located at the position coordinate, and the ending end is located at the edge of the collection range of the above-mentioned data collection device.

[0094] Exemplarily, the first preset range is 0 degrees to 10 degrees, that is, the moving semi-dynamic object and the quadruped robot move in the same direction.

[0095] Exemplarily, the second preset range is 170 degrees to 180 degrees, that is, the moving semi-dynamic object and the quadruped robot move towards each other.

[0096] If the second driving route of the mobile semi-dynamic object and the first driving route of the quadruped robot have an intersection, it means that the two may interfere with each other; if the second driving route of the mobile semi-dynamic object and the first driving route of the quadruped robot do not have an intersection, it means that the two do not have the possibility of interfering with each other.

[0097] Specifically, when the encounter state is the first type, navigation control of the quadruped robot includes:

[0098] Step 231, determine whether the first speed of the moving semi-dynamic object is greater than or equal to the second speed of the quadruped robot. If so, control the quadruped robot to travel at the second speed along the first driving route. If not, enter step 232.

[0099] Step 232, determine whether the first speed is greater than or equal to the first preset speed of the quadruped robot. If so, control the quadruped robot to travel at the first speed and along the first driving route. If not, enter step 233.

[0100] Step 233: generate a new driving route for the quadruped robot, and control the quadruped robot to drive along the new driving route.

[0101] Preferably, the first preset speed is the minimum traveling speed of the quadruped robot.

[0102] The first preset speed is set according to the experience of those skilled in the art or according to actual application scenarios, and the embodiments of the present application are not limited to this.

[0103] When the driving speed of the moving semi-dynamic object is within the allowable driving speed range of the quadruped robot, the quadruped robot is controlled to follow the moving semi-dynamic object. If the driving speed of the moving semi-dynamic object is not within the allowable driving speed range of the quadruped robot, a new driving route is generated for the quadruped robot.

[0104] Specifically, when the encounter state is of the second type, the navigation control of the quadruped robot includes: generating a new driving route for the quadruped robot, and controlling the quadruped robot to drive along the new driving route.

[0105] When the mobile semi-dynamic object and the quadruped robot are moving towards each other, a collision is inevitable, so a new driving route is directly generated for the quadruped robot.

[0106] Specifically, when the encounter state is the third type, the navigation control of the quadruped robot includes:

[0107] Step 251: define the intersection point as the meeting point, calculate the first time for the mobile semi-dynamic object to travel to the meeting point, and calculate the second time for the quadruped robot to travel to the meeting point.

[0108] Step 252, calculate the first difference between the first time and the second time, and determine whether the first difference is greater than or equal to a preset time threshold. If so, control the quadruped robot to travel along the first driving route; if not, proceed to step 253.

[0109] Step 253: Obtain a second preset speed of the quadruped robot, and calculate a third time for the quadruped robot to travel to the meeting point along the first driving route at the second preset speed, wherein the second preset speed is the first preset speed or the third preset speed.

[0110] Step 254, calculate the second difference between the first time and the third time, and determine whether the second difference is greater than or equal to the preset time threshold. If so, control the quadruped robot to travel at the second preset speed and along the first driving route. If not, enter step 255.

[0111] Step 255: generate a new driving route for the quadruped robot, and control the quadruped robot to drive along the new driving route.

[0112] Preferably, the third preset speed is the maximum traveling speed of the quadruped robot.

[0113] The preset time threshold and the third preset speed are set according to the experience of those skilled in the art or according to actual application scenarios, and the embodiments of the present application are not limited thereto.

[0114] If the first difference or the second difference is greater than or equal to the preset time threshold, it means that the moving semi-dynamic object will not collide with the quadruped robot. In this case, there is no need to generate a new driving route, and it is only necessary to control the driving speed of the quadruped robot.

[0115] Preferably, the driving speed of the quadruped robot can be set between the first preset speed and the third preset speed based on the first speed of the moving semi-dynamic object, the second speed of the quadruped robot, and the time before and after the two pass through the meeting point, so that the quadruped robot does not collide with the moving semi-dynamic object when passing the meeting point.

[0116] Specifically, when the encounter state is of the fourth type, the navigation control of the quadruped robot includes: controlling the quadruped robot to travel along the first driving route.

[0117] According to the technical solution of the present invention, the encounter types are classified, and then the quadruped robot is subjected to specific navigation control based on the specific encounter type. While ensuring that the quadruped robot safely bypasses the moving semi-dynamic object, the data processing speed can be accelerated and the reaction time can be reduced.

[0118] Specifically, in step 25, obtaining the second driving route of the mobile semi-dynamic object includes: obtaining historical operation data of the mobile semi-dynamic object within a first preset time, and generating the second driving route based on the historical operation data and the first operation data.

[0119] The first preset time is set according to the experience of those skilled in the art or according to an actual application scenario, and the embodiment of the present application is not limited to this.

[0120] It should be understood that, although each step in the flow chart of each embodiment of the present invention is shown in sequence according to the indication of the arrow, these steps are not necessarily performed in sequence according to the order indicated by the arrow. Unless there is a clear explanation in this article, the execution of these steps does not have a strict order restriction, and these steps can be performed in other orders. Moreover, at least a portion of the steps in each embodiment may include a plurality of sub-steps or a plurality of stages, and these sub-steps or stages are not necessarily performed at the same time, but can be performed at different times, and the execution order of these sub-steps or stages is not necessarily performed in sequence, but can be performed in turn or alternately with at least a portion of other steps or sub-steps or stages of other steps.

[0121] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program. The above-mentioned program can be stored in a non-volatile computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0122] The above-mentioned embodiments only express the preferred implementation mode of the present invention, and the description thereof is relatively specific and detailed, but it cannot be understood as limiting the scope of the patent of the present invention. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

Claims

1. A quadruped robot navigation system in a semi-dynamic environment based on a hierarchical positioning strategy, characterized in that: include: A data acquisition module, a first map construction module, a second map construction module, a first navigation control module, a relocation module and an obstacle perception module; The data acquisition module acquires first original point cloud data and first inertial measurement unit data in the target area, performs first preprocessing on the first original point cloud data, and acquires first point cloud data; The first map construction module constructs a first map of the target area using a graph-optimized SLAM algorithm based on the first point cloud data and the first inertial measurement unit data; The second map construction module uses a target detection algorithm to detect and select the semi-dynamic objects in the first map with a rectangular frame, determines the coordinate position of the semi-dynamic objects, and removes the semi-dynamic objects from the first map based on the coordinate position to generate a second map; The first navigation control module plans a first driving route of the quadruped robot based on the second map, and controls the quadruped robot to drive along the first driving route; The repositioning module, the quadruped robot performs obstacle detection during driving, and when a moving semi-dynamic object is detected, obtains second original point cloud data and second inertial measurement unit data collected when the moving semi-dynamic object is detected, performs second preprocessing on the second original point cloud data to obtain second point cloud data, and then determines the position coordinates of the quadruped robot based on the second map, the second point cloud data and the second inertial measurement unit data using the graph optimized SLAM algorithm; The obstacle perception module obtains the first operating data of the mobile semi-dynamic object and the second operating data of the quadruped robot, classifies the encounter state of the mobile semi-dynamic object and the quadruped robot based on the first operating data and the second operating data, and performs navigation control according to the classification result.

2. The system according to claim 1, characterized in that The first point cloud data includes third point cloud data and fourth point cloud data, and the first preprocessing includes: Step 11: obtaining a first coordinate point of a data acquisition device when acquiring the first original point cloud data, and a first acquisition area corresponding to the first original point cloud data, dividing the first acquisition area, defining an area whose distance from the first coordinate point is less than or equal to a first preset distance as a first area, and defining an area whose distance from the acquisition point coordinate point is greater than the first preset distance as a second area; Step 12, extract any area, if any of the areas is the first area, proceed to step 13, if any of the areas is the second area, proceed to step 14; Step 13, the first area is evenly divided into N1 first grids according to the first preset value corresponding to the specific parameter, any first grid is extracted, and it is determined whether the number of laser measurement points in any first grid is less than or equal to 1. If so, any first grid is not processed. If not, any first grid is processed according to a preset rule so that the number of laser measurement points in any first grid is 1. Step 13 is repeated, and after all first grids are traversed, the third point cloud data is obtained; Step 14, the second area is evenly divided into N2 second grids according to the second preset value corresponding to the specific parameter, any second grid is extracted, and it is determined whether the number of laser measurement points in any second grid is less than or equal to 1. If so, any second grid is not processed. If not, any second grid is processed according to the preset rule so that the number of laser measurement points in any second grid is 1. Step 14 is repeated, and after traversing all second grids, fourth point cloud data is obtained, wherein the second preset value is K1 times the first preset value, and K1 is a positive number greater than 1.

3. The system according to claim 2, characterized in that The preset rules are: Calculate the midpoint coordinates of multiple laser measurement points in the grid, and use the midpoint coordinates as the only laser measurement point in the grid; or, A laser measurement point located at the center among the multiple laser measurement points is used as the only laser measurement point in the grid, wherein the grid is any one of the first grids or any one of the second grids.

4. The system according to claim 1, characterized in that The second preprocessing comprises: Deleting the point cloud data corresponding to the semi-dynamic object from the second original point cloud data to obtain fifth point cloud data; A second coordinate point of the data acquisition device when acquiring the second original point cloud data is acquired, and all laser measurement points in the fifth point cloud data whose distance from the second coordinate point is greater than a third preset value are defined as the second point cloud data.

5. The system according to claim 1, characterized in that The encounter status includes four types, and the classification method is as follows: Step 21, taking the first driving route as the center line, setting the areas on both sides of the center line at a second preset distance from the center line as identification areas, wherein the identification areas are within the collection range of the data collection device; Step 22, determining whether the moving semi-dynamic object is located in the identification area, if so, proceeding to step 23, if not, proceeding to step 25; Step 23, calculating a first angle between the moving direction of the quadruped robot and the moving direction of the semi-dynamic object, and determining whether the first angle is within a first preset range, if so, setting the encounter state to the first type, if not, proceeding to step 24; Step 24, determining whether the first angle is within a second preset range, if so, setting the encounter state to the second type, if not, proceeding to step 25; Step 25: Obtain a second driving route of the mobile semi-dynamic object, and determine whether the second driving route has an intersection with the first driving route. If so, set the encounter state to the third type; if not, set the encounter state to the fourth type.

6. The system according to claim 5, characterized in that When the encounter state is the first type, performing navigation control on the quadruped robot includes: Step 231, determining whether the first speed of the mobile semi-dynamic object is greater than or equal to the second speed of the quadruped robot, if so, controlling the quadruped robot to travel along the first travel route at the second speed, if not, proceeding to step 232; Step 232, determining whether the first speed is greater than or equal to a first preset speed of the quadruped robot, if so, controlling the quadruped robot to travel along the first travel route at the first speed, if not, proceeding to step 233; Step 233: generate a new driving route for the quadruped robot, and control the quadruped robot to drive along the new driving route.

7. The system according to claim 5, characterized in that When the encounter state is the second type, the navigation control of the quadruped robot includes: generating a new driving route for the quadruped robot, and controlling the quadruped robot to drive according to the new driving route.

8. The system according to claim 5, characterized in that When the encounter state is the third type, performing navigation control on the quadruped robot includes: Step 251, defining the intersection as a meeting point, calculating a first time for the mobile semi-dynamic object to travel to the meeting point, and calculating a second time for the quadruped robot to travel to the meeting point; Step 252, calculating a first difference between the first time and the second time, and determining whether the first difference is greater than or equal to a preset time threshold; if so, controlling the quadruped robot to travel along the first driving route; if not, proceeding to step 253; Step 253: Obtain a second preset speed of the quadruped robot, and calculate a third time for the quadruped robot to travel to the meeting point along the first driving route at the second preset speed, wherein the second preset speed is the first preset speed or the third preset speed; Step 254, calculating a second difference between the first time and the third time, and determining whether the second difference is greater than or equal to the preset time threshold; if so, controlling the quadruped robot to travel along the first travel route at the second preset speed; if not, proceeding to step 255; Step 255: generate a new driving route for the quadruped robot, and control the quadruped robot to drive along the new driving route.

9. The system according to claim 5, characterized in that When the encounter state is the fourth type, the navigation control of the quadruped robot includes: controlling the quadruped robot to travel along the first driving route.

10. The system according to claim 5, characterized in that In the step 25, obtaining the second driving route of the mobile semi-dynamic object includes: obtaining historical operation data of the mobile semi-dynamic object within a first preset time, and generating the second driving route based on the historical operation data and the first operation data.

Images