Heterogeneous acceleration method, apparatus and storage medium for controlling a foot robot

Abstract

The application discloses a kind of foot type robot control heterogeneous acceleration method, device and storage medium, method includes responding to the desired state instruction of user input, with foot end support force and torque as control input variable, the standard form of alternating direction multiplier method is iteratively solved, and optimal support force and torque are obtained;Optimal support force and torque are mapped to joint space according to Jacobian matrix, and relaxation optimization is carried out according to floating base dynamics model, and the joint force after optimization is obtained;The joint motor of each joint of robot is sent to the joint force after optimization, and support leg movement is controlled;The application improves the motion control efficiency of foot type robot, reduces running time and power consumption.

Description

Technical Field

[0001] This invention relates to the field of intelligent robot technology, specifically to a heterogeneous acceleration method, device, and storage medium for controlling a legged robot. Background Technology

[0002] Legged robots, such as bipedal robots, are robots that mimic human or other bipedal walking. They aim to simulate human-like gait by using two legs, typically employing two independent leg systems, each leg usually composed of multiple joints, mimicking the walking style of humans or animals. Bipedal robots have broad application prospects, especially in service, medical, and rescue fields, enabling autonomous movement in human environments. Because bipedal robots move one leg at a time while walking, shifting their center of gravity to the other leg, similar to human gait, these robots not only face complex motion control problems but also challenges related to stability, adaptability, and flexibility associated with human gait. Maintaining stability and balance control in bipedal robots has always been one of the important problems researchers need to solve.

[0003] Currently, several control strategy frameworks exist that can effectively enable bipedal robots to perform robust locomotion in practical tasks. These include the Hybrid Zero Dynamics (HZD) model applied to bipedal robots such as MABEL, and force- and torque-based model predictive control schemes applied to legged robots such as HECTOR (Dynamic Loco-manipulation on HECTOR: Humanoid for Enhanced Control and Open-source Research). Force- and torque-based control schemes combine model predictive control with higher-frequency whole-body control (WBC) to achieve more accurate trajectory tracking. This type of control framework is used in conjunction with linear dynamics models and constraints, employing simplified single-rigid-body dynamics. It obtains foot reaction forces through model predictive control and then maps these foot forces to the torques of each joint. The main computational resources and time of this control framework lie in the model predictive control solver. The HECTOR bipedal robot developed by MIT uses the active set method to solve the problem on an AMD Ryzen 5600X@4.2GHz CPU. This method has high requirements for hardware platform computing power, high power consumption and cost, poor real-time performance, and is difficult to apply.

