Humanoid robot three-dimensional complex environment navigation method and system

Abstract

The invention relates to a humanoid robot three-dimensional complex environment navigation method and system, and belongs to the technical field of robot navigation. Comprises: obtaining a random path node; constructing a collision detection model according to the gait cycle of the robot; detecting whether the random path node passes collision detection by using a collision detection model; if not, deleting the random path node, and obtaining the random path node again; otherwise, adding the random path nodes into the random tree; for each node added to the random tree, collision detection and plane size detection at the node are carried out, and when the detection is passed, candidate nodes are added to a global path point sequence; screening the sequence according to the complexity of the current plane to obtain complex region path nodes and flat region path nodes; and corresponding motion instructions are generated according to the operation requirements of the robot on the path nodes in the different areas. According to the method, the dynamic envelope space calculation cost is reduced, and the global path planning accuracy is improved.

Description

Technical Field

[0001] This invention relates to the field of robot navigation technology, and in particular to a method and system for navigating humanoid robots in complex three-dimensional environments. Background Technology

[0002] Humanoid robot technology is experiencing explosive growth, moving from the laboratory to complex real-world scenarios. Recent advancements in humanoid robot gait pattern generation technology emphasize the importance of trajectory planning and execution for achieving stable and efficient movement. However, traditional wheeled robots' planar path planning strategies struggle to handle unstructured terrain such as stairs, ramps, and discrete footholds, while humanoid robots, due to their high degrees of freedom, exhibit greater agility and superior mobility in everyday environments. Compared to wheeled robots, their bipedal locomotion allows them to climb stairs, overcome obstacles, and traverse complex 3D environments such as discrete footholds. However, robust navigation in 3D environments remains a core challenge, as traditional wheeled robots' planar path planning strategies are ill-suited to unstructured terrain like stairs, ramps, and discrete footholds. Key breakthroughs in this field focus on the integration of humanoid motion planning and control, achieving human-like obstacle-crossing capabilities through dynamic gait adjustment, terrain-adaptive foothold decision-making, and full-body coordinated control.

[0003] Traditional navigation algorithms have significant limitations in addressing the unique 3D motion challenges of humanoid robots. Traditional 2D or 2.5D path planners (such as RRT and A*) struggle to adequately handle discrete height variations, such as steps, climbable platforms, and obstacles that can be traversed. Their node generation and collision detection mechanisms often lack vertical adaptability, failing to reliably support robots stepping up, down, and climbing structures. Furthermore, most existing methods focus solely on the accessibility and collision-free nature of path nodes, neglecting rigorous evaluation of the support area's size, flatness, and continuity. This leads to theoretical paths failing in practice due to infeasible landing points. During humanoid robot movement, the trajectory of the swinging leg may exceed its static geometric contours, increasing the risk of collisions with obstacles. Accurately calculating such dynamic envelope spaces is computationally expensive, making it difficult to meet real-time navigation requirements. Moreover, effective global path planning in 3D environments containing climbable structures (such as steps and platforms) requires algorithms that can simultaneously handle continuous surfaces and discrete height variations.

[0004] Currently, existing technologies have made some progress in humanoid robot motion planning. For example, the A* gait planner based on an energy consumption model reduces the number of iterations by 19.7% by incorporating foot angle differences into the heuristic function, while maintaining the trajectory cost with an increase of only 0.41%. Furthermore, a method fusing unscented Kalman filtering with gait control utilizes visual landmark detection to achieve real-time pose correction, keeping trajectory deviation within 15 cm. A trajectory generation method based on cubic spline interpolation, combined with a zero-moment point stability criterion, is used for stepping on / off obstacles of known size, and its dynamic stability has been verified through offline simulation and physical experiments on the BHR-2 platform. However, these methods still have shortcomings. For example, collision detection in 3D environments is often overly simplified or conservative, failing to effectively simulate the actual motion space of the humanoid robot's swinging leg during actions such as crossing and climbing stairs, leading to impractical planned paths or omission of feasible passages. Most path planners do not fully consider the actual supportability of landing points and differences in terrain complexity when generating nodes, making it difficult for subsequent gait controllers to directly utilize them or requiring extensive post-processing. Furthermore, the path planning phase fails to fully integrate the robot's specific gait constraints and the differences in control strategies required for different terrains, often resulting in complex transformations or adaptations to generate an executable gait. Summary of the Invention

[0005] Therefore, the technical problem to be solved by the present invention is to overcome the problems of overly simplified collision detection, insufficient consideration of foothold support, inadequate integration of gait constraints and terrain strategies, high computational cost of dynamic envelope space, and low accuracy of global path planning in the prior art.

