Balance control algorithm for gravity center trajectory planning and real-time tracking of humanoid robot

Abstract

The invention relates to a balance control algorithm for gravity center trajectory planning and real-time tracking of a humanoid robot, in particular to the technical field of robot control, and realizes dynamic balance control through a closed-loop process of state sensing, online gravity center trajectory re-planning, hierarchical control law calculation and joint torque output and execution. A layered dynamic modeling and control framework is constructed, a variable-height inverted pendulum model is combined with model prediction control, and a gravity center and zero moment point track feasible in dynamics is generated online; virtual restoring force is generated through a gravity center impedance controller, a task priority inverse dynamics solver is used, multi-task execution is coordinated on the premise that balance tasks are guaranteed to be absolutely preferential through a null-space projection technology, theoretical torque is converted into current instructions to drive joints to move, and through efficient collaboration and real-time updating of dynamic models of all levels, the balance tasks are optimized. According to the invention, deep collaboration of various technical features is realized, and the problems of poor adaptability, slow response and poor balance capability of a traditional method in a dynamic environment are effectively solved.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and more specifically, to a balance control algorithm for planning and real-time tracking the center of gravity trajectory of a humanoid robot. Background Technology

[0002] As a key development direction in the intelligent manufacturing equipment industry, humanoid robots have become a research hotspot due to their enormous potential to adapt to human environments. However, balance control during bipedal walking remains a core challenge, the crux of which lies in how to establish and control their dynamic model. Traditional balance control methods mainly suffer from the following problems: Static balance limitations: Early methods relied on keeping the center of gravity always projected within the supporting polygon (static balance), which resulted in stiff, slow gait, high energy consumption, and inability to cope with external disturbances, severely limiting their practicality in industrial scenarios requiring rapid movement.

[0003] The linear inverted pendulum model is oversimplified: To simplify calculations, the widely used linear inverted pendulum model concentrates the robot's mass at a single point and assumes a constant height. This oversimplification ignores the robot's actual angular momentum, multibody dynamics, and changes in center of gravity height, leading to insufficient control precision and instability in complex scenarios such as rapid movement, turning, or going uphill and downhill.

[0004] Planning and control are disconnected: Oversimplified dynamic modeling treats center of gravity trajectory planning and joint trajectory tracking as two separate problems. The pre-planned trajectory may be dynamically infeasible, and the hierarchical control law calculation can only passively track the trajectory. When subjected to external disturbances or uneven ground, it lacks the ability to replan online and cannot achieve true dynamic equilibrium.

[0005] Poor real-time performance of dynamic modeling: Optimization algorithms based on whole-body multibody dynamics models are computationally complex and difficult to run in real time on airborne computing resources, which limits their practicality in dynamic environments and makes it difficult to cope with sudden and unpredictable situations in industrial environments.

[0006] Therefore, existing technologies make it difficult for bipedal robots to have the same robust and natural balance as humans when facing uneven ground, being pushed by external forces, or performing agile movements. Summary of the Invention

[0007] This invention addresses the technical problems existing in the prior art by providing a balance control algorithm for humanoid robot center of gravity trajectory planning and real-time tracking. By generating a dynamically feasible center of gravity trajectory online and designing a hierarchical control framework, the algorithm achieves accurate trajectory tracking and disturbance compensation, thereby significantly improving the balance robustness and motion naturalness of the humanoid robot in dynamic environments, thus solving the problems mentioned in the background art.

[0008] To achieve the present invention, a balance control algorithm for humanoid robot center of gravity trajectory planning and real-time tracking is provided, comprising the following steps: State perception steps: The robot's current state is acquired in real time through onboard sensors, including the actual position and velocity of the center of gravity, torso posture, foot contact force, and the actual position of the zero torque point; and based on the distribution of foot contact force and inertial measurement data, the normal vector and friction coefficient of the ground contact plane are estimated in real time. Online center of gravity trajectory replanning steps: Based on the current state and desired motion commands, a predictive control planner is used to solve online for the dynamically feasible desired center of gravity trajectory and zero-moment point trajectory within a finite future time domain; wherein, the planner is based on a variable-height inverted pendulum model, the desired center of gravity trajectory satisfies robot dynamics constraints, and the zero-moment point trajectory ensures that the zero-moment point is always located within the supporting polygon; when constructing the optimization problem, the normal vector of the ground contact plane and the estimated friction coefficient are explicitly introduced to correct the supporting polygon constraints and generate an anti-slip zero-moment point trajectory; The hierarchical control law calculation steps are as follows: The virtual restoring force is calculated by using the center of gravity impedance controller to calculate the deviation between the desired center of gravity trajectory and the actual center of gravity state; The theoretical torque required for each joint is calculated and synthesized by using the task priority inverse dynamics solver, with maintaining dynamic equilibrium as the highest priority task and tracking the virtual restoring force as the second highest priority task. Joint torque output and execution steps: The theoretical torque is converted into a current command to drive the joint motor to execute, and joint motion data, foot contact force data and body motion data are collected in real time and fed back to the state perception step to form closed-loop control.

[0009] Preferably, in the state perception step, the center of gravity position and velocity are obtained by combining forward kinematics calculation with mass distribution parameters; the zero torque point position is obtained by calculating the readings of the foot force / torque sensor, and the pressure center coordinates are obtained by dividing the measured torque by the vertical force. During multi-foot contact, the vertical force is weighted and averaged. During the single-foot support period, the ground height at which the swing leg will land is predicted in real time using the joint encoder data of the swing leg and the kinematic model. This predicted height is used as one of the constraints for center of gravity height planning in the variable height inverted pendulum model.

