Humanoid robot navigation method and system

Abstract

The application provides a kind of humanoid robot navigation method and system, the method comprises: according to first radar environment perception data and second radar environment perception data, constructs 2.5D grid map;With preset robot speed instruction and ontology state information, input to reinforcement learning strategy model, obtain the centroid speed prediction value of humanoid robot;Based on adaptive Kalman filtering, the centroid speed prediction value and third radar environment perception data are fused and processed, and the fusion pose estimation value is obtained;2.5D grid map and fusion pose estimation value are incrementally planned, and the target planning path is obtained;Based on target waypoint sequence and fusion pose estimation value, generate target speed instruction;The joint control amount of next action of humanoid robot is obtained by inputting target speed instruction into reinforcement learning strategy model.The application improves the obstacle avoidance decision accuracy and motion execution stability of humanoid robot in dynamic narrow environment.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to a navigation method and system for a humanoid robot. Background Technology

[0002] In dynamic and confined indoor environments, existing humanoid robots, when modeling their environment using a single sensor (such as overhead LiDAR), suffer from severe sensory obstruction due to static structures (shelves, columns, etc.) and frequently moving pedestrians. This makes it difficult to acquire complete and continuous environmental information and construct a globally consistent map. Consequently, the reliability and real-time performance of path planning are significantly compromised, making it difficult for the robot to effectively achieve autonomous navigation and dynamic obstacle avoidance while maintaining stable walking, thus increasing the risk of collisions.

[0003] Therefore, there is an urgent need for a humanoid robot navigation method and system to solve the above problems. Summary of the Invention

[0004] To address the problems existing in the prior art, this invention provides a humanoid robot navigation method and system.

[0005] This invention provides a navigation method for a humanoid robot, comprising: Based on the first radar environmental perception data and the second radar environmental perception data, a 2.5D grid map corresponding to the target working environment of the humanoid robot during the operation phase is constructed. The first radar environmental perception data is obtained based on a handheld LiDAR; the second radar environmental perception data is obtained based on multiple LiDARs fixedly installed in a preset area of ​​the target working environment. The preset robot speed command and the current body state information of the humanoid robot are input into the reinforcement learning strategy model to obtain the predicted value of the center of mass velocity of the humanoid robot output by the reinforcement learning strategy model. The reinforcement learning strategy model is trained based on the Actor-Critic network constructed by the multilayer perceptron. Based on adaptive Kalman filtering, the predicted centroid velocity and the third radar environmental perception data are fused to obtain the current fused pose estimate of the humanoid robot; and incremental path planning is performed on the 2.5D grid map and the fused pose estimate to obtain the target planning path. The third radar environmental perception data is obtained based on the humanoid robot's own lidar. Based on the target waypoint sequence and the fused pose estimation value, a target velocity command is generated; and the target velocity command is input into the reinforcement learning policy model to obtain the joint control quantity of the humanoid robot's next action. The target waypoint sequence is constructed from waypoints in the target planning path that meet a preset forward distance threshold.

[0006] According to a humanoid robot navigation method provided by the present invention, the step of constructing a 2.5D grid map corresponding to the target operating environment of the humanoid robot during its operation phase based on first radar environmental perception data and second radar environmental perception data includes: Based on the data from the first inertial measurement unit, a global coordinate system transformation is performed on the first radar environmental perception data to obtain a static environmental map corresponding to the target operating environment, wherein the first inertial measurement unit data is the inertial measurement unit data corresponding to the first radar environmental perception data. Based on the data from the second inertial measurement unit, a global coordinate system transformation is performed on the second radar environmental perception data to obtain a dynamic obstacle map corresponding to the target working environment of the humanoid robot during the operation phase. The second inertial measurement unit data is the inertial measurement unit data corresponding to the second radar environmental perception data. Based on the static environment map and the dynamic obstacle map, a joint environment model is constructed. Then, based on an extended Kalman filter, the joint observation vector corresponding to the second inertial measurement unit data in the joint environment model is optimized to obtain an optimized joint environment model. The joint observation vector is obtained based on the stacking of first point-surface residuals. The first point-surface residuals are obtained by performing neighborhood point search and local plane fitting on the points corresponding to the second inertial measurement unit data in the joint environment model. Based on the height coordinates corresponding to the highest point in the optimized joint environment model, a two-dimensional raster projection is performed on the optimized joint environment model to obtain the 2.5D raster map.

[0007] According to a humanoid robot navigation method provided by the present invention, the step of fusing the predicted centroid velocity value and the third radar environmental perception data based on adaptive Kalman filtering to obtain the current fused pose estimate of the humanoid robot includes: Based on the data from the third inertial measurement unit, a global coordinate system transformation is performed on the environmental perception data from the third radar to obtain the current self-pose radar positioning data of the humanoid robot. The data from the third inertial measurement unit corresponds to the environmental perception data from the third radar. Based on the second point-plane residual, the self-pose radar positioning data is constrained, and the constrained self-pose radar positioning data is scanned and matched or mapped to obtain the planar position observation value corresponding to the humanoid robot's own lidar. The second point-plane residual is obtained by searching for neighboring points and fitting local planes in the joint environment model based on the points in the third inertial measurement unit. Based on the data from the third inertial measurement unit, the predicted value of the center of mass velocity is transformed to obtain the observed value of the planar linear velocity. Based on the planar position observations and the planar linear velocity observations, fused observations are constructed, and the fused observations are updated based on the adaptive Kalman filter to obtain the fused pose estimate.

[0008] According to a humanoid robot navigation method provided by the present invention, the method further includes: Based on the preset positioning quality index, the variance of position observation noise is adjusted to obtain the adjusted variance of position observation noise. The weight values ​​of the planar position observations are adjusted based on the adjusted position observation noise variance.

[0009] According to a humanoid robot navigation method provided by the present invention, the incremental path planning of the 2.5D grid map and the fused pose estimation value to obtain the target planned path includes: Based on D The Lite algorithm performs incremental path search on the 2.5D grid map and the fused pose estimate to obtain a discrete path sequence; The discrete path sequence is geometrically smoothed to obtain the target planned path.

[0010] According to a humanoid robot navigation method provided by the present invention, the step of generating a target velocity command based on a target waypoint sequence and the fused pose estimation value includes: Based on the coordinate difference and angular velocity difference between the target waypoint sequence and the fused pose estimate, linear velocity command information and angular velocity command information are constructed respectively; The target velocity command is generated based on the linear velocity command information and the angular velocity command information.

[0011] The present invention also provides a humanoid robot navigation system, comprising: The multi-radar environmental perception module is used to construct a 2.5D grid map corresponding to the target working environment of the humanoid robot during its operation phase based on the first radar environmental perception data and the second radar environmental perception data. The first radar environmental perception data is obtained based on a handheld LiDAR; the second radar environmental perception data is obtained based on multiple LiDARs fixedly installed in a preset area of ​​the target working environment. The speed prediction module is used to input the preset robot speed command and the current body state information of the humanoid robot into the reinforcement learning strategy model to obtain the predicted value of the center of mass speed of the humanoid robot output by the reinforcement learning strategy model. The reinforcement learning strategy model is trained based on the Actor-Critic network constructed by the multilayer perceptron. The path planning module is used to fuse the predicted centroid velocity and the third radar environmental perception data based on adaptive Kalman filtering to obtain the current fused pose estimate of the humanoid robot; and to perform incremental path planning on the 2.5D grid map and the fused pose estimate to obtain the target planned path, wherein the third radar environmental perception data is obtained based on the humanoid robot's own lidar. The navigation prediction module is used to generate a target velocity command based on the target waypoint sequence and the fused pose estimation value; and input the target velocity command into the reinforcement learning policy model to obtain the joint control quantity of the humanoid robot's next action, wherein the target waypoint sequence is constructed from waypoints in the target planning path that meet a preset forward distance threshold.