[0006] In a first aspect, to solve the above-mentioned technical problems, the present invention provides a method for navigating a humanoid robot in a three-dimensional complex environment, comprising: S1. Obtain random path nodes; S2. Construct a collision detection model based on the robot's gait cycle; use the collision detection model to detect whether the random path node passes the collision detection; if not, delete the random path node and return to S1; if yes, add the random path node to the random tree. For each node added to the random tree, perform collision detection at the node. If the collision detection passes, the node is selected as a candidate node; otherwise, return to step S1. For each candidate node, perform a plane size detection. If the detection passes, add the candidate node to the global path point sequence; otherwise, return to step S1. S3. Based on the complexity of the current plane, the global path point sequence is filtered to obtain path nodes in complex areas and path nodes in flat areas. S4. When the robot runs along the path node of the complex area, the first pose data of the robot is obtained, and a motion command is generated based on the first pose data; when the robot runs along the path node of the flat area, the second pose data of the robot is obtained; and a motion command is generated based on the second pose data and a preset threshold.

[0007] In one embodiment of the present invention, the step of constructing a collision detection model based on the robot's gait cycle is as follows: The midpoint between the robot's standing point and the new swing point is taken as the first center point, and the direction of the line connecting the standing point and the new swing point is taken as the first minor axis; the direction perpendicular to the first minor axis is taken as the first major axis; based on the first center point, the first minor axis, and the first major axis, the first ellipse is obtained. The midpoint between the robot's previous swing foot point and its standing foot point is taken as the second center point; the direction of the line connecting the previous swing foot point and the standing foot point is taken as the second minor axis; the direction perpendicular to the second minor axis is taken as the second major axis; and the second ellipse is obtained based on the second center point, the second minor axis, and the second major axis. During the bipedal gait cycle, the first ellipse and the second ellipse are iteratively combined to continuously cover the entire motion envelope of the robot, thus obtaining a collision detection model.

[0008] In one embodiment of the present invention, the lengths of the first minor axis, the first major axis, the second minor axis, and the second major axis are respectively: ; ; ; ; in, Indicates the length of the first minor axis. Indicates the length of the first major axis. Indicates the length of the second minor axis. Indicates the length of the second major axis. Indicates a new foothold. Indicates the landing point of the supporting foot. Indicates the landing point of the previous swinging foot. Indicates the width of the robot's hip. This indicates the amount of lateral sway compensation caused by the outward expansion of the legs due to the pelvic tilt of the robot.

[0009] In one embodiment of the present invention, when the robot runs along the path node of the flat area, the method for acquiring the robot's second pose data and generating motion commands based on the second pose data and a preset threshold is as follows: The second pose data includes heading angle error. When the heading angle error is not less than a preset threshold, the system immediately switches to rotation correction mode to generate a pure rotation motion command. When the heading angle error is less than a preset threshold, the error vector is converted into motion commands in the robot's body coordinate system by a rotation matrix, based on the error vector between the robot's current pose and the target point.

[0010] In one embodiment of the present invention, for each node added to the random tree, collision detection is performed at the node. If the collision detection passes, the node is selected as a candidate node; otherwise, the process returns to step S1. Expand along the direction of the node and to the size of the robot's two legs to obtain a collision area, and detect whether the collision area collides with an obstacle; if not, the current node is selected as a candidate node; if so, return to step S1.

[0011] In one embodiment of the present invention, the step of performing plane size detection on each candidate node is as follows: Obtain a preset range of the height of the parent node of the candidate node, and perform collision detection search within the preset range to obtain the critical state node; Taking each path node of the critical state node as the center, the foot projection area is obtained by extending outward along the center; Based on the foot projection area, determine whether the height of the corresponding position of the candidate node is occupied; if so, then perform plane size detection; otherwise, delete the candidate node.

[0012] In one embodiment of the present invention, the step of filtering the global path point sequence according to the complexity of the current plane to obtain path nodes in complex regions and path nodes in flat regions is as follows: Determine the complexity of the plane by dividing it into flat and complex regions; For the path nodes in the global path point sequence that are located in the flat region, one path node is captured out of every n path nodes as the stage target point to obtain the flat region path nodes; where n is a positive integer; All path nodes in the global path point sequence that are located in the complex region are complex region path nodes.

[0013] In one embodiment of the present invention, the criteria for determining the complexity of a plane and dividing it into flat and complex regions are as follows: If the occupancy density of the area containing the plane is lower than the preset density threshold, and the z-axis height of all path nodes in the area relative to their parent nodes does not change, then the area containing the plane is determined to be a flat area; otherwise, it is a complex area.

[0014] In one embodiment of the present invention, when the robot runs along the path nodes of the complex region, the step of acquiring the robot's first pose data and generating motion commands based on the first pose data is as follows: Calculate the error vector between the first pose data and the robot's next landing point; The error vector is converted into motion commands in the robot's body coordinate system using a rotation matrix.