[0004] To address the above issues, this paper considers fully utilizing the parallel computing capabilities of FPGAs on heterogeneous chips. A nonlinear model predictive control (MMDC) algorithm suitable for parallel computing is employed in the controller design to improve the real-time performance of online computation and achieve stable motion control for bipedal robots. Currently, some researchers have conducted research on parallel computing for nonlinear model predictive control. For example, the dissertation "FPGA Implementation and Application Research of Fast Model Predictive Control," by Xu Fang (Jilin University), addresses the requirements for fast and stable trajectory tracking control of wheeled mobile robots and the constraint problems of actuators. It establishes a nonlinear model predictive controller, employs the constrained particle swarm optimization (PSO) algorithm, and proposes an FPGA-based SoPC embedded implementation scheme. Combining the simple iterative logic and parallel computing characteristics of the PSO algorithm, pipelined processing and iterative expansion are applied to the algorithm, further improving the speed of online computation and enabling real-time application of the nonlinear model predictive controller. The paper "FPGA-Accelerated Solution Method for Model Predictive Control for Autonomous Driving," by Li Yunfei (Master's Thesis, Chongqing University), proposes a lateral and longitudinal model predictive control algorithm deployed on a CPU+FPGA hardware platform that can simultaneously achieve trajectory tracking and velocity tracking. The quadprog++ quadratic programming solver is deployed on the FPGA chip, resulting in significant acceleration. These solutions target autonomous mobile vehicles, which have fewer state variables and simpler dynamic models. However, for bipedal robots, there are more state variables and more complex dynamic models. Furthermore, genetic algorithms such as particle swarm optimization heavily rely on parameter selection and are prone to getting trapped in local optima.

[0005] Patent application CN114147716A proposes a solution based on a pre-defined dynamic system stability function, such as a Lyapunov function. The solution module uses 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 torques, can be directly obtained, solving the problem that directly controlling the robot based on the joint angle values ​​leads to poor robot posture tracking. However, the first-order alternating direction multiplier method used in this solution is implemented on a high-performance CPU, which results in excessively long solution times and poor real-time performance for bipedal robots with complex dynamic models. Summary of the Invention

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

[0007] The present invention solves the above-mentioned technical problems through the following technical means:

[0008] A heterogeneous acceleration method for legged robot control is proposed, the method comprising:

[0009] In response to the user's input of the desired state command, the standard form of the alternating direction multiplier method is iteratively solved using the foot support force and torque as control input variables to obtain the optimal support force and torque;

[0010] The optimal support force and torque 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;

[0011] The optimized joint forces are sent to the robot's joint motors to control the movement of the supporting legs.

[0012] Furthermore, before the method responds to the user-inputted desired state command and iteratively solves the standard form of the alternating direction multiplier method using foot support force and torque as control input variables to obtain the optimal support force and torque, the method further includes:

[0013] Using the robot's desired state as the state variable and the force and torque of the robot's foot contacting the ground as the decision variable, a state-space dynamic equation expressed in the world coordinate system is established.

[0014] Transform the state-space dynamics equations into a discrete-time representation of time steps;

[0015] The state variables and decision variables are redefined as augmented state variables, and the discrete-time representation is converted into the standard form of the alternating direction multiplier method using the augmented state variables.

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

[0017]

[0018] stAX+DZ=c

[0019] In the formula: I z (Z) is the indicator function, I is the identity matrix, Z is the introduced nonnegative relaxation variable, X is the augmented state variable, X = [x1...x2] h u0...u h-1 ], x k (k = 1, 2, ..., h) represents the state variables at the k-th prediction time, where h is the total length of the prediction domain, and u k (k = 0, 1, ..., h-1) represents the control input variables at the k-th prediction time. Θ is the robot's azimuth angle, P c Let ω be the position of the robot's center of mass, and ω be the robot's angular velocity. The velocity is the center of mass; matrix H represents the coefficient matrix of the quadratic term, and its specific form is as follows: Matrix H is derived from Q k and R k Composition (k = 0, 1, ..., h-1), Q k The adjustment parameter for the state variable, R, needs to be set manually. k To control the input adjustment parameters, manual setting is also required; matrix A is the coefficient matrix of the state variables in the equality constraint, derived from A eq Composed of C, its specific expression In this context, matrix C is the coefficient of the inequality matrix, and A... eq The intermediate matrix is ​​constructed. Matrix A k Let A be the coefficient matrix of the k-th predicted time dynamic state variable. k =I 13 +A c,k dt, Let ψ be the rotation matrix. k For the robot's yaw angle, B k =B c,k dt, dt is the time step. I b This is the robot's inertial constant, which can be obtained based on the robot's mechanical design structure. for Inverse matrix, (p i -p c F(i = 1, 2 represent the left and right legs respectively) is a skew-symmetric matrix of the vector from the robot's center of mass to the foot end. i (i = 1, 2 represent the left and right legs respectively) represents the foot force; f is the coefficient of the linear term in the cost function. The reference trajectory is h, the total length of the prediction domain is c, and the linear equality constraint vector is b. eq = [A0X0 0 ... 0], where X0 is the initial state, and vector d is the inequality residual, composed of the force constraints at the robot's feet. μ is the frictional force between the robot's foot and the ground, F i,x F i,y F i,z F represents the foot support force exerted by the ground on the robot's feet along the x, y, and z directions, respectively. min F max Let Z represent the minimum and maximum forces that the ground can withstand along the z-direction in the reaction force exerted on the robot's supporting leg feet.

[0020] Furthermore, the step of responding to the user's input of a desired state command, using foot 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:

[0021] The standard form of the alternating direction multiplier method is solved iteratively, and the residual is calculated after each iteration.

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

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

[0024] If not, the KKT matrix in the alternating direction multiplier method is solved using the minimum residual method, and the robot's actual state variables are updated before the next iteration is performed.

[0025] Furthermore, 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] Furthermore, the minimum residual method is a minimum residual algorithm applicable to parallel solution of nonlinear equation systems.