[0012] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the humanoid robot navigation method as described above.

[0013] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the humanoid robot navigation method as described above.

[0014] The present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the humanoid robot navigation method as described above.

[0015] The humanoid robot navigation method and system provided by this invention significantly reduces perception blind spots and provides more comprehensive dynamic environmental information by deploying multiple lidars to work in coordination with the humanoid robot's own radar. Simultaneously, based on reinforcement learning algorithms, it not only outputs robot joint movements but also predicts the robot's center-of-gravity velocity in real time. The predicted center-of-gravity velocity is then fused with lidar positioning information to generate a smooth and robust global state estimate. This ensures continuous and reliable position and velocity feedback even when the lidar is obstructed, improving the accuracy of obstacle avoidance decisions and the stability of motion execution for the humanoid robot in dynamic, confined environments. Attached Figure Description

[0016] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0017] Figure 1 A flowchart illustrating the humanoid robot navigation method provided by this invention; Figure 2 This is a schematic diagram of the structure of the humanoid robot navigation system provided by the present invention; Figure 3 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0018] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0019] The research and application of humanoid robots has become an important direction in the field of robotics, especially in narrow and human-centered indoor environments such as convenience stores and exhibition halls, where robots need to achieve autonomous navigation and dynamic obstacle avoidance while maintaining stable walking.

[0020] In dynamic, confined environments, humanoid robots face significant obstacle avoidance challenges: First, static structures such as shelves and pillars in narrow spaces, as well as frequently moving pedestrians, cause severe sensory occlusion, making it difficult for single-sensor-based environmental modeling methods, such as overhead airborne LiDAR, to acquire complete and continuous environmental information. Consequently, they cannot build a globally consistent map, which in turn affects the reliability and real-time performance of path planning. Second, the bipedal walking mechanism of humanoid robots inherently has dynamic constraints such as intermittent contact and torso swing. When performing sharp turns or obstacle avoidance maneuvers in narrow passages, gait instability and tracking error accumulation are easily triggered, increasing the risk of collisions.

[0021] To address the problems of large perception blind spots, frequent path obstruction by dynamic obstacles, decreased positioning accuracy, and path planning oscillations encountered by humanoid robots during navigation in narrow, dynamic, and heavily occluded environments, this invention proposes a humanoid robot navigation method based on a multi-radar environmental perception system. This multi-radar environmental perception system comprises a handheld mapping radar unit, a fixed-mounted environmental perception radar unit, and an overhead airborne positioning radar unit. The handheld mapping radar unit is used for static environmental mapping during deployment, the fixed-mounted environmental perception radar unit is used for dynamic multi-view perception during operation, and the overhead airborne positioning radar unit is used for robot self-localization. These three radar units constitute a hierarchical multi-view environmental perception system. Each radar unit is also equipped with a corresponding inertial measurement unit (IMU) to provide high-frequency predictive states.

[0022] When a humanoid robot performs autonomous navigation tasks, a highly robust global environment model with strong anti-interference capabilities due to occlusion can be constructed through a multi-radar environmental perception process. Unlike existing mapping methods that rely solely on a single onboard LiDAR, this invention uses a multi-radar collaborative deployment structure to decouple and hierarchically integrate static environment prior mapping, dynamic multi-view environment perception, and pure robot positioning calculation. This eliminates blind spots at the system structure level and improves positioning stability in dynamic environments.

[0023] Figure 1 This is a flowchart illustrating the humanoid robot navigation method provided by the present invention, as shown below. Figure 1 As shown, the present invention provides a humanoid robot navigation method, comprising: Step 101: Based on the first radar environmental perception data and the second radar environmental perception data, construct a 2.5D grid map corresponding to the target working environment of the humanoid robot during the operation phase. The first radar environmental perception data is obtained based on a handheld lidar; the second radar environmental perception data is obtained based on multiple lidars fixedly installed in a preset area of ​​the target working environment.

[0024] In this invention, during the system deployment phase, a handheld LiDAR is used to scan the working environment and collect point data, i.e., the first radar environmental perception data. This point data, after coordinate transformation (from the handheld LiDAR coordinate system to the global coordinate system), is used to construct a static environmental map to describe fixed structures such as walls, shelves, and columns. Pre-constructing this static environmental prior map improves the positioning stability in the early stages of system operation and reduces mapping uncertainty during online operation.

[0025] During the robot's operation phase, environmental point cloud data, i.e., second radar environmental perception data, is continuously collected using a fixed LiDAR. After converting the collected points to a global coordinate system, a dynamic environmental point cloud set is constructed. In this invention, the static environmental map and the dynamic environmental point cloud set can be combined through a tightly coupled fusion method (searching for a set of neighboring points in the joint environment model for each point, fitting a local plane to obtain a reference point and a unit normal vector, constructing point-surface residuals, stacking the residuals of all fixed radars to form a joint observation vector, and inputting it into a unified error state extended Kalman filter for state optimization). This achieves tightly coupled fusion of multi-radar observations in the same state space, improving pose estimation accuracy and anti-occlusion capability.

[0026] Furthermore, based on the fused and optimized point cloud, a two-dimensional grid projection is performed, and the grid height is updated using the maximum height aggregation rule, thereby constructing a global 2.5D grid map in real time. The 2.5D grid map is used to represent the spatial structure and dynamic obstacle information of the environment, providing a foundation for the navigation of humanoid robots in the working environment.

[0027] Step 102: Input the preset robot speed command and the current body state information of the humanoid robot into the reinforcement learning strategy model to obtain the predicted value of the center of mass velocity of the humanoid robot output by the reinforcement learning strategy model. The reinforcement learning strategy model is trained based on an Actor-Critic network constructed using a multilayer perceptron.

[0028] In this invention, the reinforcement learning policy model is trained using an Actor-Critic network built upon a multilayer perceptron. The Actor network outputs the joint control objective, while the Critic network outputs the state value function to guide policy updates.

[0029] Specifically, the preset robot speed command (as a high-level control input) and the current body state information of the humanoid robot (such as joint angles and angular velocities) are first input into the reinforcement learning policy model. After calculation, the reinforcement learning policy model outputs the predicted value of the humanoid robot's center of mass velocity. The predicted value of the center of mass velocity includes the predicted values ​​of the robot's x-axis and y-axis velocities in the body coordinate system, which are used for subsequent fusion with radar positioning observation data to obtain a more accurate robot pose estimation.

[0030] Step 103: Based on adaptive Kalman filtering, the predicted centroid velocity and the third radar environmental perception data are fused to obtain the current fused pose estimate of the humanoid robot; and incremental path planning is performed on the 2.5D grid map and the fused pose estimate to obtain the target planning path. The third radar environmental perception data is obtained based on the humanoid robot's own lidar. In this invention, the third radar environmental perception data is acquired based on the humanoid robot's own lidar (airborne radar). To avoid occlusion and contamination of the environmental map by the robot's structure, and to prevent instantaneous posture disturbances caused by robot movement from erroneously updating the global environmental model, the airborne radar is only used for the robot's own pose calculation and does not participate in updating the global environmental map. After the points collected by the airborne radar are transformed to the global coordinate system, a set of neighborhood points is searched in the joint environmental model and a local plane is fitted to obtain the reference point and unit normal vector. The point-surface residual is then constructed to constrain the robot's current pose estimation.