[0010] Preferably, in the online center of gravity trajectory replanning step, the dynamic relationship of the variable height inverted pendulum model specifically satisfies: the horizontal acceleration of the center of gravity is directly proportional to the difference between the horizontal position of the zero moment point and the horizontal position of the center of gravity, and inversely proportional to the current height of the center of gravity, and the scaling factor is calculated as the gravitational acceleration divided by the height of the center of gravity; in the online center of gravity trajectory replanning step, the predictive control problem is transformed into a quadratic programming problem, wherein the optimization objective function is constructed as a weighted quadratic sum of the center of gravity trajectory tracking error, the rate of change of the zero moment point, and the zero moment point tracking error, and the weight coefficients are set to satisfy: the weight of tracking the center of gravity trajectory is the highest, followed by the control effort, and the weight of tracking the zero moment point is the lowest. In the optimization objective function, the zero moment point tracking error term is constructed as a soft constraint, and its weight is dynamically reduced when the robot is detected to be in the recovery phase of a large impact disturbance, so as to prioritize ensuring the agility of center of gravity trajectory tracking.

[0011] Preferably, the hierarchical control law calculation step includes: The three-dimensional virtual restoring force is calculated based on the deviation between the desired center of gravity trajectory and the actual center of gravity state. Multiple control tasks are processed in a fixed priority order using a task priority inverse dynamics solver: maintaining the zero-moment point within the support polygon is the highest priority task, tracking the virtual restoring force is the second highest priority task, and maintaining the torso posture is the lowest priority task. The solver ensures that low-priority tasks do not interfere with high-priority tasks through null space projection and finally synthesizes and outputs the theoretical torque required for each joint. In the null space projection operation, the Jacobian matrix used to map the balance task is dynamically constructed based on the real-time estimated ground contact plane normal vector so that the generated joint torque can actively resist the robot's slippage tendency along the inclined ground.

[0012] Preferably, the task priority inverse dynamics solver processes control tasks in a fixed priority order, from high to low: maintaining the zero moment point within the support polygon, tracking the virtual restoring force, and maintaining the torso posture; when the norm of the virtual restoring force exceeds a first preset threshold, the task priority inverse dynamics solver temporarily inserts a higher priority task of whole-body angular momentum management above the virtual restoring force task and below the balance task. This task performs disturbance rejection by optimizing joint motion to generate body angular momentum opposite to the direction of disturbance.

[0013] Preferably, the task priority inverse dynamics solver ensures that low-priority tasks do not interfere with high-priority tasks through null space projection. When solving low-priority tasks, its task objective is projected onto the null space of the Jacobian matrix corresponding to all high-priority tasks for solution. The null space projection is achieved by calculating the null space projection matrix of the Jacobian matrix. The calculation of the null space projection matrix adopts a numerical robustness method based on singular value decomposition. When the Jacobian matrix is ​​detected to be close to a singularity, its smallest singular value and the corresponding singular vector are automatically filtered out to ensure the numerical stability of the solution near the kinematic singularity point.

[0014] Preferably, when the task priority inverse dynamics solver calculates joint torques, the virtual restoring force is mapped to the joint space through the center of gravity Jacobian matrix. The center of gravity Jacobian matrix is ​​calculated in real time based on the joint angles measured in real time and the pre-stored robot link model parameters. The link model parameters include the mass, center of mass position, and inertia tensor of each link relative to the joint axis. In the joint torque synthesis stage, the net load inertia of each joint is identified online based on the readings of the joint encoder and current sensor, and this identification result is fed forward and fused with the theoretical torque calculated in the previous step to compensate for model parameter mismatch and load changes.

[0015] Preferably, in the joint torque output and execution step, converting the theoretical torque into a current command means: according to the torque constant of the joint actuator, the theoretical torque value is converted into a corresponding current command, and after digital-to-analog conversion and power amplification, the joint motor is driven to execute; at the moment of impact between the foot and the ground, based on the rapid change characteristics of the foot force / torque sensor reading in the time domain, a high-frequency damping pulse with gradually decreasing amplitude that lasts for several control cycles is temporarily superimposed on the current command to actively suppress the high-frequency vibration of the whole machine caused by the foot impact.

[0016] Preferably, the prediction time domain of the predictive control planner covers 2 to 3 future gait cycles, with a time step of 1 to 10 milliseconds. Furthermore, the online solution and trajectory output process is repeated in each control cycle to achieve real-time trajectory replanning based on the latest state feedback, actively adapting to external disturbances and complex terrain. Before each optimization solution step, the predictive control planner dynamically shrinks or expands the effective boundary of the support polygon based on the trunk angular velocity in the latest state estimate, so as to plan a more conservative zero-moment point trajectory when the body sways significantly, thereby enhancing the balance margin.