[0029] Furthermore, the method also includes:

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

[0031] The robot's foot trajectory was fitted using a Bezier curve, and the robot's swing leg trajectory was tracked using PD control.

[0032] Furthermore, when iteratively solving the standard form of the alternating direction multiplier method, the NMPC solution algorithm deployed on a heterogeneous chip is used to iteratively solve the standard form of the alternating direction multiplier method. 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.

[0033] Furthermore, this invention also proposes a heterogeneous acceleration device for legged robot control, the device comprising:

[0034] The response module is used to respond to the user's input of the desired state command. It uses the foot support force and torque as control input variables and iteratively solves the standard form of the alternating direction multiplier method to obtain the optimal support force and torque.

[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 basis dynamics model to obtain the optimized joint force.

[0036] The motion control module is used to send optimized joint forces to the robot's joint motors to control the movement of the supporting legs.

[0037] Furthermore, the present invention also proposes a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the heterogeneous acceleration method for legged robot control as described above.

[0038] In addition, this invention proposes an FPGA heterogeneous acceleration platform for predictive control solution of legged robot models, 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 user's input of the desired state command. It uses the foot support force and torque as control input variables and iteratively solves the standard form of the alternating direction multiplier method to obtain the optimal support force and torque.

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

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

[0042] Furthermore, the linear system solver is used to store the coefficient matrix using a COO storage method.

[0043] The advantages of this invention are:

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

[0045] (2) The present invention uses the minimum residual method to solve complex linear systems. Compared with traditional matrix LDL decomposition and other methods, the minimum residual method has a lower computational time complexity, which further improves the real-time performance of solving NMPC problems.

[0046] (3) The present invention rationally allocates computing resources by deploying matrix and vector multiplication, matrix Lanczos decomposition, and minimum residual method suitable for parallel computing into FPGA, and rationally allocating resources for parallel solution through methods such as loop expansion and pipeline, so as to adapt to the different architectures of ARM and FPGA and make full use of the computing resources of both.

[0047] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0048] Figure 1 This is a flowchart illustrating a heterogeneous acceleration method for controlling a legged robot according to an embodiment of the present invention.

[0049] Figure 2 This is a schematic diagram of the NMPC accelerated solution process in one embodiment of the present invention;

[0050] Figure 3 This is a schematic diagram of the control architecture of a bipedal robot in one embodiment of the present invention;

[0051] Figure 4 This is a schematic diagram of the structure of a heterogeneous acceleration device for controlling a legged robot according to an embodiment of the present invention;

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

[0053] Figure 6 This is a schematic diagram of the algorithm development process for the PS and PL ends in one embodiment of the present invention;

[0054] Figure 7 This is a schematic diagram of loop expansion and pipelined operation of vector multiplication in one embodiment of the present invention, wherein (a) vector multiplication, (b) timing diagram of vector multiplication, and (c) schematic diagram of loop expansion and pipelined operation.

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

[0056] Figure 9 This is a schematic diagram comparing resource utilization and computing time under different computing deployments in one embodiment of the present invention. Detailed Implementation

[0057] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of 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 only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

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

[0059] S10. In response to the user's input of the desired state command, the foot support force and torque are used as control input variables to 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 the desired state command into the robot, and the desired state forms a column vector, including the desired body center of mass position P. d Required body center of mass speed Required rotation matrix R d and the angular velocity ω required for the robot body d Using column vectors as state variables and foot support force and torque as 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 computational efficiency and low computational cost, it can quickly obtain the optimal support force and torque, thereby improving the motion control efficiency of the legged robot and reducing 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. Then, use existing technology to perform relaxation optimization based on the floating base dynamics model 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, and M(q) is the joint space mass matrix. It is the bias force matrix, which includes forces such as gravity and Coriolis force that are independent of joint acceleration. and f c These represent the contact Jacobian and contact force, respectively; τ is the generalized joint torque; and S... jTo select a filtering matrix, the first six possible non-zero terms in the joint torque are filtered out. For joint velocity, This refers to joint acceleration.