[0031] In this invention, a fused state vector and a system state model are constructed. The filter state vector is defined to include planar position and planar velocity information, and a prediction model is established using the constant velocity assumption. The observation sources include planar position observations from the lidar positioning output and predicted linear velocities (velocity observations transformed to the world coordinate system by IMU attitude rotation) from the reinforcement learning policy network output. After constructing the fused observations, a Kalman filter update step is used to calculate the residuals, and a standard Kalman gain update is performed. Optionally, to enhance the system's robustness to radar degradation, a positioning quality index is introduced to adjust the position observation noise variance.

[0032] Through these steps, the predicted centroid velocity and the environmental perception data from the third radar are fused based on adaptive Kalman filtering to obtain the current fused pose estimate of the humanoid robot. The fused pose estimate has the effects of suppressing position jitter caused by instantaneous matching error of lidar, ensuring motion continuity, avoiding path planning input jumps, and maintaining stable positioning under dynamic occlusion or feature sparsity conditions.

[0033] Furthermore, the constructed 2.5D grid map and the fused pose estimate are input into the planner, and an incremental algorithm (such as D...) is used. Path planning is performed using the Lite algorithm. An adjacency graph is constructed based on a 2.5D raster map, and an edge cost function is defined. Lite's incremental update rule only recalculates the affected nodes, thereby achieving local replanning and obtaining a discrete path sequence, i.e., the target planning path.

[0034] Step 104: Based on the target waypoint sequence and the fused pose estimation value, generate a target velocity command; and input the target velocity command into the reinforcement learning policy model to obtain the joint control quantity of the humanoid robot's next action, wherein the target waypoint sequence is constructed from waypoints in the target planning path that meet a preset forward distance threshold.

[0035] In this invention, the target waypoint sequence is constructed from waypoints along the target planned path that satisfy a preset forward look-ahead distance threshold. During the execution phase, to avoid oscillations caused by point-by-point tracking, a sliding forward look-ahead mechanism is used to select target waypoints. The current robot planar position is defined, and waypoints that satisfy the forward look-ahead distance constraint are selected from the smooth trajectory. It is ensured that the distance between this waypoint and the current position is greater than the forward look-ahead distance threshold, ensuring that the robot always moves forward and avoiding frequent waypoint backtracking.

[0036] Furthermore, based on the fused pose estimation values ​​(including planar position and yaw angle information) and the target waypoint (including planar position and target heading angle information), planar velocity commands (linear velocity command and angular velocity command) are generated. The linear velocity command is calculated based on the position error and position proportional gain, while the angular velocity command is calculated based on the heading error and heading proportional gain, thus yielding the target velocity command.

[0037] Finally, the generated target velocity command is input into the reinforcement learning policy model. The model outputs the joint control quantity for the humanoid robot's next action, which is executed by the lower-level joint controller, enabling the humanoid robot to accurately track its trajectory and avoid obstacles in real time in the target working environment.

[0038] The humanoid robot navigation method provided by this invention significantly reduces blind spots and provides more comprehensive dynamic environmental information by deploying multiple LiDARs to work in coordination with the humanoid robot's own radar. Simultaneously, based on reinforcement learning algorithms, it not only outputs robot joint movements but also predicts the robot's center-of-gravity velocity in real time. The predicted center-of-gravity velocity is then fused with LiDAR positioning information to generate a smooth and robust global state estimate. This ensures continuous and reliable position and velocity feedback even when the LiDAR is obstructed, improving the accuracy of obstacle avoidance decisions and the stability of motion execution for the humanoid robot in dynamic, confined environments.

[0039] Based on the above embodiments, the step of constructing a 2.5D grid map corresponding to the target working environment of the humanoid robot during its operation phase, based on the first radar environmental perception data and the second radar environmental perception data, includes: Based on the data from the first inertial measurement unit, a global coordinate system transformation is performed on the first radar environmental perception data to obtain a static environmental map corresponding to the target operating environment, wherein the first inertial measurement unit data is the inertial measurement unit data corresponding to the first radar environmental perception data. Based on the data from the second inertial measurement unit, a global coordinate system transformation is performed on the second radar environmental perception data to obtain a dynamic obstacle map corresponding to the target working environment of the humanoid robot during the operation phase. The second inertial measurement unit data is the inertial measurement unit data corresponding to the second radar environmental perception data. Based on the static environment map and the dynamic obstacle map, a joint environment model is constructed. Then, based on an extended Kalman filter, the joint observation vector corresponding to the second inertial measurement unit data in the joint environment model is optimized to obtain an optimized joint environment model. The joint observation vector is obtained based on the stacking of first point-surface residuals. The first point-surface residuals are obtained by performing neighborhood point search and local plane fitting on the points corresponding to the second inertial measurement unit data in the joint environment model. Based on the height coordinates corresponding to the highest point in the optimized joint environment model, a two-dimensional raster projection is performed on the optimized joint environment model to obtain the 2.5D raster map.

[0040] In this invention, the first radar refers to a handheld mapping radar unit. The handheld mapping radar is used during the system deployment phase to scan the static structure of the environment, acquiring point cloud data of fixed structures such as walls, shelves, and columns. This data constitutes the first radar's environmental perception data. The first inertial measurement unit (IMU) data is the data collected by the IMU corresponding to the handheld mapping radar. The IMU can measure its own motion information, such as acceleration and angular velocity, which allows its pose in space to be determined.

[0041] The second radar refers to the fixed-mounted environmental perception radar unit. During the robot's operation, the fixed-mounted environmental perception radar continuously collects dynamic environmental point cloud data from an external perspective; this data constitutes the second radar's environmental perception data. The second inertial measurement unit (IMU) data corresponds to the fixed-mounted environmental perception radar.

[0042] To address the problem of incomplete environmental perception caused by occlusion in confined environments, this invention employs a multi-radar environmental perception system comprised of handheld, fixed, and airborne LiDARs. This system achieves fusion modeling of static and dynamic obstacle maps and constructs a global 2.5D grid map in real time. The airborne LiDAR, positioned on the top of the humanoid robot's head or upper torso, is used for robot localization calculations and employs a mapping and localization separation mechanism to prevent map contamination caused by robot occlusion. The fixed LiDAR can be installed upside down on the ceiling above blind spots in the working environment.

[0043] Specifically, the radar units are first deployed in the working environment and a unified coordinate modeling relationship is established: a handheld lidar is deployed for prior mapping of the static environment; two fixed lidars are deployed for continuous perception of the dynamic environment. The number of fixed lidars can be adjusted according to the actual working environment. This invention uses two fixed lidars for illustration; an airborne lidar is deployed for robot self-localization.

