Intelligent planning method and system for autonomous getting-up action of humanoid robot

Abstract

A humanoid robot autonomous getting-up motion intelligent planning method and a humanoid robot autonomous getting-up motion intelligent planning system belong to the field of robot control, and comprise the following steps: collecting and fusing the internal state and external environment information of a robot in real time, and constructing a mixed state vector; outputting a high-level action sequence according to the mixed state vector by adopting an offline trained deep reinforcement learning strategy network; a real-time optimizer based on model predictive control is adopted to convert the behavior semantic instruction into an optimal state trajectory and an optimal control trajectory; and executing the first control quantity of the optimal control trajectory, continuously monitoring the actual state, comparing the actual state with the prediction state of the real-time optimizer based on model prediction control, calculating the deviation between the actual state and the prediction state, triggering local correction if the deviation is small, and triggering global re-planning if the deviation is large. Through the hierarchical intelligent planning architecture, the robot can adaptively cope with various unknown falling postures, a safe, stable and efficient getting-up track is quickly generated, and the autonomy and robustness of the robot are improved.

Description

Technical Field

[0001] This invention belongs to the field of robot control technology, specifically relating to an intelligent planning method and system for autonomous standing motion of a humanoid robot. Background Technology

[0002] Dynamic balance and fall recovery in humanoid robots are among the core challenges in robotics research. Currently, the technical approaches to enabling robots to stand up mainly fall into two categories: one is based on pre-programmed static motion sequences, and the other is based on online trajectory optimization using dynamic models. The online trajectory optimization method based on dynamic models primarily involves establishing a multi-rigid-body dynamic model of the robot, modeling the standing process as an optimization problem subject to various physical constraints (such as joint torques, friction, and balance stability), and then using numerical optimization algorithms (such as Model Predictive Control, MPC) to solve and generate the motion trajectory online. This type of method relies on the accuracy of the model and can theoretically handle certain disturbances.

[0003] The closest prior art solution to this invention is a robot standing method based on online model predictive control (MPC), which attempts to adapt to different initial states through online optimization.

[0004] The specific implementation process of this solution is as follows: (1) State perception: The robot’s current joint angle, posture and contact point with the ground are obtained through sensors; (2) Problem Modeling: Based on a simplified robot dynamics model (such as centroidal dynamics), a finite-time optimization problem is constructed. The objective function is typically to track a predefined sequence of standing postures and minimize energy consumption; (3) Online solution: In each control cycle, the optimization problem is solved in real time according to the current state to obtain the optimal joint control sequence for a future period of time.

[0005] (4) Trajectory execution: The first set of control commands obtained by the solution is sent to the underlying servo driver for execution.

[0006] This solution has the following main drawbacks: 1. High dependence on initial conditions and model accuracy: This scheme heavily relies on accurate robot models and environmental parameters (such as the coefficient of friction). When the robot is in an unexpected and complex fall posture, the simplified robot dynamics model may be mismatched with the actual dynamic characteristics, resulting in no solution or an infeasible solution for the optimization problem, and planning failure.

[0007] 2. High computational complexity and poor real-time performance: In this scheme, the online optimization of the robot's whole-body dynamics model involves a huge amount of computation and takes a long time to solve, making it difficult to meet the high requirements for real-time feedback control during the standing process (which typically requires calculations to be completed in milliseconds). This may lead to control delays and cause the robot to lose balance.

[0008] 3. Lack of high-level intelligent decision-making and weak generalization ability: This solution is essentially a local optimizer, lacking strategic thinking. It can only fine-tune around a preset getting-up path, and cannot, like a human, autonomously decide on high-level strategies such as "what to do first and what to do later" when faced with entirely new postures, for example, whether to roll first or reach out for support. Therefore, this solution has poor generalization ability and cannot cope with a large number of unseen fall scenarios.

[0009] 4. Insufficient utilization of perception information and poor robustness: The scheme fails to fully integrate environmental perception information (such as obstacles) to actively avoid collisions, and usually lacks an effective closed-loop feedback mechanism to deal with uncertainties in the execution process (such as slippage of the support surface), resulting in insufficient system robustness. Summary of the Invention

