Humanoid robot three-dimensional complex environment navigation method and system

Abstract

The present application relates to a kind of humanoid robot three-dimensional complex environment navigation method and system, belong to robot navigation technical field.It includes: obtaining random path node;Collision detection model is constructed according to the gait cycle of robot;Whether random path node passes collision detection is detected using collision detection model;If not, delete random path node, and reacquire random path node;Otherwise, random path node is added to random tree;For each node added to random tree, carry out collision detection and plane size detection at node, when passing detection, then candidate node is added to global path point sequence;According to the complexity of current plane, the sequence is screened, and complex region path node and flat region path node are obtained;According to the running demand of robot on different region path node, corresponding motion instruction is generated.The present application reduces dynamic envelope space calculation cost and improves global path planning accuracy.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot navigation, in particular to a humanoid robot three-dimensional complex environment navigation method and system. BACKGROUND

[0002] Humanoid robot technology is experiencing an explosive development from the laboratory to complex real-world scenarios. In recent years, the latest developments in humanoid robot gait generation techniques emphasize the importance of trajectory planning and execution for achieving stable and efficient motion. However, traditional wheeled robot path planning strategies struggle to cope with unstructured terrains such as stairs, slopes, and discrete footholds. Humanoid robots, with their high degree of freedom, exhibit higher motion flexibility and superior mobility in everyday environments. Compared to wheeled robots, their bipedal motion pattern allows them to climb stairs, cross obstacles, and navigate through complex three-dimensional environments. However, robust navigation in three-dimensional environments remains a core challenge, and traditional wheeled robot path planning strategies struggle to cope with unstructured terrains such as stairs, slopes, and discrete footholds. Key breakthroughs in this field focus on the integration of humanoid motion planning and control, i.e., through dynamic gait adjustment, terrain-adaptive foothold decision, and whole-body coordinated control, to achieve human-like flexible obstacle avoidance capabilities.

[0003] Traditional navigation algorithms have significant shortcomings when dealing with the unique three-dimensional motion challenges of humanoid robots. Traditional two-dimensional or 2.5-dimensional path planners (such as RRT, A*, etc.) struggle to adequately handle discrete height variations such as steps, climbable platforms, and crossable obstacles. Their node generation and collision detection mechanisms often lack vertical adaptability, making it difficult to reliably support the robot's ability to step up, step down, and climb structures. At the same time, existing methods mostly focus on the reachability and collision-free nature of path nodes, ignoring strict evaluation of support area size, flatness, and continuity, resulting in theoretical paths that fail in actual execution due to unfeasible footholds. During the humanoid robot's walking process, the swing leg's motion trajectory may exceed its static geometric profile, increasing the risk of collision with obstacles. Accurate calculation of such dynamic envelope spaces is computationally expensive and difficult to meet real-time navigation requirements. Furthermore, effective global path planning in three-dimensional environments containing climbable structures (such as steps and platforms) requires algorithms that can handle both continuous surfaces and discrete height variations.

[0004] At present, the existing technology has made certain progress in the motion planning of humanoid robots. For example, the A* gait planner based on the energy consumption model reduces the number of iterations by 19.7% by including the foot angle difference in the heuristic function, while keeping the trajectory cost only increasing by 0.41%. In addition, the method of fusing the unscented Kalman filter with the gait control realizes real-time pose correction through visual landmark detection, and controls the trajectory deviation within 15 cm. A trajectory generation method based on cubic spline interpolation combines the zero moment point stability criterion for stepping on / stepping off obstacles of known size, and verifies its dynamic stability through offline simulation and physical experiments on the BHR-2 platform. However, the above methods still have deficiencies. For example, the collision detection in the 3D environment is often too simple or conservative, and fails to effectively simulate the actual motion space of the swinging leg of the humanoid robot in actions such as crossing and climbing stairs, resulting in unrealistic planned paths or missed feasible paths. Most path planners do not fully consider the actual supportability of the landing point and the complexity difference of the terrain when generating nodes, making it difficult for the subsequent gait controller to directly use or requiring a lot of post-processing. In addition, the specific gait constraints of the robot and the differences in control strategies required by different terrains are not fully integrated in the path planning stage, and the planning results often need complex conversion or adaptation to generate executable gaits. SUMMARY

