Four-legged robot navigation obstacle avoidance and stair climbing method and system

Abstract

The application provides a quadruped robot navigation obstacle avoidance and stair climbing method and system, comprising the following steps: S1, establishing a quadruped robot preset unstructured environment scene model; S2, acquiring and analyzing surrounding environment information by an on-board binocular camera, establishing a global map containing semantic information and a high-precision local elevation map centered on the robot; S3, realizing autonomous navigation obstacle avoidance of the quadruped robot according to the global map and an A* path planning algorithm; and S4, realizing movement control and stair climbing of the quadruped robot according to asymmetric Actor-Critic network training and in combination with the global map and the local elevation map to plan a landing point. The application avoids instability, falling and other problems that may occur when the quadruped robot moves in an unstructured environment, effectively solves the safety hazards of the quadruped robot moving in a complex terrain, improves the movement ability of the quadruped robot, and ensures the safety and efficiency of specific task execution.

Description

Technical Field

[0001] This invention relates to the field of quadruped robot technology, specifically to a method and system for quadruped robot navigation, obstacle avoidance, and stair climbing, and more specifically to a method and system for quadruped robot navigation, obstacle avoidance, and stair climbing in an unstructured environment. Background Technology

[0002] Quadruped robots, as a typical type of legged robot, exhibit better performance in terms of mechanical structural stability and movement speed compared to other legged robots, while also possessing strong load-bearing capacity. Quadruped robots have the potential to assist or replace humans in performing complex tasks such as exploration, search and rescue, and transportation in unstructured environments, greatly increasing human opportunities to explore unexplored areas. Due to their excellent stability and versatility, quadruped robots have received widespread attention.

[0003] However, as a type of mobile robot, quadruped robots present operational challenges in kinematic and dynamic system simulation design due to the complexity of their mechanical coupling structure. From a dynamic perspective, a quadruped robot is a complex dynamically coupled system, requiring consideration of the coupling effect between the body and each leg during dynamic modeling. From a kinematic perspective, a quadruped robot can be considered as a mobile platform composed of multiple serially connected robotic arms in parallel. Furthermore, various external sensing devices for the quadruped robot must also be considered.

[0004] Autonomous navigation and dynamic obstacle avoidance are the foundation and prerequisites for the safe movement and motion of quadruped robots. Autonomous navigation enables real-time localization and autonomous map building. In unstructured indoor environments, complex scenarios such as arched passages, narrow alleys, lawns, and stairs require quadruped robots to accurately detect different obstacles in real time and at close range, and make timely path adjustments, which undoubtedly increases the difficulty of dynamic obstacle avoidance. Furthermore, uneven indoor lighting and numerous shadow areas cause differences in the exposure of various obstacle surfaces. Since cameras are easily affected by light, and may even fail to function properly in poor lighting conditions, this poses a challenge to the visual navigation of quadruped robots. While laser navigation can solve the problem of sensitivity to indoor light, most laser sensors are deployed at the top, increasing the robot's height and potentially creating blind spots for low-lying obstacles. The strategy of integrating visual and laser navigation offers a new approach, but the specific integration strategy must be designed specifically to address the complexity of the indoor environment, which is another challenge for the autonomous navigation of quadruped robots.

[0005] For quadruped robots climbing stairs, constructing a high-precision local elevation map is crucial. Current challenges in stair climbing include the risk of stepping on the edge of the stairs, causing instability, and repeatedly stepping on the same step. To enable quadruped robots to climb stairs smoothly and naturally, accurate estimation of the step width and height is necessary. In soft, uneven terrain, foot slippage and ground depressions can easily occur during dynamic foot-ground contact, causing temporary inconsistencies in the estimated height. Therefore, to ensure the overall stability of the quadruped robot's posture in these situations, it is also critical to construct a high-precision local elevation map in real-time and effectively from information obtained from cameras and LiDAR.

[0006] Patent document CN115840453B (application number: 202310098617.0) discloses a method, device, electronic device and medium for adaptive foot obstacle avoidance of a quadruped robot. The method includes the following steps: obtaining the terrain height of all reference ground points and obtaining the swing trajectory of the quadruped robot's foot based on a preset trajectory equation type; adjusting the movement trajectory of the quadruped robot's foot in real time according to the terrain height of the reference ground points and the swing trajectory, so that the quadruped robot's foot does not collide with the ground. Summary of the Invention

[0007] To address the shortcomings of existing technologies, the purpose of this invention is to provide a method and system for quadruped robot navigation, obstacle avoidance, and stair climbing.

[0008] A method for navigation, obstacle avoidance, and stair climbing of a quadruped robot according to the present invention includes:

[0009] Step S1: Establish a pre-defined unstructured environment scene model for the quadruped robot;

[0010] Step S2: The airborne binocular camera acquires and analyzes the surrounding environment information to build a global map containing semantic information and a high-precision local elevation map centered on the robot.

[0011] Step S3: The quadruped robot achieves autonomous navigation and obstacle avoidance based on the global map and the A* path planning algorithm;

[0012] Step S4: The quadruped robot is trained using an asymmetric Actor-Critic network and its footing points are planned using a global map and a local elevation map, thereby enabling the quadruped robot to perform motion control and climb stairs.

[0013] Preferably, step S2 employs:

[0014] Step S2.1: Use an airborne binocular camera to acquire RGB images and depth images of the surrounding environment;

[0015] Step S2.2: Based on the initial map template, classify, label, and verify the map using machine learning and manual methods to obtain a global map containing semantic information;

