Quadruped Robot Navigation System in a Semi-Dynamic Environment Based on a Hierarchical Localization Strategy

Abstract

The present invention belongs to the technical field of robotics, and discloses a quadruped robot navigation system in a semi-dynamic environment based on a hierarchical positioning strategy, including a data acquisition module that generates first point cloud data based on first raw point cloud data, a first map construction module that constructs a first map based on the first point cloud data and first inertial measurement unit data, a second map construction module that removes semi-dynamic objects from the first map to generate a second map, a navigation control module that plans the travel route of the quadruped robot based on the second map, a repositioning module that generates second point cloud data based on second raw point cloud data when a moving semi-dynamic object is detected and re-determines the position coordinates of the quadruped robot, and an obstacle perception module that classifies the encounter state based on first operation data and second operation data and performs navigation control according to the classification result. The technical solution of the present invention can improve the positioning accuracy and stability of the robot and guide the robot to quickly avoid obstacles.

Description

Technical Field

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

[0002] In hazardous chemical industrial parks and workshops, the normal operation of equipment must be based on a full understanding of the overall production situation. Otherwise, any mistake in any link may lead to catastrophic consequences. To ensure environmental and personnel safety and the normal progress of production, special personnel are 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 synchronous promotion of smart factories, intelligent production, and informatization, it has become an inevitable choice to conduct inspection management through inspection robots.

[0003] A similar prior art is the Chinese patent application with the publication number CN118192532A, which discloses a vision navigation method and device for a trackless inspection robot for a production workshop. The environment perception module acquires environmental image data, the landmark line detection module extracts landmark line features from the environmental image data to determine 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 obstacles, enabling the inspection robot to complete the inspection task. This method is based on images to obtain the positions of the robot and obstacles, and plans the driving route of the robot based on landmark lines, with low accuracy in route planning and positioning. There is also the Chinese patent application with the publication number CN112577491A, which discloses a robot path planning method based on an improved artificial potential field method. Step 1: Initialize the information of the logistics workshop, and determine the current position, target position, and positions of static and dynamic obstacles of the robot; Step 2: According to the A* algorithm, perform global path planning to obtain an initial path, and set each inflection point in the initial path as a sub-goal point; Step 3: Set the obstacles and the boundaries in the logistics workshop environment as the repulsive level, the sub-goal points as the attractive level, and the target point as the point with the minimum potential energy; Step 4: When the robot encounters an obstacle during driving, calculate the gravitational and repulsive forces received by the current position of the robot, and calculate the direction and magnitude of the resultant force, and guide the robot to drive to the next place according to the resultant force; Step 5: Optimize the repulsive force field function of the artificial potential field method, and introduce a robot collision coefficient into the artificial potential field method; Step 6: After the robot detects unknown dynamic obstacles around it using the sensor, use the principle of repulsion between the improved artificial potential field method and the obstacles for local obstacle avoidance; Step 7: After avoiding the obstacles, 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 received by the robot in the potential field, resulting in a long processing time.

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

[0005] In view of 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 includes: a data acquisition module, a first map construction module, a second map construction module, a navigation control module, a relocalization module, and an obstacle perception module;

[0007] The data acquisition module acquires first raw point cloud data and first inertial measurement unit data within a target area, performs a first preprocessing on the first raw point cloud data, and acquires first point cloud data;

[0008] The first map construction module constructs a first map of the target area based on the first point cloud data and the first inertial measurement unit data by using a graph optimization SLAM algorithm;

[0009] The second map construction module uses an object detection algorithm to detect and frame semi-dynamic objects in the first map with rectangular boxes, 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] The navigation control module plans a first driving route for the quadruped robot based on the second map and controls the quadruped robot to drive according to the first driving route;

[0011] The relocalization module performs obstacle detection during the driving process of the quadruped robot. When a moving semi-dynamic object is detected, it acquires second raw point cloud data and second inertial measurement unit data collected when the moving semi-dynamic object is detected, performs a second preprocessing on the second raw point cloud data to acquire 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 by using a graph optimization SLAM algorithm;