[0005] To this end, the technical problem to be solved by the present application is to overcome the problems of too simple collision detection, insufficient consideration of landing point supportability, insufficient integration of gait constraints and terrain strategies, high cost of dynamic envelope space calculation, and low accuracy of global path planning in the prior art.

[0006] In a first aspect, to solve the above technical problems, the present application provides a humanoid robot three-dimensional complex environment navigation method, comprising:

[0007] S1, acquiring a random path node;

[0008] S2, constructing a collision detection model according to the gait cycle of the robot; detecting whether the random path node passes the collision detection by using the collision detection model; if not, deleting the random path node and returning to the S1; if yes, adding the random path node to a random tree;

[0009] For each node added to the random tree, performing collision detection at the node, if the collision detection is passed, taking the node as a candidate node, otherwise returning to the S1;

[0010] Performing plane size detection on each candidate node, if the detection is passed, adding the candidate node to a global path point sequence, otherwise returning to the S1;

[0011] S3, filtering the global path point sequence according to the complexity of the current plane to obtain a complex region path node and a flat region path node;

[0012] S4, acquiring first pose data of the robot when the robot runs along the complex region path node, generating a motion instruction according to the first pose data; acquiring second pose data of the robot when the robot runs along the flat region path node; generating a motion instruction according to the second pose data and a preset threshold.

[0013] In an embodiment of the present application, the step of constructing a collision detection model according to the gait cycle of the robot is:

[0014] The midpoint between the standing foot point and the new swing foot point of the robot is taken as a first center point, and the direction of the line connecting the standing foot point and the new swing foot point is taken as a first short axis; the direction perpendicular to the first short axis is taken as a first long axis; a first ellipse is obtained according to the first center point, the first short axis and the first long axis;

[0015] The midpoint between the previous swing foot point and the standing foot point of the robot is taken as a second center point, and the direction of the line connecting the previous swing foot point and the standing foot point is taken as a second short axis; the direction perpendicular to the second short axis is taken as a second long axis; a second ellipse is obtained according to the second center point, the second short axis and the second long axis;

[0016] In the biped gait cycle, the first ellipse and the second ellipse realize continuous coverage of the entire motion envelope of the robot through an iterative relationship to obtain a collision detection model.

[0017] In an embodiment of the present application, the lengths of the first short axis, the first long axis, the second short axis and the second long axis are respectively:

[0018] ;

[0019] ;

[0020] ;

[0021] ;

[0022] wherein, the first short axis length is represented by L1, the first long axis length is represented by L2, the second short axis length is represented by L3, the second long axis length is represented by L4, the new landing foot point is represented by Pn, the supporting foot landing foot point is represented by Ps, represents a swing foot landing point of the previous swing foot, represents a hip width of the robot, represents a lateral swing compensation amount generated by compensating for leg flare caused by robot pelvic tilt.

[0023] In an embodiment of the present application, when the robot runs along the flat area path node, second pose data of the robot is acquired, and according to the second pose data and a preset threshold, a method for generating a motion instruction is as follows:

[0024] The second pose data includes a heading angle error, when the heading angle error is not less than a preset threshold, a rotation correction mode is switched to immediately, and a pure rotation motion instruction is generated;

[0025] When the heading angle error is less than the preset threshold, according to an error vector of a current pose of the robot and a target point, the error vector is converted into a motion instruction in a body coordinate system of the robot through a rotation matrix.

[0026] In an embodiment of the present application, for each node added to the random tree, collision detection at the node is performed, if the collision detection is passed, the node is taken as a candidate node, otherwise, the method returns to S1, and the steps are as follows:

[0027] A collision area is obtained by expanding along the node direction and with the size of the robot biped, and whether the collision area collides with an obstacle is detected, if not, the current node is taken as a candidate node, if yes, the method returns to S1.

[0028] In an embodiment of the present application, the step of performing plane size detection on each candidate node is as follows:

[0029] A preset range of a parent node height of the candidate node is acquired, collision detection search is performed within the preset range, and a critical state node is obtained;

[0030] A foot projection area is obtained by extending outward from each path node of the critical state node as a center;

[0031] According to the foot projection area, whether the height of the position corresponding to the candidate node is an occupied state is judged, if yes, the candidate node is deleted through plane size detection, otherwise, the candidate node is deleted.