[0016] Step S2.3: Update all units based on robot motion data to create a high-precision local elevation map centered on the robot.

[0017] Preferably, step S2.2 employs the following:

[0018] Step S2.2.1: Perform processing on the sensor data, including sorting, classifying and cleaning, to obtain an initial map template without any semantic information or annotations;

[0019] Step S2.2.2: Detect static objects and classify and label them using machine learning and / or manual methods, including obstacles and stairs;

[0020] Step S2.2.3: Ensure the correctness of the automatic map creation process through manual error correction and troubleshooting;

[0021] Step S2.2.4: Publish a global map containing semantic information.

[0022] Preferably, step S2.3 employs the following:

[0023] Step S2.3.1: Update the estimated height and height variance of each unit based on the distance sensor data for units with new measurements;

[0024] Step S2.3.2: Update all units based on the robot motion data, keeping the local map centered on the robot, and obtain the estimated height and height covariance of each unit;

[0025] Step S2.3.3: Perform map data fusion on the target cells to obtain the estimated height and upper and lower confidence limits of each cell.

[0026] Preferably, step S2.3.1 employs the following method:

[0027] New measurements from the sensors are mapped onto the elevation map as points; a specific point will result in a cell (x, y) in the elevation map having a new elevation value. In the map coordinate system {M}, the altitude measurement values ​​are obtained through a Gaussian probability distribution. To estimate the position of a point P in the sensor coordinate system {S}. S r SP A single measurement can be converted into the corresponding height measurement in the map coordinate system {M};

[0028]

[0029] The projection matrix P = [0 0 1] maps the 3D measurements to height measurements; to obtain the variance of the height measurements... Derive the Jacobian matrix J for sensor measurements and sensor coordinate frame rotation. S and J Φ for

[0030]

[0031]

[0032] in:

[0033] Assumption but

[0034] variance The propagation error is

[0035]

[0036] Where, ∑ S This is the covariance matrix of the distance sensor model; The covariance matrix representing the sensor rotation amount;

[0037] Use a one-dimensional Kalman filter to process the height measurement value. and existing elevation map estimates Fusion:

[0038]

[0039] The superscript "+" indicates that the estimate was obtained after the update, and the superscript "-" indicates that the estimate was obtained before the update.

[0040] When multiple measurements at different heights fall on the same grid, the highest measurement will be merged based on Mahalanobis distance, and measurements that are a certain distance below the current estimate will be discarded. This method of merging multiple height measurements remains effective when an area is repeatedly measured or retraced.

[0041] Preferably, step S2.3.2 employs the following method:

[0042] When grid cell i receives a measurement update value, its covariance is set to...

[0043]

[0044] in, and It is an approximation of the horizontal uncertainty generated by grid discretization; for a square grid cell with side length d,

[0045] When the grid cells do not receive new measurements, the robot will continuously update the covariance matrix based on the transformation from the previous pose to the current pose. At time k, the map reference coordinate system {M} k} Through pose transformation The robot's current reference coordinate system can be obtained. At time k = 2, the estimated position of point P in the map coordinate system {M2} The estimated position of point P in map coordinate system {M1} when k=1 express

[0046]

[0047] Unify the coordinates to the reference coordinate system {M2} and represent them as follows:

[0048]

[0049] Without loss of generality, let the pose of the reference coordinate system {M2} be...

[0050]

[0051]

[0052] The reference coordinate systems {M2} and {M1} are aligned using the following definition.

[0053]

[0054]

[0055] At this point, a common reference coordinate system {M} is defined, and coordinate systems {M1} and {M2} are unified under a single coordinate system {M}. In this way, when updating the map, it is not necessary to move the data of each grid cell in the map; only the center position of the map needs to be changed as the robot moves.

[0056] for The propagation of covariance from k=1 to k=2 is as follows:

[0057]

[0058] Where, when k=1, the covariance ∑ P,1 By initialization assignment or assignment based on the last update; covariance ∑ r and ∑ Φ Representing the coordinate system and The uncertainty in motion estimation, i.e.

[0059]

[0060]

[0061] The Jacobian matrix is ​​calculated.

[0062]

[0063]

[0064]

[0065] For map coordinate system one, the error propagation formula needs to be rewritten; for the common reference coordinate system {M}, from time k to time k+1, point P on each grid i in the map i The covariance is

[0066]

[0067] in

[0068]

[0069] Preferably, step S2.3.3 employs the following method:

[0070] When a map is needed, the elevation map data of each grid cell will be used. Convert to h i,max and h i,min h represents the upper and lower confidence intervals of the height estimate. i,max and h i,min The value is inferred from the surrounding raster cell data, which falls within a 2σ elliptical confidence interval that meets preset conditions; the average height estimate is... The weighted average of the height values ​​within the confidence interval is obtained.

[0071]

[0072] Among them, h k ω represents the height value of the k-th raster cell before merging, and I represents the set of raster cells within the 2σ elliptical confidence interval of the i-th raster cell that satisfy the preset conditions; k This represents the probability weight of the k-th grid cell.

[0073] Preferably, step S4 employs:

[0074] Step M4.1: Use an asymmetric Actor-Critic reinforcement learning network to train the quadruped robot's motion and stair climbing, realize foot placement planning, and optimize it through the PPO algorithm;

[0075] Step M4.2: Use a context-aided estimation network architecture to learn latent representation information for state estimation and environment inference.

[0076] Preferably, step S4.2 employs the following:

[0077] The context-aided estimation network CENet uses a hybrid loss function for optimization, defined as follows:

[0078] L CE =L est +L VAE

[0079] Among them, L est L represents the body velocity estimate. VAE This represents the VAE loss value; for body velocity state estimation, the body velocity estimate is used. and the true value v t The mean squared error loss function between them, i.e.

[0080]

[0081] The VAE network is trained using standard β-VAE loss, which includes reconstruction loss and latent loss; mean squared error is used as the reconstruction loss, and KL divergence is used as the latent loss; therefore, the VAE loss is defined as follows:

[0082]

[0083] in, This is the next ontology-reconstructed observation. It is the potential state z t The posterior distribution value. Given and p(z) t The context prior distribution is parameterized by a Gaussian distribution; since all observations are normalized, the standard normal distribution is chosen as the prior distribution.

[0084] A quadruped robot navigation, obstacle avoidance, and stair climbing system according to the present invention includes:

[0085] Module M1: Establish a pre-defined unstructured environment scene model for the quadruped robot;

[0086] Module M2: The airborne binocular camera acquires and analyzes information about the surrounding environment, and establishes a global map containing semantic information and a high-precision local elevation map centered on the robot.

[0087] Module M3: The quadruped robot achieves autonomous navigation and obstacle avoidance based on a global map and the A* path planning algorithm;

[0088] Module M4: The quadruped robot is trained using an asymmetric Actor-Critic network and plans its footholds using a global map and a local elevation map, thereby enabling the quadruped robot to perform motion control and climb stairs.

[0089] Compared with the prior art, the present invention has the following beneficial effects:

[0090] 1. This invention constructs a global map containing semantic information and a high-precision local elevation map centered on the robot, enabling accurate perception and understanding of the surrounding environment, avoiding problems such as collisions and falls caused by map accuracy errors, and thus assisting the quadruped robot in navigation and movement in unstructured environments.

[0091] 2. This invention designs an asymmetric Actor-Critical reinforcement learning network for the complex structure of quadruped robots. It implicitly learns various terrain features, improves the robustness of quadruped robot movement in unstructured environments, effectively solves the safety hazards of quadruped robot movement in complex terrain, improves the movement capability of quadruped robots, and ensures the safety and efficiency of specific task execution. Attached Figure Description

[0092] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:

[0093] Figure 1 This is a typical unstructured environment scene for quadruped robots.

[0094] Figure 2 This is a flowchart illustrating a method for navigation, obstacle avoidance, and stair climbing of a quadruped robot in an unstructured environment.

[0095] Figure 3 A flowchart illustrating the process of constructing a high-precision local elevation map centered on a robot.

[0096] Figure 4 This is a flowchart illustrating the motion control and stair climbing process of a quadruped robot.

[0097] Figure 5 This is a schematic diagram of a quadruped robot climbing stairs.

[0098] Figure 6 This is a schematic diagram of a quadruped robot traversing a soft mat.

[0099] Figure 7 This is a schematic diagram of a quadruped robot navigating a narrow alley. Detailed Implementation

[0100] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.

[0101] Example 1

[0102] A method for navigation, obstacle avoidance, and stair climbing of a quadruped robot according to the present invention includes:

[0103] Step S1: Establish a typical unstructured environment scene model for the quadruped robot;

[0104] Specifically, step S1 includes:

[0105] In the typical unstructured environment scenario model of quadruped robots, the specific models include: Yushu Go1 quadruped robot, arched doorway, lawn, stairs, narrow alley, soft cushion, and empty pool.

[0106] The lawn is in the center of the scene model; the Yushu Go1 quadruped robot is to the right of the lawn; the archway is in front of the Yushu Go1 quadruped robot; the stairs are above the lawn; the narrow alley is to the left of the lawn; the cushions and empty pool are below the lawn; the cushions are to the left of the empty pool.

[0107] Step S2: The airborne binocular camera acquires and analyzes the surrounding environment information to build a global map containing semantic information and a high-precision local elevation map centered on the robot.

[0108] Specifically, step S2 includes:

[0109] Step S2.1: The binocular camera acquires RGB images and depth images of the surrounding environment;

[0110] Step S2.2: Establish a global map containing semantic information;

[0111] Specifically, step S2.2 includes:

[0112] Step S2.2.1: Data Processing. The sensor data is organized, classified, and cleaned to obtain an initial map template without any semantic information or annotations.

[0113] Step S2.2.2: Object Detection. Using machine learning, most common static objects are detected and classified, including obstacles, stairs, etc. A small number of complex objects are labeled manually.

[0114] Step S2.2.3: Manual verification. Manual error correction and troubleshooting ensure the automatic map creation process is correct and promptly identify any problems.

[0115] Step S2.2.4: Map Publish. Publish a global map containing semantic information.

[0116] Step S2.3: Establish a high-precision local elevation map centered on the robot.

[0117] Specifically, step S2.3 includes:

[0118] Step S2.3.1: Update the units with new measurement values ​​based on the distance sensor data to obtain the estimated height and height variance of each unit;

[0119] Specifically, step S2.3.1 includes:

[0120] New measurements from the sensors are mapped onto the elevation map as points. A specific point will result in a cell (x, y) on the elevation map having a new elevation value. In the map coordinate system {M}, the altitude measurement values ​​are obtained through a Gaussian probability distribution. To estimate the position of a point P in the sensor coordinate system {S}. S r SP A single measurement can be converted into the corresponding height measurement in the map coordinate system {M}.

