A method for controlling the joint motors of a humanoid robot with model predictive control

Abstract

This invention discloses a control method for joint motors of a humanoid robot with model predictive control, belonging to the field of robot motion control technology. It addresses the problems of control command infeasibility and model disturbance caused by voltage saturation during high-speed motion. An error state-space model of the joint motor system is established, and nonlinear terms such as gravity are removed as nominal feedforward quantities. Subsequently, the motor speed and driver bus voltage are acquired in real time, the back electromotive force is calculated, and the voltage margin is mapped to the current dynamic torque physical limit value. Simultaneously, an extended state observer is used to estimate the lumped disturbance value not modeled in the model. Finally, the dynamic limit is used as a real-time inequality constraint, and the disturbance value is introduced to correct the prediction equation. A quadratic programming problem is constructed and solved to extract the optimal compensation torque, which is superimposed with the inverse dynamic feedforward torque and converted into a current signal to drive the motor, achieving robust control with high dynamics and high precision under physical boundary constraints.

Description

Technical Field

[0001] This invention relates to the field of robot motion control technology, specifically to a method for controlling the joint motors of a humanoid robot with model predictive control. Background Technology

[0002] With the deep integration of artificial intelligence and precision manufacturing technology, humanoid robots are gradually moving from laboratories to complex application scenarios such as industrial production, home services, and disaster relief. Unlike industrial robotic arms with fixed bases, humanoid robots need to perform walking, jumping, and load-bearing operations in unstructured environments, which places extremely high demands on the dynamic response capability, torque output accuracy, and environmental adaptability of the joint drive system. As the core actuator, the joint motor's control performance directly determines the smoothness, safety, and accuracy of the robot's overall movement and task execution.

[0003] However, existing joint motor control methods have significant shortcomings when facing highly dynamic and strongly nonlinear tasks. Traditional PID control strategies essentially rely on feedback adjustment based on "after-the-fact" errors, which easily leads to phase lag when dealing with multi-joint coupled dynamics, making it difficult to meet the trajectory tracking requirements under high-speed motion. More seriously, existing control algorithms typically use fixed torque limit thresholds, failing to consider the impact of increased back EMF on the drive voltage during high-speed motor rotation. When the robot performs rapid movements, the actual available voltage margin of the actuator decreases significantly. If the controller still issues torque commands according to the fixed threshold, it will cause the actuator voltage to saturate, leading to current loop instability, torque output cutoff, or even severe body vibration. Furthermore, control methods relying on simple dynamic models cannot effectively cope with model uncertainties such as reducer friction and wear, and load changes, resulting in irreversible degradation of control accuracy after long-term operation. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention provides a method for controlling the joint motors of a humanoid robot with model predictive control, thus solving the problems mentioned above.

[0005] To achieve the above objectives, the present invention provides the following technical solution: a joint motor control method for a humanoid robot with model predictive control, comprising the following steps: S1. Establishing an error state-space model of the joint motor system, defining the joint position tracking error and its derivative as state variables, and taking the gravity term, Coriolis force term, and centrifugal force term in joint dynamics as known nominal feedforward quantities, and analytically obtaining the state transition matrix and input control matrix describing the dynamic characteristics of the system through linearization; S2. Acquiring the real-time speed of the motor and the driver bus voltage, calculating the back electromotive force at the current speed according to the motor voltage equation, calculating the difference between the bus voltage and the back electromotive force and mapping it to the target value. S3. The dynamic torque physical limit value at the current moment is used, and the lumped disturbance value, including the model's unmodeled error, is estimated using an extended state observer; S4. The lumped disturbance value is introduced into the prediction equation constructed based on the state transition matrix and the input control matrix to predict the system state trajectory in the future time domain, and the dynamic torque physical limit value is used as the real-time inequality constraint condition for the control input to construct and solve a quadratic programming problem, and the first term of the optimal control sequence is extracted as the model predicted compensation torque; S5. The model predicted compensation torque is superimposed with the feedforward torque calculated based on the inverse dynamics model to generate the final torque command and convert it into a current control signal to drive the joint motor.

[0006] Furthermore, the construction logic of the error state space model of the joint motor system is as follows: Perform dynamic decoupling to decompose the multi-rigid-body dynamic equations of the humanoid robot into a single-joint dimension, and configure a single-axis dynamic equation including the motor rotor inertia, reducer transmission ratio, and equivalent inertia of the connecting rod side; in the single-axis dynamic equation, perform a nonlinear term separation step, unifying the nonlinear dynamic terms composed of gravity, Coriolis force, and centrifugal force, as well as the disturbance terms composed of joint friction and external contact forces, as generalized external torque inputs independent of the motor input torque, simplifying the system into a second-order differential equation describing the relationship between the input torque and the joint angular acceleration, and ignoring the direct influence of higher-order nonlinear coupling terms on the predictive model structure, thus completing the construction of the physical model framework of the linear predictive controller.