[0065] S30: The optimized joint force is sent to the joint motors of the robot to control the movement of the supporting leg.

[0066] It should be noted that the legged robot motion controller controls the supporting leg and the swinging leg separately. In this embodiment, the NMPC model is used to solve the supporting leg. The desired position in the column vector is used as the robot's state variable, and the foot support force and torque are used as control inputs as decision variables. The obtained optimal support force and torque are mapped to each joint according to the Jacobian matrix. Relaxation optimization is performed according to the floating basis dynamics model, and finally the optimized joint force is obtained and sent to each joint motor. Since the NMPC solution of the supporting leg has the longest computation time and requires the most computational resources in the entire algorithm process, this embodiment constructs a constrained NMPC model based on the dynamics model of the legged robot and uses the alternating direction multiplier method for iterative solution. Compared with the original dual interior point method, obstacle function method, active set method, etc., the alternating direction multiplier method has the advantages of fast convergence speed, clear logic, and applicability to parallel computing, thereby reducing the NMPC solution running time and power consumption.

[0067] As a further preferred technical solution, in step S10: before iteratively solving the standard form of the alternating direction multiplier method in response to the user's input desired state command, using foot support force and torque as control input variables to obtain the optimal support force and torque, the method further includes the following steps:

[0068] S1. Using the robot's desired state as the state variable and the force and torque of the robot's foot contacting the ground as the decision variable, establish the state-space dynamic equation expressed in the world coordinate system.

[0069] Specifically, this embodiment takes the HECTOR5 DW bipedal robot as an example. Its simplified rigid body includes the body, shoulders, hips, and upper thighs, which are considered 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, where F i For foot support force, when the robot stands or walks on the ground, the ground must exert a support force on the supporting leg. i = 1, 2 represent the robot's left and right legs, M iF is the supporting torque exerted by the ground on the supporting leg. i,x F i,y F i,z M represents the components of the force exerted by the ground on the supporting leg along the x, y, and z directions. i,x M i,y M i,z These represent the components of the torque exerted by the ground on the supporting leg along the x, y, and z directions, respectively. HECTOR's design centers on the assumptions of rigid body dynamics, where the remaining arm and leg components account for only 15% of the robot's total mass; therefore, the inertial effects of these components are neglected in the dynamic model. It is assumed that the location of the ground reaction force and torque is a contact point on each foot, located at the projection of the ankle joint position along the z-coordinate. The complete hardware actuation of the contact point, including 3D linear motion, rolling, and yaw, is entirely achieved by applying 5D forces and torques.

[0070] This embodiment selects... For state variables, let u = [F1 F2 M1 M2] 12×1 To control the input variables, where Θ = [φ θ ψ] T φ represents the robot's roll angle, θ is the pitch angle, ψ is the yaw angle, and P... c Indicates the position of the robot's center of mass. Let ω represent the velocity of the center of mass, and ω represent the angular velocity; u = [F1 F2 M1 M2] 12×1 For NMPC optimization, the variables represent the forces and moments in contact between the robot's feet and the ground. The simplified dynamic equations expressed in world coordinates can be represented as:

[0071]

[0072] In the formula, A c,k B c,k The meaning of the symbols is the same as above.

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

[0074] It should be noted that, in order to utilize this linearized state-space dynamics equation within the MPC framework, it is transformed into a discrete-time representation at the k-th time step using the MPC step size 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 time step k+1, x[k] is the state variable at time step k, and A k B kThe meaning of the symbols is 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 multiplier method.

[0078] Specifically, the NMPC model in this embodiment mainly focuses on the supporting leg of a bipedal robot. The foot-end force of the robot's supporting leg is obtained by solving the NMPC problem. This invention transforms the QP problem of the NMPC model into a standard form suitable for the alternating direction multiplier method. The alternating direction multiplier method has the characteristics of fast convergence speed and suitability for parallel computation. The NMPC model for the supporting leg with a finite horizontal length h can be written in the following standard format:

[0079]