[0010] To address the problems of existing solutions, such as strong dependence on initial conditions and model accuracy, computational complexity, poor real-time performance, weak generalization ability, and insufficient robustness, this invention provides an intelligent planning method and system for autonomous standing movements of humanoid robots. This invention combines artificial intelligence and model predictive control, making it particularly suitable for robots to autonomously, safely, and efficiently plan and execute standing movements from any posture after a fall.

[0011] The technical solution adopted by this invention to solve the technical problem is as follows: This invention provides an intelligent planning method for autonomous standing movements of a humanoid robot, which mainly includes the following steps: Step 1: Collect and fuse the robot's internal state and external environment information in real time to construct a hybrid state vector S; Step 2: The deep reinforcement learning strategy network trained offline outputs a high-level action sequence based on the mixed state vector S; Step 3: Use a real-time optimizer based on model predictive control to transform behavioral semantic commands into optimal state trajectories and optimal control trajectories; Step 4: Execute the first control input of the optimal control trajectory, while continuously monitoring the actual state and comparing the actual state with the predicted state of the real-time optimizer based on model predictive control. Calculate the deviation between the actual state and the predicted state. If the deviation is small, trigger local correction; if the deviation is large, trigger global replanning.

[0012] Furthermore, in step three, the centroid dynamics model is used as the dynamics model to construct and solve an optimization problem, thereby obtaining the optimal state trajectory and optimal control trajectory in the future time domain.

[0013] Furthermore, the optimization problem is as follows:

[0014] Where H represents the future time domain, For the initial control input, The system state at time t. For behavioral semantic instruction a i The target reference state obtained through analysis, To control the input, both Q and R are weight matrices; The system state at time t+1, Let ZMP be the system state transition function, ZMP be the zero-moment point, and Support Polygon be the support polygon formed by the active contact points. Let be the contact force vector at the i-th contact point at time t. Let be the ground normal vector at the i-th contact point. Let be the coefficient of friction at the i-th contact point, Distance be the minimum distance between the robot and the j-th obstacle, and robot be the robot's name. For the j-th obstacle, As a safety threshold, To control the lower limit of the input, To control the upper limit of the input.

[0015] Furthermore, in step four, the deviation between the actual state and the predicted state is δ = ‖Sactual - Spredicted‖, where Sactual is the actual state and Spredicted is the predicted state.

[0016] Furthermore, in step four, the small deviation is δ≤ϵ, where ϵ is the deviation threshold. At this time, a local correction is triggered: the real-time optimizer based on model predictive control adjusts the parameters in the optimization problem and performs re-optimization in the next control cycle to compensate for the deviation.

[0017] Furthermore, in step four, the large deviation is δ>ϵ, where ϵ is the deviation threshold. At this time, global replanning is triggered: the latest actual state is fed back to the deep reinforcement learning policy network, and a new high-level action sequence is output based on the latest actual state. The system then restarts the underlying motion planning.

[0018] Furthermore, in step two, the deep reinforcement learning policy network is pre-trained on a large scale using reinforcement learning algorithms in a highly randomized simulation environment.

[0019] Furthermore, in step two, the reward function of the deep reinforcement learning policy network is designed to encourage successful standing, minimize energy consumption and time, and punish unstable postures and collisions.

[0020] This invention provides a deep reinforcement learning policy network system applied to the intelligent planning method for autonomous standing movements of a humanoid robot, comprising: The multimodal perception and hybrid state construction module collects and fuses the robot's internal state and external environment information in real time to construct a hybrid state vector S; The high-level policy decision-making module uses an offline-trained deep reinforcement learning policy network to output a high-level action sequence based on the mixed state vector S. The underlying motion planning module uses a model predictive control-based real-time optimizer to transform behavioral semantic commands into optimal state trajectories and optimal control trajectories. The closed-loop execution and adaptive adjustment module executes the first control input of the optimal control trajectory, while continuously monitoring the actual state and comparing the actual state with the predicted state of the real-time optimizer based on model predictive control. It calculates the deviation between the actual state and the predicted state. If the deviation is small, it triggers local correction; if the deviation is large, it triggers global replanning.