[0007] Furthermore, the joint position tracking error and its derivative are defined as state variables. The gravity, Coriolis force, and centrifugal force terms in joint dynamics are taken as known nominal feedforwards. The specific process of analytically deriving the state transition matrix and input control matrix describing the system's dynamic characteristics through linearization is as follows: The difference between the desired joint trajectory and the actual feedback trajectory is calculated, constructing a state vector containing position error and velocity error components. In the continuous time domain, an algebraic stripping operation is performed, subtracting the nominal feedforwards as known terms from the total control torque, transforming the remaining dynamic equations into a linear time-invariant system describing only the relationship between the error state and the control increment. A zero-order hold is used with a set control period as the sampling time to perform a discretization transformation, converting the coefficient matrix in the continuous time domain into a state transition matrix and input control matrix in the discrete time domain. The state transition matrix is ​​used to quantify the free evolution trend of the current error state under no control input, and the input control matrix is ​​used to quantify the corrective effect of the control increment on the error state at the next moment.

[0008] Furthermore, the real-time motor speed and driver bus voltage are collected, and the back electromotive force (EMF) at the current speed is calculated based on the motor voltage equation. The specific process of calculating the difference between the bus voltage and the back EMF and mapping it to the physical limit value of the dynamic torque at the current moment is as follows: In each control cycle, the DC bus voltage sensor value and the angular velocity fed back by the motor encoder are read in real time. The instantaneous back EMF is calculated by multiplying the pre-calibrated motor back EMF constant with the current angular velocity. The instantaneous back EMF value is subtracted from the bus voltage value, and the safety margin generated by the internal impedance voltage drop of the drive circuit is deducted to obtain the maximum effective drive voltage that can actually be applied to the two ends of the motor winding at the current moment. According to Ohm's law, the maximum effective drive voltage is converted into the maximum allowable phase current using the motor phase resistance parameters. Then, combined with the motor torque constant, the maximum allowable phase current is mapped to the corresponding torque value. The torque value is set as the upper limit constraint of the control variable in the model predictive control optimization solution process, so that the generated control command is always within the driver voltage saturation region at the physical level.

[0009] Furthermore, the specific process of simultaneously estimating the lumped disturbance value, including model-unmodeled errors, using the extended state observer is as follows: Configure a three-dimensional extended state vector containing joint position, joint angular velocity, and lumped disturbance terms; use the actual position fed back by the motor encoder as the system observation output; use the final torque command applied to the motor in the previous control cycle as the system control input; calculate the observation residual between the estimated position derived from the observed state and the actual position; and perform real-time correction of the derivative of the extended state vector using a pre-designed observer gain matrix. Update the extended state vector through integration operations, and separate the third-dimensional state component as a real-time estimate of the system's lumped disturbance, which reflects the combined effects of friction, external load, and model mismatch on the system at the current moment.

[0010] Furthermore, the lumped disturbance value is introduced into the prediction equation constructed based on the state transition matrix and the input control matrix. The specific process for predicting the system state trajectory in the future time domain is as follows: In the finite prediction time domain starting from the current moment, the lumped disturbance value is used as a known exogenous disturbance term and superimposed on the discrete state-space equation composed of the state transition matrix and the input control matrix to form an augmented prediction model. Starting from the error state at the current moment, the augmented prediction model is used to perform multi-step iterative deduction, and the system state at each future moment is expressed as a linear function of the current state, the lumped disturbance value, and a series of future control increments to be solved, generating the prediction trajectory expression of the future control increment sequence.

[0011] Furthermore, using the physical limit value of the dynamic torque as the real-time inequality constraint condition for the control input, a quadratic programming problem is constructed and solved. The specific process of extracting the first term of the optimal control sequence as the model prediction compensation torque is as follows: Construct a quadratic cost function containing a state tracking error term and a control increment penalty term, and use a weighted matrix to balance the trajectory tracking accuracy and the smoothness of the control action; Transform the physical limit value of the dynamic torque into a set of linear inequality constraints for the future control input sequence to ensure that the planned torque at any time in the prediction time domain does not exceed the physical boundary allowed by the voltage margin at that time; Combine the cost function and the set of linear inequality constraints to convert it into a standard quadratic programming numerical solution form, and use the effective set method or interior point method to solve for the optimal control increment sequence that minimizes the cost function. Extract the first control increment value in this sequence and add it to the predicted torque at the previous time to obtain the optimal model prediction compensation torque required at the current time.

[0012] Furthermore, the specific process of superimposing the model-predicted compensation torque with the feedforward torque calculated based on the inverse dynamics model to generate the final torque command and convert it into a current control signal to drive the articulated motor is as follows: Based on the inverse dynamics model of the rigid body dynamics equation, the nominal feedforward torque required to overcome gravity, Coriolis force and inertia is calculated according to the current desired trajectory; a torque fusion operation is performed, and the nominal feedforward torque, the optimal model-predicted compensation torque and the reverse compensation component of the lumped disturbance value are algebraically summed to obtain the total desired output torque; the total desired output torque is divided by the torque constant of the motor to obtain the target q-axis current command, and this current command is input to the current loop controller of the motor driver, which drives the inverter to generate the corresponding phase current through the field-oriented control algorithm, thereby driving the articulated motor to move.

[0013] The present invention has the following beneficial effects:

[0014] (1) A joint motor control method for a humanoid robot with model predictive control is proposed. By feedforwarding the nonlinear terms, a linear error state-space model that is easy to solve quickly is established, reducing the computational power required by the algorithm. The physical limit of the dynamic torque that the motor can actually output at the current moment is calculated based on the real-time back electromotive force, which effectively prevents control saturation and system instability caused by insufficient driving voltage under high-speed motion conditions. At the same time, the extended state observer is used to capture friction and external disturbances in real time, so that the model can perceive unmodeled errors, ensuring the robustness of the control algorithm under conditions of inaccurate model parameters or wear.