[0044] Furthermore, extrinsic parameter calibration is performed on each radar and its corresponding IMU to obtain the rigid body transformation matrix (i.e., extrinsic parameter matrix) of each radar reaching the IMU. The extrinsic parameter matrix can be expressed as: in, Indicates the first Rigid body transformation from radar coordinate system to IMU coordinate system; Represents a three-dimensional rigid body transformation group; 1 represents a handheld radar, i.e., the first radar; 1 and 2 represent fixed radars, i.e., the second radars. In this invention, there are two second radars. This refers to airborne radar, i.e., the third radar.

[0045] Meanwhile, the pose of each radar's IMU in the global coordinate system is defined as follows: in, It is estimated in real time by the error state extended Kalman filter algorithm.

[0046] Through the above spatial modeling, a unified transformation link is established: that is, through inertial measurement unit data, the radar coordinate system is transformed to the IMU coordinate system, and finally to the global coordinate system, so that multi-radar observation data can be expressed in the same global coordinate system, providing a spatial consistency basis for subsequent fusion processing.

[0047] Next, a static environment prior map, i.e., a static environment map, is constructed. Specifically, this is done using a handheld LiDAR. The working environment is scanned to obtain point clouds of fixed structures such as walls, shelves and columns, and a static environment prior map is constructed.

[0048] Specifically, let the handheld lidar collect the first... j The points are , The process of converting points collected by a handheld lidar to the global coordinate system is as follows: in, This represents the points collected by the handheld lidar after being transformed into the global coordinate system. and This is data from the first inertial measurement unit. This represents the extrinsic parameter matrix from the handheld lidar coordinate system to the IMU. This indicates the pose of the IMU corresponding to the handheld lidar to the global coordinate system.

[0049] Then, the coordinates of all points obtained by the handheld LiDAR scan are transformed in the manner described above, and a static environment map is constructed: in, This invention represents a static environment map used to describe fixed structures such as walls, shelves, and columns. By pre-constructing a static environment map, this invention can improve the positioning stability during the initial stage of system operation and reduce mapping uncertainty during online operation.

[0050] Simultaneously, dynamic environment point cloud acquisition and tight-coupled fusion are achieved based on fixed lidar. Specifically, fixed lidar... and Continuously collect environmental point cloud data to build a dynamic obstacle map in real time, and fuse it with the static environmental map in the same coordinate system. The position and attitude of the fixed LiDAR are known data.

[0051] In this invention, the first laser radar acquisition is set to... The points are ,in, This represents two fixed lidar units. Then, the points collected by the fixed lidar units are transformed to the global coordinate system: in, This represents the points acquired by the fixed lidar after being transformed into the global coordinate system; and This is data from the second inertial measurement unit. This represents the extrinsic parameter matrix from the fixed lidar coordinate system to the IMU. This represents the pose of the IMU corresponding to the fixed lidar to the global coordinate system. .

[0052] Furthermore, the point cloud from the fixed lidar is constructed into a dynamic environmental point cloud set, i.e., a dynamic obstacle map. : And define a joint environment model for: In this invention, to achieve tight coupling and fusion of multi-radar data, each point corresponding to the data of the second inertial measurement unit is defined in the joint environment model. Search for the neighborhood point set and fit a local plane to obtain the reference point. and unit normal vector Construct point-to-surface residuals: in, Indicates the first The first fixed lidar The point-to-surface residual of each point Represents the unit normal vector. This represents a reference point on a local plane.

[0053] Furthermore, the point and area residuals from all fixed lidars are stacked to form a joint observation vector, which is then input into a unified error state extended Kalman filter for state optimization. This achieves tightly coupled fusion of multi-radar observations within the same state space. The extended Kalman filter is an algorithm for estimating system state, capable of continuously updating and optimizing the system state estimate using the system's dynamic model and observation data. By processing the joint observation vector, an optimized joint environment model can be obtained, making the data in the model more accurate and reliable. Unlike simple point cloud stitching, this residual-level fusion method significantly improves pose estimation accuracy and anti-occlusion capability.

[0054] Then, based on the fused and optimized point cloud, a global 2.5D raster map is constructed and its height representation is updated in real time to provide a compact and computable environmental model for planning.

[0055] Specifically, the point cloud in the optimized joint environment model is projected onto two-dimensional raster coordinates, and the raster height is updated using the maximum height aggregation rule: in, For the discretized raster row and column indexes, This represents the height value of the corresponding grid cell in the current 2.5D grid map. For point cloud points that fall into this grid, Point The height coordinates.

[0056] In this invention, by continuously projecting and updating the dynamic point cloud collected by fixed LiDAR, the 2.5D grid map can simultaneously reflect the distribution of static structures and dynamic obstacles. This provides a globally consistent and real-time updated map input for subsequent incremental path planning, facilitating path planning and navigation for the robot based on map information. Furthermore, stacking the residuals of all fixed LiDARs to form a joint observation vector significantly improves pose estimation accuracy and anti-occlusion capability.

[0057] Based on the above embodiments, the process of fusing the predicted centroid velocity and the third radar environmental perception data using adaptive Kalman filtering to obtain the current fused pose estimate of the humanoid robot includes: Based on the data from the third inertial measurement unit, a global coordinate system transformation is performed on the environmental perception data from the third radar to obtain the current self-pose radar positioning data of the humanoid robot. The data from the third inertial measurement unit corresponds to the environmental perception data from the third radar. Based on the second point-plane residual, the self-pose radar positioning data is constrained, and the constrained self-pose radar positioning data is scanned and matched or mapped to obtain the planar position observation value corresponding to the humanoid robot's own lidar. The second point-plane residual is obtained by searching for neighboring points and fitting local planes in the joint environment model based on the points in the third inertial measurement unit. Based on the data from the third inertial measurement unit, the predicted value of the center of mass velocity is transformed to obtain the observed value of the planar linear velocity. Based on the planar position observations and the planar linear velocity observations, fused observations are constructed, and the fused observations are updated based on the adaptive Kalman filter to obtain the fused pose estimate.

[0058] In this invention, in order to avoid the humanoid robot's body structure from obscuring and polluting the environmental map (such as a dynamic obstacle map), and to prevent the instantaneous posture disturbances caused by the humanoid robot's movement from causing erroneous updates to the global environmental model (i.e., the joint environmental model), it is necessary to further adopt a mapping and localization function separation mechanism.

[0059] Specifically, after the static environment map and the dynamic environment point cloud fusion of fixed LiDAR have been completed, a joint environment model has been formed. Based on this, airborne radar... Its function is limited to calculating the pose of the humanoid robot itself, and it does not participate in updating the global environment map.

[0060] Suppose the airborne radar collects the first... The points are , will point The process of transforming to the global coordinate system is as follows: in, This represents the point acquired by the airborne radar after being transformed into the global coordinate system, i.e., the self-positioning radar positioning data; and This is data from the third inertial measurement unit. The extrinsic parameter matrix from the airborne radar coordinate system to the IMU. This represents the currently estimated global pose of the IMU.

[0061] Furthermore, for each point In the joint environment model The reference point is obtained by searching the neighborhood point set and fitting the local plane. With unit normal vector Constructing point-to-surface residuals : in, .

[0062] In this invention, point-to-surface residuals It is only used to constrain the current pose estimation of the humanoid robot and does not participate in the dynamic point cloud set. Incremental updates are implemented. This mapping and localization separation mechanism significantly avoids map contamination caused by robot occlusion and short-term dynamic interactions, improving the purity and consistency of the environmental model.