[0021] Furthermore, the multimodal perception and hybrid state construction module includes a body state perception unit, an environment perception unit, and a state fusion unit. The body state perception unit includes an inertial measurement unit, a joint encoder, and a force / torque sensor. The inertial measurement unit is used to measure the robot's three-axis acceleration and angular velocity. The joint encoder is used to measure the robot's full-body joint angles. The force / torque sensor is used to measure the contact state between each end effector and the ground, as well as the magnitude and direction of the contact force between each end effector and the ground.

[0022] The beneficial effects of this invention are: This invention employs a hierarchical intelligent planning architecture that combines data-driven macro-level decision-making with model-driven local optimization. This enables robots to adaptively respond to various unknown fall postures and quickly generate safe, stable, and efficient get-up trajectories, significantly improving their autonomy and robustness. Compared to the closest existing technology, the significant technical advantages of this invention are: 1. Excellent generalization ability: The deep reinforcement learning policy network (π) used in this invention high It possesses intelligent decision-making capabilities learned from a large amount of simulation experience, enabling the robot to cope with unprecedented and complex initial fall postures and environments, completely freeing it from dependence on pre-programmed actions.

[0023] 2. High-efficiency real-time performance: This invention uses a layered architecture of high-level intelligent decision-making (fast) + low-level model optimization (accurate) to decouple time-consuming global search from precise local optimization. The high-level strategy decision-making module quickly points out the direction, while the low-level motion planning module refines the process. The overall computational efficiency is far higher than that of a single global optimization scheme, meeting the requirements of real-time control.

[0024] 3. Extremely high robustness and safety: This invention uses closed-loop feedback and online adaptive adjustment mechanisms to effectively compensate for model errors, sensory noise, and external disturbances. Deeply fused environmental perception ensures obstacle avoidance safety during actions, greatly improving the success rate of the standing process and the safety of the robot itself.

[0025] 4. Natural movement and energy optimization: In this invention, the optimization goal of the underlying motion planning module directly considers energy consumption and motion smoothness, making the generated standing motion closer to the efficient movement mode of a living organism and minimizing wear and tear on the robot hardware. Attached Figure Description

[0026] Figure 1 This invention provides a schematic diagram of an intelligent planning system for autonomous standing motion of a humanoid robot.

[0027] Figure 2 For Deep Reinforcement Learning (DRL) Policy Network (π) high A schematic diagram of the structure of ).

[0028] Figure 3 This is a schematic diagram of the rolling optimization principle of a real-time optimizer based on model predictive control (MPC). Detailed Implementation

[0029] The present invention will be further described in detail below with reference to the accompanying drawings.

[0030] This invention combines hierarchical intelligent planning with closed-loop control. Its core lies in decomposing the complex task of getting up into two levels: high-level strategy decision-making and low-level motion planning. The system achieves intelligence and robustness through multimodal perception fusion and online adaptive adjustment.

[0031] In a first aspect, the present invention provides an intelligent planning system for autonomous standing movements of a humanoid robot.

[0032] like Figure 1 As shown, the present invention provides an intelligent planning system for autonomous standing movements of a humanoid robot, which mainly includes the following modules: Module 1: Multimodal Perception and Hybrid State Construction Module; The multimodal perception and hybrid state construction module provides accurate input data for planning and decision-making by collecting and fusing information about the robot's internal state and external environment in real time.

[0033] According to the present invention, the multimodal perception and hybrid state construction module mainly includes an ontology state perception unit, an environment perception unit, and a state fusion unit.

[0034] According to the present invention, the body state perception unit mainly includes an inertial measurement unit (IMU), a joint encoder, and a force / torque sensor; the inertial measurement unit (IMU) is mainly used to measure the robot's three-axis acceleration and angular velocity; the joint encoder is mainly used to measure the robot's whole-body joint angles; the force / torque sensor is mainly used to measure the contact state between each end effector and the ground, as well as the magnitude and direction of the contact force between each end effector and the ground.