[0032] In an embodiment of the present application, according to the complexity of a current plane, the global path point sequence is screened to obtain complex area path nodes and flat area path nodes, and the steps are as follows:

[0033] The complexity of the plane is judged, and the plane is divided into a flat area and a complex area;

[0034] Capture one path node in every n path nodes in the global path point sequence as a stage target point, to obtain a flat region path node; wherein n is a positive integer;

[0035] The path nodes in the global path point sequence in the complex region are all complex region path nodes.

[0036] In an embodiment of the present application, the complexity of the plane is judged, and the standard for dividing the plane into a flat region and a complex region is:

[0037] When the occupancy density of the region where the plane is located 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, the region where the plane is located is determined as a flat region; otherwise, it is a complex region.

[0038] In an embodiment of the present application, when the robot runs along the complex region path node, first pose data of the robot is obtained, and the step of generating a motion instruction according to the first pose data is:

[0039] Calculate the error vector between the first pose data and the next landing point of the robot;

[0040] Convert the error vector into a motion instruction in the robot body coordinate system through a rotation matrix.

[0041] The second aspect is to solve the above technical problems, and the present application provides a three-dimensional complex environment navigation system for a humanoid robot, comprising:

[0042] A node acquisition module is configured to acquire a random path node;

[0043] A first detection module is configured to construct a collision detection model according to the gait cycle of the robot; the collision detection model is used to detect whether the random path node passes the collision detection; if not, the random path node is deleted, and the node acquisition module is returned to;

[0044] A second detection module is configured to add the random path node to a random tree if the collision detection is passed;

[0045] For each node added to the random tree, collision detection at the node is performed; if the collision detection is passed, the node is taken as a candidate node, otherwise the node acquisition module is returned to;

[0046] Plane size detection is performed on each candidate node; if the detection is passed, the candidate node is added to a global path point sequence, otherwise the node acquisition module is returned to;

[0047] The screening module is configured to screen the global path point sequence according to the complexity of the current plane, and obtain a complex region path node and a flat region path node.

[0048] The instruction generation module is configured to acquire first pose data of the robot when the robot runs along the complex region path node, and generate motion instructions according to the first pose data; acquire second pose data of the robot when the robot runs along the flat region path node; and generate motion instructions according to the second pose data and a preset threshold.

[0049] The above technical solution of the present application has the following beneficial effects compared with the prior art:

[0050] (1) The humanoid robot three-dimensional complex environment navigation method and system can screen random path nodes through a collision detection model, ensure the safety of the path nodes, effectively avoid collisions of the robot in the walking process, and improve the reliability of path planning. Meanwhile, the method classifies the global path point sequence according to the complexity, provides more suitable paths for the robot according to different terrain characteristics, and further enhances the scientificity and adaptability of path planning. In addition, the present application acquires different pose data in the complex region and the flat region respectively, generates motion instructions in combination with a preset threshold, the robot can control its motion more accurately, flexibly adapts to various terrains, and thus improves its walking ability in complex environments.

[0051] (2) The present application reduces the calculation and processing of invalid nodes by dynamically screening random path nodes, and saves computing resources. Meanwhile, the motion instructions are generated in a targeted manner after the classification of the path point sequence, avoiding the unified processing of all path points, and further improving the efficiency of path planning.

[0052] (3) The present application proposes a three-dimensional environment navigation and real-time gait planning framework for humanoid robots based on the RRT algorithm. The framework realizes efficient collision detection by approximating the motion envelope space of the swinging leg with an ellipsoid. Meanwhile, a terrain adaptive mechanism is introduced for the three-dimensional environment, which combines plane supportability analysis and local terrain complexity evaluation to differentially screen the dense path nodes generated based on RRT. Finally, the framework integrates a gait controller, which can fully utilize the motion ability of the robot and improve the planning efficiency, effectively solving the navigation and gait planning problems faced by humanoid robots in three-dimensional environments. BRIEF DESCRIPTION OF DRAWINGS

[0053] In order to make the content of the present application more easily understood, the present application will be further described in detail below according to specific embodiments of the present application and in combination with the drawings, in which:

[0054] Figure 1 A flow chart of a three-dimensional complex environment navigation method of a humanoid robot in a preferred embodiment of the present application;

[0055] Figure 2 A schematic diagram of an additional collision region beyond the self-projection region caused by the swing leg in a gait cycle in a preferred embodiment of the present application;

[0056] Figure 3 A schematic diagram of an undesirable position in path node screening in a preferred embodiment of the present application;

[0057] Figure 4 A schematic diagram of a position where the robot foot collides with an obstacle or a plane is insufficient to place the robot foot in a preferred embodiment of the present application;

[0058] Figure 5 A flow chart of a three-dimensional complex environment navigation algorithm of a humanoid robot in a preferred embodiment of the present application;

[0059] Figure 6 A result graph of a Roban robot simulation experiment in a preferred embodiment of the present application;

[0060] Figure 7 A result graph of a real experiment of a Roban robot in a preferred embodiment of the present application. DETAILED DESCRIPTION

[0061] The present application will be further described below with reference to the drawings and specific embodiments, so that those skilled in the art can better understand the present application and implement it, but the embodiments are not limiting to the present application.

[0062] Embodiment one: refer to Figure 1 As shown in the drawings, the embodiment of the present application provides a three-dimensional complex environment navigation method of a humanoid robot, including but not limited to the following steps:

[0063] S1, obtaining a random path node;

[0064] S2, constructing a collision detection model according to the gait cycle of the robot; using the collision detection model to detect whether the random path node passes the collision detection; if not, deleting the random path node and returning to S1; if yes, adding the random path node to the random tree;

[0065] For each node added to the random tree, performing collision detection at the node; if the collision detection is passed, the node is taken as a candidate node, otherwise returning to S1;

[0066] Performing plane size detection on each candidate node; if the detection is passed, adding the candidate node to a global path point sequence, otherwise returning to S1;

[0067] S3, screening the global path point sequence according to the complexity of the current plane to obtain complex region path nodes and flat region path nodes;

[0068] S4, acquiring first pose data of the robot when the robot runs along the complex region path nodes, generating motion instructions according to the first pose data; acquiring second pose data of the robot when the robot runs along the flat region path nodes; generating motion instructions according to the second pose data and a preset threshold.

[0069] The embodiment of the application provides a humanoid robot three-dimensional complex environment navigation method, which screens random path nodes through a collision detection model, ensures the safety of the path nodes, avoids collision of the robot in the walking process, and thus improves the reliability of path planning. At the same time, the global path point sequence is classified according to the complexity, which can provide more suitable paths for the robot according to different terrain characteristics, further enhancing the scientificity and adaptability of path planning. Secondly, the design enhances the terrain adaptability of the robot. Different pose data are acquired in the complex region and the flat region respectively, and motion instructions are generated in combination with a preset threshold, which can make the robot more accurately control its own motion, flexibly adapt to different terrains, and improve its walking ability in complex environments. In addition, the design also optimizes the path planning efficiency. By dynamically screening random path nodes, the calculation and processing of invalid nodes are reduced, and the dynamic envelope space calculation cost is reduced. After the path point sequence is classified, motion instructions are generated accordingly, avoiding uniform processing of all path points, and further improving the efficiency of path planning. Finally, the method helps to improve the walking stability of the robot. In the complex region, motion instructions are generated according to the first pose data, which can more accurately control the walking posture of the robot on the complex terrain; in the flat region, motion instructions are generated in combination with the second pose data and the preset threshold, which ensures that the robot walks in a more stable and efficient manner, reduces unnecessary posture adjustment, and thus improves its walking stability on different terrains.

[0070] Specifically, in step S1, random path nodes are randomly generated by using an RRT algorithm (such as 、 、 ), and a dense path with an interval of 0.03m is generated in a three-dimensional environment based on the original RRT algorithm, aiming to modify and add collision detection and path node screening strategies in the original RRT path planning algorithm.

[0071] Specifically, unlike wheeled robots, during the walking process of the humanoid robot, the trajectory of the swing leg when moving from the current supporting leg to the new foot landing point can exceed the size of the self shape and collide with obstacles in the environment. Considering the high computational cost required for high-precision motion space, in the embodiment of the present application, an ellipsoid approximation method is used to model the motion envelope of the swing leg, which reduces the computational complexity and at the same time realizes the rapid collision detection. Therefore, in step S2, the motion envelope space of the swing leg in the walking gait cycle of the humanoid robot is approximated by an ellipse, and the specific steps are as follows:

[0072] S211, the midpoint between the standing foot point and the new swing foot point of the robot is taken as the first center point, the direction of the line connecting the standing foot point and the new swing foot point is taken as the first short axis, and the direction perpendicular to the first short axis is taken as the first long axis; according to the first center point, the first short axis and the first long axis, a first ellipse is obtained.

[0073] Exemplarily, the area of the robot from the standing foot to the new swing foot, including the motion space of the current foot during single-step transition, is represented by ellipse I. The center point is the midpoint between the standing foot point and the new swing foot point, the short axis is along the direction of the line connecting the two feet, and the length of the first short axis is :

[0074] ;

[0075] wherein, represents the new foot landing point, represents the supporting foot landing point.

[0076] Exemplarily, the first long axis is perpendicular to the first short axis, and the length of the first long axis is :

[0077] ;

[0078] wherein, represents the width of the hip of the robot, represents the lateral swing compensation amount to compensate for the lateral swing caused by the lateral tilting of the pelvis of the robot.

[0079] S212, the midpoint between the previous swing foot point and the standing foot point of the robot 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 short axis, and the direction perpendicular to the second short axis is taken as the second long axis; according to the second center point, the second short axis and the second long axis, a second ellipse is obtained.

[0080] Exemplarily, the area of the robot from the previous swing foot to the standing foot, including the motion space of the previous foot during single-step transition, is represented by ellipse II. The length of the second short axis is :

[0081] ;

[0082] wherein, represents the footfall point of the previous swing leg.

[0083] Exemplarily, the second long axis is perpendicular to the second short axis, and the length of the second long axis is .

[0084] .

[0085] S213, in the biped gait cycle, two ellipse regions (i.e. the first ellipse and the second ellipse) realize continuous coverage of the entire motion envelope of the robot through an iterative relationship, and then a collision detection model is obtained.

[0086] Further, the collision detection model is used to detect whether the collision region formed by the random sampling path node generated by the RRT algorithm and the previous two nodes collides with the obstacle, and if no collision occurs, the path node is added to the random tree. Specifically, for the random path node planned by the RRT algorithm, it is checked whether the collision region formed by the line connecting the previous two path nodes collides with the obstacle. The collision region can be referred to as shown in Figure 2 . Wherein, Figure 2 the symbol in the formula (1) represents the current step, and the symbol represents the previous step of the current step. The region contains the entire gait cycle of the robot walking due to leg lifting and swinging, ensuring the feasibility of the path node as the robot footfall point. Only the nodes passing the collision detection are added to the random tree.

[0087] Specifically, the nodes added to the random tree are subjected to collision detection at the node and plane size detection.

[0088] Further, referring to Figure 3 and Figure 4 (the first picture from left to right), for each node added to the random tree, the position of the node is subjected to collision detection, and the collision region is subjected to collision detection at the node in the direction of the node with the size of the biped robot. When it is detected that the region does not collide with the obstacle, the current node is taken as a candidate node. Through the above collision detection, it can be ensured that the footfall will not collide with the obstacle.

[0089] Further, for the candidate node, a collision detection search is performed within a preset range of the parent node height to identify a feasible z value, ensuring that the path node is on a plane and the height difference is within the maximum height that the robot can step. The parent node refers to the node selected as the starting point for growth in the random tree. For the candidate node, the specific steps of performing a collision detection search within a preset range of the parent node height to identify a feasible z value are: within a preset range of the parent node height (representing the maximum step height), such as a range of ±0.15 m, a collision detection search is performed from bottom to top to identify a feasible z value. If there is no collision at a certain height, the height of the state is adjusted to this position, and the state is recorded as a critical state node.

[0090] The above collision detection search method can ensure that the path node does not hang in the air, falls on a three-dimensional plane, and does not exceed the maximum height of the robot step.