[0012] The obstacle perception module acquires first running data of the moving semi-dynamic object and second running data of the quadruped robot, classifies the encounter state between the moving semi-dynamic object and the quadruped robot based on the first running data and the second running 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: 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 where the distance from the first coordinate point is less than or equal to the first preset distance as the first area, and define the area where the distance from the acquisition point coordinate is greater than the first preset distance as the second area;

[0015] Step 12: Extract any area. If any area is the first area, go to Step 13; if any area is the second area, go to Step 14;

[0016] Step 13: Evenly divide the first area into N1 first grids according to the first preset value corresponding to the specific parameter. Extract any first grid. 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. After traversing all the first grids, obtain the third point cloud data;

[0017] Step 14: Evenly divide the second area into N2 second grids according to the second preset value corresponding to the specific parameter. Extract any second grid. 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. After traversing all the second grids, obtain the fourth point cloud data, where the second preset value is K1 times the first preset value, and K1 is a positive number greater than 1.

[0018] Specifically, the preset rule is:

[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] Use the laser measurement point at the center among multiple laser measurement points as the only laser measurement point in the grid, where the grid is any first grid or any second grid.

[0021] Specifically, the second preprocessing includes:

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

[0023] Obtain the second coordinate point of the data acquisition device when collecting the second original point cloud data, and define all laser measurement points in the fifth point cloud data whose distance from the second coordinate point is greater than the third preset value as the second point cloud data.

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

[0025] Step 21: Taking the first driving route as the center line, set the area on both sides of the center line at a second preset distance from the center line as the recognition area, and the recognition area is within the acquisition range of the data acquisition device;

[0026] Step 22: Determine whether the moving semi-dynamic object is within the recognition area. If so, proceed to Step 23; if not, proceed to Step 25;

[0027] Step 23: Calculate the first included angle between the movement direction of the quadruped robot and the movement direction of the moving semi-dynamic object, and determine whether the first included angle is within the first preset range. If so, set the encounter state as the first type; if not, proceed to Step 24;

[0028] Step 24: Determine whether the first included angle is within the second preset range. If so, set the encounter state as the second type; if not, proceed to Step 25;

[0029] Step 25: Obtain the second driving route of the moving semi-dynamic object, and determine whether there is an intersection between the second driving route and the first driving route. If so, set the encounter state as the third type; if not, set the encounter state as the fourth type.

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

[0031] 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 along the first driving route at the second speed; if not, proceed to Step 232;

[0032] 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 along the first driving route at the first speed; if not, proceed to Step 233;

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

[0034] Specifically, 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 travel 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: Define the intersection point as the meeting point, calculate the first time for the moving semi-dynamic object to travel to the meeting point, and calculate the second time for the quadruped robot to travel to the meeting point;

[0037] 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 travel route. If not, proceed to Step 253;

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

[0039] 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 along the first travel route at the second preset speed. If not, proceed to Step 255;

[0040] Step 255: Generate a new travel route for the quadruped robot, and control the quadruped robot to travel along the new travel route.

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

[0042] Specifically, in Step 25, obtaining the second travel route of the moving semi-dynamic object includes: obtaining the historical operation data of the moving semi-dynamic object within the first preset time, and generating the second travel 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 the map, first perform the first preprocessing on the collected point cloud data to generate the first point cloud data, then construct the first map based on the first point cloud data and the first inertial measurement unit data, and then perform hierarchical processing on the point cloud to exclude the interference of semi-dynamic objects and construct the second map containing only static features. Plan the first travel route of the quadruped robot based on the second map to improve the stability and robustness of route planning.

[0045] 2. When detecting a moving semi-dynamic object, perform the second preprocessing on the collected point cloud data to remove the point cloud corresponding to the semi-dynamic object, and use static features to locate the quadruped robot, improving the positioning accuracy and stability.