[0015] (2) A joint motor control method for a humanoid robot with model predictive control incorporates dynamically changing physical limits as hard constraints into the quadratic programming solver, enabling the model predictive control algorithm to predict future voltage boundaries and thus plan an optimal control curve that satisfies both physical constraints and is extremely smooth, avoiding the impact of traditional hard limiting. Finally, by superimposing and fusing the compensation torque calculated by model predictive control with the feedforward torque calculated by inverse dynamics, both the feedforward is used to ensure a fast response to high-frequency commands, and the model predictive control is used to eliminate tracking errors, thereby achieving trajectory correction while ensuring the agility of joint movements.

[0016] Of course, any product implementing this invention does not necessarily need to achieve all of the advantages described above at the same time. Attached Figure Description

[0017] Figure 1 This is a flowchart of a joint motor control method for a humanoid robot with model predictive control according to the present invention. Detailed Implementation

[0018] This application provides a humanoid robot joint motor control method with model predictive control, which solves the problems of existing technologies such as the saturation of drive voltage caused by ignoring back electromotive force during high-speed humanoid robot movement, resulting in system jitter, as well as the low trajectory tracking accuracy and lag dynamic response caused by model nonlinearity and external disturbances.

[0019] The overall concept of the solution in this application embodiment is as follows:

[0020] This application employs a composite control architecture combining feedforward decoupling, predictive compensation, and dynamic constraints. First, an inverse dynamics model is used to handle key nonlinear factors such as gravity and Coriolis force, simplifying the complex system into a linear error model. Second, a dynamic constraint mechanism based on the voltage equation is established to calculate the motor's torque capability boundary at the current speed in real time, and an observer is used to estimate system disturbances. Finally, aided by these dynamic boundary and disturbance information, model predictive control (MPC) is used to solve for the optimal compensation torque online, which is then fused with the feedforward torque, thereby achieving safe, precise, and highly dynamic control of the articulated motor across the entire speed range.

[0021] Please see Figure 1 This invention provides a technical solution: a method for controlling the joint motors of a humanoid robot with model predictive control, comprising the following steps: S1. Establishing an error state-space model of the joint motor system, defining the joint position tracking error and its derivative as state variables, and taking the gravity term, Coriolis force term, and centrifugal force term in joint dynamics as known nominal feedforwards, and parsing the state transition matrix and input control matrix describing the dynamic characteristics of the system through linearization; S2. Acquiring the real-time speed of the motor and the driver bus voltage, calculating the back electromotive force at the current speed according to the motor voltage equation, calculating the difference between the bus voltage and the back electromotive force and mapping it to the current speed. S3. The dynamic torque physical limit value at time t is obtained, and the lumped disturbance value, including the model's unmodeled error, is estimated using an extended state observer; S4. The lumped disturbance value is introduced into the prediction equation constructed based on the state transition matrix and the input control matrix to predict the system state trajectory in the future time domain, and the dynamic torque physical limit value is used as the real-time inequality constraint condition for the control input to construct and solve a quadratic programming problem, and the first term of the optimal control sequence is extracted as the model predicted compensation torque; S5. The model predicted compensation torque is superimposed with the feedforward torque calculated based on the inverse dynamics model to generate the final torque command and convert it into a current control signal to drive the joint motor.

[0022] In this implementation scheme, step S1 forms the basis of system modeling. The error state-space model defines the deviation of the system from the desired trajectory (position error and velocity error) as new state variables, thereby transforming the complex nonlinear system model into a linear time-invariant system around a reference trajectory. The nominal feedforward quantity refers to the torque required to counteract known nonlinear effects (such as gravity terms and Coriolis force terms), calculated offline or online. By resolving the state transition matrix and input control matrix in S1, this step decouples the complex multi-joint dynamics problem into a linear predictive model that can be calculated online, laying the computational foundation for subsequent fast rolling optimization. Step S2 is the core of the system's perception and constraint definition. Back electromotive force (EMF) is the reverse EMF generated internally when the motor rotates; its magnitude is proportional to the rotational speed and reduces the actual usable voltage of the driver. By calculating the difference between the bus voltage and the back EMF in real time and mapping it to the physical limit value of the dynamic torque, this step dynamically introduces the voltage saturation boundary of the motor driver into the control algorithm for the first time, effectively avoiding the traditional controller issuing saturation commands that cannot be executed at high speeds. Simultaneously, the Extended State Observer (ESO) is used to uniformly estimate all unmodeled nonlinear terms (such as friction and external shocks) as lumped disturbance values. These estimates are used to correct the prediction model in subsequent step S3, significantly enhancing the system's anti-interference capability and robustness. Step S3 is the core decision-making step of Model Predictive Control (MPC). First, the lumped disturbance values ​​estimated in step S2 are introduced into the prediction equations, enabling the prediction model of MPC to reflect the current real operating environment. By performing rolling optimization in the future time domain, MPC can proactively plan control actions and avoid lag. The physical limit value of the dynamic torque is used as an inequality constraint, and a quadratic programming (QP) problem is constructed to ensure that the optimal control sequence is physically feasible throughout the entire prediction time domain. This step achieves optimal, smooth, and real-time physical constraint dynamic compensation for tracking errors by selecting the first term of the sequence as the model prediction compensation torque. Step S4 is the fusion and execution of control commands. The inverse dynamics model is used to calculate the nominal torque required to maintain the desired trajectory. As a feedforward torque, it ensures the basic response speed and most of the energy requirements of the control system. This step combines the feedforward torque with the model-predicted compensation torque obtained in step S3 (used to eliminate errors and disturbances) to form the final total desired torque. Finally, this torque is converted into a current control signal (q-axis current command) and input to the motor driver, achieving high dynamic and high-precision torque output, thereby completing the precise drive of the robot joints.

