Introduction
As an important branch of artificial intelligence, the development of motion control technology for humanoid robots is crucial for achieving autonomy, flexibility, and intelligence. With technological advancements, advanced control strategies such as MPC and WBC have been widely applied in the motion control of humanoid robots, greatly enhancing their performance.
Principle Introduction
Model Predictive Control (MPC) Principle Introduction
Model Predictive Control is a model-based control strategy that optimizes the current control output by predicting the system states and control inputs over a future time horizon. The core of MPC lies in establishing an accurate system model and using this model to forecast future behaviors to achieve optimal control of the system.
Basic Concept of Optimal Control
Optimal control refers to the problem of adjusting the control parameters of a system under given constraints to achieve the best state under certain performance metrics. Optimal control typically requires optimization over the entire time domain to achieve the best performance, and the optimal control at the current moment may not necessarily be the optimal value at the next moment in the presence of disturbances. Optimal control has wide applications in practice, such as optimizing vehicle acceleration and steering angles in autonomous cars to safely reach a destination within a specified time; maximizing production efficiency by adjusting control parameters in manufacturing; and optimizing investment portfolios to achieve maximum asset returns in economics.
Overall Process of the MPC Algorithm
Model Predictive Control requires three steps at time k:
• Step 1: Obtain the current state of the system;
• Step 2: Perform optimization based on u(k), u(k+1), u(k+2), u(k+j), with the cost function being
• where EN represents the terminal value of the error, which is also a measure of performance quality.
• Step 3: Only take u(k) as the control input applied to the system.
Repeat the above three steps at the next moment, using the state value from the next step for the prediction. This approach is called Receding Horizon Control.
Key Features of MPC
• Predictive: MPC can predict the system behavior over a future time horizon.
• Optimization: Achieves optimal system performance by optimizing control inputs.
• Constraint Handling: Capable of handling constraints on system states and control inputs.
Whole Body Control (WBC) Principle Introduction
Whole Body Control is a method used to achieve stability in humanoid robots. Its core idea is to treat the robot as a whole, optimizing the movements of various joints to achieve overall balance and stability. This differs from traditional local control methods, which typically focus only on the stability of individual joints or localized areas.
Overall Process of the WBC Algorithm
1. System Modeling: First, it is necessary to establish the dynamic model of the robot, including the position of the center of mass and the kinematic and dynamic characteristics of the joints.
2. Stability Analysis: Analyze the robot’s stability by calculating the positions of ZMP (Zero Moment Point) and CMP (Center of Mass Projection). If the ZMP is not within the support polygon, the robot needs to adjust its posture or gait.
3. Control Objective Setting: Based on task requirements and the results of stability analysis, set control objectives, such as keeping the ZMP at the center of the support polygon or minimizing energy consumption.
4. Optimization Problem Solving: Transform the control objectives into optimization problems and solve them using mathematical methods. This typically involves linear or nonlinear programming, aiming to find a set of optimal joint motion commands.
5. Real-time Control: Apply the optimized joint motion commands to the robot system in real-time to achieve dynamic balance.
6. Feedback Adjustment: Adjust the control strategy online based on the robot’s actual motion state and changes in the external environment.
Deployment Environment Introduction
Taking the OpenLoong project as an example, we can analyze its application in humanoid robot motion control. OpenLoong provides an open-source humanoid robot training platform and a whole-body dynamic control software package. Based on the content from its official website [https://atomgit.com/openloong/openloong-dyn-control], related deployment environments can be referenced.
Hardware Environment
The hardware environment is the foundation for implementing robot motion control algorithms, such as Model Predictive Control (MPC) and Whole Body Control (WBC). Below is a general hardware environment based on the OpenLoong project introduction:
Basic Hardware Components
1. Control Unit: Typically based on high-performance computing platforms (such as NVIDIA Jetson series, Intel Nuc, etc.), responsible for running motion control algorithms and real-time processing of sensor data.
2. Sensor Suite: Includes but is not limited to:
￮ Torque sensors: Measure joint torque for feedback control.
￮ Attitude sensors: Such as gyroscopes and accelerometers, monitor the robot’s attitude changes.
￮ Visual sensors: Such as cameras, used for environmental perception and target recognition.
￮ Tactile sensors: Sense contact forces for performing fine operations.
3. Drive Unit: Responsible for converting control commands into motor movements, including motor drivers and motor controllers.
4. Motors: Actuators used to drive the movements of robot joints, commonly including DC motors, AC motors, and stepper motors.
5. Power Management System: Provides stable power for the entire system, including batteries and power converters.
6. Communication Interfaces: Used for data exchange between internal modules of the robot and communication with external devices, such as Ethernet, Wi-Fi, Bluetooth, etc.
Advanced Hardware Components
1. AI Computing Unit: High-performance GPUs or dedicated AI chips for processing complex algorithms and machine learning tasks.
2. Real-Time Operating System (RTOS): Ensures the real-time and stability of control algorithms, such as FreeRTOS, VxWorks, etc.
3. Storage Devices: Used for storing system software, algorithm models, and temporary data, such as SSDs, eMMCs, etc.
4. Redundant Systems: Implement redundancy on critical components (such as power supply, control unit) to enhance system reliability and fault tolerance.
5. Safety Monitoring System: Monitors hardware status in real-time to ensure safe operation of the robot.
6. Interface Expansion Boards: Provide additional I/O interfaces for the robot, such as GPIO, PWM, SPI, I2C, etc.
Of course, if we really deploy a humanoid robot, the cost could be very high, so here we still use the simulation environment provided by the Qinglong platform. This content can be referenced [https://atomgit.com/openloong/openloong-dyn-control].
Simulation Deployment Process
The official documentation provides a series of simulation deployment processes, such as Linux version, compiler version, etc. You can follow the content below to operate:
Environment Recommendations
• Operating System: Ubuntu 22.04.4 LTS
• Compiler: g++ 11.4.0
Dependency Installation
The simulation interface requires system support for openGL
# Update & Install Dependencies
sudo apt-get update
sudo apt install git cmake gcc-11 g++-11
sudo apt install libglu1-mesa-dev freeglut3-devCode Retrieval and Compilation
# Clone the code repository
git clone https://atomgit.com/openloong/openloong-dyn-control.git
# Compile the code
cd openloong-dyn-control
mkdir build
cd build
cmake ..
make
# Run the simulation
./walk_mpc_wbc #or ./walk_wbc or ./jump_mpcResult Demonstration
After running, you will see a walking humanoid following a specific gait.
Some developers have also recorded related videos on Bilibili; interested friends can watch:
https://www.bilibili.com/video/BV1q2aheREic/?spm_id_from=333.337.search-card.all.click&vd_source=11e208184319df67805026f6ffaf9eac
Code Interpretation
The entire code repository structure is shown in the above image, which looks quite clear. The most important part is the algorithm section, where the algorithm storage folder is located.
The above two folders are the code repositories for WBC and MPC implementations.
Main Implementation of MPC
First, let’s look at the MPC part, which can be divided into seven key components.
1. Constructor MPC::MPC(double dtIn)
Initializes the member variables of the MPC class, sets the simulation time step dt, and initializes various matrices and vectors to zero.
MPC::MPC(double dtIn):QP(nu*ch, nc*ch) { // ...}Here, QP represents the dimensions of the quadratic programming problem, nu is the dimension of control inputs, nc is the number of constraints, and ch is the number of time steps.
2. Set Weight Function MPC::set_weight
Sets the weight matrices in the MPC problem, including state weight L and control input weight K.
void MPC::set_weight(double u_weight, Eigen::MatrixXd L_diag, Eigen::MatrixXd K_diag) { // ...}u_weight is the weight coefficient for control inputs, while L_diag and K_diag are the diagonal matrices for state and control inputs, respectively.
3. Data Read Function MPC::dataBusRead
Reads the current system state from the data bus and updates the current state estimate of the MPC.
void MPC::dataBusRead(DataBus &Data) { // ...}DataBus is a structure or class containing all the necessary sensor data and system states.
4. Calculation Function MPC::cal
Performs the main calculations of MPC, including establishing the optimization problem, solving it, and updating the system state.
void MPC::cal() { if (EN) { // ... // Establish MPC's dynamic model and constraints // ... // Call qpOASES solver // ... // Update system state prediction // ... }}Here, the qpOASES library is used to solve the quadratic programming problem.
5. Data Write Function MPC::dataBusWrite
Writes the results of the MPC calculations back to the data bus for system execution.
void MPC::dataBusWrite(DataBus &Data) { // ...}The data written includes the desired control inputs and system state predictions.
6. Enable/Disable MPC Function
Allows the user to enable or disable the MPC controller.
void MPC::enable() { EN = true; }void MPC::disable() { EN = false; }EN is a boolean variable indicating whether the MPC is enabled.
7. Auxiliary Function MPC::copy_Eigen_to_real_t
Converts Eigen matrices to the real_t array format required by qpOASES.
void MPC::copy_Eigen_to_real_t(qpOASES::real_t* target, Eigen::MatrixXd source, int nRows, int nCols) { // ...}This function is used for data format conversion, as the qpOASES library uses C-style arrays.
Integrating into the Three Processes of MPC Principle
Step 1: Obtain the Current State of the System
In the MPC framework, the first step is to obtain the current state information of the system, which includes state variables such as position, velocity, and acceleration. In the code, this step is typically done by reading sensor data or obtaining it from the data bus.
void MPC::dataBusRead(DataBus &Data) { // ... X_cur.block<3,1>(0,0) = Data.base_rpy; // Get current base attitude angles X_cur.block<3,1>(3,0) = Data.q.block<3,1>(0,0); // Get current position X_cur.block<3,1>(6,0) = Data.dq.block<3,1>(3,0); // Get current angular velocity X_cur.block<3,1>(9,0) = Data.dq.block<3,1>(0,0); // Get current linear velocity // ...}Step 2: Perform Optimization Based on u(k), u(k+1), u(k+2), u(k+j)
In this step, MPC calculates a series of future control inputs by solving an optimization problem based on the current state information and the predetermined target state. The goal of the optimization problem is to minimize the cost function, which typically includes tracking errors and the smoothness of control inputs.
void MPC::cal() { if (EN) { // ... H = 2 * (Bqp.transpose() * L * Bqp + alpha * K) + 1e-10*Eigen::MatrixXd::Identity(nx*mpc_N, nx*mpc_N); c = 2 * Bqp.transpose() * L * (Aqp * X_cur - Xd) + 2 * alpha * K * delta_U; // ... res = QP.init(qp_H, qp_c, qp_As, qp_lu, qp_uu, qp_lbA, qp_ubA, nWSR, &cpu_time, xOpt_iniGuess); // ... }}In this code, H and c are the Hessian matrix and linear term used to construct the optimization problem, and QP.init is the function that initializes the optimization solver.
Step 3: Only Take u(k) as the Control Input Applied to the System
After optimization, MPC only uses the first control input u(k) from the optimized sequence to control the system at the current moment, and then repeats this process in the next control cycle.
void MPC::cal() { if (EN) { // ... qpOASES::real_t xOpt[nu * ch]; QP.getPrimalSolution(xOpt); if (qp_Status == 0) { for (int i = 0; i < nu * ch; i++) Ufe(i) = xOpt[i]; } // ... }}In this code, Ufe stores the optimized control input sequence, but in practice, only Ufe.block(0, 0) is usually taken as the control input at the current moment.
Main Implementation of WBC
1. Constructor WBC_priority::WBC_priority
Initializes the member variables of the WBC class, sets the degrees of freedom of the model, friction coefficients, time steps, etc., and configures the QP problem.
WBC_priority::WBC_priority(int model_nv_In, int QP_nvIn, int QP_ncIn, double miu_In, double dt) : QP_prob(QP_nvIn, QP_ncIn) { // ...}This initializes the QP problem, including the number of control variables (QP_nvIn) and the number of constraints (QP_ncIn).
2. Data Read Function WBC_priority::dataBusRead
Reads the current state of the robot from the data bus, including joint positions, velocities, accelerations, etc.
void WBC_priority::dataBusRead(const DataBus &robotState) { // ...}This function is responsible for obtaining the current state information of the robot from the data bus for control calculations.
3. Data Write Function WBC_priority::dataBusWrite
Writes the calculation results back to the data bus, including the final joint torques and forces.
void WBC_priority::dataBusWrite(DataBus &robotState) { // ...}This function is responsible for writing the calculation results (such as joint torques and contact forces) back to the data bus for the robot to execute.
4. Calculation Function WBC_priority::computeTau
Constructs and solves the QP problem to calculate the joint torques of the robot.
void WBC_priority::computeTau() { // ...}This function is the core of WBC, constructing the QP problem, including the objective function and constraints, and calling the solver to solve it. The results are used to calculate joint torques and contact forces.
5. Auxiliary Function WBC_priority::copy_Eigen_to_real_t
Converts Eigen matrices to the real_t array format required by qpOASES.
void WBC_priority::copy_Eigen_to_real_t(qpOASES::real_t *target, const Eigen::MatrixXd &source, int nRows, int nCols) { // ...}This function is used for data format conversion, as the qpOASES library uses C-style arrays.
6. Calculation Function WBC_priority::computeDdq
Calculates the robot’s accelerations and velocities.
void WBC_priority::computeDdq(Pin_KinDyn &pinKinDynIn) { // ...}This function is responsible for calculating the robot’s accelerations and velocities, including handling various tasks (such as maintaining balance, tracking target positions, etc.).
Integrating into the Six Processes of WBC Principle
1. System Modeling
In the WBC constructor, the dynamic model is initialized, and the robot’s degrees of freedom are defined, laying the foundation for system modeling.
WBC_priority::WBC_priority(int model_nv_In, int QP_nvIn, int QP_ncIn, double miu_In, double dt) : QP_prob(QP_nvIn, QP_ncIn) { // ...}Here, model_nv_In represents the number of degrees of freedom in the robot model, which is a key parameter for system modeling.
2. Stability Analysis
Stability analysis typically involves calculating ZMP (Zero Moment Point) and CMP (Center of Mass Projection). In the code, although there is no direct function to calculate ZMP, the stability considerations are implicitly included in the optimization problem.
void WBC_priority::computeTau() { // ... Eigen::MatrixXd eigen_qp_A1 = Eigen::MatrixXd::Zero(6, QP_nv); // ... Eigen::VectorXd eqRes = Eigen::VectorXd::Zero(6); // ...}When constructing the QP problem, by setting equality and inequality constraints, the stability of the robot is ensured, for example, by constraining forces and moments to keep the ZMP within the support polygon.
3. Control Objective Setting
The control objectives are set during the formalization of the optimization problem. In WBC, these objectives may include tracking specific trajectories, maintaining balance, or optimizing energy consumption.
void WBC_priority::computeTau() { // ... Eigen::MatrixXd eigen_qp_H = Eigen::MatrixXd::Zero(QP_nv, QP_nv); // ...}Here, eigen_qp_H and qp_g define the Hessian matrix and linear term of the optimization problem, reflecting the control objectives.
4. Optimization Problem Solving
The optimization problem is solved using the qpOASES library, which is an efficient quadratic programming solver.
void WBC_priority::computeTau() { // ... res = QP_prob.init(qp_H, qp_g, qp_A, NULL, NULL, qp_lbA, qp_ubA, nWSR, &cpu_time, xOpt_iniGuess); // ...}Here, the parameters of the optimization problem (such as qp_H, qp_g, qp_A, etc.) are passed to the solver, which returns the optimal solution.
5. Real-Time Control
Real-time control is reflected in applying the results of the optimization solution to the robot system.
void WBC_priority::dataBusWrite(DataBus &robotState) { robotState.wbc_ddq_final = eigen_ddq_Opt; robotState.wbc_tauJointRes = tauJointRes; // ...}In the dataBusWrite function, the calculated accelerations eigen_ddq_Opt and joint torques tauJointRes are written to the data bus for controlling the robot.
6. Feedback Adjustment
Feedback adjustment involves adjusting the control strategy based on the robot’s actual state and changes in the external environment. In WBC, this is typically achieved by recalculating the optimization problem in each control cycle.
void WBC_priority::dataBusRead(const DataBus &robotState) { // ...}In the dataBusRead function, the latest sensor data and state information are read from the data bus, which will be used for the optimization calculations in the next control cycle.
Reference Links
https://zhuanlan.zhihu.com/p/633715817
https://atomgit.com/openloong/openloong-dyn-control/tree/master/algorithm