[0017] Preferably, the joint torque output and execution steps include: The theoretical torque required for each joint is converted into a corresponding current command and used to drive the joint motor. Real-time acquisition of joint motion data from the joint encoder, foot contact force data from the foot force / torque sensor, and body motion data from the inertial measurement unit is used. The acquired sensor data is used as input to the sensing and state estimation steps to initiate the closed-loop control cycle of the next control cycle. Furthermore, cross-correlation analysis is performed between the high-frequency dynamic components of the foot contact force data and the current ripple signal of the joint motor actuator. If a significant correlation occurs within a specific frequency band, it is determined that there is abnormal backlash or wear in the transmission system, triggering a maintenance alarm. Simultaneously, the control mode is switched to a conservative gain strategy that suppresses excitation in that frequency band.

[0018] To achieve the present invention, the present invention provides an electronic device, comprising: Memory, used to store computer software programs; The processor is used to read and execute the computer software program, thereby implementing the balance control algorithm for humanoid robot center of gravity trajectory planning and real-time tracking described above.

[0019] To achieve the present invention, the present invention provides a non-transitory computer-readable storage medium storing a computer software program, which, when executed by a processor, implements the balance control algorithm for planning the center of gravity trajectory and real-time tracking of the humanoid robot.

[0020] The beneficial effects of this invention are: (1) This invention fundamentally improves the dynamic balance control capability by constructing a closed-loop control framework of online trajectory replanning, hierarchical control, and real-time execution. Specifically, a predictive control planner based on a variable-height inverted pendulum model is used to generate dynamically feasible trajectories of the center of gravity and zero-moment point online; combined with a center of gravity impedance controller, the trajectory deviation is converted into a virtual restoring force; and then, through a task priority inverse dynamics solver, the zero-space projection technique is used to coordinate the execution of multiple tasks under the premise of ensuring the absolute priority of the balance task, effectively solving the technical problems of poor adaptability and easy instability in dynamic environments caused by excessive model simplification and the disconnect between planning and control in traditional methods.

[0021] (2) This invention achieves emergent optimization of overall performance through deep synergy of various technical features. The variable height model provides a precise dynamic basis for predictive planning; the real-time replanning mechanism ensures that the trajectory is always feasible; impedance control naturally transforms high-level planning into low-level mechanical instructions; and the task priority framework intelligently resolves multi-objective control conflicts. This multi-level coupling mechanism enables the robot to maintain balance like a human by actively shifting its center of gravity and coordinating the swinging of its limbs when facing sudden external forces, uneven ground, or performing agile movements. It effectively solves the core technical bottlenecks of traditional methods, such as stiff gait, slow response, and inability to balance and naturalness of movement. Ultimately, it achieves significant breakthroughs in anti-disturbance capability, naturalness of movement, and energy efficiency. Attached Figure Description

[0022] Figure 1 This is a flowchart of the balance control algorithm for center-of-gravity trajectory planning and real-time tracking of the humanoid robot of the present invention. Detailed Implementation The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0023] In the description of this application, the term "for example" is used to indicate that it is used as an example, illustration, or illustration. Any embodiment described as "for example" in this application is not necessarily to be construed as being more preferred or advantageous than other embodiments. The following description is provided to enable any person skilled in the art to make and use the invention. Details are set forth in the following description for purposes of explanation. It should be understood that those skilled in the art will recognize that the invention can be made without using these specific details. In other instances, well-known structures and processes will not be described in detail to avoid obscuring the description of the invention with unnecessary detail. Therefore, the invention is not intended to be limited to the embodiments shown, but is consistent with the broadest scope of the principles and features disclosed in this application.

[0024] Example 1, see Figure 1 The flowchart of the balance control algorithm for center of gravity trajectory planning and real-time tracking of a humanoid robot is shown. The balance control algorithm for center of gravity trajectory planning and real-time tracking of a humanoid robot includes the following steps: State perception steps: The robot's current state is acquired in real time through onboard sensors (such as inertial measurement units, joint encoders, force / torque sensors, etc.), including the actual position and velocity of the center of gravity, torso posture, foot contact force, and the actual position of the zero torque point. In one possible embodiment, based on the distribution of plantar contact force and inertial measurement data, the normal vector and friction coefficient of the ground contact plane are estimated in real time; during the single-leg support period, the ground height at which the swing leg will land is predicted in real time using the joint encoder data and kinematic model of the swing leg, and this predicted height is used as one of the constraints for the center of gravity height planning in the variable height inverted pendulum model.

[0025] The explanation explains that the state perception step acquires real-time full-body state data of the robot through an onboard sensor array. Specifically, encoders distributed at each joint measure the rotation angle and angular velocity of each joint; an inertial measurement unit (IMU) located on the robot's torso measures the robot's three-dimensional linear acceleration and three-dimensional angular velocity; and six-dimensional force / torque sensors installed on the soles of the feet measure the three-dimensional contact force and three-dimensional contact torque between the soles and the ground. Based on this raw sensor data, the following processing flow is used to obtain the required state information: through forward kinematics calculations, the joint angle data is converted into the spatial pose of each link, and then, combined with the known robot mass distribution parameters, the three-dimensional position and three-dimensional velocity of the robot's overall center of gravity in the world coordinate system are calculated; through filtering and fusion of the IMU data, the pitch and roll angles of the torso relative to the direction of gravity are estimated; and through the readings of the six-dimensional force / torque sensors on the soles of the feet, based on the physical definition of the zero-torque point (i.e., the point where the horizontal torque component is zero), the two-dimensional coordinates of the zero-torque point on the support plane are calculated. This calculation process involves dividing the measured torque component by the corresponding vertical force component.