[0063] Next, based on the calculated second point-plane residual, constraints are applied to the robot's own pose radar positioning data. If the second point-plane residual is large, it indicates a significant deviation between the current positioning data and the surrounding local planar environment, potentially indicating a positioning error. In this case, the positioning data can be adjusted or its range of variation limited to better reflect the actual environment. If the second point-plane residual is small, it indicates relatively accurate positioning data, which can maintain stability to some extent. Through this constraint, constrained self-pose radar positioning data is obtained, which can then be used in subsequent scanning matching or map positioning processes to obtain the planar position observations corresponding to the humanoid robot's own LiDAR, thus improving the accuracy and reliability of the humanoid robot's positioning data.

[0064] In this invention, an end-to-end reinforcement learning policy network is trained in a simulation environment. When the network receives a high-level velocity command, it can directly output the robot joint control torque and simultaneously predict the robot's current center of mass velocity.

[0065] Specifically, a reinforcement learning policy network based on the Actor-Critic architecture is first constructed, where the Actor network is used to output robot control actions, and the Critic network is used to output state value functions to guide policy parameter updates.

[0066] To meet the real-time requirements of engineering deployment, the Actor and Critic networks in this invention can be built using a multilayer perceptron (MLP) structure. The number of network layers and the size of the hidden layers can be set according to the number of degrees of freedom and observation dimensions of the humanoid robot, so as to balance inference efficiency and control expression capabilities.

[0067] In one embodiment, the Actor network at time... The output action can be represented as: in, The target vector for joint control; This is the predicted value of the center of mass velocity for a humanoid robot.

[0068] Furthermore, the predicted value of the center of mass velocity can be expressed as: in, and The plane representing the humanoid robot in the body coordinate system shaft and Axis velocity prediction. The center-of-mass velocity prediction is used to reflect the robot's short-term motion trend and provides a continuity constraint for state estimation under conditions of radar positioning degradation or obstruction.

[0069] Unlike traditional strategies that only output control quantities, this invention introduces a velocity prediction branch at the output of the Actor network, enabling the strategy to provide motion state estimates while outputting joint control actions, thus facilitating subsequent fusion with external observations.

[0070] Then, a reinforcement learning training task is constructed in a simulation environment, with the training objective being to enable the policy to stably follow speed commands and maintain bipedal gait stability.

[0071] Specifically, a high-level speed command is set as the task input, enabling the policy to learn stable walking under different speed and disturbance conditions. The policy function can be expressed as: in, For state observation vectors, These are the policy parameters. After training, a convergent policy network is obtained, which is then used for actual deployment.

[0072] Optionally, randomization factors can be introduced into the simulation environment to improve the robustness of the strategy, such as ground friction disturbances, mass parameter disturbances, and joint delay disturbances, so that the trained strategy can better adapt to model errors and external disturbances in actual deployment.

[0073] Furthermore, during training, the Critic network guides the updates of the Actor network based on the value function, thereby improving training convergence efficiency and policy performance. After training, a converged policy network, i.e., a reinforcement learning policy model, is obtained and deployed in a real-world robotic system for real-time inference execution.

[0074] During the actual deployment phase, the reinforcement learning strategy model performs inference once in each control cycle. Specifically, the velocity command output by the planning module and the robot's body state are input into the reinforcement learning strategy model, which outputs joint control variables and simultaneously outputs the predicted value of the center of mass linear velocity. In this invention, the joint control quantity output by the reinforcement learning policy model The control input is directly fed into the lower-level joint servo execution module, which converts the control input into actual control signals that drive the joints, thereby driving the robot to produce stable gait motion.

[0075] The predicted centroid velocity output by the reinforcement learning policy model The system integrates airborne radar positioning observations with policy-predicted velocities to obtain interference-resistant global pose and velocity estimates, i.e., fused pose estimates. This process fuses airborne radar positioning observations with policy-predicted velocities via an adaptive Kalman filter to obtain interference-resistant global pose and velocity estimates for the robot. These estimated poses, along with a dynamic 2.5D grid map, are then input into a planner for incremental path search. This addresses the problems of positioning jitter, jumps, and short-term failures caused by airborne radar occlusion, sparse features, or matching degradation in narrow, dynamic environments.

[0076] Specifically, to achieve joint estimation of position and velocity, this invention first constructs a filter state vector, defining the discrete time or control cycle index as... The state vector is defined as: in, For control period, it represents discrete time index or control period index; They represent the first One control cycle, the planar position of the humanoid robot in the world coordinate system; and They represent the first One control cycle, the planar velocity of the humanoid robot in the world coordinate system; Indicates the first The state vector of each control cycle.

[0077] To ensure motion continuity, this invention can employ a constant velocity assumption to establish a prediction model. Optionally, the control cycle time interval is denoted as... The predicted position update relationship is then: in, Indicates the control cycle time interval. and Indicates the predicted location. and This represents the posterior position estimate from the previous period. and This represents the estimated velocity value for the previous cycle.

[0078] Furthermore, the speed prediction model is defined as follows: In this invention, the velocity prediction model reflects the physical characteristic of humanoid robots whose velocity changes slowly over short timescales, and is used to maintain the temporal continuity of the state under short-term occlusion or observation degradation.

[0079] Next, a fused observation value is constructed based on the planar position observations from the airborne radar and the centroid velocity predictions output by the reinforcement learning policy model. Specifically, the planar position observations from the airborne radar are constructed by obtaining planar position observations through scan matching or map localization, defined as follows: in, and This represents the planar position measurement value obtained by using airborne radar to locate the self-pose radar positioning data after constraints.

[0080] The predicted center-of-mass velocity output by the reinforcement learning policy model can originally be defined in the robot's body coordinate system. To ensure consistency with the state variables in the world coordinate system, this invention utilizes the yaw angle from the data of the third inertial measurement unit output by the IMU. Perform a coordinate transformation on this velocity to obtain the velocity observation in the world coordinate system: in, and This represents the observed planar linear velocity after attitude transformation.

[0081] Furthermore, by fusing observation vectors and residuals, position and velocity observations are stitched together to form fused observation values. : And calculate the observation residuals: in, Represents the observation residual; Indicates the predicted state.

[0082] Then, a standard Kalman gain update is performed to obtain the posterior state estimate, i.e., the fused pose estimate. : in, Indicates Kalman gain, This represents the posterior state estimate after fusion and update.

[0083] After the fusion update is completed, a smooth and robust state estimate is obtained. To facilitate use by the path planning and control layers, the robot's planar pose expression in the world coordinate system is further output. In some embodiments, the output planar pose can be represented as: in, and These represent the plane positions after filtering and smoothing. This represents the yaw angle provided by the IMU and optionally corrected by filtering.

[0084] Meanwhile, in path planning and waypoint selection, the current position's planar coordinates are represented as: .

[0085] Smoothing pose estimation can suppress high-frequency position jitter caused by instantaneous matching error of airborne radar, maintain the temporal continuity of position and velocity under dynamic occlusion or feature degradation conditions, avoid path oscillation and frequent replanning caused by path planning input jumps, and provide a stable and reliable state reference for subsequent incremental path planning and trajectory smoothing.

[0086] This invention fuses the centroid velocity prediction output by the reinforcement learning policy model with the planar position observation output by the airborne radar. Through an adaptive Kalman filter, a continuous, smooth, and interference-resistant global position and velocity estimate is obtained. This estimate is then used as input for path planning, waypoint selection, and velocity command generation. This avoids occlusion and contamination of the environmental map by the robot's structure and prevents instantaneous attitude disturbances caused by robot movement from incorrectly updating the global environmental model.