[0015] Secondly, to solve the above-mentioned technical problems, the present invention provides a three-dimensional complex environment navigation system for humanoid robots, comprising: The node acquisition module is used to acquire nodes along random paths. The first detection module is used to construct a collision detection model based on the robot's gait cycle; use the collision detection model to detect whether the random path node passes the collision detection; if not, delete the random path node and return to the node acquisition module; The second detection module is used to add the random path node to the random tree if the collision detection is passed; For each node added to the random tree, perform collision detection at the node. If the collision detection passes, the node is selected as a candidate node; otherwise, return to the node acquisition module. For each candidate node, a plane size detection is performed. If the detection passes, the candidate node is added to the global path point sequence; otherwise, the process returns to the node acquisition module. The filtering module is used to filter the global path point sequence according to the complexity of the current plane to obtain path nodes in complex areas and path nodes in flat areas. The instruction generation module is used to acquire the robot's first pose data and generate motion instructions based on the first pose data when the robot runs along the path nodes of the complex area; and to acquire the robot's second pose data and generate motion instructions based on the second pose data and a preset threshold when the robot runs along the path nodes of the flat area.

[0016] Compared with the prior art, the above-described technical solution of the present invention has the following advantages: (1) The humanoid robot three-dimensional complex environment navigation method and system described in this invention uses a collision detection model to screen random path nodes, ensuring the safety of path nodes and effectively avoiding collisions during robot walking, thereby improving the reliability of path planning. Simultaneously, this method classifies the global path point sequence according to complexity, providing more suitable paths for the robot based on different terrain characteristics, further enhancing the scientific nature and adaptability of path planning. Furthermore, by acquiring different pose data in complex and flat areas respectively, and combining this with preset thresholds to generate motion commands, the robot can more accurately control its own movement and flexibly adapt to various terrains, thereby improving its walking ability in complex environments.

[0017] (2) This invention reduces the calculation and processing of invalid nodes by dynamically filtering random path nodes, thus saving computing resources. At the same time, it generates motion instructions in a targeted manner after classifying the path point sequence, avoiding uniform processing of all path points and further improving the efficiency of path planning.

[0018] (3) Based on the RRT algorithm, this invention proposes a three-dimensional environment navigation and real-time gait planning framework for humanoid robots. This framework uses an elliptic approximation to represent the motion envelope space of the swinging leg, achieving efficient collision detection. Simultaneously, a terrain adaptation mechanism is introduced for the three-dimensional environment, integrating planar supportability analysis and local terrain complexity assessment to differentiate the dense path nodes generated based on RRT. Finally, the framework integrates a gait controller, which can fully utilize the robot's motion capabilities and improve planning efficiency, effectively solving the navigation and gait planning problems faced by humanoid robots in three-dimensional environments. Attached Figure Description

[0019] To make the content of this invention easier to understand, the invention will be further described in detail below with reference to specific embodiments and accompanying drawings, wherein: Figure 1 This is a flowchart of a three-dimensional complex environment navigation method for a humanoid robot according to a preferred embodiment of the present invention; Figure 2 This is a schematic diagram of the additional collision area beyond its own projection area caused by the swing leg during the gait cycle in a preferred embodiment of the present invention. Figure 3 This is a schematic diagram of the defective locations in the path node filtering process in a preferred embodiment of the present invention; Figure 4 This is a schematic diagram showing the position where the robot's foot collides with an obstacle and the plane is insufficient to accommodate the robot's foot in a preferred embodiment of the present invention; Figure 5 This is a flowchart of the three-dimensional complex environment navigation algorithm for a humanoid robot in a preferred embodiment of the present invention; Figure 6 This is a simulation experiment result diagram of the Roban robot in a preferred embodiment of the present invention; Figure 7 The image shows the actual experimental results of the Roban robot in a preferred embodiment of the present invention. Detailed Implementation

[0020] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand and implement the present invention. However, the embodiments described are not intended to limit the present invention.

[0021] Example 1: Refer to Figure 1 As shown, this embodiment of the invention provides a method for navigating a humanoid robot in a complex three-dimensional environment, including but not limited to the following steps: S1. Obtain random path nodes; S2. Construct a collision detection model based on the robot's gait cycle; use the collision detection model to detect whether random path nodes pass the collision detection; if not, delete the random path node and return to S1; if yes, add the random path node to the random tree. For each node added to the random tree, perform collision detection at the node. If the collision detection passes, the node is selected as a candidate node; otherwise, return to S1. Perform plane size detection on each candidate node. If the detection passes, add the candidate node to the global path point sequence; otherwise, return to S1. S3. Based on the complexity of the current plane, filter the global path point sequence to obtain path nodes for complex regions and path nodes for flat regions. S4. When the robot runs along a path node in a complex area, acquire the robot's first pose data and generate motion commands based on the first pose data; when the robot runs along a path node in a flat area, acquire the robot's second pose data; generate motion commands based on the second pose data and a preset threshold.