[0026] The explanation explains that the forward kinematics calculation refers to the standard process of establishing a homogeneous transformation matrix from the base coordinate system to the coordinate system of each link based on the pre-calibrated Denavit-Hartenberg parameter table of the robot, and obtaining the pose of the end effector through chain multiplication. The robot mass distribution parameters include the mass, center of mass position, and inertia tensor of each link, which are pre-loaded during controller initialization. The specific calculation process of the zero torque point is as follows: For each foot, let the measured three-dimensional force be (Fx, Fy, Fz) and the three-dimensional torque be (Mx, My, Mz), then the coordinate calculation formula of the pressure center of the foot is x = -My / Fz, y = Mx / Fz. When both feet are in contact with the ground, the overall zero torque point is obtained by weighted averaging of the coordinates of the pressure centers of each foot according to the vertical force they bear.

[0027] The explanation explains that by calculating the angle between the force vector direction measured by the plantar force / torque sensor and the gravity direction measured by the inertial measurement unit, the normal vector of the ground contact plane is estimated in real time. Simultaneously, based on the ratio of the plantar tangential force to the normal force and its historical variation, the current static friction coefficient of the ground is estimated online. When constructing the optimization problem, the ground normal vector and the friction coefficient estimate are explicitly introduced. Specifically, the supporting polygon constraint originally on the horizontal plane is rotated to align with the actual contact plane according to the ground normal vector; at the same time, the constraint range of the zero-torque point trajectory is contracted inward based on the friction coefficient estimate to ensure that the generated zero-torque point trajectory is not only dynamically balanced but also possesses anti-slip capability.

[0028] Online center of gravity trajectory replanning steps: Based on the current state and desired motion commands (such as user-specified forward speed, turning angle, or gait parameters such as stride length and stride frequency, which come from the high-level control layer or user input), a predictive control planner is used to solve online for the dynamically feasible desired center of gravity trajectory and zero-moment point trajectory within a finite future time domain; wherein, the planner is based on a variable-height inverted pendulum model, the desired center of gravity trajectory satisfies robot dynamics constraints, and the zero-moment point trajectory ensures that the zero-moment point is always located within the support polygon; The explanation explains that the variable-height inverted pendulum model predictive control planner is invoked. This planner uses the gait cycles of the next few steps as a window to quickly solve online for the dynamically feasible desired center-of-gravity trajectory and zero-moment point trajectory. The desired center-of-gravity trajectory includes the sequence of the position and velocity of the center of gravity in three-dimensional space over time, satisfying robot dynamics constraints (such as acceleration limits and energy consumption optimization). The zero-moment point trajectory includes the sequence of the position of the zero-moment point on the support plane over time, ensuring that the zero-moment point always lies within the support polygon (i.e., dynamic equilibrium constraint) and optimizing the smoothness of motion and energy consumption. The online center-of-gravity trajectory replanning process includes: Step 201: Model Construction and Parameter Definition First, to balance model accuracy and computational efficiency, the robot's complex multibody dynamics are simplified into a variable-height inverted pendulum model. In this model, the robot's entire mass is concentrated at its center of gravity and connected to a ground support point via a retractable virtual rod. This model explicitly considers the vertical height variation of the center of gravity, thus more accurately describing the dynamic characteristics of the robot's movements such as going up and down slopes or squatting and standing up. Key parameters involved in the model include: the robot's three-dimensional position and velocity of the center of gravity, the two-dimensional position of the zero-moment point calculated from the foot contact force, and the support polygon determined from the foot position data. The core dynamic relationship is that the acceleration of the center of gravity in the horizontal plane is directly proportional to the difference between the zero-moment point position and the horizontal position of the center of gravity, and inversely proportional to the current height of the center of gravity. Specifically, the scaling factor is calculated as the gravitational acceleration divided by the height of the center of gravity. Through this relationship, given the current state of the center of gravity and the position of the zero-moment point, the motion state of the center of gravity at the next moment can be predicted. Step 202 Planner Configuration: The planner takes the current robot state and the desired motion command as input; the prediction time domain is set to cover several future gait cycles (e.g., 2 to 3 steps), and the time step is consistent with the control cycle (typically 1 to 10 milliseconds); the planner's state variables include center of gravity position, velocity, and height, while the control variables are the zero-torque point position or center of gravity acceleration. Step 203: Construct the optimization problem for online solution: The primary objective is to minimize the deviation between the predicted center-of-gravity trajectory and a reference center-of-gravity trajectory generated by motion commands, ensuring the achievement of the motion intent. Secondary objectives include minimizing the rate of change of control variables (e.g., the speed of movement at the zero-torque point) to reduce energy consumption and minimizing the tracking error at the zero-torque point. These objectives are balanced using pre-determined weighting coefficients that have been experimentally tested. The reference center of gravity trajectory is generated online based on the desired motion command. Specifically, when the desired command is forward speed, the reference center of gravity trajectory is a straight line moving at a constant speed on the horizontal plane; its vertical height can be preset to a constant value or a specific curve according to the terrain or mission requirements. A steering command corresponds to a continuously curving arc trajectory. Step 204: Construct constraints for online solution: The primary balance constraint is to ensure that the predicted zero-moment point trajectory lies entirely within the dynamically changing support polygon; the secondary constraint is the dynamic constraint, namely, the center of gravity acceleration must be within the range limited by the capabilities of the robot's joint actuators. Additionally, upper and lower limits for the variation in center of gravity height must be set, and this range is determined by the robot's mechanical structure. Step 205: Online Solving and Trajectory Output Within each control cycle, the planner executes a complete online solution loop. This loop begins with initialization, using the currently estimated robot state as the initial condition for the optimization problem. Then, prediction and solution are performed: using the variable-height inverted pendulum model, the future evolution of the system state under different control sequences is predicted. Simultaneously, the optimization problem is transformed into a standard quadratic programming problem, and a high-efficiency online solver (such as the interior-point method) is invoked to solve it, thereby obtaining a set of optimal control sequences in the future time domain, i.e., the ideal center-of-gravity trajectory and the zero-moment point trajectory. Finally, the output is: employing a rolling time-domain strategy, only the trajectory values ​​of the first time step in the optimal control sequence obtained from the solution (i.e., the desired center-of-gravity position, velocity, and desired zero-moment point position that should be tracked immediately at the current moment) are output to subsequent control steps. Step 206 Real-time replanning mechanism: The above online solution process is repeated in each control cycle. This means that the robot can continuously replan its future motion trajectory based on the latest state feedback.