[0121]

[0122] The projection matrix P =

[001] maps the 3D measurements to height measurements. To obtain the variance of the height measurements... Derive the Jacobian matrix J for sensor measurements and sensor coordinate frame rotation. S and J Φ for

[0123]

[0124]

[0125] in:

[0126] Assumption but

[0127] variance The propagation error is

[0128]

[0129] Where, ∑ SThis is the covariance matrix of the distance sensor model. The covariance matrix representing the sensor rotation amount (∑ IS (submatrix).

[0130] One-dimensional Kalman filtering can be used to convert height measurements. and existing elevation map estimates Fusion:

[0131]

[0132] The superscript "+" indicates that the estimate was obtained after the update, and the superscript "-" indicates that the estimate was obtained before the update.

[0133] When multiple measurements at different heights fall on the same grid (such as a vertical wall), the highest measurement is fused based on Mahalanobis distance, and measurements that are a certain distance below the current estimate are discarded. This method of fusing multiple height measurements remains effective when an area is repeatedly measured or retraced (such as when a robot turns or moves in the opposite direction).

[0134] Step S2.3.2: Update all units based on the robot motion data, keeping the local map centered on the robot, and obtain the estimated height and height covariance of each unit;

[0135] Specifically, step S2.3.2 includes:

[0136] When grid cell i receives a measurement update value, its covariance is set to...

[0137]

[0138] in and It is an approximation of the horizontal uncertainty generated by grid discretization, that is, for a square grid cell with side length d,

[0139] When the grid cells do not receive new measurements, the robot will continuously update the covariance matrix based on the transformation from the previous pose to the current pose. At time k, the map reference coordinate system {M} k} Through pose transformation The robot's current reference coordinate system can be obtained. At time k = 2, the estimated position of point P in the map coordinate system {M2} The position of point P in map coordinate system {M1} when k=1 can be estimated. express

[0140]

[0141] Unifying the coordinates to the reference coordinate system {M2} can be expressed as follows:

[0142]

[0143] Without loss of generality, the pose of the reference coordinate system {M2} can be set as follows:

[0144]

[0145]

[0146] The reference coordinate systems {M2} and {M1} can be aligned using the following definition.

[0147]

[0148]

[0149] At this point, a common reference coordinate system {M} can be defined, meaning that coordinate systems {M1} and {M2} are unified under a single coordinate system {M}. This way, when updating the map, it's unnecessary to move the data of each individual grid cell; only the center position of the map needs to be changed as the robot moves.

[0150] for The propagation of covariance from k=1 to k=2 is as follows:

[0151]

[0152] Where, when k=1, the covariance ∑ P,1 Values ​​are assigned either through initialization or based on the last update. Covariance ∑ r and ∑ Φ Representing the coordinate system and The uncertainty in motion estimation, i.e.

[0153]

[0154]

[0155] The Jacobian matrix is ​​calculated.

[0156]

[0157]

[0158]

[0159] For the map coordinate system 1, the error propagation formula needs to be rewritten. For the common reference coordinate system {M}, from time k to time k+1, point P on each grid i in the map... i The covariance is

[0160]

[0161] in

[0162]

[0163] Step S2.3.3: Perform map data fusion on the target cells to obtain the estimated height and upper and lower confidence limits of each cell.

[0164] Specifically, step S2.3.3 includes:

[0165] Map fusion is the process of combining the elevation map data of each raster when a map is needed. Convert to h i,max and h i,min This represents the upper and lower confidence intervals of the height estimate. i,max and h i,min The value is inferred from surrounding raster cell data that falls within a 95% 2σ elliptical confidence interval. Mean height estimate. The weighted average of the height values ​​within the confidence interval is obtained.

[0166]

[0167] Among them, h k Let ω represent the height value of the k-th raster cell before merging, and I represent the set of raster cells within the 95% 2σ elliptic confidence interval of the i-th raster cell. k This represents the probability weight of the k-th grid cell.

[0168] Step S3: The quadruped robot achieves autonomous navigation and obstacle avoidance based on the global map and the A* path planning algorithm;

[0169] Specifically, step S3 includes:

[0170] Step S3.1: Set the location of the navigation target point;

[0171] Step S3.2: Calculate the path cost corresponding to the surrounding map units based on the global map information;

[0172] Step S3.3: Use the A* algorithm to calculate the optimal path and achieve navigation and obstacle avoidance.

[0173] Step S4: The quadruped robot is trained using an asymmetric Actor-Critic network and its foot placement is planned using a combination of global and local maps, thereby enabling motion control and stair climbing for the quadruped robot.

[0174] Specifically, step S4 includes:

[0175] Step S4.1: Use an asymmetric Actor-Critic reinforcement learning network to train the quadruped robot's motion and stair climbing, realize foot placement planning, and optimize it through the PPO algorithm;

[0176] Specifically, step S4.1 includes:

[0177] An asymmetric Actor-Critic architecture is employed to implicitly learn terrain features. A robust motion policy is learned through the interaction between the policy and the value network, which implicitly imagines privileged information within a given time. The policy (Actor) receives partial observations. As input, the value network (Critic) receives the complete state s. t Furthermore, the policy was optimized using the Proximity Policy Optimization (PPO) algorithm.

[0178] A game-inspired curriculum was used to achieve progressive motion strategy learning in complex terrain. The terrain consisted of smooth, rough, discretized, and stair-like terrain with slopes within [0°, 22°]. Furthermore, a grid-adaptive curriculum was used for low-speed movement, further improving turning speed and stability.