[0023] Specifically, the construction logic of the error state space model of the joint motor system is as follows: Perform dynamic decoupling to decompose the multi-rigid-body dynamic equations of the humanoid robot into a single-joint dimension, and configure a single-axis dynamic equation including the motor rotor inertia, reducer transmission ratio, and equivalent inertia of the connecting rod side; in the single-axis dynamic equation, perform a nonlinear term separation step, unifying the nonlinear dynamic terms composed of gravity, Coriolis force, and centrifugal force, as well as the disturbance terms composed of joint friction and external contact forces, as generalized external torque inputs independent of the motor input torque, simplifying the system into a second-order differential equation describing the relationship between the input torque and the joint angular acceleration, and ignoring the direct influence of higher-order nonlinear coupling terms on the predictive model structure, thus completing the construction of the physical model framework of the linear predictive controller.

[0024] In this implementation scheme, firstly, a dynamic decoupling operation is performed. This involves decomposing the multi-rigid-body dynamic equations of the humanoid robot into independent single-axis dynamic equations for individual joints through coordinate transformation and simplification. This avoids repeatedly calculating the complex coupling effects between all joints in each control cycle, significantly reducing the computational burden on the controller and enabling the algorithm to meet the real-time requirements of the robot's rapid movement. When configuring these single-axis dynamic equations, only core physical parameters such as motor rotor inertia, reducer transmission ratio, and equivalent inertia of the link side are retained, ensuring the accuracy of the model. Secondly, the nonlinear term separation step is crucial for achieving model linearization. This step categorizes inherent nonlinear terms in joint dynamics (such as gravity, Coriolis force, and centrifugal force) and terms that are difficult to model accurately (joint friction and external contact forces). These terms are uniformly treated as generalized external torque inputs independent of the controller output. Through this separation, the original nonlinear differential equations are transformed into a second-order linear differential equation that only describes the relationship between the motor input torque and joint acceleration. This approach simplifies the model structure significantly, providing a linear computational foundation for the subsequent quadratic programming solver of Model Predictive Control (MPC) and avoiding the enormous computational power required to solve nonlinear optimization problems.

[0025] Specifically, the joint position tracking error and its derivative are defined as state variables. The gravity, Coriolis force, and centrifugal force terms in joint dynamics are taken as known nominal feedforwards. The specific process of parsing the state transition matrix and input control matrix describing the dynamic characteristics of the system through linearization is as follows: Calculate the difference between the desired joint trajectory and the actual feedback trajectory, and construct a state vector containing position error components and velocity error components; In the continuous time domain, perform an algebraic stripping operation to subtract the nominal feedforwards as known terms from the total control torque, so that the remaining dynamic equations are transformed into a linear time-invariant system that only describes the relationship between the error state and the control increment; Through a zero-order hold with a set control period as the sampling time, perform a discretization transformation to transform the coefficient matrix in the continuous time domain into the state transition matrix and input control matrix in the discrete time domain. The state transition matrix is ​​used to quantify the free evolution trend of the current error state under no control input, and the input control matrix is ​​used to quantify the correction effectiveness of the control increment on the error state at the next moment.

[0026] In this implementation scheme, the process aims to further transform the established simplified physical model into a discrete-time state-space expression required for MPC, clarifying the quantitative impact of control input on error evolution. First, the difference between the desired joint trajectory and the actual feedback trajectory is calculated, and the state vector X is defined. Err Includes position error components and velocity error components By defining this error state, the controller's objective becomes driving X. Err The rapid convergence to zero significantly improves the controller's targeting. Subsequently, an algebraic stripping operation is performed in the continuous-time domain. This operation eliminates the direct impact of the nominal feedforward (i.e., the torque calculated from inverse dynamics) on the total control torque by subtracting it from the total control torque. The remaining dynamic equations are then transformed into equations describing only the error state vector X. Err For a linear time-invariant system with an evolving trend, its continuous-time state-space expression can be written as: ;wherein: F con : Continuous-time state matrix; G con : Continuous-time input matrix; X Err (t): A continuous-time state vector containing position and velocity errors; U Err (t): Continuous-time error control input (compensation torque). Finally, the zero-order hold is used for the set control period T. C For the sampling time, a discretization transformation is performed. This step transforms the continuous-time system model into a discrete-time form suitable for the operation of the digital controller, ensuring that the MPC algorithm can accurately predict the effect of its control action at the next sampling time. The discretization process is achieved through the following relationship: ; Where: Φ: Discrete-time state transition matrix, which quantifies the trend of the current error state evolving freely to the next time step without control input; Γ: Discrete-time input control matrix, which quantifies the corrective effect of the currently applied control increment on the error state at the next time step; T C : Control the sampling period. Regarding the determination of weighting coefficients: In practical applications, the error penalty weighting coefficient matrix (e.g., the Q matrix and R matrix) used for subsequent quadratic programming (QP) solutions is typically determined based on offline optimization methods using LQR control theory. Specifically, the optimal LQR feedback gain of the linear system can be calculated offline, and the Q and R matrices in the MPC can be initialized according to the weights corresponding to this gain. Subsequently, in simulations and real-world tests, fine-tuning and optimization are performed based on performance indicators such as system response overshoot, convergence speed, and control torque smoothness.

