Heterogeneous acceleration method and device for foot type robot control and storage medium

Abstract

The invention discloses a heterogeneous acceleration method and device for foot type robot control and a storage medium, and the method comprises the steps: taking a foot end supporting force and a moment as control input variables in response to an expected state instruction input by a user, carrying out the iterative solution of a standard form of an alternating direction multiplier method, and obtaining the optimal supporting force and moment; mapping the optimal supporting force and moment to a joint space according to a Jacobian matrix, and performing relaxation optimization according to a floating base dynamic model to obtain an optimized joint force; the optimized joint force is sent to all joint motors of the robot, and supporting legs are controlled to move; the motion control efficiency of the foot type robot is improved, and the running time and power consumption are reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent robots, and particularly relates to a heterogeneous acceleration method, device and storage medium for controlling a legged robot. Background Art

[0002] A legged robot, such as a biped robot, is a robot that mimics the walking of humans or other bipedal animals. It aims to simulate a human-like gait through two legs, usually using two independent leg systems, and each leg is usually composed of multiple joints to imitate the walking manner of humans or animals. Biped robots have broad application prospects, especially in the fields of service, medical treatment, rescue, etc., and can move autonomously in the human environment. When a biped robot walks, the robot moves one leg at a time and then transfers the center of gravity to the other leg, similar to the human gait. Therefore, such robots not only face complex motion control problems but also need to face challenges such as stability, adaptability, and flexibility related to the human gait. How to maintain the stable and balanced control of a biped robot has always been one of the important problems to be solved by researchers.

[0003] Currently, there are already various control strategy frameworks that can effectively enable a biped robot to perform robust motion walking in practical tasks. Such as the Hybrid Zero Dynamics (HZD) model applied to biped robots such as MABEL, and the model predictive control scheme based on force and torque applied to legged robots such as HECTOR (Dynamic Loco-manipulation on HECTOR: Humanoid for Enhanced Control and Open-source Research); the control scheme based on force and torque combines model predictive control with higher-frequency whole body control (WBC) to achieve more accurate trajectory tracking. This type of control framework is used together with a linear dynamics model and constraints, adopts a simplified single-rigid-body dynamics, obtains the foot-end reaction force through model predictive control, and then maps the foot-end force to each joint torque. The main computing resources and computing time of this control framework lie in the model predictive control solver. The HECTOR biped robot developed by MIT uses the active set method to solve in an AMD Ryzen 5600X@4.2GHz CPU; it has high requirements for the computing power of the hardware platform, high power consumption and cost, poor real-time performance, and great application difficulty.

[0004] To address the above problems, consider making full use of the parallel computing power of FPGAs on heterogeneous chips, adopting a nonlinear model predictive control algorithm suitable for parallel computing to design the controller, improving the real-time performance of online computing, and achieving stable motion control of biped robots. Currently, some researchers have conducted research on the parallel computing of nonlinear model predictive control: For example, in the literature "FPGA Implementation and Application Research of Fast Model Predictive Control, Xu Fang, Ph.D. Thesis of Jilin University", a nonlinear model predictive controller was established for the requirements of fast and stable trajectory tracking control of wheeled mobile robots and the constraints of actuators. The constrained particle swarm optimization algorithm (PSO) was adopted, and an SoPC embedded implementation scheme based on FPGA was proposed. Combining the characteristics of the particle swarm algorithm such as simple iterative logic and parallel computability, the algorithm was processed by pipeline and loop unrolling, further improving the speed of online computing and realizing the real-time application of the nonlinear model predictive controller. In the literature "FPGA Acceleration Solving Method for Model Predictive Control for Autonomous Driving, Li Yunfei, Master Thesis of Chongqing University", a longitudinal and lateral model predictive control algorithm that can simultaneously achieve trajectory tracking and speed tracking and is deployed on a CPU+FPGA hardware platform was proposed. The quadprog++ quadratic programming solver was deployed into the FPGA chip, and the acceleration effect was obvious. These solutions are aimed at autonomous driving mobile vehicles, which have fewer state variables and simple dynamic models. However, for biped robot objects, there are more state variables and more complex dynamic models, and genetic algorithms such as particle swarm are seriously dependent on parameter selection and are prone to falling into local optimal solutions.

[0005] In the patent application document with the publication number CN114147716A, a method is proposed to solve the quadratic programming problem based on a preset dynamic system stability function such as the Lyapunov function, and the solving module is an open-source quadratic programming solver such as the first-order alternating direction multiplier method or the OSQP solver. By solving the quadratic programming problem, the optimal control parameters of each joint such as joint torque can be directly obtained, solving the problem that directly controlling the robot according to the angle values of each joint will result in poor tracking of the desired posture. However, the first-order alternating direction multiplier method adopted in this solution is implemented on a high-performance CPU, and for a biped robot with a complex dynamic model, its solving time is too long and the real-time performance is poor. Summary of the Invention

[0006] The technical problem to be solved by the present invention is how to improve the motion control efficiency of legged robots, reduce the running time and power consumption.

[0007] The present invention solves the above technical problems by the following technical means:

[0008] A heterogeneous acceleration method for controlling a legged robot is proposed, and the method includes:

[0009] In response to the desired state instruction input by the user, taking the foot-end support force and moment as control input variables, iteratively solve the standard form of the alternating direction multiplier method to obtain the optimal support force and moment;

[0010] Map the optimal support force and moment to the joint space according to the Jacobian matrix, and perform relaxation optimization according to the floating-base dynamics model to obtain the optimized joint force;