[0022] This invention provides a method for navigating a humanoid robot in a complex 3D environment. By using a collision detection model to filter random path nodes, the safety of these nodes is ensured, preventing collisions during robot movement and improving the reliability of path planning. Simultaneously, the global path point sequence is categorized according to complexity, providing more suitable paths for the robot based on different terrain characteristics, further enhancing the scientific rigor and adaptability of path planning. Secondly, this design enhances the robot's terrain adaptability. Acquiring different pose data in complex and flat areas, and combining this with preset thresholds to generate motion commands, allows the robot to control its movement more precisely, flexibly adapt to different terrains, and improve its walking ability in complex environments. Furthermore, this design optimizes path planning efficiency. Dynamically filtering random path nodes reduces the calculation and processing of invalid nodes, lowering the computational cost of the dynamic envelope space. Classifying the path point sequence and generating motion commands specifically avoids uniform processing of all path points, further improving path planning efficiency. Finally, this method helps improve the robot's walking stability. In complex areas, motion commands are generated based on the first pose data, enabling more precise control of the robot's walking posture on complex terrain. In flat areas, motion commands are generated by combining the second pose data and preset thresholds, ensuring that the robot walks in a more stable and efficient manner, reducing unnecessary posture adjustments, and thus improving its walking stability on different terrains.

[0023] Specifically, in step S1, random path nodes are randomly generated using the RRT algorithm (e.g., ...). , , The algorithm generates dense paths with an interval of 0.03m in a 3D environment based on the original RRT algorithm. The purpose is to modify and add collision detection and path node selection strategies in the original RRT path planning algorithm.

[0024] Specifically, unlike wheeled robots, when a humanoid robot moves from its current supporting foot to a new foothold during walking, the trajectory of its swinging leg may exceed its own size and collide with obstacles in the environment. Considering the high computational cost required for high-precision motion space, this embodiment of the invention uses an ellipsoidal approximation method to model the motion envelope of the swinging leg. This simplification reduces computational complexity while enabling rapid collision detection. Therefore, in step S2, a collision detection model is constructed using the ellipsoidal approximation method for the motion envelope space of the swinging leg during the humanoid robot's walking gait cycle. The specific steps are as follows: S211. Take the midpoint between the robot's standing point and the new swing point as the first center point, take the direction of the line connecting the standing point and the new swing point as the first minor axis, and take the direction perpendicular to the first minor axis as the first major axis; obtain the first ellipse based on the first center point, the first minor axis and the first major axis.

[0025] For example, the area of ​​the robot from the standing foot to the new swinging foot is determined, including the motion space of the current foot during a single step transition, denoted by ellipse I. The center point is the midpoint between the standing foot and the new swinging foot point, the minor axis is along the line connecting the two feet, and the length of the first minor axis is... for: ; in, Indicates a new foothold. Indicates the landing point of the supporting foot.

[0026] For example, the first major axis is perpendicular to the first minor axis, and the length of the first major axis is... for: ; in, Indicates the width of the robot's hip. This indicates the amount of lateral sway compensation caused by the outward expansion of the legs due to the pelvic tilt of the robot.

[0027] S212. Take the midpoint between the robot's previous swing foot point and the standing foot point as the second center point, take the direction of the line connecting the previous swing foot point and the standing foot point as the second minor axis, and take the direction perpendicular to the second minor axis as the second major axis; based on the second center point, the second minor axis and the second major axis, obtain the second ellipse.

[0028] For example, the area of ​​the robot from the previous swinging foot to the standing foot is determined, including the motion space of the single step transition of the previous stepping foot, denoted by ellipse II. Second minor axis length for: ; in, This indicates the landing point of the previous swinging foot.

[0029] For example, the second major axis is perpendicular to the second minor axis, and the length of the second major axis is... for: .

[0030] S213. During the bipedal gait cycle, two elliptical regions (i.e., the first ellipse and the second ellipse) achieve continuous coverage of the robot's entire motion envelope through iterative relationships, thereby obtaining a collision detection model.

[0031] Furthermore, a collision detection model is used to check whether the collision area formed by the randomly sampled path node generated by the RRT algorithm and the previous two nodes collides with an obstacle. If no collision occurs, the path node is added to the random tree. Specifically, for the random path node planned by the RRT algorithm, the connection between it and the previous two path nodes is checked to see if there is a collision with an obstacle. The collision area can be referenced... Figure 2 As shown. Among them, Figure 2 symbols in Indicates the current step taken, symbol This represents the previous step. This area includes the extra area generated by leg lifting and swaying during the robot's entire gait cycle, ensuring the feasibility of path nodes as robot landing points. Only nodes that pass collision detection are added to the random tree.

[0032] Specifically, collision detection and plane size detection are performed on the nodes added to the random tree.

[0033] Furthermore, referring to Figure 3 and Figure 4 As shown in the first image from left to right, for each node added to the random tree, collision detection is performed on its own position. The collision area is measured along the node direction and is the size of the robot's two feet. If no collision is detected between this area and an obstacle, the current node is selected as a candidate node. This collision detection ensures that the robot will not collide with obstacles when landing.

[0034] Furthermore, for candidate nodes, collision detection is performed within a preset range of the parent node's height to identify feasible z-values, ensuring that path nodes are on a plane and their height difference is within the robot's maximum step height. Here, the parent node refers to the node selected as the growth starting point in the random tree. The specific steps for identifying feasible z-values ​​through collision detection within the preset range of the parent node's height (representing the maximum step height), such as ±0.15m, are as follows: Collision detection is performed from bottom to top within the preset range of the parent node's height (representing the maximum step height) to identify feasible z-values. If no collision is found at a certain height, the state's height is adjusted to that position, and this state is recorded as a critical state node.