[0080] stx[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 Q represents the system state and control input at time step k. k R k This is a matrix defining the weights of each state and control input variable. The inequality constraints are mainly related to the contact force and torque. The contact forces in the x and y directions are located within the friction pyramid, where μ is the coefficient of friction; x k+1 Let x be the system state at time step k+1. k+1,ref For 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) represent the foot support forces exerted by the ground on the robot's feet along the x, y, and z directions, respectively, F min F max Let Z represent the minimum and maximum forces that the ground can withstand along the z-direction in the reaction force exerted on the robot's supporting leg feet.

[0085] Redefine the augmented state variable X = [x1 ... x h u0 ... u h-1 The standard MPC problem, consisting of state x and decision variable u, can be transformed into the following form:

[0086]

[0087] stA eq X = b eq

[0088] CX <d

[0089] The definitions of each function in the formula are consistent with those above.

[0090] Finally, the standard form of the alternating direction multiplier method is established as follows:

[0091]

[0092] stAX+DZ=c

[0093] In the formula: I z (Z) is an indicator function.

[0094] As a further preferred technical solution, such as Figure 2 As shown, step S10: In response to the user's input of the desired state command, using the foot 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, specifically including the following steps:

[0095] S11. Iterate through the standard form of the alternating direction multiplier method and calculate the residual after each iteration.

[0096] Specifically, the steps for solving the optimization problem using the alternating direction multiplier method include:

[0097] Input matrices H, f, A, D, c; input initial values ​​X0, Z0, τ0;

[0098] ADMM algorithm iterative solution:

[0099] (H+ρA T A)X k+1 =-[f+ρA T (DZ k +τ k -c)]

[0100]

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

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

[0103] In the formula, ρ is a manually set penalty factor, and Z... k Let τ be the value of the nonnegative relaxation variable Z in the k-th iteration. k Let r be the dual variable of the k-th iteration. k Let X0, Z0, and τ0 be the residual of the k-th iteration, and let X0, Z0, and τ0 be the state variable, slack variable, and dual variable in the initial state, respectively. Let g be the first g numbers of the dual variables in the k-th iteration, where g has the same number of columns as matrix C. z () is used to project the vector along the z-direction; the other variables have been defined previously.

[0104] S12. Determine whether the termination iteration condition is met based on the residual. If yes, proceed to step S13; otherwise, proceed to step S14.

[0105] S13, Output optimal support force and torque;

[0106] S14. Solve the KKT matrix in the alternating direction multiplier method using the minimum residual method, update the robot's actual state variables, and then execute step S11 to perform the next iteration.

[0107] Specifically, the existing method, the minimum residual method, is a minimum residual algorithm suitable for parallel solving of linear equation systems. in The algorithm execution steps include:

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

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

[0110] Solve for the Givens transformation matrix:

[0111]

[0112] in, Let γ be the 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] Recursion 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] get:

[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 This is the residual from the (k+1)th iteration.

[0129] This embodiment uses the minimum residual method to solve complex linear systems. Compared with traditional methods such as matrix LDL decomposition, the minimum residual method has lower computational time complexity.

[0130] It should be noted that model predictive controllers for bipedal robots generally use the robot's single-rigid-body dynamics model as equality constraints to design the model predictive control solver. The common solving algorithms are the active set method or the interior point method, and the algorithm implementation is mainly on high-performance CPU hardware platforms. This embodiment also uses a single-rigid-body dynamics model as its basis. However, unlike other inventions or methods, this embodiment introduces a non-negative relaxation variable when designing the model predictive controller solver, transforming the nonlinear model predictive controller into the standard form of the alternating direction multiplier method. Furthermore, the nonlinear model predictive control algorithm is implemented on an embedded SoC platform, fully utilizing the parallel characteristics of the alternating direction multiplier method. By deploying the alternating direction multiplier method on a parallelizable FPGA chip with lower computing power, the solution time can be shortened and real-time performance improved through parallel acceleration, achieving real-time response of the nonlinear model predictive control algorithm.

[0131] The NMPC model constructed in this embodiment mainly focuses on the supporting leg of a bipedal robot, obtaining the foot-end force of the robot's supporting leg by solving the NMPC problem. This invention transforms the QP problem of the NMPC model into a standard form suitable for the alternating direction multiplier method, which has the characteristics of fast convergence speed and suitability for parallel computing.

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

[0133] In response to the user's input of desired state commands, plan the robot's landing point;

[0134] The robot's foot trajectory was fitted using a Bezier curve, and the robot's swing leg trajectory was tracked using PD control.

[0135] As a further preferred technical solution, when iteratively solving the standard form of the alternating direction multiplier method, the NMPC solution algorithm deployed on a heterogeneous chip is used to iteratively solve the standard form of the alternating direction multiplier method. 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.

[0136] Specifically, the control architecture of bipedal robots is as follows: Figure 3As shown in the diagram, the user first inputs the desired state command to the robot. The robot's motion controller then controls the supporting leg and the swinging leg separately. For the supporting leg, the NMPC algorithm is used, with column vectors representing the robot's state and foot support force and torque as control inputs. The obtained optimal support force and torque are mapped to each joint according to the Jacobian matrix, and relaxation optimization is performed based on the floating basis dynamics model. Finally, the optimized joint forces are obtained and sent to each joint motor. For the swinging leg, its landing point is first planned, then its foot trajectory is fitted using a Bézier curve, and PD control is used to track its swinging leg trajectory. Simultaneously, the robot evaluates its actual state based on the state estimator and continues to the next control loop, completing the next motion control cycle of the bipedal robot. In the entire algorithm process, the NMPC solver for the supporting leg has the longest computation time and requires the most computational resources. This invention, for the NMPC controller of a legged robot, deploys the core component, the NMPC solver module, on a heterogeneous chip ZYNQ, that is, by allocating relevant algorithms on the CPU and FPGA to complete the algorithm solution acceleration task.

[0137] In addition, such as Figure 4 As shown, another embodiment of the present invention proposes a heterogeneous acceleration device for legged robot control, the device comprising:

[0138] The response module 10 is used to respond to the user's input of the desired state command, and to iteratively solve the standard form of the alternating direction multiplier method with the foot support force and torque as control input variables to obtain the optimal support force and torque.

[0139] The joint force optimization module 20 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.

[0140] The motion control module 30 is used to send the optimized joint forces to the various joint motors of the robot to control the movement of the supporting leg.

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

[0142] The dynamic equation construction module is used to establish state-space dynamic equations in world coordinate system, with the robot's desired state as the state variable and the force and torque of the robot's foot contacting the ground as the decision variable.

[0143] The first conversion module is used to convert the state-space dynamic equations into a discrete-time representation of time steps;

[0144] The second transformation module is used to redefine the state variables and decision variables as augmented state variables, and use the augmented state variables to transform the discrete-time representation into the standard form of the alternating direction multiplier method.

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

[0146]

[0147] stAX+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] The standard form of the alternating direction multiplier method is solved iteratively, and the residual is calculated after each iteration.

[0151] Based on the residual, determine whether the termination iteration condition is met;

[0152] If so, output the optimal support force and torque;

[0153] If not, the KKT matrix in the alternating direction multiplier method is solved using the minimum residual method, and the robot's actual state variables are updated before the next iteration is performed.