[0091] Specifically, in step S2, if it passes the collision detection, it is selected as a candidate node, and if it does not pass, a random path node is reselected and returned to step S1. For each candidate node, refer to Figure 4 (From left to right, the second figure) Strictly verify the ground assurance. The foot projection area extends outward from the node center, and the longitudinal span (1 / 3 of the foot length) and the transverse span (1 / 2 of the foot width) are used to define the sampling area to evaluate the continuity of the plane and whether there is enough foothold area. If the plane detection fails, the path node is removed. For the candidate node, the area is expanded in the node direction to check whether the plane is completely occupied, and the specific steps are:

[0092] S221, obtain the above critical state node, for each path node of the critical state node , extend the foot projection area in the front and rear direction (1 / 3 of the foot length) and the left and right direction (1 / 2 of the foot width) of the node direction with the node as the center, and the foot projection area The expression of the foot projection area is:

[0093] ;

[0094] ;

[0095] Wherein, represents the heading angle of the path node, represents the foot length of the robot, represents the foot width of the robot.

[0096] S222, grid sampling is performed at a map resolution, and an occupancy state at a height corresponding to a position in the map is queried to determine whether the node is on a plane and whether the plane size is sufficient to place a foot. If the occupancy state indicates that the plane size detection is passed, the node is taken as a candidate path node. If the occupancy state indicates that the plane size detection is not passed, the node is deleted.

[0097] The above plane detection fully considers the support of the foot placement point, and ensures that all path nodes are accurately placed on a plane suitable for the robot to stand on, and the planes have sufficient area and stability to provide reliable support for stable walking of the robot.

[0098] Further, in the path optimization stage, a dynamic adaptive strategy is used to screen the original path nodes generated by the above process.

[0099] Specifically, in step S3, according to the complexity of the current plane, the global path point sequence is screened to obtain complex region path nodes and flat region path nodes. The steps for screening are as follows:

[0100] S310, determine the complexity of the current plane, and specifically distinguish between flat regions and complex regions, the purpose of which is to dynamically screen dense path nodes and provide accurate data support for subsequent gait planning of the robot.

[0101] Further, 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, the region is classified as a flat region; otherwise, it is classified as a complex region.

[0102] S320, for path nodes in the global path point sequence that are determined to be in a flat region, every n path nodes capture one path node as a stage target point (also referred to as a temporary target), thereby obtaining flat region path nodes. Wherein, n is a positive integer, and in the embodiment of the present application, it takes a value of 6.

[0103] S330, for path nodes in the global path point sequence that are determined to be in a complex region, all of them are complex region path nodes, i.e., no screening is performed on the nodes. Specifically, the path nodes are directly taken as foot placement points of the robot, and a set of human-like steps that satisfy the motion of the robot are generated by the planner.

[0104] Based on the above steps S320 and S330, the controller in the robot will generate human-like turning and advancing steps that satisfy the motion of the robot.

[0105] Specifically, in step S4, the robot pose data (including real-time three-dimensional position and yaw angle) is continuously received and converted into the global path point sequence obtained by the above algorithm, and the maximum step length and the maximum rotation angle of the robot during walking are set. If the robot is 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 the motion instruction in the robot body coordinate system by a rotation matrix, wherein the error vector includes distance error and angle error.

[0106] For example, the position and quaternion of the humanoid robot and the path node are read, and an error threshold is set. The distance error between the current pose and the next target path node is calculated. In a complex area, the calculated distance error is sent to the robot as the step size.

[0107] Further, when the robot runs along the path node in a flat area, the second pose data of the robot is obtained, and according to the second pose data and the preset threshold, the method for generating the motion instruction is: the second pose data includes the heading angle error, when the heading angle error is not less than the preset threshold, the rotation correction mode is switched to immediately, and the pure rotation motion instruction is generated; when the heading angle error is less than the preset threshold, the error vector between the current pose of the robot and the target point is converted into the motion instruction in the robot body coordinate system by a rotation matrix.

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

[0109] Further, in a flat area, when the next stage target point is the final target point, the focus mode is switched, at this time the distance threshold between the target path node positions is reduced to ensure that the robot can accurately reach the target position.

[0110] In order to better understand step S4, the following exemplary implementation cases are given.