[0087] Based on the above embodiments, the method further includes: Based on the preset positioning quality index, the variance of position observation noise is adjusted to obtain the adjusted variance of position observation noise. The weight values ​​of the planar position observations are adjusted based on the adjusted position observation noise variance.

[0088] In this invention, to enhance the robustness of the system under occlusion or matching degradation conditions, a preset positioning quality index is introduced. And adaptively adjust the variance of location observation noise: in, Indicates the observation variance at the nominal location. This represents the adjustment coefficient. This represents the positioning quality index. When the radar positioning quality deteriorates, the position observation weight is automatically reduced, thereby enhancing the role of velocity prediction in supporting state continuity.

[0089] In this invention, when A lower value indicates a decrease in radar positioning quality. Increasing the weight of position observations in the fusion process allows velocity prediction to provide stronger support for state continuity in the short term; when A higher value indicates that the positioning is reliable, the position observation weights are restored, and the positioning accuracy is improved.

[0090] Based on the above embodiments, the incremental path planning of the 2.5D grid map and the fused pose estimation value to obtain the target planned path includes: Based on D The Lite algorithm performs incremental path search on the 2.5D grid map and the fused pose estimate to obtain a discrete path sequence; The discrete path sequence is geometrically smoothed to obtain the target planned path.

[0091] In this invention, D is first used The Lite algorithm performs path planning, abstracting the 2.5D raster map into a set of raster nodes. Assess adjacency relationships and construct an adjacency graph: in, Represents a set of grid nodes. This represents the set of adjacent edges between grid cells.

[0092] Specifically, set Represents any grid node, Indicates and For adjacent grid nodes, the edge cost function is defined as follows: in, Represents any grid node, Indicates and Adjacent grid nodes, This represents the Euclidean distance between the centers of the grid. Indicates the grid height value. Indicates the grid clearance. The weight for the height difference penalty. As for the net clearance penalty weight, To prevent constants with a denominator of zero; In this invention, to improve the efficiency of online replanning in dynamic environments, incremental D can be adopted. The Lite algorithm performs path planning and maintains a one-step lookahead value for each node. The one-step lookahead value is defined as: in, Represents a node Cost estimate from the current point of origin to the target node. Represents a node The set of successor nodes, This represents the cost of local consistency for a node. (Through D) Lite's incremental update rule updates only the affected local nodes when dynamic obstacles cause changes in local costs, thereby reducing the number of global replanning attempts and lowering the risk of path oscillation.

[0093] After the planning is completed, a discrete path sequence is obtained: in, Indicates the first The position of each path point in the plane coordinate system Indicates the number of path points.

[0094] Due to the discrete path sequence output by incremental grid planning As discrete polylines, they exhibit abrupt changes in angles, local jagged edges, and discontinuities. Directly using them for speed command generation would cause the robot to make sharp turns and experience motion oscillations. Therefore, this invention addresses the discrete path sequence... Geometric smoothing is performed to obtain a waypoint sequence suitable for stable execution of the bipedal robot; In one embodiment, a three-point sliding window weighted smoothing rule can be used to update intermediate path points: in, Indicates the smoothed path points. Represents the smoothing coefficient. satisfy .

[0095] Optionally, the endpoints can be kept unchanged to ensure the start and end point constraints: The smoothed trajectory is represented as: In one embodiment, curvature constraints can further limit the directional changes of adjacent segments: in, Indicates local curvature; Indicates the first Segment direction angle, if If the threshold is exceeded, a second smoothing process is performed.

[0096] This invention significantly reduces abrupt changes in direction between adjacent path segments and decreases speed command jumps by smoothing discrete path sequences, thereby improving the executability and motion stability of humanoid robots in narrow passages and turning areas.

[0097] Based on the above embodiments, the step of generating a target velocity command based on the target waypoint sequence and the fused pose estimation value includes: Based on the coordinate difference and angular velocity difference between the target waypoint sequence and the fused pose estimate, linear velocity command information and angular velocity command information are constructed respectively; The target velocity command is generated based on the linear velocity command information and the angular velocity command information.

[0098] During the path execution phase, tracking waypoints point by point can easily lead to waypoint index rollback or frequent switching due to local disturbances, resulting in speed oscillations. Therefore, this invention employs a sliding forward-looking mechanism to select the target waypoint. Specifically, the current planar position of the humanoid robot is represented as: in, The fused pose estimate obtained from the above embodiments.

[0099] In each control cycle, from the smooth waypoint sequence Select forward waypoints in (i.e., the target planning path) (i.e., target waypoint), the selection rules are as follows: And satisfy the forward sight distance constraint: in, Indicates forward waypoints, This indicates a preset forward-looking distance threshold. This mechanism ensures that the humanoid robot always moves forward and avoids frequent waypoint regressions.

[0100] In this invention, by setting This ensures that the target waypoint is located a certain distance ahead of the robot, giving the robot a "look-ahead" characteristic, thereby improving the continuity and stability of path following.

[0101] Then, based on the current fused pose estimation and forward waypoint error, a high-level velocity command (i.e., target velocity command) is generated and input into the reinforcement learning policy network for execution.

[0102] Specifically, based on fused pose estimation With forward waypoints The formula for generating planar velocity commands and linear velocity commands is as follows: The formula for calculating angular velocity command information is: in, and Indicates position proportional gain. Indicates the heading proportional gain. Indicates the target direction angle. This indicates the current yaw angle. Next, the target speed command is generated: Furthermore, the aforementioned target velocity command is used as a high-level command and input into the reinforcement learning policy model. The reinforcement learning policy model outputs the joint control quantity corresponding to the target velocity command. The lower-level joint servo controller converts the torque into joint driving torque and drives the humanoid robot to perform actions, thereby enabling it to move along the planned path and avoid dynamic obstacles.

[0103] This invention significantly reduces the impact of the dispersion of the planning output on control execution by smoothing the path and generating proportional velocity from forward waypoints, thereby reducing sharp turns and oscillations and improving the stability margin for passage through narrow passages.

[0104] The humanoid robot navigation method provided by this invention can be applied to humanoid robots performing autonomous navigation and obstacle avoidance tasks in narrow and dynamically changing indoor environments. Through a hierarchical closed-loop structure of "multi-radar environmental perception and fusion localization - incremental path planning - reinforcement learning control execution," overall coordination is achieved. Specifically: the upper layer relies on a multi-radar environmental perception system consisting of a handheld mapping radar, a fixed environmental perception radar, and an airborne positioning radar to construct a globally covering 2.5D grid map. It then obtains continuous, smooth, and interference-resistant robot pose estimation by fusing LiDAR localization observations and policy prediction linear velocities through adaptive Kalman filtering. The middle layer performs incremental path planning based on the fused pose and global map, outputting updated reference paths and velocity commands in real time. The lower layer inputs the velocity commands into a reinforcement learning policy network and transforms them into joint control targets. The joint servo controller outputs driving torque, thereby driving the robot to execute stably.