[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 suitable for parallel solution of nonlinear equation systems.

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

[0159] In response to the user's input of desired state commands, plan the robot's landing point;

[0160] The robot's foot trajectory was fitted using a Bezier curve, and the robot's swing leg trajectory was tracked using PD control.

[0161] As a further preferred technical solution, when iteratively solving the standard form of the alternating direction multiplier method, 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.

[0162] It should be noted that other embodiments or specific implementation methods of the heterogeneous acceleration device controlled by the legged robot described in this invention can refer to the above-described method embodiments, and will not be repeated here.

[0163] In addition, another embodiment of the present invention proposes an FPGA heterogeneous acceleration platform for predictive control solution of a legged robot model, 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 user's input of the desired state command. It uses the foot support force and torque as control input variables and iteratively solves the standard form of the alternating direction multiplier method to 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 using the minimum residual method, and to update the robot's actual state variables.

[0166] Specifically, such as Figure 5 As shown, this 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 on the development board via the UART serial interface transmitter, and then the data is sent to the NMPC solver in the ARM core. The PL end reads the coefficient matrix and residual vector from the PS end via the AXI interface, and then obtains the solution to the equation system at the PL end. The solution is then passed to the NMPC solver at the PS end. The construction steps of the NMPC solver are as follows: Figure 6 As shown, firstly, a minimum residual algorithm (Minres) for solving linear equations was written and deployed on the FPGA chip. This algorithm was written in C language. Then, the C code was converted into hardware description language (HDL) code using the Vivado high-level synthesis tool HLS. The Vivado tool was used for synthesis, placement, routing, and IP core generation. Then, block design was performed to design the communication between the PS and PL sides. Finally, binary files were generated. Then, the PS side code was written in the Vivado SDK, mainly responsible for the logic control and macro definition of the NMPC algorithm. Finally, board-level verification was performed.

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

[0168] It should be noted that when using high-level synthesis tools to convert C code into a hardware description language, parallel computing design is required. For the product of vectors V1 and V2, assuming both vectors have dimension N, the hardware computation, multiplying and adding the first element of each vector, would take approximately four time cycles, including data read, multiplication, addition, and write. Therefore, we will perform loop unrolling and pipelined operations on the product of the vectors, such as... Figure 7As shown, this method can fully utilize on-chip system resources and improve computational efficiency. For the product of a matrix and a vector, since the coefficient matrix H in this algorithm is a sparse matrix with most elements being zero, we adopt a COO storage method to save storage resources and shorten computation time. Figure 8 As shown, this storage method uses three arrays to store a sparse matrix: a row index array, a column index array, and a value index array, which respectively store the row, column, and value corresponding to the non-zero elements in the array. Therefore, the product of a matrix and a vector is decomposed into the product of vectors.

[0169] This invention utilizes PROTOIP for rapid algorithm deployment. PROTOIP is an open-source framework that can be easily deployed into VIVADO software. Using Matlab, the tcl commands within the PROTOIP toolbox can be automatically invoked, automating tasks such as C code to HDL conversion, HDL code synthesis, placement and routing, and IP core generation. This significantly reduces the workload for developers on FPGA development, allowing them to focus on algorithm development. Employing the PROTOIP toolbox for rapid generation and deployment of bipedal robot NMPC solution algorithm code reduces the difficulty of deploying bipedal robot NMPC models to hardware, thus improving development efficiency.

[0170] It should be noted that this invention primarily tested four computational allocation schemes:

[0171] (1) All algorithms are deployed in the ARM core of the development board, with a main frequency of 660MHz, and are marked as SW;

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

[0173] (3) Deploy the matrix Lanczos decomposition onto the FPGA chip, and deploy other algorithms onto the ARM core, labeled as HW2;

[0174] (4) Deploy the MINRES algorithm onto the FPGA chip and deploy other algorithms onto the ARM core, labeled HW3;

[0175] Its deployment effect is as follows Figure 9 As shown, deploying the MINRES algorithm on an FPGA can significantly reduce computation time. With a prediction domain of 10, the computation time is approximately 8.1ms, while also improving the resource utilization of the FPGA.

[0176] This embodiment develops a set of NMPC solving algorithms that are suitable for low power consumption, low cost, and parallel computing. It uses the minimum residual method to solve the KKT matrix in the alternating direction multiplier method and deploys the minimum residual method on an FPGA chip. The product of the matrix and vector is accelerated by cyclic expansion and pipeline operation, which makes full use of the parallel characteristics of the FPGA chip and improves the real-time performance of solving NMPC problems.

[0177] Furthermore, one embodiment of the present invention also proposes a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the heterogeneous acceleration method for legged robot control as described in the above embodiment.

[0178] It should be noted that the logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Alternatively, the computer-readable medium may be paper or other suitable media on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in a computer memory.

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

[0180] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0181] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this invention, "a plurality of" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0182] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled 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 includes: In response to the user's input of the desired state command, the standard form of the alternating direction multiplier method is iteratively solved using the foot support force and torque as control input variables to obtain the optimal support force and torque; The optimal support force and torque 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 torque; The optimized joint torque is sent to each joint motor of the robot to control the movement of the supporting leg; Specifically, when iteratively solving the standard form of the alternating direction multiplier method, the NMPC solution algorithm deployed on a heterogeneous chip is used to iteratively solve the standard form of the alternating direction multiplier method. 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.