[0179] Step S4.2: Use a Context-Aided Estimation Network (CENet) architecture to learn latent representation information for state estimation and environment inference.

[0180] Specifically, step S4.2 includes:

[0181] The context-aided estimation network CENet uses a hybrid loss function for optimization, defined as follows:

[0182] L CE =L est +L VAE

[0183] Where L est L represents the body velocity estimate. VAE This represents the VAE loss value. For body velocity state estimation, the body velocity estimate is used. and the true value v t The mean squared error loss function between them, i.e.

[0184]

[0185] The VAE network is trained using standard β-VAE loss, which includes reconstruction loss and latent loss. Mean squared error is used as the reconstruction loss, and KL divergence is used as the latent loss. Therefore, the VAE loss is defined as follows:

[0186]

[0187] in This is the next ontology-reconstructed observation. It is the potential state z t The posterior distribution value. Given and p(z) t The context prior distribution is parameterized by a Gaussian distribution. Since all observations are normalized, the standard normal distribution is chosen as the prior distribution.

[0188] The typical unstructured environment scene model of the quadruped robot includes a quadruped robot body model, a specific scene model in the environment, and its operating status.

[0189] First, a global environment map is proposed based on the three-dimensional information of the surrounding environment, while the local elevation map is obtained by updating and fusing distance sensor information and robot body kinematic data. The combination of the two enables accurate navigation and obstacle avoidance in unstructured environments.

[0190] Based on the asymmetric Actor-Critic reinforcement learning algorithm, this invention achieves robust control of a quadruped robot. The ultimate goal of this invention is to understand the surrounding environment through environmental mapping and reinforcement learning algorithms, and to selectively choose appropriate motion control schemes.

[0191] The present invention also provides a quadruped robot navigation, obstacle avoidance and stair climbing system. The quadruped robot navigation, obstacle avoidance and stair climbing system can be implemented by executing the process steps of the quadruped robot navigation, obstacle avoidance and stair climbing method. That is, those skilled in the art can understand the quadruped robot navigation, obstacle avoidance and stair climbing method as a preferred embodiment of the quadruped robot navigation, obstacle avoidance and stair climbing system.

[0192] Example 2

[0193] Example 2 is a preferred example of Example 1.

[0194] The following preferred embodiments further illustrate the present invention in detail:

[0195] like Figure 2 As shown, this embodiment discloses a method for navigation, obstacle avoidance, and stair climbing of a quadruped robot in an unstructured environment, including the following steps S00 to S30:

[0196] S00. Establish typical unstructured environmental scenarios for quadruped robots, such as... Figure 1 As shown, the scenes include stairs, narrow alleys, lawns, and soft cushions;

[0197] S10. Analyze the surrounding environment information and establish a global map containing semantic information and a high-precision local elevation map centered on the robot.

[0198] S20. Perform path planning based on the global map to enable the quadruped robot to navigate and avoid obstacles autonomously;

[0199] S30. By using reinforcement learning algorithms to plan foot placement points, the quadruped robot can achieve motion control and climb stairs.

[0200] Specifically, the establishment of a typical unstructured environment scenario for a quadruped robot is as follows:

[0201] In the typical unstructured environment scenario model of quadruped robots, the specific models include: Yushu Go1 quadruped robot, arched doorway, lawn, stairs, narrow alley, soft cushion, and empty pool.

[0202] It should be noted that, as Figure 1 As shown, the lawn is in the center of the scene model; the Yushu Go1 quadruped robot is to the right of the lawn; the arched gate is in front of the Yushu Go1 quadruped robot; the stairs are above the lawn; the narrow alley is to the left of the lawn; the cushion and the empty pool are below the lawn; the cushion is to the left of the empty pool.

[0203] Specifically, the process of analyzing the surrounding environmental information is as follows:

[0204] The quadruped robot's binocular camera scans the 3D information of the surrounding environment. Combined with the robot's built-in encoder and inertial sensors, it collects pose information for the robot to build maps.

[0205] It should be noted that the quadruped robot's binocular camera can be used to acquire RGB images and point cloud images of the surrounding environment, and thus obtain depth information of surrounding objects.

[0206] Specifically, such as Figure 3 As shown, the process of creating a high-precision local elevation map centered on the robot includes the following steps S100 to S102:

[0207] S100. Update the units with new measurements based on the distance sensor data to obtain the estimated height and height variance for each unit, including:

[0208] New measurements from the sensors are mapped onto the elevation map as points. A specific point will result in a cell (x, y) on the elevation map having a new elevation value. In the map coordinate system {M}, the altitude measurement values ​​are obtained through a Gaussian probability distribution. To estimate the position of a point P in the sensor coordinate system {S}. S r SP A single measurement can be converted into the corresponding height measurement in the map coordinate system {M}.

[0209]

[0210] The projection matrix P =

[001] maps the 3D measurements to height measurements. To obtain the variance of the height measurements... Derive the Jacobian matrix J for sensor measurements and sensor coordinate frame rotation. S and J Φ for

[0211]

[0212]

[0213] in:

[0214] Assumption but

[0215] variance The propagation error is

[0216]