[0035] According to the present invention, the environmental perception unit is mainly realized through visual sensors, such as depth cameras and lidar. The visual sensors construct a local three-dimensional environment map around the robot in real time and identify the geometric dimensions and positions of feasible support planes and obstacles.

[0036] According to the present invention, the state fusion unit is mainly used to fuse the robot's internal state information with the external environment information to construct a unified hybrid state vector S.

[0037] Module Two: High-Level Strategy Decision-Making Module; The high-level policy decision-making module is mainly implemented using a reinforcement learning-based decision maker, specifically an offline-trained deep reinforcement learning (DRL) policy network (π). high Based on the current mixed state vector S, output the macroscopic policy sequence for the get-up task, i.e., the high-level action sequence A. high ={a1, a2, ..., a k}, where a i (i ) represents abstract behavioral semantic instructions. This deep reinforcement learning policy network (π) high Large-scale pre-training is performed in a highly randomized simulation environment using reinforcement learning algorithms.

[0038] According to the present invention, such as Figure 2 As shown, the Deep Reinforcement Learning (DRL) policy network (π) high Specifically, it can be composed of an input layer, a hidden layer, and an output layer. The input data of the input layer includes a mixed state vector S, robot state, contact state, and environmental information. The hidden layer is mainly implemented using a multilayer perceptron (MLP), with ReLU as the activation function. The output data of the output layer is a high-level action sequence A. high And abstract behavioral semantic instructions, such as instruction 1: roll, instruction 2: support, etc.

[0039] Module 3: Underlying Motion Planning Module; The underlying motion planning module is mainly implemented using an optimizer based on model predictive control (MPC). Specifically, a real-time optimizer based on MPC can be used to process the behavioral semantic instructions a. i This is transformed into a specific, safe, and feasible whole-body movement trajectory, namely the optimal joint trajectory.

[0040] Module 4: Closed-Loop Execution and Adaptive Adjustment Module; The closed-loop execution and adaptive adjustment module ensures that the system can cope with uncertainties in real-world environments, forming a closed loop. This module is primarily implemented through the underlying servo controller and is mainly used for trajectory execution and status monitoring, deviation assessment, and decision-making.

[0041] During closed-loop execution and adaptive adjustment, the multimodal perception and hybrid state construction module continuously monitors the actual state (Sactual). Simultaneously, the underlying servo controller executes the first control input output by the underlying motion planning module. The underlying servo controller compares the actual state (Sactual) with the predicted state (Spredicted) of the real-time optimizer based on model predictive control (MPC). If the deviation between the actual state (Sactual) and the predicted state (Spredicted) is a small deviation signal, the underlying motion planning module is triggered to perform local correction. If the deviation between the actual state (Sactual) and the predicted state (Spredicted) is a large deviation signal, the higher-level strategy decision module is triggered to perform global replanning.

[0042] Secondly, the present invention provides an intelligent planning method for autonomous standing motion of a humanoid robot, which is implemented using an intelligent planning system for autonomous standing motion of a humanoid robot provided in the first aspect.

[0043] This invention provides an intelligent planning method for autonomous standing movements of a humanoid robot, which specifically includes the following steps: Step 1: Multimodal perception and hybrid state construction; By collecting and fusing information about the robot's internal state and external environment in real time, accurate input data is provided for planning and decision-making. The specific implementation process is as follows: S1.1: Hardware initialization and data acquisition; The robot's full-body joint angles, angular velocities, torso orientation, contact status of each end effector with the ground, and the magnitude and direction of the contact force between each end effector and the ground are acquired in real time using an inertial measurement unit (IMU), joint encoders, and force / torque sensors. Based on this data, the real-time center of mass (CoM) position, velocity, and zero-moment point (ZMP) are calculated.

[0044] Using visual sensors such as depth cameras and LiDAR, a local 3D environment map of the robot's surroundings is constructed in real time, identifying the geometric dimensions and locations of feasible support planes and obstacles.

[0045] S1.2: State estimation and information fusion; The robot's internal state information is fused with external environment information to construct a unified hybrid state vector S. This hybrid state vector S forms the basis for subsequent planning and decision-making, and specifically includes: Robot body state: joint angles q ,speed dq / dt The position of the center of mass p com speed v com , Zero Torque Point ZMP.