[0027] Specifically, the process of collecting real-time motor speed and driver bus voltage, calculating back electromotive force (EMF) at the current speed based on the motor voltage equation, and mapping the difference between the bus voltage and back EMF to the current dynamic torque physical limit is as follows: Within each control cycle, the DC bus voltage sensor value and the angular velocity fed back by the motor encoder are read in real time. The instantaneous back EMF is calculated using the product of the pre-calibrated motor back EMF constant and the current angular velocity. The instantaneous back EMF value is subtracted from the bus voltage value, and the safety margin generated by the internal impedance voltage drop of the drive circuit is deducted to obtain the maximum effective drive voltage that can actually be applied across the motor windings at the current moment. Based on Ohm's law, the maximum effective drive voltage is converted into the maximum allowable phase current using the motor phase resistance parameters. Then, combined with the motor torque constant, the maximum allowable phase current is mapped to the corresponding torque value. The torque value is set as the upper limit constraint of the control variables in the model predictive control optimization solution process, ensuring that the generated control commands are always within the driver voltage saturation region at the physical level.

[0028] In this implementation scheme, firstly, an instantaneous back electromotive force calculation is performed within each control cycle. This calculation utilizes the real-time readout of the motor encoder angular velocity ω. Z and the pre-calibrated back electromotive force constant K of the motor B The instantaneous back electromotive force Ψ is calculated through a linear relationship. EMF The purpose of this step is to quantify the degree of drive voltage consumption when the motor is rotating at high speed: Ψ EMF =K B ·ω Z Among them: Ψ EMF Instantaneous back electromotive force; K B ω: Back electromotive force constant of the motor; Z : Real-time angular velocity of the motor. Then, the voltage margin calculation step is performed. This step calculates the real-time bus voltage V. SSubtract the instantaneous back electromotive force Ψ EMF And deduct the safety margin V of the internal impedance drop of the reserved drive circuit. D This allows us to obtain the maximum effective drive voltage V that the driver can actually apply to the motor windings at the current moment. M The V M This is the physical basis for the planned torque, ensuring that the algorithm will not saturate on the power supply side: V M =V S -Ψ EMF -V D ; where: V M Maximum effective driving voltage; V S DC bus voltage sensor value; V D The internal impedance drop safety margin of the drive circuit. Finally, the physical quantity mapping step is performed. According to Ohm's law, the phase resistance parameter R of the motor is used. P The maximum effective driving voltage V M This is converted into the maximum allowable phase current, and then combined with the motor torque constant K. T Map the maximum permissible phase current to the corresponding physical limit value of dynamic torque τ. L The τ L That is, the upper bound of the optimization solution for the model predictive controller: ;wherein: τ L : The physical limit value of the dynamic torque at the current moment; K T Motor torque constant; R P Motor phase resistance parameters. By performing the constraint boundary update step, this τ... L Setting an upper limit constraint on the control variables in the model predictive control optimization process ensures that the generated control commands are always within the non-saturation region of the driver voltage at the physical level, fundamentally eliminating the problems of decreased control accuracy and system oscillation caused by voltage saturation.

[0029] Specifically, the process of simultaneously estimating the lumped disturbance value, including model-unmodeled errors, using the extended state observer is as follows: A three-dimensional extended state vector containing joint position, joint angular velocity, and lumped disturbance term is configured. The actual position fed back by the motor encoder is used as the system observation output, and the final torque command applied to the motor in the previous control cycle is used as the system control input. The observation residual between the estimated position derived from the observed state and the actual position is calculated. The derivative of the extended state vector is corrected in real time using a pre-designed observer gain matrix. The extended state vector is updated through integration, and the third-dimensional state component is separated as a real-time estimate of the system's lumped disturbance, reflecting the combined effects of friction, external load, and model mismatch on the system at the current moment.

[0030] In this implementation scheme, the process of simultaneously estimating the lumped disturbance value, including unmodeled errors, using an extended state observer is explained. This process aims to acquire, in real time, the lumped disturbance value generated by friction, model uncertainty, or external shocks in the system through soft-sensor technology, and use this disturbance value to enhance the robustness of the controller. First, a three-dimensional extended state vector Z is configured, including estimates of joint position z1, joint angular velocity z2, and lumped disturbance z3. This configuration elevates external disturbances in traditional control theory to state variables requiring real-time estimation. Subsequently, the observer inputs the final torque command U applied in the previous control cycle. CMD (As control input) and the actual position q fed back by the motor encoder C (As system output). The observer internally utilizes the nominal inertia M of the motor. OB The system dynamics are simulated, and the estimated position derived from the observed state is compared with the actual position q. C The observation residuals between [the points]. In the continuous-time domain, the observer is represented by the derivative of the state vector with respect to the following differential equation. Perform real-time correction Where: Z: expanded state vector z1 is the location estimate, z3 is the lumped disturbance estimate; U CMD : The torque command applied in the previous control cycle; M OB : Nominal inertia of the motor used inside the observer; q C : The actual position fed back by the motor encoder; L G1 ,L G2 ,L G3 A pre-designed observer gain matrix is ​​used to control the observer's convergence speed and noise suppression capability. Finally, the expanded state vector Z is updated through integration, and the third-dimensional state component z3 is extracted as a real-time estimate of the system's lumped disturbance. This estimate accurately reflects the combined effects of friction, external load, and model mismatch on the system at the current moment, providing accurate real-time corrections for subsequent feedforward compensation by the model predictive controller.