[0029] In one possible embodiment, the zero-moment point tracking error term in the optimization objective function is constructed as a soft constraint, the weight of which is dynamically reduced when the robot is detected to be in the recovery phase of a large impact disturbance, so as to prioritize the agility of center of gravity trajectory tracking.

[0030] In one possible embodiment, the trajectory planning problem is transformed into a quadratic programming problem and solved through the following steps: (1) State space establishment: Discretize the dynamic relationship of the variable height inverted pendulum model at the sampling point to form a linear equation describing the evolution of the system state (center of gravity position, velocity) over time; (2) Construction of the objective function: The objective function is specifically constructed as a weighted sum of three quadratic terms: the first term is the sum of squares of the deviations between the predicted center-of-gravity trajectory and the reference center-of-gravity trajectory; the second term is the sum of squares of the rate of change of the predicted zero-moment point trajectory, used to measure control effort; and the third term is the sum of squares of the deviations between the predicted zero-moment point and the desired position. The weight coefficients of each quadratic term are preset according to the dynamic performance of the robot, with the highest weight for tracking the center-of-gravity trajectory, followed by control effort, and the lowest weight for tracking the zero-moment point. (3) Constraint linearization: The balance constraint, namely the zero moment point must be located within the supporting polygon, is expressed as a series of linear inequalities; the boundary of the supporting polygon is determined by the current and predicted coordinates of the foot contact point. (4) Problem Solving and Output: After constructing the above quadratic programming problem, the interior point method solver is called to solve it. The solver output is the optimal control sequence in the future time domain, i.e., the zero-moment point trajectory; based on the dynamic model, the corresponding dynamically feasible expected center of gravity trajectory (including position and velocity) is calculated from the zero-moment point trajectory. Finally, the first element of the optimal control sequence, i.e., the expected center of gravity position, velocity, and expected zero-moment point position to be tracked in the current control cycle, is output to the hierarchical control law calculation.

[0031] The hierarchical control law calculation steps are as follows: The virtual restoring force is calculated by using the center of gravity impedance controller to calculate the deviation between the desired center of gravity trajectory and the actual center of gravity state; The theoretical torque required for each joint is calculated and synthesized by using the task priority inverse dynamics solver, with maintaining dynamic equilibrium as the highest priority task and tracking the virtual restoring force as the second highest priority task. The explanation explains that the hierarchical control law calculation step is used to convert the desired center of gravity trajectory and the desired zero-moment point trajectory into joint torque commands that can directly drive the robot. The virtual restoring force is obtained by multiplying the position deviation by a preset virtual stiffness coefficient to obtain the spring force. The value of the virtual stiffness coefficient is usually set to 5 to 10 times the total mass of the robot (unit: kg), and its unit is Newton / meter. The velocity deviation is multiplied by a preset virtual damping coefficient to obtain the damping force. Then, the spring force and the damping force are vectored together. The values ​​of the virtual stiffness coefficient and the virtual damping coefficient are preset according to the total mass of the robot and the desired dynamic response characteristics. In the zero-space projection operation, the Jacobian matrix used to map the balance task is dynamically constructed based on the real-time estimated ground contact plane normal vector so that the generated joint torque can actively resist the robot's slippage tendency along the inclined ground.

[0032] The process of acquiring the virtual resilience includes: Deviation calculation: The difference between the desired center of gravity position and the actual center of gravity position is calculated in real time to obtain the position deviation; at the same time, the difference between the desired center of gravity velocity and the actual center of gravity velocity is calculated to obtain the velocity deviation. The virtual restoring force is composed of two parts: one part is proportional to the position deviation, and its proportionality coefficient is called virtual stiffness; the other part is proportional to the velocity deviation, and its proportionality coefficient is called virtual damping. Adding these two force vectors yields the final three-dimensional virtual restoring force acting on the center of gravity. The values ​​of the virtual stiffness and virtual damping are preset based on the robot's total mass and the desired dynamic response characteristics. For example, the virtual stiffness is typically set to 5 to 10 times the robot's total mass, while the virtual damping is adjusted to a value close to the critical damping state to ensure a rapid response without overshoot.