[0046] Contact state: C = {c i | i=1,...,n}, where each contact point c i Including its position p i Contact force f i Friction coefficient estimation μ i And a flag indicating whether it is active.

[0047] Environment state: O = {o j | j=1,...,m}, where each obstacle o j It is described by its bounding box or geometric model.

[0048] Step Two: High-Level Strategic Decision-Making; Employing an offline-trained deep reinforcement learning (DRL) policy network (π) high Based on the current mixed state vector S, output the macroscopic policy sequence for the get-up task.

[0049] This deep reinforcement learning policy network (π) high The input to the input is a mixed state vector S, and the output is a high-level action sequence A. high ={a1, a2, ..., a k}, where a i (i () represents abstract behavioral semantic instructions, for example: a1: "Roll over into a prone position"; a2: "Establish a three-point stable configuration supported by the left hand and both knees"; a3: "Shift your center of gravity forward and lift your hips"; a4: "Execute the sequence of movements to get up by supporting yourself on all fours".

[0050] This deep reinforcement learning policy network (π) highIn a highly randomized simulation environment (including various fall postures, ground friction, obstacles, etc.), the algorithm is pre-trained on a large scale using reinforcement learning algorithms (such as Proximal Policy Optimization, PPO). The reward function R is designed to encourage successful standing, minimize energy consumption and time, and penalize unstable postures and collisions.

[0051] Step 3: Underlying motion planning; like Figure 3 As shown, a real-time optimizer based on model predictive control (MPC) is used to process the behavioral semantic instructions a i This is transformed into a concrete, safe, and feasible full-body movement trajectory. The specific implementation process is as follows: S3.1: Construct a dynamic model; Specifically, a computationally efficient centroid dynamic model can be used as the dynamic model to balance computational accuracy and real-time requirements.

[0052] S3.2: Optimize the formalization of the problem; In each control cycle, solve a finite-time optimization problem of the following form:

[0053] Where H represents the future time domain; The initial control input (i.e., the first control quantity of the optimal control trajectory U*, is directly sent to the underlying servo controller for execution). The system state at the current time t (e.g., the state of the centroid). For behavioral semantic instruction a i The target reference state obtained through analysis; For control inputs (such as joint torque or acceleration); Q and R are both weight matrices; The system state (e.g., the centroid state) at time t+1. Let be the system state transition function, describing the system state at time t. and control input Under the influence of the action, the system state transitions to the system state at time t+1. The dynamic relationship is derived based on the center-of-mass dynamic model; ZMP is the zero-moment point; Support Polygon is the support polygon formed by the active contact points; Let be the contact force vector at the i-th contact point at time t, which is measured by the force / torque sensor and reflects the interaction force between the robot's end effector and the ground. is the ground normal vector of the i-th contact point, perpendicular to the support plane, used to define the directional reference of the friction cone constraint; Let be the friction coefficient at the i-th contact point, estimated from the ground material and contact state, and a key parameter of the friction cone constraint; Distance is the distance between the robot and the j-th obstacle. o j The minimum distance between () is calculated using 3D environmental data collected by the environmental sensing unit; robot refers to a robot. For the j-th obstacle, it is identified and modeled by the environmental perception unit, including its geometric dimensions, position coordinates and other information; This is a safety threshold; The lower limit of the control input is determined by the physical properties of the robot actuator (such as joint motor), limiting the control input to not be lower than this threshold. To control the upper limit of the input, the same reason is determined by the physical performance of the actuator, limiting the control input to not exceeding this threshold.

[0054] S3.3: Trajectory output; Solving the above finite-time optimization problem yields the optimal state trajectory X* and optimal control trajectory U* in the future time domain H; the first control variable of the optimal control trajectory U* is then used. This is sent as the current instruction to the underlying servo controller.

[0055] Step 4: Closed-loop execution and adaptive adjustment; S4.1: Trajectory Execution and Status Monitoring; The underlying servo controller executes the first control input. Meanwhile, the multimodal perception and hybrid state construction module continuously monitors the actual state (Sactual), and the underlying servo controller compares the actual state (Sactual) with the predicted state (Spredicted) of the real-time optimizer based on model predictive control (MPC). S4.2: Deviation Assessment and Decision Making; Define a deviation threshold ϵ, and calculate the deviation δ = ||Sactual - Spredicted||. If δ≤ϵ (small deviation), a local correction is triggered. The parameters in the optimization problem (such as the target reference state) are adjusted by a real-time optimizer based on model predictive control (MPC). (or constraint boundary), and then re-optimize in the next control cycle to compensate for the deviation.

[0056] If δ > ϵ (large deviation, such as severe slippage), a global replanning is triggered. The latest actual state is fed back to the high-level strategy decision-making module, which re-evaluates based on the latest actual state and outputs a new high-level action sequence A. newhigh The system then restarts the underlying motion planning.

[0057] The technical solutions of the present invention will be clearly and completely described below with reference to the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0058] Imagine a 1.6-meter-tall humanoid robot working on a smooth indoor floor. It loses its balance and falls, ending up in a side-lying position with its right arm partially tucked under its body and its left leg bent. 0.4 meters to its left front, there is a 30-centimeter-high cubic obstacle (like a small stool).