[0046] 3. During the navigation process, obtain the real-time status of moving semi-dynamic objects in the environment, classify the encounter status of the moving semi-dynamic objects and the quadruped robot based on the running data of the moving semi-dynamic objects and the running data of the quadruped robot, and perform navigation control according to the classification results, so as to accelerate the data processing speed while ensuring that the quadruped robot safely bypasses the moving semi-dynamic objects. Brief Description of the Drawings

[0047] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings described below are only the embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on the provided drawings without creative efforts.

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

[0049] In order to make the purpose, technical solutions and advantages of the present invention clearer, the following will further describe the present invention in detail in conjunction with the drawings and embodiments. Obviously, the specific embodiments described here are only used to explain the present invention, which 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 those of ordinary skill in the art without creative efforts belong to the protection scope 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 for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the quantity of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include at least one of such features. In addition, the technical solutions between the various embodiments can be combined with each other, but it must be based on the fact that those of ordinary skill in the art can implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the protection scope required by the present invention.

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

[0052] Figure 1 Shown is a schematic structural diagram 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. As Figure 1 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 relocalization module 50, and an obstacle perception module 60.

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

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

[0055] The second map construction module 30 detects semi-dynamic objects in the first map by using an object detection algorithm, frames them with rectangular boxes, 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 for the quadruped robot based on the second map, and controls the quadruped robot to drive according to the first driving route.

[0057] The relocalization module 50 performs obstacle detection during the driving process of the quadruped robot. When a moving semi-dynamic object is detected, it acquires second raw point cloud data and second inertial measurement unit data collected when the moving semi-dynamic object is detected, performs a second preprocessing on the second raw point cloud data to acquire second point cloud data, and then determines the position coordinates of the quadruped robot by using the graph optimization SLAM algorithm based on the second map, the second point cloud data, and the second inertial measurement unit data.

[0058] The obstacle perception module 60 acquires first running data of the moving semi-dynamic object and second running data of the quadruped robot, classifies the encounter states of the moving semi-dynamic object and the quadruped robot based on the first running data and the second running data, and performs navigation control according to the classification results.

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

[0060] Generally, when the semi-dynamic objects are in a non-operating state, they are all located at positions that do not interfere with the robot. Based on the second map that only contains static features, a first driving route is generated. The first driving route has a high utilization rate. When the semi-dynamic objects affect 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 the route planning.

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

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

[0063] Step 11: Obtain the first coordinate point of the data acquisition device when collecting the first raw point cloud data, and the first acquisition area corresponding to the first raw point cloud data. Divide the first acquisition area, and define the area with a distance less than or equal to the first preset distance from the first coordinate point as the first area, and define the area with a distance greater than the first preset distance from the acquisition point coordinate as the second area.

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

[0065] Step 13: Uniformly divide the first area 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. After traversing all the first grids, obtain the third point cloud data.

[0066] Step 14: Uniformly divide the second area 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. After traversing all the second grids, obtain the fourth point cloud data, where 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 the actual application scenario, and the embodiments of the present application do not limit this.

[0068] Preferably, the above-mentioned first grid and 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 region, and N2 is a positive integer determined according to the second preset value and the size of the second region.

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

[0070] The lidar point cloud of each object in the acquisition area is obtained through the data acquisition device. The closer to the data acquisition device, the greater the point cloud density, and the farther from the data acquisition device, the smaller the point cloud density. Generally speaking, the point cloud data collected by the data acquisition device is generally huge in quantity. In order to improve the processing speed of the point cloud data, it is necessary to perform thinning processing on the point cloud to 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. Subsequently, grid division is performed on the first area and the second area respectively, and the laser measurement points in each grid are thinned according to a preset rule. Among them, 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 this, the point cloud density in the first area after thinning processing can be made greater than that in the second area.

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

[0073] Specifically, the preset rule is:

[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] Use the laser measurement point located at the center among multiple laser measurement points as the only laser measurement point in the grid, where the grid is any first grid or any second grid.