[0035] The collision detection search method described above can ensure that path nodes are not suspended in mid-air, but fall on the three-dimensional plane and do not exceed the maximum step height of the robot.

[0036] Specifically, in step S2, if the collision detection passes, it is designated as a candidate node; otherwise, a random path node is selected again, and the process returns to step S1. For each candidate node, refer to... Figure 4(Second image from left to right) shows a rigorous verification of ground reliability. The foot projection area extends outward from the node center, and the sampling area is defined by the longitudinal span (1 / 3 of the foot length) and the lateral span (1 / 2 of the foot width) to evaluate the continuity of the plane and whether there is sufficient foothold area. If the plane detection fails, the path node is removed. For candidate nodes, the area is expanded along the node direction to check whether the plane is completely occupied. The specific steps are as follows: S221. Obtain the above-mentioned critical state nodes. For each path node of the critical state node... Centered on this node, extend the foot projection area forward and backward (1 / 3 of the foot length) and left and right (1 / 2 of the foot width) in the direction of this node. The expression is: ; ; in, This indicates the heading angle of the path node. Indicates the length of the robot's legs. This indicates the width of the robot's footsteps.

[0037] S222. Perform grid sampling at map resolution and query the occupancy status at the corresponding height on the map to determine whether the node is on a plane and whether the plane size is sufficient to accommodate its position. If it is in an occupied state, it passes the plane size check and is included as a candidate path node. If it is not in an occupied state, it fails the plane size check and is deleted.

[0038] The aforementioned plane detection fully considers the support of the landing point, ensuring that all path nodes accurately land on a plane suitable for the robot to stand on, and that these planes have sufficient area and stability to provide reliable support for the robot's stable walking.

[0039] Furthermore, in the path optimization stage, a dynamic adaptive strategy is used to filter the original path nodes generated in the above process.

[0040] Specifically, in step S3, the global path point sequence is processed according to the complexity of the current plane. The steps to filter and obtain path nodes for complex and flat regions are as follows: S310. Determine the complexity of the current plane, specifically distinguishing between flat and complex areas. The purpose is to dynamically filter dense path nodes and provide accurate data support for gait planning during subsequent robot movement.

[0041] Furthermore, the criteria for determining whether the current plane is a flat region or a complex region are as follows: if the occupancy density of a region is lower than a preset density threshold, and the z-axis height of all path nodes in the region relative to their parent nodes does not change, then the region is classified as a flat region; otherwise, it is classified as a complex region.

[0042] S320. For path nodes in the global path point sequence that are determined to be in a flat region, capture one path node every n path nodes as a stage target point (also called a temporary target), thereby obtaining the path nodes in the flat region. Wherein, n is a positive integer, and in this embodiment of the invention, its value is 6.

[0043] S330. For path nodes in the global path point sequence that are determined to be in complex regions, they are all considered complex region path nodes, meaning that these nodes are not filtered. Specifically, the path nodes are directly used as the robot's landing points, and the planner subsequently generates a set of humanoid steps that satisfy the robot's movement.

[0044] Based on the above steps S320 and S330, the controller in the robot will generate humanoid turning and forward steps that satisfy the robot's movement.

[0045] Specifically, in step S4, the robot pose data (including real-time 3D position and yaw angle) is continuously received and converted with the global path point sequence obtained by the above algorithm. At the same time, the maximum step length and maximum rotation angle of the robot during walking are set. If the robot is located in a complex area, based on the error vector between the current pose (i.e., the first pose data) and the next landing point, the global coordinate error is converted into motion commands in the robot body coordinate system through a rotation matrix. The error vector includes distance error and angle error.

[0046] For example, the positions and quaternions of the humanoid robot and path nodes are read, an error threshold is set, and the distance error between the current pose and the next target path node is calculated. In complex areas, the calculated distance error is used as the step size and sent to the robot.

[0047] Furthermore, if the robot is running along a path node in a flat area, the method for acquiring the robot's second pose data and generating motion commands based on the second pose data and a preset threshold is as follows: the second pose data includes heading angle error; when the heading angle error is not less than the preset threshold, the robot immediately switches to rotation correction mode to generate pure rotation motion commands; when the heading angle error is less than the preset threshold, the error vector is converted into motion commands in the robot's body coordinate system through a rotation matrix based on the error vector between the robot's current pose and the target point.

[0048] For example, in a flat area, when a heading angle error greater than or equal to a threshold of 20° is detected, a rotation priority mechanism is adopted; when a heading angle error less than the threshold of 20° is detected, based on the error vector between the current pose and the target point, the global coordinate error is converted into motion commands in the robot body coordinate system through a rotation matrix, while ensuring that the step size and rotation angle are less than the set maximum step size and rotation angle.

[0049] Furthermore, in flat areas, when the next stage target point is the final target point, switch to focus mode. At this time, reduce the distance threshold between the target path node positions to ensure that the robot can reach the target position accurately.