[0059] The intelligent planning method for autonomous standing motion of a humanoid robot according to the present invention is used to intelligently plan the autonomous standing motion of the aforementioned humanoid robot. The specific implementation steps are as follows: Step 1: Multimodal perception and hybrid state construction; 1. Hardware initialization and data acquisition; Activate the robot's full-body sensor system: Inertial Measurement Unit (IMU): Acquires the three-axis acceleration and angular velocity of the robot's torso at a frequency of 1000Hz; Joint encoder: Reads the current angles q=[q1, q2, ...,q'] of n joints throughout the robot's body. n ]; A six-dimensional force / torque sensor is installed on the robot's wrist and ankle to acquire the contact force vector (fcontact) between the right elbow, right hip, and right ankle and the ground.

[0060] A depth camera, mounted on the robot's head, acquires environmental point cloud data at a frequency of 30Hz.

[0061] 2. State estimation and information fusion; Robot body state calculation: Using the three-axis acceleration and angular velocity data of the robot torso and joint angles q, the position p of the end effectors (hands, feet, hips) in the world coordinate system is calculated using the robot's forward kinematics model. ee Then calculate the position p of the robot's global center of mass (CoM). com and speed v com Based on contact force information, estimate the current support polygon and zero moment point (ZMP).

[0062] Environmental state analysis: The environmental point cloud data collected by the depth camera is processed in real time. Plane segmentation and clustering algorithms are used to identify the ground plane and detect the cube obstacle o1 in the left front. Its pose and size relative to the robot's base coordinate system are accurately calculated.

[0063] Construct a hybrid state vector S: Integrate the above information into a structured state vector for subsequent planning and decision-making.

[0064] Step Two: High-Level Strategic Decision-Making; 1. Policy network reasoning; The constructed mixed state vector S is input into the pre-trained high-level policy neural network (π). high This high-level policy neural network (π) high It can be deployed on the robot's embedded computing unit (such as NVIDIA Jetson AGX Orin) using the PyTorch or TensorFlow framework.

[0065] 2. Generate high-level action sequences; This high-level policy neural network (π) high The mixed state vector S is forward propagated. Based on the learned policy, an abstract high-level action sequence A is output. high ={a1, a2, a3, a4}: a1: "Roll to the right into a prone position"; a2: "Establish three-point support (left palm, both knees)"; a3: "Shift your center of gravity forward and lift your hips"; a4: "Execute standing actions in sequence".

[0066] This high-level action sequence is based on an intelligent assessment of the environment (unobstructed right side) and one's own state (right arm under pressure, left side relatively free).

[0067] Step 3: Basic motion planning; 1. Instruction parsing and target generation; The real-time optimizer based on model predictive control (MPC) receives the instruction to be executed, which is parsed into a specific control objective, such as rotating the robot's torso 90 degrees around the vertical axis from its current side-lying posture to a prone posture within 1.5 seconds, while simultaneously translating the center of mass a specified distance on the horizontal plane.