[0076] Exemplarily, if there are 3 laser measurement points in the grid, then the midpoint coordinates of these 3 laser measurement points are (x1, y1, z1), (x2, y2, z2), (x3, y3, z3) are the coordinates of the above 3 laser measurement points respectively.

[0077] Exemplarily, there are three laser measurement points a1, a2, and a3 in the grid. Among them, a2 is the laser measurement point near / at the center, so a2 is used as the only laser measurement point in the grid.

[0078] Specifically, the second preprocessing includes:

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

[0080] Obtain the second coordinate point of the data acquisition device when collecting the second original point cloud data, and define all laser measurement points in the fifth point cloud data whose distance from the second coordinate point is greater than the third preset value as the 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 embodiments of the present application do not limit this. The third preset value and the second preset value may be equal or not equal.

[0082] When a moving semi-dynamic object is detected, delete the point cloud data corresponding to the semi-dynamic object from the second original point cloud data, and perform positioning on the quadruped robot 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 acquisition range of the data acquisition device (located at point P), A is close to the data acquisition device and B is far from the data acquisition device. Two laser measurement points (P1, P2) on both sides of A and two laser measurement points (P3, P4) 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 far-distance point cloud is smaller than the jitter error caused by the near-distance point cloud. Therefore, positioning the quadruped robot through the far-distance point cloud data can improve the positioning accuracy.

[0084] According to the technical solution of the present invention, when the robot is driving normally, it is positioned according to the general situation. When performing precise positioning on the quadruped robot based on the precise positioning opportunity, while reducing the workload of the robot and the amount of data processing, it can ensure the robot's precise obstacle avoidance. In addition, using the static features at a long distance for positioning improves the positioning accuracy and stability.

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

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

[0087] Step 21: Taking the first driving route as the center line, set the area on both sides of the center line at a second preset distance from the center line as the recognition area, and the recognition area is within the acquisition range of the data acquisition device.

[0088] Step 22: Determine whether the moving semi-dynamic object is within the recognition area. If so, proceed to Step 23; if not, proceed to Step 25.

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

[0090] Step 24: Determine whether the first included angle is within the second preset range. If so, set the encounter state as the second type; if not, proceed to Step 25.

[0091] Step 25: Obtain the second driving route of the moving semi-dynamic object, and determine whether there is an intersection between the second driving route and the first driving route. If so, set the encounter state as the third type; if not, set the encounter state as 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 the actual application scenario, 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 recognition area is located at the position coordinate, and the ending end is located at the edge of the acquisition range of the above data acquisition device.

[0094] Exemplarily, the first preset range is 0 degrees to 10 degrees, that is, the moving semi-dynamic object and the quadruped robot are traveling 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 are traveling in opposite directions.

[0096] If there is an intersection between the second driving route of the moving semi-dynamic object and the first driving route of the quadruped robot, it indicates that there may be mutual interference between the two; if there is no intersection between the second driving route of the moving semi-dynamic object and the first driving route of the quadruped robot, it indicates that there is no possibility of mutual interference between the two.

[0097] Specifically, when the encounter state is the first type, the 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 along the first travel route at the second speed. If not, proceed to 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 along the first travel route at the first speed. If not, proceed to Step 233.

[0100] Step 233: Generate a new travel route for the quadruped robot and control the quadruped robot to travel along the new travel route.

[0101] Preferably, the first preset speed is the minimum travel 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 the actual application scenario, and the embodiments of the present application do not limit this.

[0103] When the travel speed of the moving semi-dynamic object is within the allowable travel speed range of the quadruped robot, control the quadruped robot to follow the moving semi-dynamic object. If the travel speed of the moving semi-dynamic object is not within the allowable travel speed range of the quadruped robot, generate a new travel route 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 travel route for the quadruped robot and controlling the quadruped robot to travel along the new travel route.

[0105] When the moving semi-dynamic object and the quadruped robot are traveling towards each other and a collision cannot be avoided, directly generate a new travel route for the quadruped robot.

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