[0050] To better understand step S4, the following exemplary implementation case is given.

[0051] For example, the robot's pose data (including Cartesian coordinates and yaw angle) is continuously received and processed along a global path point sequence to calculate the error vector between the current pose and the target point. A rotation matrix is ​​used to convert this global coordinate error into motion commands within the robot's body frame. Before gait planning, the robot's maximum stride length and maximum rotation angle during walking are set to [0.1 m, 0.1 m, 8 yaw], and the distance threshold between the robot's position and the target path node position is set to [0.1 m, 0.1 m]. In complex areas, the error value is directly sent to the robot as the stride length. In flat areas, when the detected yaw angle error exceeds the 20° threshold (including 20°), the system immediately switches to rotation correction mode, generating a pure rotation command of size [0.01, 0, θ] to ensure the robot prioritizes alignment. When the yaw angle error is less than the 20° threshold, the error value is sent to the robot as the stride length, while ensuring the stride length command is less than the robot's set maximum stride length, until the distance to the stage target point is less than the set distance threshold, at which point the following path node is designated as the stage target point. As the robot continuously approaches the target point along the path, when the next stage target point becomes the final target point, it switches to a focus mode. At this point, the distance threshold between target path nodes is reduced to [0.06 m, 0.06 m], ensuring the robot can accurately reach the target location. Upon approaching the final path point, the endpoint mode is automatically activated. Here, the position tolerance is reduced by 60% to 0.06 meters, and the angle tolerance is reduced to 2°, ensuring the robot accurately reaches the target location.

[0052] The framework described above achieves a seamless transition from planar movement to vertical ascent through the synergistic effect of rotation mode, endpoint mode, and environmental information response, providing an efficient and complete solution for the autonomous navigation of humanoid robots in complex three-dimensional environments.

[0053] Specifically, based on the above steps, this embodiment of the invention provides a gait planning algorithm for a humanoid robot. The algorithm includes input parameters, output parameters, and algorithm steps, as detailed below: The input parameters include the robot's current position and orientation, a series of waypoints, the error threshold between the robot and the waypoints, and the maximum step size limit.

[0054] Obtain the robot's current position and orientation, including 3D coordinates. x , y , z and orientation angle θ Marked as: .

[0055] The position and orientation information contained in each waypoint in a series of waypoints is labeled as follows: .

[0056] Set the error threshold between the robot and the path points This is used to determine if the robot is close to a pathpoint. It sets the maximum step size limit, including... x , y The maximum step size in direction and the maximum adjustment amount in angle are marked as follows: .

[0057] The output parameters are motion commands, which include... The step size in the direction is marked as: .

[0058] For the algorithm steps, please refer to [link / reference]. Figure 5 The specific steps are as follows: Step 1: Initialize the global pathpoint sequence list ; Set the starting point Add to list Define a function Distance(·) to calculate the distance and angle deviation between the robot's current position and orientation and the path points. The return value is represented as... .

[0059] Step 2: To achieve autonomous navigation of the humanoid robot in a 3D environment, the robot needs to efficiently plan paths and generate collision-free gaits in various complex 3D environments. Therefore, the set of path points is traversed, and different processing is performed based on the region where the path point is located (complex region or flat region): In complex regions: For each path point located in a complex region, the distance and angular deviation between the robot and that path point are directly calculated and used as motion commands.

[0060] In flat areas, the following three cases are included: Case 1: If the angle of the path point is deviated If the angle is greater than 20°, the motion command is to... x Move 0.01 meters in the direction. y Do not move in direction, adjust the angle to not exceed the maximum allowable angle. error .

[0061] Case 2: If the current path point is not the last path point, when the distance between the robot and the path point is less than the error threshold. When the motion command is to ensure that the step size in each direction does not exceed the smaller of the maximum step size and the distance deviation.

[0062] Case 3: If the current path point is the last path point, and the distance between the robot and the path point is less than 0.6 times the error threshold. When the motion command is to ensure that the step size in each direction does not exceed the smaller of the maximum step size and the error, the motion command is to ensure that the motion size in each direction does not exceed the smaller of the maximum step size and the error.

[0063] The algorithm described above dynamically adjusts the robot's motion commands by calculating the distance and angular deviation between the robot and pathpoints. In complex areas, the algorithm primarily focuses on angle adjustment; in flat areas, it dynamically adjusts the step size based on distance and angular deviations, and improves accuracy as it approaches the target point, ensuring the robot accurately reaches its target location. In this way, the algorithm achieves gait planning for humanoid robots in complex 3D environments, ensuring efficient and safe navigation and movement.

[0064] The improved RRT path planning method based on double elliptic envelope collision space and 3D terrain perception proposed in this invention effectively solves the problems of low efficiency in leg motion collision detection and insufficient 3D path planning accuracy for humanoid robots in unstructured environments. Furthermore, this invention also proposes a terrain-adaptive hierarchical motion control method, which can improve the path tracking accuracy of humanoid robots in complex terrains and achieve synergistic optimization of motion efficiency and accuracy. To further verify the path planning algorithm proposed in this invention, a 3D scene for humanoid robot navigation and path planning, including both simulated and real-world scenarios, was designed. The real-world scene includes obstacles and climbable steps, while the simulated scene uses V-REP software to add obstacles and steps and visualize them, aiming to provide dual verification of the method.