2. The heterogeneous acceleration method for legged robot control as described in claim 1, characterized in that, Before responding to the user-inputted desired state command and iteratively solving the standard form of the alternating direction multiplier method using foot support force and torque as control input variables to obtain the optimal support force and torque, the method further includes: Using the robot's desired state as the state variable and the force and torque of the robot's foot contacting the ground as the decision variable, a state-space dynamic equation expressed in the world coordinate system is established. Transform 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 discrete-time representation is converted into the standard form of the alternating direction multiplier method using the augmented state variables.

3. The heterogeneous acceleration method for legged robot control as described in claim 1, characterized in that, The standard form of the alternating direction multiplier method is: In the formula: , For indicator functions, X For augmented state variables, Z To introduce non-negative relaxation variables, f The coefficients of the linear term in the cost function are... H This is the coefficient matrix of the quadratic terms; A This is the coefficient matrix of the state variables for the equality constraints. C The coefficients of the inequality matrix are... A eq This is the intermediate matrix; It is the identity matrix; c The linear equality constraint vector. , This is the initial state. d The remainder of the inequality. T This is the transpose symbol.

4. The heterogeneous acceleration method for legged robot control as described in claim 1, characterized in that, The response to the user-inputted desired state command, using foot 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, including: The standard form of the alternating direction multiplier method is solved iteratively, and the residual is calculated after each iteration. Based on the residual, determine whether the termination iteration condition is met; If so, output the optimal support force and torque; If not, the KKT matrix in the alternating direction multiplier method is solved using the minimum residual method, and the robot's actual state variables are updated before the next iteration is performed.

5. The heterogeneous acceleration method for legged robot control as described in claim 4, characterized in that, The termination iteration condition satisfied by the residual is: In the formula: , For the set threshold, Norm symbol, , A This is the coefficient matrix of the state variables for the equality constraints. C The coefficients of the inequality matrix are... A eq This is the intermediate matrix; It is the identity matrix; c The linear equality constraint vector. , This is the initial state. d The remainder is the inequality remainder. Let Z be the value of the nonnegative relaxation variable in the k-th iteration. Let be the value of the augmented state variable in the k-th iteration.

6. The heterogeneous acceleration method for legged robot control as described in claim 4, characterized in that, The minimum residual method is a minimum residual algorithm applicable to parallel solution of linear equation systems.

7. The heterogeneous acceleration method for legged robot control as described in claim 1, characterized in that, The method further includes: In response to the user's input of desired state commands, plan the robot's landing point; The robot's foot trajectory was fitted using a Bezier curve, and the robot's swing leg trajectory was tracked using PD control.

8. A heterogeneous acceleration device controlled by a legged robot, characterized in that, The device includes: The response module is used to respond to the user's input of the desired state command. It uses the foot support force and torque as control input variables and iteratively solves the standard form of the alternating direction multiplier method to obtain 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 torque; The motion control module is used to send the optimized joint torque to the various joint motors of the robot to control the movement of the supporting leg; Specifically, when iteratively solving the standard form of the alternating direction multiplier method, the NMPC solution algorithm deployed on a heterogeneous chip is used to iteratively solve the standard form of the alternating direction multiplier method. 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 computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1-7.

10. An FPGA heterogeneous acceleration platform for predictive control solution of a legged robot model, characterized in that, It includes a real-time simulation system platform, an MPC controller deployed 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. The MPC controller is used to respond to the user's input of the desired state command. It uses the foot support force and torque as control input variables and iteratively solves the standard form of the alternating direction multiplier method to 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, and to update the robot's actual state variables; The MPC controller is also 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 torque; the optimized joint torque is sent to the robot's joint motors to control the movement of the support leg.

11. The FPGA heterogeneous acceleration platform for predictive control solving of legged robot models as described in claim 10, 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.

12. The FPGA heterogeneous acceleration platform for predictive control solving of legged robot models as described in claim 10, characterized in that, The linear system solver uses a COO storage method to store the coefficient matrix.

Images