[0068] 2. Model Predictive Control (MPC) optimization solution; The optimization problem is as follows: Dynamic model: A linearized center-of-mass dynamic model is used as the dynamic model.

[0069] Objective function: Minimize tracking error (deviation from target attitude and center of mass position) and control (sum of squares of joint torques).

[0070] The constraints are as follows: Dynamic constraints: State transitions satisfy = ; Balance constraint: The zero moment point ZMP in the prediction time domain must always fall within the support polygon formed by the active contact points.

[0071] Friction cone constraint: The contact force at the active contact point (right elbow, right hip, right foot) must be located within the friction cone.

[0072] Collision avoidance constraint: A constraint based on the signed distance function (SDF) is introduced into the optimization problem to ensure that the distance between all links of the robot and obstacle o1 is always greater than a safety threshold. = 5cm.

[0073] Actuator constraints: Joint angle, speed, and torque shall not exceed physical limits.

[0074] Online solution: Solve the optimization problem using an efficient numerical optimization solver (such as OSQP or ACADO) to obtain the optimal state trajectory X* and optimal control trajectory U* in the future time domain H.

[0075] Trajectory Output: The first control variable of the optimal control trajectory U*. The target torque or position of each joint at the current moment is sent as the current command to the underlying servo controller.

[0076] Step 4: Closed-loop execution and adaptive adjustment; 1. Trajectory execution and status monitoring; The underlying servo controller executes the first control input. The robot began to roll to the right.

[0077] The multimodal perception and hybrid state construction module runs continuously, updating the hybrid state vector S at a frequency of 100Hz.

[0078] 2. Deviation assessment and decision-making; The deviation δ = ||pactualcom| - |ppredictedcom| is calculated between the actual centroid position (pactualcom) and the centroid position (ppredictedcom) predicted by the real-time optimizer based on model predictive control (MPC).

[0079] Scenario A (Small Deviation): After 0.5 seconds of execution, δ=2cm<ϵ (ϵ=3cm) was detected, which indicates that there may be slight unevenness on the ground, resulting in a slightly slower rolling speed.

[0080] Action: Trigger local correction. The underlying motion planning module does not perform global replanning. Instead, in the next control cycle, it uses the current actual state as the new initial state to continue solving the optimization problem, naturally compensating for accumulated errors.

[0081] Scenario B (Large Deviation): Suppose that during the roll, the right elbow support point suddenly slips, causing the deviation δ to increase instantly to 8cm>ϵ.

[0082] Action: Immediately trigger global replanning. Interrupt the current underlying motion planning loop and feed the latest state (Sactual) (including slippage information) back to the higher-level policy neural network (π). high High-level policy neural network (π) high Based on the latest slippage status, the system may make a new decision and output a completely new sequence of high-level actions. For example, the instruction may be changed to: roll backward first, and the system will then re-plan the underlying motion based on the new instruction.

[0083] Looping and Termination: Once the first high-level instruction is successfully executed and the robot reaches a prone position, the system will automatically input the latest hybrid state vector S into the high-level policy neural network (π). high ), requesting the next instruction. This loop continues until the high-level policy neural network (π) is reached. high Output a termination flag to indicate that the robot has successfully achieved stable standing.

[0084] 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. However, 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 method for intelligent planning of autonomous standing motions in humanoid robots, characterized in that, Includes the following steps: Step 1: Collect and fuse the robot's internal state and external environment information in real time to construct a hybrid state vector S; Step 2: The deep reinforcement learning strategy network trained offline outputs a high-level action sequence based on the mixed state vector S; Step 3: Use a real-time optimizer based on model predictive control to transform behavioral semantic commands into optimal state trajectories and optimal control trajectories; Step 4: Execute the first control input of the optimal control trajectory, while continuously monitoring the actual state and comparing the actual state with the predicted state of the real-time optimizer based on model predictive control. Calculate the deviation between the actual state and the predicted state. If the deviation is small, trigger local correction; if the deviation is large, trigger global replanning.