[0105] In practical applications, such as narrow corridors, convenience store-style indoor spaces, or areas where humans and machines coexist, the environment typically contains fixed structures like shelves and pillars, as well as dynamic obstacles that frequently appear and disappear. Relying solely on a single airborne LiDAR for perception can easily lead to large blind spots due to occlusion, resulting in an incomplete environmental model and consequently causing frequent path oscillations, repeated replanning, or decision lag. This invention employs a multi-radar cooperative deployment approach, where fixed radars continuously observe dynamic environmental changes from an external perspective, while airborne radars are dedicated to high-precision positioning. This separates mapping and localization functions, preventing robot movement, short-term occlusion, or localized dynamic interference from contaminating the global map and improving the integrity and stability of the environmental model.

[0106] Simultaneously, at the localization level, this invention introduces an adaptive Kalman filter mechanism to fuse the planar position observations provided by airborne radar with the linear velocity predicted by the reinforcement learning policy network. By maintaining state continuity through a constant-velocity motion model and dynamically adjusting the observation noise weights based on radar matching quality, the system can maintain stable pose estimation even under radar feature degradation or short-term obstruction. This fusion mechanism effectively suppresses high-frequency localization jitter and jumps, improves the temporal consistency and robustness of global position estimation, provides accurate and reliable state input for the path planning module, and enhances the autonomous navigation capability of humanoid robots in confined dynamic environments.

[0107] The humanoid robot navigation system provided by the present invention is described below. The humanoid robot navigation system described below can be referred to in correspondence with the humanoid robot navigation method described above.

[0108] Figure 2 This is a schematic diagram of the humanoid robot navigation system provided by the present invention, as shown below. Figure 2As shown, this invention provides a humanoid robot navigation system, including a multi-radar environment perception module 201, a speed prediction module 202, a path planning module 203, and a navigation prediction module 204. The multi-radar environment perception module 201 is used to construct a 2.5D grid map corresponding to the target working environment of the humanoid robot during its operation phase based on first radar environment perception data and second radar environment perception data. The first radar environment perception data is obtained based on a handheld LiDAR; the second radar environment perception data is obtained based on multiple LiDARs fixedly installed in a preset area of ​​the target working environment. The speed prediction module 202 is used to input a preset robot speed command and the current body state information of the humanoid robot into a reinforcement learning strategy model to obtain a predicted value of the humanoid robot's center of mass velocity output by the reinforcement learning strategy model. The strategy model is trained using an Actor-Critic network built on a multilayer perceptron. The path planning module 203 is used to fuse the predicted centroid velocity and the third radar environmental perception data based on adaptive Kalman filtering to obtain the current fused pose estimate of the humanoid robot. It then performs incremental path planning on the 2.5D grid map and the fused pose estimate to obtain the target planned path. The third radar environmental perception data is obtained based on the humanoid robot's own LiDAR. The navigation prediction module 204 generates a target velocity command based on the target waypoint sequence and the fused pose estimate. This target velocity command is then input into the reinforcement learning strategy model to obtain the joint control quantity for the humanoid robot's next action. The target waypoint sequence is constructed from waypoints along the target planned path that satisfy a preset forward-looking distance threshold.

[0109] The humanoid robot navigation system provided by this invention significantly reduces blind spots and provides more comprehensive dynamic environmental information by deploying multiple fixed LiDARs that work in conjunction with the robot's own radar. Simultaneously, based on reinforcement learning algorithms, it not only outputs robot joint movements but also predicts the robot's center-of-gravity velocity in real time. The predicted velocity is then fused with LiDAR positioning information to generate a smooth and robust global state estimate. This ensures continuous and reliable position and velocity feedback even when the LiDAR is obstructed, improving the accuracy of obstacle avoidance decisions and the stability of motion execution for the humanoid robot in dynamic, confined environments.

[0110] The system provided by this invention is used to execute the above-described method embodiments. For specific processes and details, please refer to the above embodiments, which will not be repeated here.

[0111] Figure 3 This is a schematic diagram of the structure of the electronic device provided by the present invention, such as... Figure 3As shown, the electronic device may include: a processor 301, a communication interface 302, a memory 303, and a communication bus 304. The processor 301, communication interface 302, and memory 303 communicate with each other via the communication bus 304. The processor 301 can call logical instructions in the memory 303 to execute a humanoid robot navigation method. This method includes: constructing a 2.5D grid map corresponding to the target working environment of the humanoid robot during its operation phase based on first radar environmental perception data and second radar environmental perception data. The first radar environmental perception data is obtained based on a handheld LiDAR; the second radar environmental perception data is obtained based on multiple LiDARs fixedly installed in a preset area of ​​the target working environment. A preset robot speed command and the current body state information of the humanoid robot are input into a reinforcement learning strategy model to obtain a predicted value of the humanoid robot's center of mass velocity output by the reinforcement learning strategy model. The reinforcement learning strategy model is constructed based on a multilayer perceptron. The humanoid robot is trained using an Actor-Critic network. Based on adaptive Kalman filtering, the predicted centroid velocity and the third radar environmental perception data are fused to obtain the current fused pose estimate. Incremental path planning is then performed on the 2.5D grid map and the fused pose estimate to obtain the target planned path. The third radar environmental perception data is obtained from the humanoid robot's own LiDAR. A target velocity command is generated based on the target waypoint sequence and the fused pose estimate. This target velocity command is then input into the reinforcement learning policy model to obtain the joint control variables for the humanoid robot's next action. The target waypoint sequence is constructed from waypoints along the target planned path that satisfy a preset forward-looking distance threshold.

[0112] Furthermore, the logical instructions in the aforementioned memory 303 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, essentially, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0113] On the other hand, the present invention also provides a computer program product, the computer program product comprising a computer program stored on a non-transitory computer-readable storage medium, the computer program comprising program instructions, wherein when the program instructions are executed by a computer, the computer is able to execute the humanoid robot navigation method provided by the above methods, the method comprising: constructing a 2.5D grid map corresponding to the target working environment of the humanoid robot during the operation phase based on first radar environmental perception data and second radar environmental perception data, wherein the first radar environmental perception data is obtained based on a handheld lidar; the second radar environmental perception data is obtained based on multiple lidars fixedly installed in a preset area of ​​the target working environment; inputting a preset robot speed command and the current body state information of the humanoid robot into a reinforcement learning strategy model to obtain the humanoid robot's current body state information output by the reinforcement learning strategy model. The humanoid robot's center-of-gravity velocity prediction value is obtained by training an Actor-Critic network based on a multilayer perceptron, using an adaptive Kalman filter to fuse the predicted velocity value and third radar environmental perception data to obtain the current fused pose estimate of the humanoid robot. Incremental path planning is then performed on the 2.5D grid map and the fused pose estimate to obtain a target planning path, where the third radar environmental perception data is obtained from the humanoid robot's own LiDAR. A target velocity command is generated based on the target waypoint sequence and the fused pose estimate. This target velocity command is then input into the reinforcement learning strategy model to obtain the joint control variables for the humanoid robot's next action, where the target waypoint sequence is constructed from waypoints along the target planning path that satisfy a preset forward-looking distance threshold.