[0033] The task priority inverse dynamics solution is used to decompose virtual forces into joint moments. Its principle is to complete as many secondary tasks as possible while ensuring the highest priority task (dynamic equilibrium) is absolutely satisfied. This process specifically includes: Task definition and priority allocation: Identify all tasks to be executed and fix their priority order, from highest to lowest: Highest priority task (balancing task): Ensure the zero-moment point always lies within the supporting polygon. This task is achieved by optimizing the contact force between the foot and the ground. Second highest priority task (center of gravity task): accurately generate the required virtual resilience to track the desired center of gravity trajectory; Lower priority tasks (posture tasks, etc.): maintaining an upright posture of the torso, or accurately tracking the desired trajectory of the swinging foot.

[0034] Among them, priority-based serialization solution: the solution process strictly follows the priority order mentioned above. Low-priority tasks must be solved within the null space of high-priority tasks. This means that the actions of low-priority tasks must never interfere with the execution of high-priority tasks, including: First, address the balancing task: using the plantar contact force as the optimization variable, construct a quadratic programming problem with the goal of making the zero-moment point most stably located at the center of the supporting polygon, and solve for the preliminary joint space solution that satisfies this task. Next, the center of gravity task is addressed: without disrupting the aforementioned balance, the virtual restoring force is mapped to the joint space using the center of gravity Jacobian matrix (which is calculated in real time based on the joint angles measured in real time, combined with the robot link model parameters pre-stored in the controller. The link model parameters include the mass, center of mass position, and inertia tensor relative to the joint axis of each link), and the joint torque compensation amount that satisfies center of gravity tracking without interfering with balance is solved. Finally, attitude and other tasks are processed: Under the premise of ensuring that all higher priority tasks are not interfered with, the joint torque compensation amount that satisfies the attitude and other tasks is solved by tools such as the torso attitude Jacobian matrix (also calculated based on real-time state). Theoretical torque synthesis and output: The joint torques obtained from the step-by-step solution above, which meet the task priority, are synthesized and substituted into the dynamic equations that take into account the robot's overall mass, inertia and other inherent parameters for verification and adjustment. Finally, the theoretical torque required for each joint is calculated.

[0035] Furthermore, to ensure that low-priority tasks do not interfere with high-priority tasks, this invention employs null space projection as a mathematical tool. Specifically, when solving low-priority tasks, their task objectives are projected onto the null space (i.e., the invalid subspace) of the resultant moment space of all current high-priority tasks. This means that the joint solutions sought by the solver are strictly limited to the solution space dimension that will not affect high-priority tasks. This operation is achieved by calculating the null space projection matrix of the Jacobian matrix corresponding to the high-priority tasks, a calculation process that is a well-known numerical linear algebra method in the art.

[0036] Furthermore, in the final torque synthesis stage, the complete dynamic model of the robot must be considered. The dynamic equations include inertial terms determined by the robot's mass distribution, Coriolis and centrifugal force terms determined by the motion velocity, and gravity terms. These dynamic parameters (such as the mass of each link, the position of the center of mass, and the inertia tensor) are inherent properties of the robot and must be pre-loaded during controller initialization. The final theoretical torque is the least-squares solution that enables the robot to generate the expected motion satisfying all task priority sequences under this dynamic model.

[0037] In one possible embodiment, before each optimization solution step, the planner dynamically shrinks or expands the effective boundary of the supporting polygon based on the magnitude of the trunk angular velocity in the latest state estimate, so as to plan a more conservative zero-moment point trajectory when the body sways significantly, thereby enhancing the balance margin.

[0038] Joint torque output and execution steps: The theoretical torque is converted into a current command to drive the joint motor to execute, and joint motion data, foot contact force data and body motion data are collected in real time and fed back to the state perception step to form closed-loop control.

[0039] This step, as explained, is the final execution stage of the control loop. It is responsible for converting the calculated theoretical torque into the robot's actual motion and providing the lowest-level sensory feedback to the entire control system, including: Physical output of joint torque: The controller outputs the theoretical torque required for each joint, calculated in step three, as command values ​​to the robot's joint actuators. For common motor actuators, this process specifically involves converting the theoretical torque values ​​into corresponding current commands based on the actuator's torque constant. Subsequently, this electrical signal passes through a digital-to-analog converter and a power amplifier to drive the joint motors to generate the corresponding physical torque, thereby moving the linkages.

[0040] Closed-loop control at the actuator level: Within each joint, the actuator typically has a built-in current loop controller. This controller targets the received current command, monitors the actual current of the motor in real time, and adjusts the output voltage to accurately track the current command, thereby ensuring that the torque output to the joint matches the theoretical torque. This process operates at extremely high frequencies within the actuator, forming the innermost torque closed-loop control.

[0041] The formation of system-level closed-loop feedback: After joint movement occurs, the robot's state changes. At this time, the actual angles and angular velocities of each joint are measured by joint encoders, the actual foot contact force distribution is measured by foot force / torque sensors, and the acceleration and angular velocity of the torso are measured by an inertial measurement unit. The real-time readings of all these sensors are synchronously acquired and used as the robot's actual state data, which is then transmitted to the input of step one for a new round of state estimation at the start of the next control cycle.

[0042] In one possible embodiment, the high-frequency dynamic component in the foot contact force data is cross-correlation analyzed with the current ripple signal of the joint motor actuator. If a significant correlation is found within a specific frequency band, it is determined that there is abnormal backlash or wear in the transmission system, and a maintenance alarm is triggered. At the same time, the control mode is switched to a conservative gain strategy to suppress the excitation of that frequency band. At the moment of impact between the foot and the ground, based on the rapid change characteristics of the foot force / torque sensor reading in the time domain, a high-frequency damping pulse with gradually decreasing amplitude that lasts for several control cycles is temporarily superimposed on the current command to actively suppress the high-frequency vibration of the whole machine caused by the foot impact.