[0111] Exemplarily, the pose data (including rectangular coordinates and yaw angle) of the robot is continuously received and processed along the global path point sequence to calculate the error vector between the current pose and the target point. The global coordinate error is converted into motion commands in the robot body frame using a rotation matrix. Before gait planning, the maximum step length and the maximum rotation angle of the robot during walking are set as: [0.1 m, 0.1 m, 8 yaw], and the distance threshold between the robot position and the target path node position is set as: [0.1 m, 0.1 m]. In complex areas, the error value is directly sent to the robot as a step length. In flat areas, when the yaw angle error is detected to exceed the 20° threshold (including 20°), the rotation correction mode is immediately switched to, a pure rotation command with a size of [0.01, 0, θ] is generated to ensure that the robot aligns the direction first. When the yaw angle error is less than the 20° threshold, the error value is sent to the robot as a step length, while ensuring that the step length command is less than the maximum step length set by the robot, until the distance to the stage target point is less than the set distance threshold, the next path node is taken as the stage target point. Along the stage target point, the target point is approached, and when the next stage target point is the final target point, the focus mode is switched, at this time the distance threshold between the target path node positions is reduced to: [0.06 m, 0.06 m], ensuring that the robot can accurately reach the target position. The end point mode is automatically activated when approaching the final path point, at this time the position tolerance is reduced by 60% to 0.06 meters, and the angle tolerance is reduced to 2°, ensuring that the robot accurately reaches the target position.

[0112] The above framework realizes seamless transition from planar movement to vertical climbing through the cooperation of rotation mode, end point mode and environmental information response, and provides an efficient and complete solution for autonomous navigation of humanoid robots in complex three-dimensional environments.

[0113] Specifically, based on the above steps, the gait planning algorithm for the humanoid robot is given, which includes input parameters, output parameters and algorithm steps, and the specific content is as follows:

[0114] The input parameters include the current position and pose of the robot, a series of path points, error threshold and maximum step length limit between the robot and the path points.

[0115] The current position and pose of the robot are obtained, including three-dimensional coordinates x , y , z and orientation angle θ , which are marked as:

[0116] .

[0117] The position and pose information contained in each path point in the series of path points are marked as:

[0118] .

[0119] Setting an error threshold between the robot and the waypoints , to determine whether the robot is close to the waypoints. Set the maximum step limit, including the maximum step size in x 、 y direction and the maximum adjustment amount of angle, marked as:

[0120] .

[0121] The output parameter is the motion instruction, which includes the step size in direction, marked as:

[0122] .

[0123] For algorithm steps, refer to Figure 5 , the specific steps are:

[0124] Step 1: Initialize the global path point sequence list ; add the starting point to the list ; define a function Distance(·) to calculate the distance and angular deviation between the current position and attitude of the robot and the path point, and the return value is represented as .

[0125] Step 2: To achieve autonomous navigation of humanoid robots in three-dimensional environments, the robot needs to efficiently plan paths and generate collision-free gaits in various complex three-dimensional environments. Therefore, traverse the path point set and process differently according to the area where the path point is located (complex area or flat area):

[0126] In complex areas: for each path point located in a complex area, directly calculate the distance and angular deviation between the robot and the path point, and use it as the motion instruction.

[0127] In flat areas, there are three cases:

[0128] Case 1: If the angular deviation of the path point is greater than 20°, the motion instruction is to move 0.01 meters in the x direction, y no movement in the direction, and the angle adjustment is not more than the error of the maximum allowed angle .

[0129] 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 distance between the robot and the path point is less than 0.6 times the error threshold, the motion instruction is the smaller value between the maximum step size and the distance deviation in each direction.

[0130] Case 3: If the current path point is the last path point, when the distance between the robot and the path point is less than 0.6 times the error threshold , the motion instruction is the smaller value between the maximum step size and the error in each direction.

[0131] The above algorithm dynamically adjusts the motion instruction of the robot by calculating the distance and angular deviation between the robot and the path point. In complex areas, the algorithm mainly focuses on angular adjustment; in flat areas, the algorithm dynamically adjusts the step size according to the distance and angular deviation, and increases the accuracy when approaching the target point, ensuring that the robot can accurately reach the target position. In this way, the algorithm realizes the gait planning of humanoid robots in complex three-dimensional environments, ensuring that the robot can efficiently and safely navigate and walk.