[0065] Specifically, virtual and real-world scenarios with different terrain environments were established. In the simulation environment, the robot needed to avoid obstacles measuring 0.3m × 0.3m × 0.5m and 0.2m × 0.3m × 0.2m (because their height exceeded the robot's capabilities) and climb a step with a height of 0.12m. In the real-world environment, the robot needed to avoid obstacles measuring 0.3m × 0.3m × 0.5m and climb a step with a height of 0.06m.

[0066] Furthermore, the above method was validated using the Roban humanoid robot in both simulated and real-world environments. Simulation and real-world experimental results show that the method described in this embodiment of the invention can plan path nodes (such as...) that match the humanoid robot's movement in a 3D environment containing obstacles and steps. Figure 6-7 (As shown). Through Figures 6 to 7 As can be observed from the slice images, the proposed method can generate gait commands in real time during obstacle avoidance and step-climbing. Before stepping onto a step, the robot maintains a gap of about 2 centimeters between its feet and the step, thereby achieving a smooth transition between different gait states and effectively avoiding the risk of collision with obstacles.

[0067] This invention proposes a 3D environment navigation and real-time gait planning framework for humanoid robots based on the traditional RRT algorithm. This framework uses an elliptic approximation to represent the motion envelope space of the swinging leg, achieving efficient collision detection. Simultaneously, it introduces a terrain adaptation mechanism for the 3D environment, integrating planar supportability analysis and local terrain complexity assessment to differentially filter dense path nodes generated based on RRT. Finally, the framework integrates a gait controller, which can fully utilize the robot's motion capabilities and significantly improve planning efficiency, effectively solving the navigation and gait planning problems faced by humanoid robots in 3D environments.

[0068] Example 2: Based on the same inventive concept, this example provides a three-dimensional complex environment navigation system for humanoid robots. The principle of solving the problem is similar to that of the three-dimensional complex environment navigation method for humanoid robots provided in Example 1, and the repeated parts will not be described again.

[0069] This embodiment provides a three-dimensional complex environment navigation system for a humanoid robot, including: The node acquisition module is used to acquire nodes along random paths. The first detection module is used to construct a collision detection model based on the robot's gait cycle; it uses the collision detection model to detect whether random path nodes pass the collision detection; if not, it deletes the random path nodes and returns to the node acquisition module. The second detection module is used to add random path nodes to the random tree if the collision detection is passed; For each node added to the random tree, perform collision detection at the node. If the collision detection passes, the node is selected as a candidate node; otherwise, return to the node acquisition module. For each candidate node, perform a plane size check. If the check passes, add the candidate node to the global path point sequence; otherwise, return to the node acquisition module. The filtering module is used to filter the global path point sequence based on the complexity of the current plane to obtain path nodes in complex areas and path nodes in flat areas. The instruction generation module is used to acquire the robot's first pose data when the robot is running along a path node in a complex area, and generate motion instructions based on the first pose data; when the robot is running along a path node in a flat area, it acquires the robot's second pose data; and generates motion instructions based on the second pose data and a preset threshold.

[0070] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0071] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0072] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0073] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0074] Obviously, the above embodiments are merely illustrative examples for clear explanation and are not intended to limit the implementation. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively list all possible implementations here. However, obvious variations or modifications derived therefrom are still within the scope of protection of this invention.

Claims

1. A method for navigating a humanoid robot in a complex three-dimensional environment, characterized in that, include: S1. Obtain random path nodes; S2. Construct a collision detection model based on the robot's gait cycle; use the collision detection model to detect whether the random path node passes the collision detection; if not, delete the random path node and return to S1; if yes, add the random path node to the random tree. For each node added to the random tree, perform collision detection at the node. If the collision detection passes, the node is selected as a candidate node; otherwise, return to step S1. For each candidate node, perform a plane size detection. If the detection passes, add the candidate node to the global path point sequence; otherwise, return to step S1. S3. Based on the complexity of the current plane, the global path point sequence is filtered to obtain path nodes in complex areas and path nodes in flat areas. S4. When the robot runs along the path node of the complex area, the first pose data of the robot is obtained, and a motion command is generated based on the first pose data; when the robot runs along the path node of the flat area, the second pose data of the robot is obtained; and a motion command is generated based on the second pose data and a preset threshold.