[0043] Example 2: An electronic device provided in this embodiment of the invention is used to implement a balance control algorithm for center-of-gravity trajectory planning and real-time tracking of a humanoid robot. It includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it performs the following steps: State perception steps: The robot's current state is acquired in real time through onboard sensors, including the actual position and velocity of the center of gravity, torso posture, foot contact force, and the actual position of the zero torque point; and based on the distribution of foot contact force and inertial measurement data, the normal vector and friction coefficient of the ground contact plane are estimated in real time. Online center of gravity trajectory replanning steps: Based on the current state and desired motion commands, a predictive control planner is used to solve online for the dynamically feasible desired center of gravity trajectory and zero-moment point trajectory within a finite future time domain; wherein, the planner is based on a variable-height inverted pendulum model, the desired center of gravity trajectory satisfies robot dynamics constraints, and the zero-moment point trajectory ensures that the zero-moment point is always located within the supporting polygon; when constructing the optimization problem, the normal vector of the ground contact plane and the estimated friction coefficient are explicitly introduced to correct the supporting polygon constraints and generate an anti-slip zero-moment point trajectory; The hierarchical control law calculation steps are as follows: The virtual restoring force is calculated by using the center of gravity impedance controller to calculate the deviation between the desired center of gravity trajectory and the actual center of gravity state; The theoretical torque required for each joint is calculated and synthesized by using the task priority inverse dynamics solver, with maintaining dynamic equilibrium as the highest priority task and tracking the virtual restoring force as the second highest priority task. Joint torque output and execution steps: The theoretical torque is converted into a current command to drive the joint motor to execute, and joint motion data, foot contact force data and body motion data are collected in real time and fed back to the state perception step to form closed-loop control.

[0044] Furthermore, an embodiment of the present invention provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, performs the following steps: State perception steps: The robot's current state is acquired in real time through onboard sensors, including the actual position and velocity of the center of gravity, torso posture, foot contact force, and the actual position of the zero torque point; and based on the distribution of foot contact force and inertial measurement data, the normal vector and friction coefficient of the ground contact plane are estimated in real time. Online center of gravity trajectory replanning steps: Based on the current state and desired motion commands, a predictive control planner is used to solve online for the dynamically feasible desired center of gravity trajectory and zero-moment point trajectory within a finite future time domain; wherein, the planner is based on a variable-height inverted pendulum model, the desired center of gravity trajectory satisfies robot dynamics constraints, and the zero-moment point trajectory ensures that the zero-moment point is always located within the supporting polygon; when constructing the optimization problem, the normal vector of the ground contact plane and the estimated friction coefficient are explicitly introduced to correct the supporting polygon constraints and generate an anti-slip zero-moment point trajectory; The hierarchical control law calculation steps are as follows: The virtual restoring force is calculated by using the center of gravity impedance controller to calculate the deviation between the desired center of gravity trajectory and the actual center of gravity state; The theoretical torque required for each joint is calculated and synthesized by using the task priority inverse dynamics solver, with maintaining dynamic equilibrium as the highest priority task and tracking the virtual restoring force as the second highest priority task. Joint torque output and execution steps: The theoretical torque is converted into a current command to drive the joint motor to execute, and joint motion data, foot contact force data and body motion data are collected in real time and fed back to the state perception step to form closed-loop control.

[0045] It should be noted that the descriptions of each embodiment in the above embodiments have different focuses. For parts that are not described in detail in a certain embodiment, please refer to the relevant descriptions in other embodiments.

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

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

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

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

[0050] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention.

[0051] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. A balance control algorithm for center-of-gravity trajectory planning and real-time tracking of a humanoid robot, characterized in that, Includes the following steps: State perception step: The robot's current state is acquired in real time through onboard sensors, including the actual position and velocity of the center of gravity, the posture of the torso, the contact force of the feet, and the actual position of the zero torque point; Online center of gravity trajectory replanning steps: Based on the current state and the desired motion command, the predictive control planner solves online the dynamically feasible desired center of gravity trajectory and zero-moment point trajectory within a finite future time domain; wherein, the planner is based on a variable height inverted pendulum model, the desired center of gravity trajectory satisfies the robot dynamics constraints, and the zero-moment point trajectory ensures that the zero-moment point is always located within the support polygon; The hierarchical control law calculation steps are as follows: The virtual restoring force is calculated by using the center of gravity impedance controller to calculate the deviation between the desired center of gravity trajectory and the actual center of gravity state; The theoretical torque required for each joint is calculated and synthesized by using the task priority inverse dynamics solver, with maintaining dynamic equilibrium as the highest priority task and tracking the virtual restoring force as the second highest priority task. Joint torque output and execution steps: The theoretical torque is converted into a current command to drive the joint motor to execute, and joint motion data, foot contact force data and body motion data are collected in real time and fed back to the state perception step to form closed-loop control.