[0132] The improved RRT path planning method based on double-ellipsoid envelope collision space and three-dimensional terrain perception proposed by the embodiment of the application effectively solves the problems of low leg movement collision detection efficiency and insufficient three-dimensional path planning accuracy of humanoid robots in unstructured environments. In addition, the application also proposes a terrain-adaptive hierarchical motion control method that can improve the path tracking accuracy of humanoid robots in complex terrain and achieve coordinated optimization of motion efficiency and accuracy. In order to further verify the path planning algorithm proposed by the embodiment of the application, a humanoid robot navigation and path planning three-dimensional scene containing simulation and real scenes is designed. The real scene contains obstacles and steps that can be stepped on, while the simulation scene adds obstacles and steps through V-REP software and visualizes them, aiming to verify the method twice.

[0133] Specifically, virtual scenes and real scenes with different terrain environments are established. In the simulation environment, the robot needs to avoid obstacles with sizes of 0.3 meters x 0.3 meters x 0.5 meters and 0.2 meters x 0.3 meters x 0.2 meters (because their height exceeds the robot's capability range), and climb a step with a height of 0.12 meters. In the real environment, the robot needs to avoid obstacles with sizes of 0.3 meters x 0.3 meters x 0.5 meters, and climb a step with a height of 0.06 meters.

[0134] Further, the Roban humanoid robot is used to verify the above method in simulation and real environments. Simulation and real experiment results show that the method described in the embodiment of the application can plan a path node (as shown in Figures 6-7 ) that matches the walking of a humanoid robot in a three-dimensional environment containing obstacles and steps. Through Figures 6 to 7As 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.

[0135] 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.

[0136] 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.

[0137] This embodiment provides a three-dimensional complex environment navigation system for a humanoid robot, including:

[0138] The node acquisition module is used to acquire nodes along random paths.

[0139] 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.

[0140] The second detection module is used to add random path nodes to the random tree if the collision detection is passed;

[0141] 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.

[0142] 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.

[0143] The filtering module is used to filter the global path point sequence based on the complexity of the current plane, and obtain path nodes in complex areas and path nodes in flat areas.

[0144] The instruction generation module is configured to acquire first pose data of the robot when the robot runs along the complex area path node, and generate a motion instruction according to the first pose data; acquire second pose data of the robot when the robot runs along the flat area path node; and generate a motion instruction according to the second pose data and a preset threshold.

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

[0146] The present application is described with reference to flowcharts and / or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, as well as combinations of flows and / or blocks in the flowcharts 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, a special-purpose computer, an embedded processor, or other programmable data processing apparatus to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing apparatus produce a device that implements the functions specified in the flowcharts and / or block diagrams. Figure One one or more flows and / or blocks Figure One means for performing the functions specified in the flowchart

[0147] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to work in a specific manner, so that the instructions stored in the computer-readable memory produce a manufactured product including instruction means, which implements the functions specified in the flowcharts and / or block diagrams. Figure One one or more flows and / or blocks Figure One means for performing the functions specified in the flowchart

[0148] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus, so that a series of operation steps are performed on the computer or other programmable data processing apparatus to produce a computer-implemented process, so that the instructions executed on the computer or other programmable data processing apparatus provide a process for implementing the functions specified in the flowcharts and / or block diagrams. Figure One one or more flows and / or blocks Figure One means for performing the functions specified in the flowchart

[0149] Obviously, the above embodiments are merely example for clearly illustrating, and are not limitation to the embodiments. For ordinary skilled in the art, other different forms of changes or variations can be made on the basis of the above description. Here, all the embodiments need not and can not be exhausted. The obvious changes or variations derived therefrom are still within the scope of the present 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 support foot landing point and the new landing point is taken as the first center point; the direction of the line connecting the support foot landing point and the new landing point is taken as the first minor axis; the direction perpendicular to the first minor axis is taken as the first major axis; and a first ellipse is obtained based on the first center point, the first minor axis, and the first major axis. The midpoint between the landing point of the robot's previous swing foot and the landing point of the supporting foot is taken as the second center point; the direction of the line connecting the landing point of the previous swing foot and the landing point of the supporting foot is taken as the second minor axis; the direction perpendicular to the second minor axis is taken as the second major axis; based on the second center point, the second minor axis, and the second major axis, the second ellipse is obtained. 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 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.

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