[0114] In another aspect, the present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the humanoid robot navigation method provided in the above embodiments. The method includes: constructing a 2.5D grid map corresponding to the target working environment of the humanoid robot during its operation phase based on first radar environmental perception data and second radar environmental perception data, wherein the first radar environmental perception data is obtained based on a handheld LiDAR; the second radar environmental perception data is obtained based on multiple LiDARs fixedly installed in a preset area of ​​the target working environment; inputting a preset robot speed command and the current body state information of the humanoid robot into a reinforcement learning strategy model to obtain a predicted value of the humanoid robot's center of mass velocity output by the reinforcement learning strategy model, wherein... The reinforcement learning strategy model is trained on an Actor-Critic network built on a multilayer perceptron. Based on adaptive Kalman filtering, the predicted centroid velocity and the third radar environmental perception data are fused to obtain the current fused pose estimate of the humanoid robot. Incremental path planning is then performed on the 2.5D grid map and the fused pose estimate to obtain the target planned path. The third radar environmental perception data is obtained from the humanoid robot's own LiDAR. A target velocity command is generated based on the target waypoint sequence and the fused pose estimate. This target velocity command is then input into the reinforcement learning strategy model to obtain the joint control variables for the humanoid robot's next action. The target waypoint sequence is constructed from waypoints along the target planned path that satisfy a preset forward-looking distance threshold.

[0115] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0116] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0117] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A navigation method for a humanoid robot, characterized in that, include: Based on the first radar environmental perception data and the second radar environmental perception data, a 2.5D grid map corresponding to the target working environment of the humanoid robot during the operation phase is constructed. The first radar environmental perception data is obtained based on a handheld LiDAR; the second radar environmental perception data is obtained based on multiple LiDARs fixedly installed in a preset area of ​​the target working environment. The preset robot speed command and the current body state information of the humanoid robot are input into the reinforcement learning strategy model to obtain the predicted value of the center of mass velocity of the humanoid robot output by the reinforcement learning strategy model. The reinforcement learning strategy model is trained based on the Actor-Critic network constructed by the multilayer perceptron. Based on adaptive Kalman filtering, the predicted centroid velocity and the third radar environmental perception data are fused to obtain the current fused pose estimate of the humanoid robot; and incremental path planning is performed on the 2.5D grid map and the fused pose estimate to obtain the target planning path. The third radar environmental perception data is obtained based on the humanoid robot's own lidar. Based on the target waypoint sequence and the fused pose estimation value, a target velocity command is generated; and the target velocity command is input into the reinforcement learning policy model to obtain the joint control quantity of the humanoid robot's next action. The target waypoint sequence is constructed from waypoints in the target planning path that meet a preset forward distance threshold.

2. The humanoid robot navigation method according to claim 1, characterized in that, The step of constructing a 2.5D grid map corresponding to the target operating environment of the humanoid robot during its operation phase, based on the first radar environmental perception data and the second radar environmental perception data, includes: Based on the data from the first inertial measurement unit, a global coordinate system transformation is performed on the first radar environmental perception data to obtain a static environmental map corresponding to the target operating environment, wherein the first inertial measurement unit data is the inertial measurement unit data corresponding to the first radar environmental perception data. Based on the data from the second inertial measurement unit, a global coordinate system transformation is performed on the second radar environmental perception data to obtain a dynamic obstacle map corresponding to the target working environment of the humanoid robot during the operation phase. The second inertial measurement unit data is the inertial measurement unit data corresponding to the second radar environmental perception data. Based on the static environment map and the dynamic obstacle map, a joint environment model is constructed. Then, based on an extended Kalman filter, the joint observation vector corresponding to the second inertial measurement unit data in the joint environment model is optimized to obtain an optimized joint environment model. The joint observation vector is obtained based on the stacking of first point-surface residuals. The first point-surface residuals are obtained by performing neighborhood point search and local plane fitting on the points corresponding to the second inertial measurement unit data in the joint environment model. Based on the height coordinates corresponding to the highest point in the optimized joint environment model, a two-dimensional raster projection is performed on the optimized joint environment model to obtain the 2.5D raster map.

3. The humanoid robot navigation method according to claim 2, characterized in that, The process of fusing the predicted centroid velocity and the third radar environmental perception data based on adaptive Kalman filtering to obtain the current fused pose estimate of the humanoid robot includes: Based on the data from the third inertial measurement unit, a global coordinate system transformation is performed on the environmental perception data from the third radar to obtain the current self-pose radar positioning data of the humanoid robot. The data from the third inertial measurement unit corresponds to the environmental perception data from the third radar. Based on the second point-plane residual, the self-pose radar positioning data is constrained, and the constrained self-pose radar positioning data is scanned and matched or mapped to obtain the planar position observation value corresponding to the humanoid robot's own lidar. The second point-plane residual is obtained by searching for neighboring points and fitting local planes in the joint environment model based on the points in the third inertial measurement unit. Based on the data from the third inertial measurement unit, the predicted value of the center of mass velocity is transformed to obtain the observed value of the planar linear velocity. Based on the planar position observations and the planar linear velocity observations, fused observations are constructed, and the fused observations are updated based on the adaptive Kalman filter to obtain the fused pose estimate.

4. The humanoid robot navigation method according to claim 3, characterized in that, The method further includes: Based on the preset positioning quality index, the variance of position observation noise is adjusted to obtain the adjusted variance of position observation noise. The weight values ​​of the planar position observations are adjusted based on the adjusted position observation noise variance.

5. The humanoid robot navigation method according to claim 1, characterized in that, The incremental path planning process, which involves combining the 2.5D grid map and the fused pose estimate to obtain the target planned path, includes: Based on D The Lite algorithm performs incremental path search on the 2.5D grid map and the fused pose estimate to obtain a discrete path sequence; The discrete path sequence is geometrically smoothed to obtain the target planned path.

6. The humanoid robot navigation method according to claim 1, characterized in that, The step of generating a target velocity command based on the target waypoint sequence and the fused pose estimate includes: Based on the coordinate difference and angular velocity difference between the target waypoint sequence and the fused pose estimate, linear velocity command information and angular velocity command information are constructed respectively; The target velocity command is generated based on the linear velocity command information and the angular velocity command information.

7. A humanoid robot navigation system, characterized in that, include: The multi-radar environmental perception module is used to construct a 2.5D grid map corresponding to the target working environment of the humanoid robot during its operation phase based on the first radar environmental perception data and the second radar environmental perception data. The first radar environmental perception data is obtained based on a handheld LiDAR; the second radar environmental perception data is obtained based on multiple LiDARs fixedly installed in a preset area of ​​the target working environment. The speed prediction module is used to input the preset robot speed command and the current body state information of the humanoid robot into the reinforcement learning strategy model to obtain the predicted value of the center of mass speed of the humanoid robot output by the reinforcement learning strategy model. The reinforcement learning strategy model is trained based on the Actor-Critic network constructed by the multilayer perceptron. The path planning module is used to fuse the predicted centroid velocity and the third radar environmental perception data based on adaptive Kalman filtering to obtain the current fused pose estimate of the humanoid robot; and to perform incremental path planning on the 2.5D grid map and the fused pose estimate to obtain the target planned path, wherein the third radar environmental perception data is obtained based on the humanoid robot's own lidar. The navigation prediction module is used to generate a target velocity command based on the target waypoint sequence and the fused pose estimation value; and input the target velocity command into the reinforcement learning policy model to obtain the joint control quantity of the humanoid robot's next action, wherein the target waypoint sequence is constructed from waypoints in the target planning path that meet a preset forward distance threshold.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and running on the processor, characterized in that, When the processor executes the computer program, it implements the humanoid robot navigation method as described in any one of claims 1 to 6.

9. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the humanoid robot navigation method as described in any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the humanoid robot navigation method as described in any one of claims 1 to 6.

Images