2. The balance control algorithm for humanoid robot center of gravity trajectory planning and real-time tracking according to claim 1, characterized in that, In the online center of gravity trajectory replanning step, the dynamic relationship of the variable height inverted pendulum model specifically satisfies: the horizontal acceleration of the center of gravity is directly proportional to the difference between the horizontal position of the zero moment point and the horizontal position of the center of gravity, and inversely proportional to the current height of the center of gravity, and the scaling factor is calculated as the gravitational acceleration divided by the height of the center of gravity. In the center-of-gravity trajectory replanning step, the predictive control problem is transformed into a quadratic programming problem. The optimization objective function is constructed as a weighted quadratic sum of the center-of-gravity trajectory tracking error, the rate of change of the zero-moment point, and the zero-moment point tracking error. The weight coefficients are set to satisfy the following: the weight of tracking the center-of-gravity trajectory is the highest, followed by the control effort, and the weight of tracking the zero-moment point is the lowest. In the optimization objective function, the zero-moment point tracking error term is constructed as a soft constraint, and its weight is dynamically reduced when the robot is detected to be in the recovery phase of a large impact disturbance, so as to prioritize the agility of center-of-gravity trajectory tracking.

3. The balance control algorithm for humanoid robot center of gravity trajectory planning and real-time tracking according to claim 2, characterized in that, The hierarchical control law calculation steps include: The three-dimensional virtual restoring force is calculated based on the deviation between the desired center of gravity trajectory and the actual center of gravity state. Multiple control tasks are processed in a fixed priority order using a task priority inverse dynamics solver: maintaining the zero-moment point within the support polygon is the highest priority task, tracking the virtual restoring force is the second highest priority task, and maintaining the torso posture is the lowest priority task. The solver ensures that low-priority tasks do not interfere with high-priority tasks through null space projection and finally synthesizes and outputs the theoretical torque required for each joint. In the null space projection operation, the Jacobian matrix used to map the balance task is dynamically constructed based on the real-time estimated ground contact plane normal vector so that the generated joint torque can actively resist the robot's slippage tendency along the inclined ground.

4. The balance control algorithm for humanoid robot center of gravity trajectory planning and real-time tracking according to claim 3, characterized in that, The task priority inverse dynamics solver processes control tasks in a fixed priority order, from high to low: maintaining the zero moment point within the support polygon, tracking the virtual restoring force, and maintaining the torso posture.

5. The balance control algorithm for humanoid robot center of gravity trajectory planning and real-time tracking according to claim 3, characterized in that, The task priority inverse dynamics solver ensures that low-priority tasks do not interfere with high-priority tasks through null space projection. When solving low-priority tasks, its task objective is projected onto the null space of the Jacobian matrix corresponding to all high-priority tasks for solution. The null space projection is achieved by calculating the null space projection matrix of the Jacobian matrix. The calculation of the null space projection matrix adopts a numerical robustness method based on singular value decomposition. When the Jacobian matrix is ​​detected to be close to a singularity, its smallest singular value and the corresponding singular vector are automatically filtered out to ensure the numerical stability of the solution near the kinematic singular point.

6. The balance control algorithm for humanoid robot center of gravity trajectory planning and real-time tracking according to claim 1, characterized in that, When calculating joint torques, the task priority inverse dynamics solver maps the virtual restoring force to the joint space through the center of gravity Jacobian matrix. The center of gravity Jacobian matrix is ​​calculated in real time based on the measured joint angles and the pre-stored robot link model parameters. The link model parameters include the mass, center of mass position, and inertia tensor of each link relative to the joint axis. In the joint torque synthesis stage, the net load inertia of each joint is identified online based on the readings of the joint encoder and current sensor. This identification result is then fed forward and fused with the theoretical torque calculated in the previous step to compensate for model parameter mismatch and load changes.

7. The balance control algorithm for humanoid robot center of gravity trajectory planning and real-time tracking according to claim 1, characterized in that, In the joint torque output and execution steps, converting the theoretical torque into a current command means: based on the torque constant of the joint actuator, converting the theoretical torque value into a corresponding current command, and then driving the joint motor to execute it after digital-to-analog conversion and power amplification; At the moment of impact between the foot and the ground, based on the rapid change in the time domain of the foot force / torque sensor readings, a high-frequency damping pulse with gradually decreasing amplitude, lasting for several control cycles, is temporarily superimposed on the current command to actively suppress the high-frequency vibration of the whole machine caused by the foot impact.

8. The balance control algorithm for humanoid robot center of gravity trajectory planning and real-time tracking according to claim 1, characterized in that, The predictive control planner's prediction time domain covers 2 to 3 future gait cycles, with a time step of 1 to 10 milliseconds. Furthermore, the online solution and trajectory output process is repeated in each control cycle to achieve real-time trajectory replanning based on the latest state feedback, actively adapting to external disturbances and complex terrain. Before each optimization solution step, the predictive control planner dynamically shrinks or expands the effective boundary of the support polygon based on the trunk angular velocity in the latest state estimate, thus planning a more conservative zero-moment point trajectory when the body sways significantly, thereby enhancing the balance margin.

9. An electronic device, characterized in that, include: Memory, used to store computer software programs; A processor is configured to read and execute the computer software program, thereby implementing the balance control algorithm for humanoid robot center of gravity trajectory planning and real-time tracking as described in any one of claims 1-8.

10. A non-transitory computer-readable storage medium, characterized in that, The storage medium stores a computer software program, which, when executed by a processor, implements the steps of the balance control algorithm for humanoid robot center of gravity trajectory planning and real-time tracking as described in any one of claims 1-8.

Images