[0031] Specifically, the process of introducing the lumped disturbance value into the prediction equation constructed based on the state transition matrix and the input control matrix to predict the system state trajectory in the future time domain is as follows: In the finite prediction time domain starting from the current moment, the lumped disturbance value is used as a known exogenous disturbance term and superimposed on the discrete state-space equation composed of the state transition matrix and the input control matrix to form an augmented prediction model; Starting from the error state at the current moment, the augmented prediction model is used to perform multi-step iterative deduction, and the system state at each future moment is expressed as a linear function of the current state, the lumped disturbance value, and a series of future control increments to be solved, generating the prediction trajectory expression of the future control increment sequence.

[0032] In this implementation scheme, this process forms the basis for model predictive control to make forward-looking decisions. By incorporating real-time observed environmental disturbances into the predictive model, the system's prediction accuracy and anti-interference capability for future states are improved. The future finite prediction time domain N starts from the current time k. P Within this, the lumped disturbance value estimated by the extended state observer in step S2 is... As a known exogenous disturbance term, it is assumed to remain constant (or follow a pre-defined low rate of change model) in the prediction time domain. This disturbance term is then superimposed onto the discrete state-space equations consisting of the state transition matrix Φ and the input control matrix Γ, forming the augmented prediction model. The discrete state equations of the augmented prediction model can be expressed as: Where: X k : Current error state vector; Φ: State transition matrix; Γ: Input control matrix; ΔU k The control increment input to be solved at the current moment; : The lumped disturbance value estimated by the extended state observer; B Dist The disturbance input matrix is ​​used to quantify the impact of disturbances on the system state. It is represented by the error state X at the current time k. k Starting with N, we use an augmented prediction model to perform N P Step-by-step iterative deduction, the system state X at each future time k+j. k+j Represented as the current state X k Lumped disturbance value And a series of control increment sequences ΔU to be solved in the future k The linear function is then used to generate the predicted trajectory expression for the future control increment sequence, providing a complete prediction objective for the subsequent quadratic programming solver.

[0033] Specifically, using the physical limit value of dynamic torque as the real-time inequality constraint condition for the control input, the process of constructing and solving a quadratic programming problem and extracting the first term of the optimal control sequence as the model prediction compensation torque is as follows: A quadratic cost function containing state tracking error and control increment penalty terms is constructed, and a weighted matrix is ​​used to balance trajectory tracking accuracy and control smoothness. The physical limit value of dynamic torque is transformed into a set of linear inequality constraints for the future control input sequence, ensuring that the planned torque at any time in the prediction time domain does not exceed the physical boundary allowed by the voltage margin at that time. The cost function and the set of linear inequality constraints are combined and transformed into a standard quadratic programming numerical solution form. The optimal control increment sequence that minimizes the cost function is solved using the effective set method or interior point method. The first control increment value in this sequence is extracted and added to the predicted torque at the previous time to obtain the optimal model prediction compensation torque required at the current time.

[0034] In this implementation scheme, this process is the core of model predictive control, aiming to find the sequence that minimizes system error and provides the smoothest control actions while satisfying physical constraints through optimization. First, a quadratic cost function J is constructed. This function aims to balance the system's control objective and control energy consumption, and its structure is as follows: Where: J: the quadratic cost function to be minimized; X k+j Predicting the time domain N P Error state vector within; ΔU k+j Control time domain N C The control increment sequence within; Q W : The weighted matrix of the state tracking error terms, used to balance trajectory tracking accuracy; R W The first step is to use a weighted matrix for the incremental penalty term to balance the smoothness of the control action. The second step is to apply the constraint. The dynamic torque physical limit value τ calculated in step S2 is then applied. L Transformed into a set of linear inequality constraints C for future control input sequences k This set of constraints ensures that the total planned torque (including feedforward and compensation torque) at any time within the prediction time domain does not exceed the allowable τ of the voltage margin at that time. L Physical boundary. Subsequently, the cost function J is compared with the set of linear inequality constraints C. k The simultaneous equations are transformed into a standard quadratic programming numerical solution. The optimal control increment sequence that minimizes the cost function is then obtained using efficient algorithms such as the effective set method or the interior point method. Finally, the rolling optimization extraction step is performed. Based on the principle of rolling time-domain optimization, the first control increment value in the optimal control sequence is extracted. Then, it is added to the predicted torque of the previous moment to obtain the optimal model predicted compensation torque τ required at the current moment. COMP This compensating torque is the core signal for correcting system errors and eliminating disturbances.