[0011] Send the optimized joint force to each joint motor of the robot to control the movement of the supporting leg.

[0012] Further, before the step of "In response to the desired state instruction input by the user, taking the foot-end support force and moment as control input variables, iteratively solve the standard form of the alternating direction multiplier method to obtain the optimal support force and moment", the method further includes:

[0013] Taking the desired state of the robot as the state variable and the force and moment of the robot's sole in contact with the ground as the decision variables, establish a state-space dynamics equation expressed in the world coordinate system;

[0014] Convert the state-space dynamics equation into a discrete-time representation form of time steps;

[0015] Redefine the state variable and the decision variable as the augmented state variable, and use the augmented state variable to convert the discrete-time representation form into the standard form of the alternating direction multiplier method.

[0016] Further, the standard form of the alternating direction multiplier method is:

[0017]

[0018] s.t. AX + DZ = c

[0019] Where: I z (Z) is the indicator function, I is the identity matrix, Z is the introduced non-negative relaxation variable, X is the augmented state variable, X = [x1...x h u0...u h-1 , x k (k = 1, 2,..., h) is the state variable at the k-th prediction time, the variable h is the total length of the prediction domain, u k (k = 0, 1,..., h - 1) is the control input variable at the k-th prediction time, Θ is the azimuth angle of the robot, P c is the position of the robot's center of mass, ω is the angular velocity of the robot, is the velocity of the center of mass; the matrix H represents the quadratic term coefficient matrix, and its specific expression form is as The matrix H consists of Q k and R k for (k = 0, 1,..., h - 1). Q k is the state variable adjustment parameter, which needs to be set manually. R k is the control input adjustment parameter, which also needs to be set manually. The matrix A is the coefficient matrix of the equality constraint state variable, which is composed of A eq and C. The matrix C in its specific expression is the coefficient of the inequality matrix, and A eq is the constructed intermediate matrix. The matrix A k is the coefficient matrix of the k-th predicted time dynamic state variable. A k = I 13 + A c,k dt, is the rotation matrix, ψ k is the yaw angle of the robot, and B k = B c,k dt, dt is the time step. I b is the inertia constant of the robot, which can be obtained according to the mechanical design structure of the robot. is the inverse matrix. (p i - p c ) × (i = 1, 2 represent the left and right legs respectively) is the skew-symmetric matrix of the vector from the center of mass of the robot to the foot end. F i (i = 1, 2 represent the left and right legs respectively) is the foot end force. f is the coefficient of the linear term of the cost function. is the reference trajectory, and h is the total length of the prediction domain. c is the linear equality constraint vector, and b eq = [A0X0 0... 0], where X0 is the initial state. The vector d is the inequality margin, which is composed of the force constraints at the foot end of the robot. μ is the friction between the foot end of the robot and the ground. F i,x , F i,y , F i,z are the foot end support forces of the ground on the robot along the x, y, and z directions respectively. F min , F max are the minimum and maximum forces that the ground can bear on the z direction of the reaction force at the foot end of the supporting leg of the robot.

[0020] Further, in response to the desired state instruction input by the user, taking the foot-end support force and moment as control input variables, iteratively solving the standard form of the alternating direction multiplier method to obtain the optimal support force and moment, including:

[0021] Iteratively solve the standard form of the alternating direction multiplier method, and calculate the residual after each iterative solution;

[0022] Based on the residual, determine whether the termination iteration condition is satisfied;

[0023] If so, output the optimal support force and moment;

[0024] If not, use the minimum residual method to solve the KKT matrix in the alternating direction multiplier method, update the actual state variables of the robot, and then perform the next iterative solution.

[0025] Further, the termination iteration condition satisfied by the residual is:

[0026] ||r k || ≤ ε

[0027] In the formula: r k = Ax (k) + Bz (k) - c, ε is a manually set threshold, and || || is the absolute value.

[0028] Further, the minimum residual method is the minimum residual algorithm applicable to parallel solving of non-linear equations.

[0029] Further, the method further includes:

[0030] In response to the desired state instruction input by the user, plan the robot's landing point;

[0031] Use Bezier curve to fit the robot's foot-end trajectory, and use PD control to track the robot's swinging leg trajectory.

[0032] Further, when iteratively solving the standard form of the alternating direction multiplier method, use the NMPC solving algorithm deployed on the heterogeneous chip to iteratively solve the standard form of the alternating direction multiplier method, wherein the standard form iterative solving algorithm of the alternating direction multiplier method is deployed in the ARM core, and the minimum residual algorithm is deployed in the FPGA chip.

[0033] In addition, the present invention also proposes a heterogeneous acceleration device for controlling a legged robot, and the device includes:

[0034] A response module, configured to, in response to the desired state instruction input by the user, take the foot-end support force and moment as control input variables, iteratively solve the standard form of the alternating direction multiplier method, and obtain the optimal support force and moment;

[0035] The joint force optimization module is used to map the optimal support force and torque to the joint space according to the Jacobian matrix, and perform relaxation optimization according to the floating base dynamics model to obtain the optimized joint force;

[0036] The motion control module is used to send the optimized joint force to each joint motor of the robot to control the movement of the support legs.

[0037] In addition, the present invention also proposes a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the heterogeneous acceleration method for controlling a legged robot as described above is implemented.

[0038] In addition, the present invention also proposes an FPGA heterogeneous acceleration platform for solving model predictive control of a legged robot, including a real-time simulation system platform, an MPC controller arranged in an ARM core, and a linear system solver deployed in an FPGA chip. The real-time simulation system platform is connected to the MPC controller, and the MPC controller is connected to the linear system solver;

[0039] The MPC controller is used to respond to the desired state instruction input by the user, take the foot end support force and torque as control input variables, and iteratively solve the standard form of the alternating direction method of multipliers to obtain the optimal support force and torque;

[0040] The linear system solver is used to solve the KKT matrix in the alternating direction method of multipliers by using the minimum residual method to update the actual state variables of the robot.

[0041] Further, the real-time simulation system platform is connected to the MPC controller through a UART interface, and the MPC controller is connected to the linear system solver through an AXI interface.

[0042] Further, the linear system solver is used to store the coefficient matrix in a COO storage manner.

[0043] The advantages of the present invention are as follows:

[0044] (1) The standard form of the alternating direction method of multipliers in the present invention is obtained by converting the nonlinear model predictive control (NMPC) model of the legged robot. Then, the alternating direction method of multipliers can be used to solve the standard form of the alternating direction method of multipliers. Compared with the original dual interior point method, the active set method, etc., the alternating direction method of multipliers is suitable for parallel computing, has the advantages of high computing efficiency, low computing cost, fast convergence speed, and easy to find the global optimal solution, etc., so as to improve the motion control efficiency of the legged robot, reduce the running time and power consumption.

[0045] (2) The present invention uses the minimum residual method to solve the solution of a complex linear system. Compared with traditional methods such as matrix LDL decomposition, the operation time complexity of the minimum residual method is lower, further improving the real-time performance of solving the NMPC problem.

[0046] (3) The present invention reasonably allocates computing resources. By deploying matrix-vector multiplication, matrix Lanczos decomposition, and the minimum residual method suitable for parallel computing to the FPGA, and reasonably deploying resources for parallel solution through methods such as loop unrolling and pipelining to adapt to the different architectures of ARM and FPGA, making full use of the computing power resources of both.

[0047] Additional aspects and advantages of the present invention will be given in part in the following description, become apparent in part from the following description, or be learned through the practice of the present invention. Brief Description of the Drawings

[0048] Figure 1 is a schematic flowchart of a heterogeneous acceleration method for legged robot control proposed in an embodiment of the present invention;

[0049] Figure 2 is a schematic flowchart of the NMPC acceleration solution process in an embodiment of the present invention;

[0050] Figure 3 is a schematic diagram of the control architecture of a biped robot in an embodiment of the present invention;

[0051] Figure 4 is a schematic structural diagram of a heterogeneous acceleration device for legged robot control proposed in an embodiment of the present invention;

[0052] Figure 5 is a control block diagram of an FPGA heterogeneous acceleration platform for solving model predictive control of a legged robot proposed in an embodiment of the present invention;

[0053] Figure 6 is a schematic flowchart of the algorithm development process between the PS side and the PL side in an embodiment of the present invention;

[0054] Figure 7 is a schematic diagram of loop unrolling and pipelining operations for the product of vector and vector in an embodiment of the present invention, where (a) is the multiplication of vector and vector, (b) is the timing schematic diagram of vector-vector multiplication, and (c) is the schematic diagram of loop unrolling and pipelining operations;

[0055] Figure 8 is a schematic diagram of the COO storage principle in an embodiment of the present invention;

[0056] Figure 9 is a schematic diagram of the comparison of resource utilization rate and computing time under different computing deployments in an embodiment of the present invention. Detailed implementation mode

[0057] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without making creative efforts belong to the scope of protection of the present invention.

[0058] As Figure 1 shown, an embodiment of the present invention proposes a heterogeneous acceleration method for controlling a legged robot, and the method includes the following steps:

[0059] S10. In response to the desired state instruction input by the user, using the foot-end support force and torque as control input variables, iteratively solve the standard form of the alternating direction multiplier method to obtain the optimal support force and torque;

[0060] It should be noted that the user first inputs a desired state instruction to the robot. The required state forms a column vector, including the required body center-of-mass position P d , the required body center-of-mass velocity the required rotation matrix R d and the required angular velocity ω of the robot body d . Using the column vector as the state variable and the foot-end support force and torque as the decision variables, the standard form of the alternating direction multiplier method is iteratively solved using the alternating direction multiplier method. Since the alternating direction multiplier method has the advantages of high calculation efficiency and low calculation cost, the optimal support force and torque can be quickly obtained, thereby improving the motion control efficiency of the legged robot and reducing the running time and power consumption.

[0061] S20. Multiply the optimal support force and torque by the Jacobian matrix and map them to the joint space, and perform relaxation optimization according to the floating-base dynamics model using the existing technology to obtain the optimized joint force;

[0062] Specifically, the optimized joint force calculated in this embodiment is:

[0063]

[0064] In the formula, q is the generalized joint space vector, M(q) is the joint space mass matrix, is the bias force matrix, which includes forces such as gravity and Coriolis force that are independent of joint acceleration, and f c and the contact force are the contact Jacobian and the contact force respectively, τ is the generalized joint torque, and S jis a selection filter matrix used to filter out the first 6 possible non-zero terms in the joint torque. is the joint velocity. is the joint acceleration.

[0065] S30. Send the optimized joint forces to each joint motor of the robot to control the movement of the supporting legs.

[0066] It should be noted that the motion controller of the legged robot controls the supporting legs and the swinging legs respectively. In this embodiment, the NMPC model is used to solve for the supporting legs. The desired position in the column vector is used as the state variable of the robot, and the foot-end support force and torque are used as the control inputs as decision variables. The obtained optimal support force and torque are mapped to each joint according to the Jacobian matrix, and relaxation optimization is performed according to the floating-base dynamics model. Finally, the optimized joint forces are obtained and sent to each joint motor. Since in the entire algorithm process, the NMPC solution of the supporting legs takes the longest calculation time and requires the most computing resources, this embodiment constructs a constrained NMPC model based on the dynamics model of the legged robot and uses the alternating direction method of multipliers for iterative solution. Compared with methods such as the primal-dual interior point method, the barrier function method, and the active set method, the alternating direction method of multipliers has the advantages of fast convergence speed, clear logic, and suitability for parallel computing, thus reducing the running time and power consumption of the NMPC solution.

[0067] As a further preferred technical solution, before the step S10: in response to the desired state instruction input by the user, taking the foot-end support force and torque as the control input variables, and performing iterative solution on the standard form of the alternating direction method of multipliers to obtain the optimal support force and torque, the method further includes the following steps:

[0068] S1. Establish a state-space dynamics equation represented in the world coordinate system with the desired state of the robot as the state variable and the force and torque of the robot's sole in contact with the ground as the decision variables.

[0069] Specifically, in this embodiment, taking the HECTOR 5-degree-of-freedom biped robot as an example, its simplified rigid body includes the body, shoulders, hips, and upper thighs, which are regarded as a combined rigid body. Its control input is the ground reaction force F i =[F i,x F i,y F i,z and M i =[M i,x M i,y M i,z , i = 1, 2. Among them, F i is the foot-end support force. When the robot stands or walks on the ground, the ground must have a support force on the supporting legs. i = 1, 2 represents the left and right legs of the robot, and M iis the supporting force moment of the ground on the supporting leg, F i,x , F i,y , F i,z are respectively the components of the force of the ground on the supporting leg along the x, y, and z directions, M i,x , M i,y , M i,z are respectively the components of the supporting force moment of the ground on the supporting leg along the x, y, and z directions. The design of HECTOR is centered around the assumptions of rigid body dynamics, where the mass of the remaining arm and leg components only accounts for 15% of the total mass of the robot. Therefore, the inertial effects of these components are ignored in the dynamic model. It is assumed that the position of the ground reaction force and moment is a contact point on each foot, located at the projection of the ankle joint position along the z coordinate. The complete hardware drive of the contact point, including 3-D linear motion, rolling, and yaw, is fully realized by applying 5-D forces and moments.

[0070] This embodiment selects as the state variable, and u = [F1 F2 M1 M2] 12×1 as the control input variable, where, Θ = [φ θ ψ] T , φ represents the rolling angle of the robot body, θ is the pitch angle, ψ is the yaw angle, P c represents the position of the robot's center of mass, represents the center of mass velocity, ω represents the angular velocity; u = [F1 F2 M1 M2] 12×1 is the NMPC optimization solution variable, representing the force and moment of the contact between the robot's sole and the ground. The simplified dynamic equation represented in the world coordinate system can be expressed as:

[0071]

[0072] In the formula, A c,k , B c,k The symbol meanings are the same as above.

[0073] S2. Convert the state space dynamic equation into a discrete time representation form of time steps;

[0074] It should be noted that in order to utilize this linearized state space dynamic equation in the MPC framework, it is converted into a discrete time representation at the k-th time step using the MPC time step dt:

[0075] x[k + 1] = A k x[k] + B k u[k]

[0076] In the formula, x[k + 1] is the state variable at the (k + 1)-th time step, x[k] is the state variable at the k-th time step, A k , B kThe symbol meanings are the same as above.

[0077] S3. Redefine the state variables and decision variables as augmented state variables, and use the augmented state variables to convert the discrete-time representation into the standard form of the alternating direction method of multipliers.

[0078] Specifically, the NMPC model in this embodiment mainly targets the supporting leg of the biped robot. By solving the NMPC problem, the foot-end force of the robot's supporting leg is obtained. The present invention converts the QP problem of the NMPC model into the standard form applicable to the alternating direction method of multipliers, and the alternating direction method of multipliers has the characteristics of fast convergence speed and applicability to parallel computing. The MPC problem of the NMPC model for the supporting leg with a finite horizon length h can be written in the following standard format:

[0079]

[0080] s.t. x[k + 1] = A k x[k] + B k u[k]

[0081] -μF i,z ≤ F i,x ≤ μF i,z

[0082] -μF i,z ≤ F i,y ≤ μF i,z

[0083] 0 < F min ≤ F i,z ≤ F max

[0084] In the formula, x k , u k are the system state and control input at time step k, Q k , R k are matrices defining the weights of each state and control input variable. The inequality constraints are mainly related to the contact force and moment. The contact forces in the x and y directions are controlled to be within the friction pyramid, where μ is the friction coefficient; x k+1 is the system state at time step k + 1, x k+1,ref is the reference system state at time step k + 1, F i,x , F i,y , F i,z (i = 1, 2 represent the left and right legs respectively) are the foot-end support forces of the ground on the robot's foot along the x, y, and z directions, and F min , F max are the minimum and maximum tolerable forces of the ground reaction force on the robot's supporting leg foot-end along the z direction.

[0085] Redefine the augmented state variable \(X = [x_1\cdots x_n\) h \(u_0\cdots u_m]\) h-1 composed of the state \(x\) and the decision variable \(u\). Then the standard MPC problem can be transformed into the following form:

[0086]

[0087] s.t. \(A\) eq \(X = b\) eq

[0088] \(CX < d\)

[0089] The definitions of the functions in the formula are the same as those above.

[0090] Finally, establish the standard form of the alternating direction method of multipliers as:

[0091]

[0092] s.t. \(AX + DZ = c\)

[0093] Where: \(I\) z \((Z)\) is the indicator function.

[0094] As a further preferred technical solution, as Figure 2 shown, the step S10: In response to the desired state instruction input by the user, use the foot-end support force and torque as the control input variables, and perform iterative solution on the standard form of the alternating direction method of multipliers to obtain the optimal support force and torque, which specifically includes the following steps:

[0095] S11. Perform iterative solution on the standard form of the alternating direction method of multipliers, and calculate the residual after each iterative solution;

[0096] Specifically, the optimization steps of the alternating direction method of multipliers specifically include:

[0097] Input the matrices \(H\), \(f\), \(A\), \(D\), \(c\); input the initial values \(X_0\), \(Z_0\), \(\tau_0\);

[0098] Iterative solution of the ADMM algorithm:

[0099] \((H+\rho A^TA)X\) T \(= -[f+\rho A^T(DZ\) k+1 \(+\tau - c)]\) T \(\tau\) k \(=\tau\) k \(-\rho A^T(DZ\)

[0100]

[0101] \(\tau\) k \(=\tau\) k+(AX k +DZ k -c)

[0102] Determine the termination iteration condition: ||r k || ≤ ε, r k = Ax (k) + DZ (k) - c,

[0103] where ρ is the artificially set penalty factor, Z k is the value of the non - negative slack variable Z at the k - th iteration, τ k is the dual variable at the k - th iteration, r k is the residual at the k - th iteration, X0, Z0, τ0 are the state variable, slack variable, and dual variable in the initial state, respectively, are the first g numbers of the dual variable at the k - th iteration, the size of g is the same as the number of columns of matrix C, proj z () is to project the vector along the z - direction, and the other variables have been defined above.

[0104] S12. Based on the residual, determine whether the termination iteration condition is satisfied. If so, execute step S13; otherwise, execute step S14;

[0105] S13. Output the optimal supporting force and moment;

[0106] S14. Use the minimum residual method to solve the KKT matrix in the alternating direction multiplier method, update the actual state variables of the robot, and then execute step S11 for the next iteration solution.

[0107] Specifically, the existing minimum residual method is the minimum residual algorithm applicable to parallel solving of linear equations. For the linear equation system where The algorithm running steps include:

[0108] Initialization: X0 = 0, β0 is the quadratic norm of the vector q1 is the direction of ;

[0109] Lanzos decomposition: β1 = ||ν||2, q2 = ν / β1;

[0110] Solve the Givens transformation matrix:

[0111]

[0112] where, is a Givens rotation matrix, c1 and s1 are elements in the Givens rotation matrix, representing cosθ1 and sinθ1 respectively, θ1 is an intermediate variable, and γ1 is the first diagonal element of the tridiagonal matrix obtained after performing the first Givens rotation.

[0113] Recursive formula: p1 = q1 / γ1, ρ1 = -β0s1, τ1 = β0c1, x1 = x0 + b / α1, x1 = x0 + τ1p1

[0114] Iteration:

[0115]

[0116] v = Aq k+1 -α k+1 q k+1 -β k q k

[0117] β k+1 = ||v||2

[0118] ε k-1 = s k-1 β k

[0119]

[0120] Solve the linear equation:

[0121]

[0122] Obtain:

[0123] τ k+1 = ρ k c k+1

[0124] ρ k+1 = -ρ k s k+1

[0125] p k+1 = (q k+1 - ε k-1 p k-1 - δ k p k ) / γ k+1

[0126] x k+1 = x k + τ k+1 p k+1

[0127] k = k + 1

[0128] where p k+1 is the residual of the (k + 1)-th iteration.

[0129] In this embodiment, the minimum residual method is used to solve the solution of a complex linear system. Compared with traditional methods such as matrix LDL decomposition, the operation time complexity of the minimum residual method is lower.

[0130] It should be noted that the model predictive controller of a biped robot generally uses the single-rigid-body dynamics model of the robot as an equality constraint, and then designs a model predictive control solver. The general solution algorithms are the active set method or the interior point method, and the algorithm implementation is mainly on a high-computing-power CPU hardware platform. This embodiment is also based on the single-rigid-body dynamics model. Different from other inventions or methods, when designing the model predictive controller solver in this embodiment, a non-negative relaxation variable is introduced to transform the non-linear model predictive controller into the standard form of the alternating direction method of multipliers, and the implementation of the non-linear model predictive control algorithm is realized on an embedded SoC platform. By making full use of the parallel characteristics of the alternating direction method of multipliers, the alternating direction method of multipliers is deployed to an FPGA chip with parallel computing ability and lower computing power. The solution time can be shortened by the method of parallel acceleration, the real-time performance can be improved, and the real-time response of the non-linear model predictive control algorithm can be realized.

[0131] The NMPC model constructed in this embodiment mainly aims at the support leg of the biped robot, and the foot end force of the robot support leg is obtained by solving the NMPC problem. The present invention converts the QP problem of the NMPC model into the standard form applicable to the alternating direction method of multipliers, and the alternating direction method of multipliers has the characteristics of fast convergence speed and applicability to parallel computing.

[0132] As a further preferred technical solution, the method further includes:

[0133] planning the robot's landing point in response to the desired state instruction input by the user;

[0134] fitting the robot's foot end trajectory with a Bessel curve and using PD control to track the robot's swing leg trajectory.

[0135] As a further preferred technical solution, when performing iterative solution on the standard form of the alternating direction method of multipliers, the NMPC solution algorithm deployed on the heterogeneous chip is used to perform iterative solution on the standard form of the alternating direction method of multipliers, wherein the standard form iterative solution algorithm of the alternating direction method of multipliers is deployed in the ARM core, and the minimum residual algorithm is deployed in the FPGA chip.

[0136] Specifically, the biped robot control architecture is as Figure 3As shown in the figure. First, the user inputs the desired state instruction to the robot. The robot motion controller controls the support legs and swing legs respectively. For the support legs, the NMPC algorithm is adopted, with the column vector as the robot state, the foot-end support force and torque as the control inputs, and the obtained optimal support force and torque are mapped to each joint according to the Jacobian matrix, and relaxation optimization is carried out according to the floating-base dynamics model. Finally, the optimized joint forces are obtained and sent to each joint motor. For the swing legs, first plan their landing points, then use Bezier curves to fit their foot-end trajectories, and use PD control to track their swing leg trajectories. At the same time, the robot evaluates its actual state according to the state estimator and continues the next control loop to complete the next motion control cycle of the biped robot. In the entire algorithm process, the NMPC solver of the support legs takes the longest calculation time and requires the most computing resources. In the present invention, for the NMPC controller of the legged robot, the core component NMPC solving module is deployed on the heterogeneous chip ZYNQ, that is, the relevant algorithms are allocated on the CPU and FPGA to complete the algorithm solving acceleration task.

[0137] In addition, as Figure 4 shown, another embodiment of the present invention proposes a heterogeneous acceleration device for controlling a legged robot, and the device includes:

[0138] A response module 10, configured to respond to the desired state instruction input by the user, take the foot-end support force and torque as the control input variables, perform iterative solution on the standard form of the alternating direction multiplier method, and obtain the optimal support force and torque;

[0139] A joint force optimization module 20, configured to map the optimal support force and torque to the joint space according to the Jacobian matrix, and perform relaxation optimization according to the floating-base dynamics model to obtain the optimized joint forces;

[0140] A motion control module 30, configured to send the optimized joint forces to each joint motor of the robot to control the movement of the support legs.

[0141] As a further preferred technical solution, the device further includes:

[0142] A dynamic equation construction module, configured to establish a state-space dynamic equation represented in the world coordinate system with the desired state of the robot as the state variable and the force and torque of the robot's sole in contact with the ground as the decision variables;

[0143] A first conversion module, configured to convert the state-space dynamic equation into a discrete-time representation form of time steps;

[0144] A second conversion module, configured to re-define the state variables and decision variables as augmented state variables, and convert the discrete-time representation form into the standard form of the alternating direction multiplier method by using the augmented state variables.

[0145] As a further preferred technical solution, the standard form of the alternating direction method of multipliers is as follows:

[0146]

[0147] s.t. AX + DZ = c

[0148] The definitions of the symbols in the formula are the same as those above.

[0149] As a further preferred technical solution, the response module is specifically used for:

[0150] Iteratively solve the standard form of the alternating direction method of multipliers, and calculate the residual after each iterative solution;

[0151] Judge whether the termination iteration condition is satisfied based on the residual;

[0152] If so, output the optimal supporting force and moment;

[0153] If not, solve the KKT matrix in the alternating direction method of multipliers by using the minimum residual method, update the actual state variables of the robot, and then perform the next iterative solution.

[0154] The termination iteration condition satisfied by the residual is:

[0155] ||r k || ≤ ε

[0156] The definitions of the symbols in the formula are the same as those above.

[0157] As a further preferred technical solution, the minimum residual method is a minimum residual algorithm applicable to parallel solution of nonlinear equations.

[0158] As a further preferred technical solution, the response module is further used for:

[0159] In response to the desired state instruction input by the user, plan the landing point of the robot;

[0160] Use Bessel curves to fit the trajectory of the robot's foot end, and use PD control to track the trajectory of the robot's swinging leg.

[0161] As a further preferred technical solution, when iteratively solving the standard form of the alternating direction method of multipliers, the standard form iterative solution algorithm of the alternating direction method of multipliers is deployed in the ARM core, and the minimum residual algorithm is deployed in the FPGA chip.

[0162] It should be noted that other embodiments or specific implementation methods of the heterogeneous acceleration device for controlling the legged robot of the present invention can refer to the above method embodiments, and will not be elaborated here.

[0163] In addition, another embodiment of the present invention further provides an FPGA heterogeneous acceleration platform for solving model predictive control of a legged robot, including a real-time simulation system platform, an MPC controller arranged in an ARM core, and a linear system solver deployed in an FPGA chip. The real-time simulation system platform is connected to the MPC controller, and the MPC controller is connected to the linear system solver;

[0164] The MPC controller is used to respond to the desired state instruction input by the user, take the foot-end support force and torque as control input variables, perform iterative solution on the standard form of the alternating direction multiplier method, and obtain the optimal support force and torque;

[0165] The linear system solver is used to solve the KKT matrix in the alternating direction multiplier method by using the minimum residual method and update the actual state variables of the robot.

[0166] Specifically, as Figure 5 shown, the present invention uses the Xilinx Zynq-7020 hardware platform to accelerate the model predictive control algorithm. The desired position is sent to the UART interface receiver of the development board through the UART serial interface sender, and then the data is sent to the NMPC solver in the ARM core. The PL side reads the coefficient matrix and the residual vector from the PS side through the AXI interface, solves the equations on the PL side, and then transfers the solution of the equations to the NMPC solver on the PS side. The construction steps of the NMPC solver are as Figure 6 shown. First, write the minimum residual algorithm (Minres) for solving the linear equations deployed on the FPGA chip. This algorithm is written in C language. Then, use the Vivado high-level synthesis tool HLS to convert the C code into hardware description language HDL code, use the Vivado tool for synthesis, placement, routing and generate an IP core, then perform block design to design the communication between the PS side and the PL side, and finally generate a binary file. Then, write the PS side code in the Vivado SDK, which is mainly responsible for the logic control, macro definition, etc. of the NMPC algorithm, and finally perform board-level verification.

[0167] As a further preferred technical solution, the linear system solver is used to store the coefficient matrix in the COO storage mode.

[0168] It should be noted that when converting C code into hardware description language using a high-level synthesis tool, parallel computing design needs to be carried out. For the product of vector V1 and vector V2, assuming that the dimensions of the vectors are both N, when the hardware performs calculations, for the multiplication and addition of the first element of the vector, assuming that it takes 4 time periods, including four steps of data reading, multiplication, addition, and writing. At this time, we will perform loop unrolling and pipelining operations on the product of the vectors, as Figure 7As shown, this method can make full use of the system-on-chip resources and improve the computing efficiency. For the product of a matrix and a vector, since the coefficient matrix H is a sparse matrix in this algorithm and most of its elements are zero elements, in order to save storage resources and shorten the computing time, we adopt the COO storage method as Figure 8 shown. This storage method uses three arrays to store a sparse matrix, namely the row index array, the column index array, and the value index array, which store the corresponding row, column, and value of the non-zero elements in the array respectively. Therefore, the product of the matrix and the vector is split into the product of vectors.

[0169] The present invention uses PROTOIP for rapid algorithm deployment. PROTOIP is an open-source framework that can be conveniently deployed into the VIVADO software. By using the Matlab tool, the tcl commands in the PROTOIP toolbox can be automatically called, and then tasks such as the automatic conversion of C code to HDL language, the automatic synthesis of HDL code, layout and wiring, and the generation of IP cores can be automatically completed. This can greatly reduce the work of developers for FPGA development and then focus on the development of algorithms. Using the PROIOIP toolbox to quickly generate and deploy the biped robot NMPC solution algorithm code reduces the difficulty of deploying the biped robot NMPC model to hardware and improves the development efficiency.

[0170] It should be noted that the present invention mainly tests four computing allocation schemes:

[0171] (1) The algorithm is all deployed in the ARM core of the development board with a main frequency of 660 MHz, marked as SW;

[0172] (2) The product of the matrix and the vector is deployed in the FPGA chip with a frequency of 120 MHz, and other algorithms are deployed to the ARM core, marked as HW1;

[0173] (3) The matrix Lanczos decomposition is deployed to the FPGA chip, and other algorithms are deployed to the ARM core, marked as HW2;

[0174] (4) The MINRES algorithm is deployed to the FPGA chip, and other algorithms are deployed to the ARM core, marked as HW3;

[0175] The deployment effect is as Figure 9 shown. It can be seen that deploying the MINRES algorithm to the FPGA can achieve a significant reduction in computing time. The prediction domain is 10, and the computing time is about 8.1 ms. At the same time, the resource utilization rate of the FPGA is higher.

[0176] In this embodiment, a set of NMPC solving algorithms applicable to low power consumption, low cost, and parallel computing are developed. The minimum residual method is used to solve the KKT matrix in the alternating direction multiplier method, and the minimum residual method is deployed on the FPGA chip. The multiplication of matrices and vectors is accelerated and parallel computed by means of loop unrolling and pipelining operations, making full use of the parallel characteristics of the FPGA chip and improving the real-time performance of solving the NMPC problem.

[0177] In addition, an embodiment of the present invention also proposes a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the heterogeneous acceleration method for the control of the legged robot as described in the above embodiment is realized.

[0178] It should be noted that the logic and / or steps represented in the flowchart or described in other ways herein, for example, can be considered as a definite sequence list of executable instructions for implementing logical functions, and can be specifically implemented in any computer-readable medium for use by an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processor, or other systems that can fetch and execute instructions from the instruction execution system, apparatus, or device), or in combination with these instruction execution systems, apparatus, or devices. For the purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device. More specific examples (non-exhaustive list) of the computer-readable medium include the following: an electrical connection part with one or more wirings (electronic device), a portable computer diskette (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). In addition, the computer-readable medium can even be paper or other suitable media on which the program can be printed, because the program can be obtained electronically, for example, by optically scanning the paper or other media, followed by editing, interpretation, or other appropriate processing as necessary, and then stored in a computer memory.

[0179] It should be understood that each part of the present invention can be implemented by hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, any one or a combination of the following well-known technologies in the art can be used: discrete logic circuits with logic gate circuits for implementing logical functions on data signals, application-specific integrated circuits with appropriate combinational logic gate circuits, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0180] In the description of this specification, the descriptions referring to terms such as "one embodiment", "some embodiments", "examples", "specific examples", or "some examples", etc. mean that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic descriptions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in a suitable manner in any one or more embodiments or examples.

[0181] In addition, the terms "first" and "second" are used only for descriptive purposes and cannot be construed as indicating or implying relative importance or implicitly specifying the quantity of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include at least one of such features. In the description of the present invention, "a plurality of" means at least two, such as two, three, etc., unless otherwise specifically and clearly defined.

[0182] Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

Claims

1. A heterogeneous acceleration method for controlling a legged robot, characterized in that: The method comprises: In response to the desired state command input by the user, the foot-end support force and torque are used as control input variables, and the standard form of the alternating direction multiplier method is iteratively solved to obtain the optimal support force and torque; The optimal support force and moment are mapped to the joint space according to the Jacobian matrix, and relaxation optimization is performed according to the floating basis dynamics model to obtain the optimized joint force; The optimized joint force is sent to each joint motor of the robot to control the movement of the supporting legs.

2. The heterogeneous acceleration method for controlling a legged robot according to claim 1, characterized in that: Before the method responds to the desired state command input by the user, takes the foot end support force and torque as control input variables, iteratively solves the standard form of the alternating direction multiplier method to obtain the optimal support force and torque, the method further includes: The desired state of the robot is taken as the state variable, and the force and torque between the robot's sole and the ground are taken as the decision variables, and the state space dynamic equation expressed in the world coordinate system is established; Convert the state-space dynamics equations into a discrete-time representation of time steps; The state variables and decision variables are redefined as augmented state variables, and the augmented state variables are used to convert the discrete-time representation into the standard form of the alternating direction multiplier method.

3. The heterogeneous acceleration method for controlling a legged robot according to claim 1, characterized in that: The standard form of the alternating direction multiplier method is: stAX+DZ=c Where: I z (Z) is the indicator function, X is the augmented state variable, Z is the introduced non-negative slack variable, f is the coefficient of the linear term of the cost function, H is the coefficient matrix of the quadratic term; A is the coefficient matrix of the equality constraint state variable, C is the coefficient matrix of the inequality matrix, A eq is the intermediate matrix; I is the identity matrix; c is the linear equality constraint vector, b eq =[A0X0 0 ... 0], X0 is the initial state, d is the inequality residue, and T is the transpose symbol.

4. The heterogeneous acceleration method for controlling a legged robot according to claim 1, characterized in that: The method of responding to the desired state command input by the user, taking the foot end support force and torque as control input variables, iteratively solving the standard form of the alternating direction multiplier method to obtain the optimal support force and torque includes: Iterate the standard form of the alternating direction multiplier method and calculate the residual after each iterative solution; Determine whether a termination condition of the iteration is met based on the residual; If so, the optimal support force and moment are output; If not, the minimum residual method is used to solve the KKT matrix in the alternating direction multiplier method, and the actual state variables of the robot are updated before the next iterative solution.

5. The heterogeneous acceleration method for controlling a legged robot according to claim 4, characterized in that: The termination iteration condition satisfied by the residual is: ||r k ||≤ε Where: r k =Ax (k) +Bz (k) -c, ε is the set threshold, || || is the absolute value.

6. The heterogeneous acceleration method for controlling a legged robot according to claim 4, characterized in that: The minimum residual method is a minimum residual algorithm suitable for solving nonlinear equations in parallel.

7. The heterogeneous acceleration method for controlling a legged robot according to claim 1, characterized in that: The method further comprises: In response to the desired state command input by the user, planning the robot's landing point; The Bezier curve is used to fit the trajectory of the robot's foot, and PD control is used to track the trajectory of the robot's swinging legs.

8. The heterogeneous acceleration method for controlling a legged robot according to any one of claims 1 to 7, characterized in that: When iteratively solving the standard form of the alternating direction multiplier method, the NMPC solution algorithm deployed in the heterogeneous chip is used to iteratively solve the standard form of the alternating direction multiplier method, wherein the standard form iterative solution algorithm of the alternating direction multiplier method is deployed in the ARM core, and the minimum residual algorithm is deployed in the FPGA chip.

9. A heterogeneous acceleration device controlled by a legged robot, characterized in that: The device comprises: A response module is used for responding to the desired state command input by the user, taking the foot end support force and torque as the control input variables, iteratively solving the standard form of the alternating direction multiplier method, and obtaining the optimal support force and torque; The joint force optimization module is used to map the optimal support force and torque to the joint space according to the Jacobian matrix, and perform relaxation optimization according to the floating basis dynamics model to obtain the optimized joint force; The motion control module is used to send the optimized joint force to each joint motor of the robot to control the movement of the supporting legs.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 8 is implemented.

11. An FPGA heterogeneous acceleration platform for solving model predictive control of a legged robot, characterized in that: It includes a real-time simulation system platform, an MPC controller arranged in an ARM core, and a linear system solver deployed in an FPGA chip, wherein the real-time simulation system platform is connected to the MPC controller, and the MPC controller is connected to the linear system solver; The MPC controller is used to respond to the desired state command input by the user, take the foot-end support force and torque as the control input variables, iteratively solve the standard form of the alternating direction multiplier method, and obtain the optimal support force and torque; The linear system solver is used to solve the KKT matrix in the alternating direction multiplier method using the minimum residual method to update the actual state variables of the robot.

12. The FPGA heterogeneous acceleration platform for solving the legged robot model predictive control as claimed in claim 11, characterized in that: The real-time simulation system platform is connected to the MPC controller via a UART interface, and the MPC controller is connected to the linear system solver via an AXI interface.

13. The FPGA heterogeneous acceleration platform for solving the legged robot model predictive control as claimed in claim 11, characterized in that: The linear system solver is used to store the coefficient matrix in a COO storage manner.

Images