[0107] Step 251: Define the intersection point as the encounter point, calculate the first time for the moving semi-dynamic object to travel to the encounter point, and calculate the second time for the quadruped robot to travel to the encounter 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 the preset time threshold. If so, control the quadruped robot to travel along the first travel route. If not, proceed to Step 253.

[0109] Step 253: Obtain the second preset speed of the quadruped robot, and calculate the third time for the quadruped robot to travel to the meeting point along the first travel route at the second preset speed, where 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 along the first travel route at the second preset speed; if not, proceed to Step 255.

[0111] Step 255: Generate a new travel route for the quadruped robot, and control the quadruped robot to travel along the new travel route.

[0112] Preferably, the third preset speed is the maximum travel 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 the actual application scenario, and the embodiments of the present application do not limit this.

[0114] If the first difference or the second difference is greater than or equal to the preset time threshold, it indicates that the moving semi-dynamic object will not collide with the quadruped robot, so there is no need to generate a new travel route, and only the travel speed of the quadruped robot needs to be controlled.

[0115] Preferably, the travel speed of the quadruped robot can also 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 sequence of the two passing through the meeting point, as long as the quadruped robot does not collide with the moving semi-dynamic object when passing through the meeting point.

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

[0117] According to the technical solution of the present invention, the meeting types are classified, and then specific navigation control is performed on the quadruped robot based on specific meeting types, which can accelerate the data processing speed and reduce the reaction time while ensuring that the quadruped robot safely bypasses the moving semi-dynamic object.

[0118] Specifically, in Step 25, obtaining the second travel route of the moving semi-dynamic object includes: obtaining the historical operation data of the moving semi-dynamic object within the first preset time, and generating the second travel 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 the actual application scenario, and the embodiments of the present application do not limit this.

[0120] It should be understood that although the steps in the flowcharts of the embodiments of the present invention are shown in sequence according to the indications of the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless there is a clear description in this article, there is no strict order limit for the execution of these steps, and these steps can be executed in other orders. Moreover, at least a part of the steps in each embodiment may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed alternately or in turn with at least a part of other steps or sub-steps or stages of other steps.