2. The method for navigating a humanoid robot in a three-dimensional complex environment according to claim 1, characterized in that, The steps for constructing a collision detection model based on the robot's gait cycle are as follows: The midpoint between the robot's standing point and the new swing point is taken as the first center point, and the direction of the line connecting the standing point and the new swing point is taken as the first minor axis; the direction perpendicular to the first minor axis is taken as the first major axis; based on the first center point, the first minor axis, and the first major axis, the first ellipse is obtained. The midpoint between the robot's previous swing foot point and the standing foot point is taken as the second center point, and the direction of the line connecting the previous swing foot point and the standing foot point is taken as the second minor axis. The direction perpendicular to the second minor axis is taken as the second major axis; The second ellipse is obtained based on the second center point, the second minor axis, and the second major axis; During the bipedal gait cycle, the first ellipse and the second ellipse are iteratively combined to continuously cover the entire motion envelope of the robot, thus obtaining a collision detection model.

3. The method for navigating a humanoid robot in a three-dimensional complex environment according to claim 2, characterized in that, The lengths of the first minor axis, the first major axis, the second minor axis, and the second major axis are respectively: ； ； ； ； in, Indicates the length of the first minor axis. Indicates the length of the first major axis. Indicates the length of the second minor axis. Indicates the length of the second major axis. Indicates a new foothold. Indicates the landing point of the supporting foot. Indicates the landing point of the previous swinging foot. Indicates the width of the robot's hip. This indicates the amount of lateral sway compensation caused by the outward expansion of the legs due to the pelvic tilt of the robot.

4. The method for navigating a humanoid robot in a three-dimensional complex environment according to claim 1, characterized in that, When the robot runs along the path nodes of the flat area, the method for acquiring the robot's second pose data and generating motion commands based on the second pose data and a preset threshold is as follows: The second pose data includes heading angle error. When the heading angle error is not less than a preset threshold, the system immediately switches to rotation correction mode to generate a pure rotation motion command. When the heading angle error is less than a preset threshold, the error vector is converted into motion commands in the robot's body coordinate system by a rotation matrix, based on the error vector between the robot's current pose and the target point.

5. The method for navigating a humanoid robot in a three-dimensional complex environment according to claim 1, characterized in that, For each node added to the random tree, a collision detection is performed at the node. If the collision detection passes, the node is selected as a candidate node; otherwise, the process returns to step S1. Expand along the direction of the node and to the size of the robot's two legs to obtain a collision area, and detect whether the collision area collides with an obstacle; if not, the current node is selected as a candidate node; if so, return to step S1.

6. The method for navigating a humanoid robot in a three-dimensional complex environment according to claim 1, characterized in that, The steps for performing plane size detection on each candidate node are as follows: Obtain a preset range of the height of the parent node of the candidate node, and perform collision detection search within the preset range to obtain the critical state node; Taking each path node of the critical state node as the center, the foot projection area is obtained by extending outward along the center; Based on the foot projection area, determine whether the height of the corresponding position of the candidate node is occupied; if so, then perform plane size detection; otherwise, delete the candidate node.

7. The method for navigating a humanoid robot in a three-dimensional complex environment according to claim 1, characterized in that, The steps for filtering the global path point sequence based on the complexity of the current plane to obtain path nodes for complex and flat regions are as follows: Determine the complexity of the plane by dividing it into flat and complex regions; For the path nodes in the global path point sequence that are located in the flat region, one path node is captured out of every n path nodes as the stage target point to obtain the path nodes in the flat region; where n is a positive integer. All path nodes in the global path point sequence that are located in the complex region are complex region path nodes.

8. A method for navigating a humanoid robot in a three-dimensional complex environment according to claim 7, characterized in that, The criteria for judging the complexity of a plane, dividing it into flat and complex regions, are as follows: If the occupancy density of the area containing the plane is lower than the preset density threshold, and the z-axis height of all path nodes in the area relative to their parent nodes does not change, then the area containing the plane is determined to be a flat area; otherwise, it is a complex area.

9. A method for navigating a humanoid robot in a three-dimensional complex environment according to claim 1, characterized in that, When the robot runs along the path nodes of the complex area, the steps of acquiring the robot's first pose data and generating motion commands based on the first pose data are as follows: Calculate the error vector between the first pose data and the robot's next landing point; The error vector is converted into motion commands in the robot's body coordinate system using a rotation matrix.

10. A three-dimensional complex environment navigation system for a humanoid robot, characterized in that, include: The node acquisition module is used to acquire nodes along random paths. The first detection module is used to construct a collision detection model based on the robot's gait cycle; use the collision detection model to detect whether the random path node passes the collision detection; if not, delete the random path node and return to the node acquisition module; The second detection module is used to add the random path node to the random tree if the collision detection is passed; For each node added to the random tree, perform collision detection at the node. If the collision detection passes, the node is selected as a candidate node; otherwise, return to the node acquisition module. For each candidate node, a plane size detection is performed. If the detection passes, the candidate node is added to the global path point sequence; otherwise, the process returns to the node acquisition module. The filtering module is used to filter the global path point sequence according to the complexity of the current plane to obtain path nodes in complex areas and path nodes in flat areas. The instruction generation module is used to acquire the robot's first pose data and generate motion instructions based on the first pose data when the robot runs along the path nodes of the complex area; and to acquire the robot's second pose data and generate motion instructions based on the second pose data and a preset threshold when the robot runs along the path nodes of the flat area.

Images