[0035] Specifically, the process of superimposing the model-predicted compensation torque with the feedforward torque calculated based on the inverse dynamics model to generate the final torque command and convert it into a current control signal to drive the articulated motor is as follows: Based on the inverse dynamics model of the rigid body dynamics equation, the nominal feedforward torque required to overcome gravity, Coriolis force and inertia is calculated according to the current desired trajectory; a torque fusion operation is performed, and the nominal feedforward torque, the optimal model-predicted compensation torque and the reverse compensation component of the lumped disturbance value are algebraically summed to obtain the total desired output torque; the total desired output torque is divided by the motor torque constant to obtain the target q-axis current command, and this current command is input to the current loop controller of the motor driver, which drives the inverter to generate the corresponding phase current through the field-oriented control algorithm, thereby driving the articulated motor to move.

[0036] In this implementation scheme, this process is the end-point fusion and execution of the control system, aiming to convert the algorithm output into physical signals for the drive motor. First, based on the inverse dynamics model of the rigid body dynamics equations, the nominal feedforward torque τ required to overcome gravity, Coriolis force, and inertia is calculated according to the current desired trajectory. FF This torque constitutes the basic torque envelope of the robot's movements, ensuring the system's rapid response to the desired trajectory. Secondly, a multi-source torque fusion operation is performed. The nominal feedforward torque τ... FF The optimal model output predicts the compensation torque τ. COMP and the estimated lumped disturbance value The total expected output torque τ is obtained by algebraically summing the reverse compensation components. TOT . ;wherein: τ TOT : Total expected output torque; τ FF τ is the nominal feedforward torque calculated by the inverse dynamics model. COMP : The optimal compensation torque for model predictive control; The extended state observer estimates the lumped disturbance value (the negative sign indicates cancellation of the disturbance's effect). Finally, the current signal conversion step is performed. The total desired output torque τ is then converted. TOT Divide by the motor's torque constant K TQ Receive the target q-axis current command I Q : Among them: I Q : Target q-axis current command; K TQ The motor torque constant is then input into the current loop controller of the motor driver. The inverter is driven by the field-oriented control (FOC) algorithm to generate the corresponding phase current, thereby efficiently and accurately driving the joint motor.

[0037] In summary, this application has at least the following effects:

[0038] A method for controlling the joint motors of a humanoid robot with model predictive control is proposed. By sensing the motor speed and bus voltage in real time, the back electromotive force is calculated and converted into a dynamic torque physical limit as a hard constraint for quadratic programming. This fundamentally solves the problems of infeasible control commands and system oscillation caused by drive voltage saturation during high-speed motion, significantly improving the system's safety and effective torque utilization. At the same time, this method combines an extended state observer to compensate for unmodeled errors and external disturbances online, achieving microsecond-level precision correction and smooth control of joint trajectories in highly dynamic and strongly disturbed environments.

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

[0040] This invention is described with reference to flowchart illustrations and / or block diagrams of systems, 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, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart 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.

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

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

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

[0044] 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 method for controlling the joint motors of a humanoid robot with model predictive control, characterized in that, Includes the following steps: S1. Establish the error state-space model of the joint motor system, define the joint position tracking error and its derivative as state variables, take the gravity term, Coriolis force term and centrifugal force term in the joint dynamics as known nominal feedforwards, and analyze the state transition matrix and input control matrix describing the dynamic characteristics of the system through linearization. S2. Collect the real-time speed of the motor and the bus voltage of the driver, calculate the back electromotive force at the current speed according to the motor voltage equation, calculate the difference between the bus voltage and the back electromotive force and map it to the physical limit value of the dynamic torque at the current moment, and at the same time use the extended state observer to estimate the lumped disturbance value including the model unmodeled error. S3. Introduce the lumped disturbance value into the prediction equation constructed based on the state transition matrix and the input control matrix to predict the system state trajectory in the future time domain. Use the physical limit value of the dynamic torque as the real-time inequality constraint condition of the control input to construct and solve the quadratic programming problem, and extract the first term of the optimal control sequence as the model prediction compensation torque. S4. The model-predicted compensation torque is superimposed with the feedforward torque calculated based on the inverse dynamics model to generate the final torque command and convert it into a current control signal to drive the joint motor. The specific process of collecting real-time motor speed and driver bus voltage, calculating back electromotive force at the current speed based on the motor voltage equation, calculating the difference between the bus voltage and back electromotive force, and mapping it to the current dynamic torque physical limit value is as follows: In each control cycle, the DC bus voltage sensor value and the angular velocity fed back by the motor encoder are read in real time, and the instantaneous back EMF is calculated by multiplying the pre-calibrated motor back EMF constant with the current angular velocity. Subtract the instantaneous back electromotive force value from the bus voltage value and deduct the safety margin generated by the internal impedance voltage drop of the drive circuit to obtain the maximum effective drive voltage that can actually be applied across the motor windings at the current moment. According to Ohm's law, the maximum effective drive voltage is converted into the maximum allowable phase current using the motor phase resistance parameters. Then, combined with the motor torque constant, the maximum allowable phase current is mapped to the corresponding torque value. Setting the torque value as the upper limit constraint of the control variable in the model predictive control optimization process ensures that the generated control command is always within the driver voltage saturation region at the physical level. Using the physical limit value of dynamic torque as the real-time inequality constraint condition for the control input, a quadratic programming problem is constructed and solved. The first term of the optimal control sequence is extracted as the model's prediction compensation torque. The specific process is as follows: A quadratic cost function containing state tracking error terms and control increment penalty terms is constructed, and a weighting matrix is ​​used to balance trajectory tracking accuracy and control smoothness. The physical limit value of dynamic torque is transformed into a set of linear inequality constraints for future control input sequences, ensuring that the planned torque at any time in the prediction time domain does not exceed the physical boundary allowed by the voltage margin at that time. The cost function is combined with the set of linear inequality constraints to transform it into a standard quadratic programming numerical solution. The optimal control increment sequence that minimizes the cost function is obtained by using the effective set method or the interior point method. The first control increment value in the sequence is extracted and added to the predicted torque at the previous time step to obtain the optimal model prediction compensation torque required at the current time step.

2. The method for controlling the joint motors of a humanoid robot with model predictive control according to claim 1, characterized in that: The logic for constructing the error state-space model of the joint motor system is as follows: Perform dynamic decoupling operation to decompose the multi-rigid-body dynamic equations of the humanoid robot to the single-joint dimension, and configure the single-axis dynamic equations that include the motor rotor inertia, reducer transmission ratio and link side equivalent inertia. In the uniaxial dynamic equation, a nonlinear term separation step is performed. The nonlinear dynamic terms consisting of gravity, Coriolis force, and centrifugal force, as well as the disturbance terms consisting of joint friction and external contact force, are uniformly regarded as generalized external torque input independent of the motor input torque. The system is simplified to a second-order differential equation describing the relationship between the input torque and the joint angular acceleration. The direct influence of higher-order nonlinear coupling terms on the predictive model structure is ignored, and the physical model framework of the linear predictive controller is completed.

3. The method for controlling the joint motors of a humanoid robot with model predictive control according to claim 2, characterized in that: The joint position tracking error and its derivative are defined as state variables. The gravity, Coriolis force, and centrifugal force terms in joint dynamics are taken as known nominal feedforwards. The specific process of analytically deriving the state transition matrix and input control matrix describing the system's dynamic characteristics through linearization is as follows: Calculate the difference between the desired joint trajectory and the actual feedback trajectory, and construct a state vector containing position error components and velocity error components; In the continuous time domain, an algebraic stripping operation is performed to subtract the nominal feedforward quantity from the total control torque as a known term, so that the remaining dynamic equations are transformed into a linear time-invariant system that only describes the relationship between the error state and the control increment. By using a zero-order hold with a set control period as the sampling time, a discretization transformation is performed to convert the coefficient matrix in the continuous time domain into a state transition matrix and an input control matrix in the discrete time domain. The state transition matrix is ​​used to quantify the free evolution trend of the current error state under no control input, and the input control matrix is ​​used to quantify the correction effectiveness of the control increment on the error state at the next time step.

4. The method for controlling the joint motors of a humanoid robot with model predictive control according to claim 1, characterized in that: The specific process of estimating the lumped disturbance value, including unmodeled errors, using the extended state observer is as follows: Configure a three-dimensional extended state vector including joint position, joint angular velocity and lumped disturbance term, take the actual position fed back by the motor encoder as the system observation output, take the final torque command applied to the motor in the previous control cycle as the system control input, calculate the observation residual between the estimated position derived from the observed state and the actual position, and correct the derivative of the extended state vector in real time through a pre-designed observer gain matrix. The expanded state vector is updated by integral operation, and the third-dimensional state component is separated from it as a real-time estimate of the lumped disturbance of the system, which reflects the combined effects of friction, external load and model mismatch on the system at the current moment.

5. The method for controlling the joint motors of a humanoid robot with model predictive control according to claim 1, characterized in that: The specific process of incorporating the lumped disturbance value into the prediction equation constructed based on the state transition matrix and the input control matrix to predict the future system state trajectory in the time domain is as follows: Within the finite future prediction time domain starting from the current moment, the lumped disturbance value is treated as a known exogenous disturbance term and superimposed onto the discrete state-space equation composed of the state transition matrix and the input control matrix to form an augmented prediction model. Starting from the error state at the current moment, the augmented prediction model is used to perform multi-step iterative deduction. The system state at each future moment is represented as a linear function of the current state, the lumped disturbance value, and a series of future control increments to be solved, generating a predicted trajectory expression for the future control increment sequence.

6. The method for controlling the joint motors of a humanoid robot with model predictive control according to claim 1, characterized in that: The specific process of superimposing the model-predicted compensation torque with the feedforward torque calculated based on the inverse dynamics model to generate the final torque command and converting it into a current control signal to drive the joint motor is as follows: Based on the inverse dynamics model of rigid body dynamics equations, the nominal feedforward torque required to overcome gravity, Coriolis force and inertia is calculated according to the current desired trajectory. The torque fusion operation is performed by algebraically summing the nominal feedforward torque, the optimal model predicted compensation torque, and the reverse compensation component of the lumped disturbance value to obtain the total expected output torque. Divide the total expected output torque by the motor's torque constant to obtain the target q-axis current command. Input this current command into the current loop controller of the motor driver. The inverter is then driven by the field-oriented control algorithm to generate the corresponding phase current, which drives the joint motor to move.

Images