[0121] Those of ordinary skill in the art can understand that all or part of the processes of implementing the methods in the above embodiments can be completed by instructing relevant hardware through a computer program. The above 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 methods. Among them, any reference to a memory, storage, database, or other medium used in the embodiments provided in the present application can include non-volatile and / or volatile memories. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of 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 (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and Rambus dynamic RAM (RDRAM), etc.

[0122] The above embodiments only express the preferred implementation modes of the present invention. The description is relatively specific and detailed, but it cannot be understood as a limitation on the scope of the patent of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present invention, several deformations and improvements can still be made, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention should be subject to the appended claims.

Claims

1. A quadruped robot navigation system in a semi-dynamic environment based on a hierarchical positioning strategy, characterized in that Including: A data acquisition module, a first map construction module, a second map construction module, a navigation control module, a relocalization module, and an obstacle perception module; The data acquisition module acquires first raw point cloud data and first inertial measurement unit data within a target area, performs first preprocessing on the first raw point cloud data, and acquires first point cloud data; The first map construction module constructs a first map of the target area based on the first point cloud data and the first inertial measurement unit data by using a graph optimization SLAM algorithm; The second map construction module uses an object detection algorithm to detect and frame semi-dynamic objects in the first map with rectangular boxes, 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; The navigation control module plans a first driving route for the quadruped robot based on the second map and controls the quadruped robot to drive according to the first driving route; The relocalization module, during the driving process of the quadruped robot, performs obstacle detection. When a moving semi-dynamic object is detected, it acquires second raw point cloud data and second inertial measurement unit data collected when the moving semi-dynamic object is detected, performs second preprocessing on the second raw point cloud data to acquire 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 by using the graph optimization SLAM algorithm; The obstacle perception module acquires first motion data of the moving semi-dynamic object and second motion data of the quadruped robot, classifies the encounter state between the moving semi-dynamic object and the quadruped robot based on the first motion data and the second motion data, and performs navigation control according to the classification result, where the first motion data and the second motion data respectively include the current position, driving speed, angular velocity, and acceleration; The second preprocessing includes: Deleting the point cloud data corresponding to the semi-dynamic object from the second raw point cloud data to acquire fifth point cloud data; Acquiring a second coordinate point of the data acquisition device when the second raw point cloud data is collected, and defining all laser measurement points in the fifth point cloud data whose distance from the second coordinate point is greater than a third preset value as the second point cloud data; The encounter state includes four types, and the classification method is: Step 21: Taking the first driving route as the center line, set an identification area with a second preset distance from the center line on both sides of the center line, and the identification area is within the acquisition range of the data acquisition device; Step 22: Determine whether the moving semi-dynamic object is within the identification area. If so, go to step 23; if not, go to step 25; Step 23: Calculate a first included angle between the motion direction of the quadruped robot and the motion direction of the moving semi-dynamic object, and determine whether the first included angle is within a first preset range. If so, set the encounter state to the first type; if not, go to step 24; Step 24: Determine whether the first included angle is within a second preset range. If so, set the encounter state to the second type; if not, proceed to Step 25; Step 25: Obtain the second travel route of the moving semi-dynamic object, and determine whether there is an intersection between the second travel route and the first travel route. If so, set the encounter state to the third type; if not, set the encounter state to the fourth type.

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: Obtain the first coordinate point of the data acquisition device when collecting the first raw point cloud data, and the first acquisition area corresponding to the first raw point cloud data. Divide the first acquisition area, and define the area with a distance less than or equal to the first preset distance from the first coordinate point as the first area, and define the area with a distance greater than the first preset distance from the acquisition point coordinate as the 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: Evenly divide the first area into N1 first grids according to the first preset value corresponding to a 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 a preset rule so that the number of laser measurement points in any first grid is 1. Repeat Step 13. After traversing all first grids, obtain the third point cloud data, where the specific parameter is volume or side length; Step 14: Evenly divide the second area 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. After traversing all second grids, obtain the fourth point cloud data, where 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 rule is: 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, Use the laser measurement point at the center among multiple laser measurement points as the only laser measurement point in the grid, where the grid is any first grid or any second grid.

4. The system according to claim 1, wherein When the encounter state is the first type, the navigation control of the quadruped robot includes: 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 along the first travel route at the second speed; if not, proceed to Step 232; 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 along the first travel route at the first speed. If not, proceed to Step 233; Step 233: Generate a new travel route for the quadruped robot and control the quadruped robot to travel along the new travel route.

5. The system according to claim 1, wherein When the encounter state is of the second type, the navigation control of the quadruped robot includes: generating a new travel route for the quadruped robot and controlling the quadruped robot to travel along the new travel route.

6. The system according to claim 1, characterized in that, When the encounter state is of the third type, the navigation control of the quadruped robot includes: Step 251: Define the intersection point as the encounter point, calculate the first time for the moving semi-dynamic object to travel to the encounter point, and calculate the second time for the quadruped robot to travel to the encounter point; 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 the preset time threshold. If so, control the quadruped robot to travel along the first travel route. If not, proceed to Step 253; Step 253: Obtain the second preset speed of the quadruped robot, and calculate the third time for the quadruped robot to travel to the encounter point along the first travel route at the second preset speed, where the second preset speed is the first preset speed or the third preset speed; 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 along the first travel route at the second preset speed. If not, proceed to Step 255; Step 255: Generate a new travel route for the quadruped robot and control the quadruped robot to travel along the new travel route.

7. The system according to claim 1, wherein 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 travel route.

8. The system according to claim 1, wherein In Step 25, the obtaining of the second travel route of the moving semi-dynamic object includes: obtaining the historical operation data of the moving semi-dynamic object within the first preset time, and generating the second travel route based on the historical operation data and the first operation data.

Images