[0217] Where, ∑ S This is the covariance matrix of the distance sensor model. The covariance matrix representing the sensor rotation amount (∑ IS (submatrix).

[0218] One-dimensional Kalman filtering can be used to convert height measurements. and existing elevation map estimates Fusion:

[0219]

[0220] The superscript "+" indicates that the estimate was obtained after the update, and the superscript "-" indicates that the estimate was obtained before the update.

[0221] It should be noted that when multiple measurements at different heights fall on the same grid (such as a vertical wall), the highest measurement will be fused based on Mahalanobis distance, and measurements that are a certain distance below the current estimate will be discarded. This method of fusing multiple height measurements remains effective when an area is repeatedly measured or retraced (such as when the robot turns or moves in the opposite direction).

[0222] S101. Update all units based on the robot's motion data, keeping the local map centered on the robot, and obtain the estimated height and height covariance for each unit, including:

[0223] When grid cell i receives a measurement update value, its covariance is set to...

[0224]

[0225] in and It is an approximation of the horizontal uncertainty generated by grid discretization, that is, for a square grid cell with side length d,

[0226] When the grid cells do not receive new measurements, the robot will continuously update the covariance matrix based on the transformation from the previous pose to the current pose. At time k, the map reference coordinate system {Mk} undergoes a pose transformation. The robot's current reference coordinate system can be obtained. At time k = 2, the estimated position of point P in the map coordinate system {M2} The position of point P in map coordinate system {M1} when k=1 can be estimated. express

[0227]

[0228] Unifying the coordinates to the reference coordinate system {M2} can be expressed as follows:

[0229]

[0230] Without loss of generality, the pose of the reference coordinate system {M2} can be set as follows:

[0231]

[0232]

[0233] The reference coordinate systems {M2} and {M1} can be aligned using the following definition.

[0234]

[0235]

[0236] At this point, a common reference coordinate system {M} can be defined, meaning that coordinate systems {M1} and {M2} are unified under a single coordinate system {M}. This way, when updating the map, it's unnecessary to move the data of each individual grid cell; only the center position of the map needs to be changed as the robot moves.

[0237] for The propagation of covariance from k=1 to k=2 is as follows:

[0238]

[0239] Where, when k=1, the covariance ∑ P,1 Values ​​are assigned either through initialization or based on the last update. Covariance ∑ r and ∑ Φ Representing the coordinate system and The uncertainty in motion estimation, i.e.

[0240]

[0241]

[0242] The Jacobian matrix is ​​calculated.

[0243]

[0244]

[0245]

[0246] For the map coordinate system 1, the error propagation formula needs to be rewritten. For the common reference coordinate system {M}, from time k to time k+1, point P on each grid i in the map... i The covariance is

[0247]

[0248] in

[0249]

[0250] S102. Perform map data fusion on the target cells to obtain the estimated height and upper and lower confidence limits of each cell, including:

[0251] Map fusion is the process of combining the elevation map data of each raster when a map is needed. Convert to h i,max and h i,min This represents the upper and lower confidence intervals of the height estimate. i,max and h i,minThe value is inferred from surrounding raster cell data that falls within a 95% 2σ elliptical confidence interval. Mean height estimate. The weighted average of the height values ​​within the confidence interval is obtained.

[0252]

[0253] It should be noted that, among them, h k Let ω represent the height value of the k-th raster cell before merging, and I represent the set of raster cells within the 95% 2σ elliptic confidence interval of the i-th raster cell. k This represents the probability weight of the k-th grid cell.

[0254] Specifically, such as Figure 4 As shown, the process of realizing the motion control and stair climbing of the quadruped robot includes the following steps S200 to S304:

[0255] S200: Based on the global map, path planning is performed to achieve localization and navigation of the quadruped robot, including:

[0256] First, the location of the navigation target point is set. Then, the path cost corresponding to the surrounding map units is calculated based on the global map information. The A* algorithm is then used to calculate the optimal path to achieve navigation and obstacle avoidance.

[0257] S103. Determine the type of obstacle in front of you based on the semantic map and select the corresponding motion mode, including:

[0258] Sensor data is processed, classified, and cleaned to obtain an initial map template without any semantic information or annotations. Machine learning is used to detect and classify most common static objects, including obstacles and stairs. A small number of complex objects are manually labeled. Manual error correction and review ensure the accuracy of the automated map creation process and promptly identify any issues. Finally, a global map containing semantic information is published.

[0259] It should be noted that, based on semantic map information, the quadruped robot will select different movement modes to adapt to different terrains in a targeted manner.

[0260] S300 can traverse obstacles such as lawns, stairs, and mats, including:

[0261] When faced with obstacles that can be crossed, the quadruped robot will choose to cross them directly, maintaining its posture stability during movement through an adaptive control algorithm.

[0262] like Figure 5 As shown, a quadruped robot climbs stairs. Figure 6 As shown, the quadruped robot traverses the soft mat.

[0263] It should be noted that when encountering complex obstacles such as stairs, the quadruped robot will adjust the angle of its torso during movement to make it parallel to the slope, thereby increasing its climbing speed.

[0264] S301. An asymmetric Actor-Critic reinforcement learning network is used to implement foot placement planning for a quadruped robot, including:

[0265] An asymmetric Actor-Critic architecture is employed to implicitly learn terrain features. A robust motion policy is learned through the interaction between the policy and the value network, which implicitly imagines privileged information within a given time. The policy (Actor) receives partial observations. As input, the value network (Critic) receives the complete state s. t Furthermore, the policy was optimized using the Proximity Policy Optimization (PPO) algorithm.

[0266] It should be noted that a game-inspired learning curriculum was used to achieve progressive motion strategy learning in complex terrain. The terrain consists of smooth, rough, discretized, and stair-like terrain with slopes within [0°, 22°]. Furthermore, a grid-adaptive curriculum was used for low-speed movement, further improving turning speed and stability.

[0267] S302. Use context-aided estimation network architecture to learn latent representation information for state estimation and environment inference, including:

[0268] The context-aided estimation network CENet uses a hybrid loss function for optimization, defined as follows:

[0269] L CE =L est +L VAE

[0270] It should be noted that L est L represents the estimate of body velocity. VAE This represents the VAE loss value. For body velocity state estimation, the body velocity estimate is used. and the true value v t The mean squared error loss function between them, i.e.

[0271]

[0272] The VAE network is trained using standard β-VAE loss, which includes reconstruction loss and latent loss. Mean squared error is used as the reconstruction loss, and KL divergence is used as the latent loss. Therefore, the VAE loss is defined as follows:

[0273]

[0274] It should be noted that, among them This is the next ontology-reconstructed observation. It is the potential state z t The posterior distribution value. Given and p(z) t The context prior distribution is parameterized by a Gaussian distribution. Since all observations are normalized, the standard normal distribution is chosen as the prior distribution.

[0275] S303. Obstacles that cannot be crossed, such as walls, including:

[0276] The insurmountable obstacle was much taller than the robot itself, making it impossible for the quadruped robot to climb over it.

[0277] like Figure 7 As shown, the quadruped robot avoids walls and traverses a narrow alley.

[0278] It should be noted that when a quadruped robot approaches an obstacle, the binocular camera usually cannot obtain all the information about the object. Therefore, the judgment is made based on the semantic map annotation information.

[0279] S304. Use the A* algorithm to calculate the optimal path and implement dynamic navigation and obstacle avoidance, including:

[0280] The formula for calculating the priority of each node using the A* algorithm is:

[0281] f(n) = g(n) + h(n)

[0282] It should be noted that f(n) is the overall priority of node n. When selecting the next node to traverse, the node with the highest overall priority (smallest value) will always be selected. g(n) is the cost of node n from the starting point. h(n) is the expected cost of node n from the ending point, which is the heuristic function of the A* algorithm.

[0283] It should be further explained that during the operation of the A* algorithm, the node with the smallest f(n) value (highest priority) is selected from the priority queue each time as the next node to be traversed.

[0284] This invention constructs a navigation, obstacle avoidance, and stair-climbing system for quadruped robots by modeling typical unstructured environmental scenarios. Through the construction of global and local maps, it further enhances the quadruped robot's perception of its surroundings, enabling it to adopt different movement modes for different environmental scenarios. Therefore, this invention effectively identifies the environmental scenario model surrounding the quadruped robot, provides reliable algorithmic support for robust quadruped robot movement, effectively solves the movement hazards of quadruped robots in complex terrain, and improves the movement capability and efficiency of quadruped robots.

[0285] Those skilled in the art will understand that, besides implementing the system and its various devices, modules, and units provided by this invention in the form of purely computer-readable program code, the same functions can be achieved entirely through logical programming of the method steps, making the system and its various devices, modules, and units of this invention function in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers. Therefore, the system and its various devices, modules, and units provided by this invention can be considered as a hardware component, and the devices, modules, and units included therein for implementing various functions can also be considered as structures within the hardware component; alternatively, the devices, modules, and units for implementing various functions can be considered as both software modules implementing the method and structures within the hardware component.

[0286] In the description of this application, it should be understood that the terms "upper", "lower", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this application and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this application.

[0287] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A method for navigation, obstacle avoidance, and stair climbing of a quadruped robot, characterized in that, include: Step S1: Establish a pre-defined unstructured environment scene model for the quadruped robot; Step S2: The airborne binocular camera acquires and analyzes the surrounding environment information to build a global map containing semantic information and a high-precision local elevation map centered on the robot. Step S3: The quadruped robot achieves autonomous localization and navigation based on the global map and the A* path planning algorithm, and determines the type of obstacle in front of it based on the semantic map and selects the corresponding movement mode; Step S4: When the obstacle in front is an traversable obstacle, the quadruped robot is trained based on the asymmetric Actor-Critic network and plans the landing points by combining the global map and the local elevation map, thereby realizing the motion control of the quadruped robot and climbing stairs; when the obstacle in front is an insurmountable obstacle, the A* algorithm is used to calculate the optimal path to realize dynamic navigation and obstacle avoidance. Step S2 employs the following: Step S2.1: Use an airborne binocular camera to acquire RGB images and depth images of the surrounding environment; Step S2.2: Based on the initial map template, classify, label, and verify the map using machine learning and manual methods to obtain a global map containing semantic information; Step S2.3: Update all units based on robot motion data to create a high-precision local elevation map centered on the robot; Step S2.3 adopts the following: Step S2.3.1: Update the estimated height and height variance of each unit based on the distance sensor data for units with new measurements; Step S2.3.2: Update all units based on the robot motion data, keeping the local map centered on the robot, and obtain the estimated height and height covariance of each unit; Step S2.3.3: Perform map data fusion on the target cells to obtain the estimated height and upper and lower confidence limits of each cell; Step S4 employs the following: Step M4.1: Use an asymmetric Actor-Critic reinforcement learning network to train the quadruped robot's motion and stair climbing, realize foot placement planning, and optimize it through the PPO algorithm; Step M4.2: Use a context-aided estimation network architecture to learn latent representations for state estimation and environment inference; Step S4.2 adopts the following: The context-aided estimation network CENet uses a hybrid loss function for optimization, defined as follows: in, This represents an estimate of body speed. This represents the VAE loss value; for body velocity state estimation, the body velocity estimate is used. and the true value The mean squared error loss function between them, i.e. VAE networks are based on standards The VAE loss is used for training and includes reconstruction loss and latent loss; mean squared error is used as the reconstruction loss, and KL divergence is used as the latent loss; therefore, the VAE loss is defined as follows: in, This is the next ontology-reconstructed observation. It is a potential state The posterior distribution value; given and The context prior distribution is parameterized by the Gaussian distribution; since all observations are normalized, the standard normal distribution is chosen as the prior distribution. Step S2.2 adopts the following: Step S2.2.1: Perform processing on the sensor data, including sorting, classifying and cleaning, to obtain an initial map template without any semantic information or annotations; Step S2.2.2: Detect static objects and classify and label them using machine learning and / or manual methods, including obstacles and stairs; Step S2.2.3: Ensure the correctness of the automatic map creation process through manual error correction and troubleshooting; Step S2.2.4: Publish a global map containing semantic information. The global map containing semantic information is used to determine the type of obstacle in front of the camera in order to select the corresponding motion mode.

2. The quadruped robot navigation, obstacle avoidance, and stair climbing method according to claim 1, characterized in that, Step S2.3.1 adopts the following: New measurements from the sensors are mapped onto the elevation map as points; a specific point will result in a cell on the elevation map. Have a new height value In map coordinate system In the middle, the height measurement values ​​are obtained through a Gaussian probability distribution. To estimate; for the sensor coordinate system a certain point below Location A single measurement can be converted into a map coordinate system. The corresponding height measurement value; Wherein, projection matrix Mapping 3D measurements to height measurements; in order to obtain the variance of the height measurements. Derive the Jacobian matrix of sensor measurements and sensor coordinate frame rotation. and for in: Assumption ,but variance The propagation error is in, This is the covariance matrix of the distance sensor model; The covariance matrix representing the sensor rotation amount; Use a one-dimensional Kalman filter to process the height measurement value. and existing elevation map estimates Fusion: Among them, superscript Indicates that the estimated value was obtained after the update, indicated by the superscript. This indicates that the estimated value was obtained before the update; When multiple measurements at different heights fall on the same grid, the highest measurement will be merged based on Mahalanobis distance, and measurements that are a certain distance below the current estimate will be discarded. This method of merging multiple height measurements remains effective when an area is repeatedly measured or retraced.

3. The quadruped robot navigation, obstacle avoidance, and stair climbing method according to claim 1, characterized in that, Step S2.3.2 adopts the following: When grid unit When a measurement update value is received, its covariance is set to... in, and It is an approximation of the horizontal uncertainty generated by raster discretization; for a side length of... Square grid cells, ; When the grid cells do not receive new measurements, the robot will continuously update the covariance matrix based on the transformation from the previous pose to the current pose. ; in time At that time, map reference coordinate system Through pose transformation The robot's current reference coordinate system can be obtained. ; in time At time, point In map coordinate system The estimated position below Depend on Time Point In map coordinate system The estimated position below express Unify coordinates to the reference coordinate system The following is represented as Without loss of generality, a reference coordinate system is set. The position is Align the reference coordinate system using the following definition. and At this point, a common reference coordinate system is defined. coordinate system and Unified in a coordinate system This way, when updating the map, it is not necessary to move the data of each grid cell in the map; only the center position of the map needs to be changed as the robot moves. for covariance from arrive The spread of Among them At that time, covariance Covariance is determined by initialization or based on the previous update. and Representing the coordinate system and The uncertainty in motion estimation, i.e. The Jacobian matrix is ​​calculated. For map coordinate system 1, the error propagation formula needs to be rewritten; for common reference coordinate system... From time At the time Each grid in the map On point The covariance is in 。 4. The method for navigation, obstacle avoidance, and stair climbing of a quadruped robot according to claim 1, characterized in that, Step S2.3.3 adopts the following: When a map is needed, the elevation map data of each grid cell will be used. Convert to ; and This represents the upper and lower confidence intervals of the height estimate; and The value is inferred from the surrounding raster cell data, which fall within a preset condition. Within the elliptical confidence interval; mean height estimate The weighted average of the height values ​​within the confidence interval is obtained. in, Indicates the first The height value of each grid cell before merging. Indicates the first Each grid cell meets the preset conditions. The set of grid cells within an elliptical confidence interval; Indicates the first The probability weight of each grid cell.

5. A quadruped robot navigation, obstacle avoidance, and stair climbing system, used to execute the quadruped robot navigation, obstacle avoidance, and stair climbing method as described in claim 1, characterized in that, include: Module M1: Establish a pre-defined unstructured environment scene model for the quadruped robot; Module M2: The airborne binocular camera acquires and analyzes information about the surrounding environment, and establishes a global map containing semantic information and a high-precision local elevation map centered on the robot. Module M3: The quadruped robot achieves autonomous localization and navigation based on the global map and A* path planning algorithm, and determines the type of obstacle in front of it based on the semantic map and selects the corresponding movement mode; Module M4: When the obstacle in front is an traversable obstacle, the quadruped robot is trained based on an asymmetric Actor-Critic network and plans its footing points by combining a global map and a local elevation map, thereby realizing the quadruped robot's motion control and climbing stairs; when the obstacle in front is an insurmountable obstacle, the A* algorithm is used to calculate the optimal path to achieve dynamic navigation and obstacle avoidance.

Images