2. The intelligent planning method for autonomous standing motion of a humanoid robot according to claim 1, characterized in that, In step three, the centroid dynamics model is used as the dynamics model to construct and solve an optimization problem, thereby obtaining the optimal state trajectory and optimal control trajectory in the future time domain.

3. The intelligent planning method for autonomous standing motion of a humanoid robot according to claim 2, characterized in that, The optimization problem is as follows: Where H represents the future time domain, For the initial control input, The system state at time t. For behavioral semantic instruction a i The target reference state obtained through analysis, To control the input, both Q and R are weight matrices; The system state at time t+1, Let ZMP be the system state transition function, ZMP be the zero-moment point, and Support Polygon be the support polygon formed by the active contact points. Let be the contact force vector at the i-th contact point at time t. Let be the ground normal vector at the i-th contact point. Let be the coefficient of friction at the i-th contact point, Distance be the minimum distance between the robot and the j-th obstacle, and robot be the robot's name. For the j-th obstacle, As a safety threshold, To control the lower limit of the input, To control the upper limit of the input.

4. The intelligent planning method for autonomous standing motion of a humanoid robot according to claim 1, characterized in that, In step four, the deviation between the actual state and the predicted state is δ = ||Sactual - Spredicted||, where Sactual is the actual state and Spredicted is the predicted state.

5. The intelligent planning method for autonomous standing motion of a humanoid robot according to claim 1, characterized in that, In step four, the small deviation is δ≤ϵ, where ϵ is the deviation threshold. At this time, local correction is triggered: the real-time optimizer based on model predictive control adjusts the parameters in the optimization problem and performs re-optimization in the next control cycle to compensate for the deviation.

6. The intelligent planning method for autonomous standing motion of a humanoid robot according to claim 1, characterized in that, In step four, the large deviation is δ>ϵ, where ϵ is the deviation threshold. At this time, global replanning is triggered: the latest actual state is fed back to the deep reinforcement learning policy network, and a new high-level action sequence is output based on the latest actual state. The system then restarts the underlying motion planning.

7. The intelligent planning method for autonomous standing motion of a humanoid robot according to claim 1, characterized in that, In step two, the deep reinforcement learning policy network is pre-trained on a large scale using reinforcement learning algorithms in a highly randomized simulation environment.

8. The intelligent planning method for autonomous standing motion of a humanoid robot according to claim 1, characterized in that, In step two, the reward function of the deep reinforcement learning policy network is designed to encourage successful standing, minimize energy consumption and time, and punish unstable postures and collisions.

9. The deep reinforcement learning policy network system applied to the intelligent planning method for autonomous standing motion of a humanoid robot according to any one of claims 1-8, characterized in that, include: The multimodal perception and hybrid state construction module collects and fuses the robot's internal state and external environment information in real time to construct a hybrid state vector S; The high-level policy decision-making module uses an offline-trained deep reinforcement learning policy network to output a high-level action sequence based on the mixed state vector S. The underlying motion planning module uses a model predictive control-based real-time optimizer to transform behavioral semantic commands into optimal state trajectories and optimal control trajectories. The closed-loop execution and adaptive adjustment module executes the first control input of the optimal control trajectory, while continuously monitoring the actual state and comparing the actual state with the predicted state of the real-time optimizer based on model predictive control. It calculates the deviation between the actual state and the predicted state. If the deviation is small, it triggers local correction; if the deviation is large, it triggers global replanning.

10. The intelligent planning system for autonomous standing motion of a humanoid robot according to claim 9, characterized in that, The multimodal perception and hybrid state construction module includes a body state perception unit, an environment perception unit, and a state fusion unit. The body state perception unit includes an inertial measurement unit, a joint encoder, and a force / torque sensor. The inertial measurement unit is used to measure the robot's three-axis acceleration and angular velocity. The joint encoder is used to measure the robot's full-body joint angles. Force / torque sensors are used to measure the contact state between each end effector and the ground, as well as the magnitude and direction of the contact force between each end effector and the ground.

Images