Biped robot nonlinear model predictive controller FPGA heterogeneous acceleration method

Abstract

The invention relates to a biped robot nonlinear model predictive controller FPGA heterogeneous acceleration method, which comprises the following steps: establishing an NMPC model according to a biped robot dynamic model, iteratively solving a quadratic programming form of the NMPC model, and completing a single iteration process: solving by a primitive-dual interior point method and solving a symmetric linear equation set by a minres algorithm; the process of solving the solution of the linear equation set constructed by using the KKT condition based on the quadratic programming form through the primitive-dual interior point method is configured to be executed by a CPU; the process of solving the symmetric linear equation set by the minres algorithm is configured to be executed by the FPGA. According to different calculation characteristics of a solution operator in a biped robot NMPC model, a logic reusable minres algorithm in the solution process is deployed to an FPGA calculation platform, and nonlinear operation is deployed to a CPU for implementation, so that the calculation power characteristic of a heterogeneous chip is fully utilized. According to the method, robot dynamics modeling, an efficient optimization solution algorithm and appropriate computing power resource allocation are considered, and the acceleration effect and verification of the algorithm can be ensured.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to an FPGA heterogeneous acceleration method for a nonlinear model predictive controller of a biped robot. Background Art

[0002] Currently, deploying NMPC algorithms for bipedal robots in embedded systems faces several challenges. Implementing complex NMPC control algorithms requires a CPU with high computing power and low response latency, which hinders reducing controller hardware costs and size. Real-time predictive control of complex systems is constrained by memory space and computation time, making it difficult to rapidly iterate and find the optimal solution. This, to a certain extent, limits the application of fast model predictive controllers for real-time control problems. These issues hinder the development of real-time NMPC control for bipedal robots, necessitating innovative solutions to improve the controller's real-time performance and reduce power consumption and cost. This paper provides a bipedal robot NMPC controller based on a heterogeneous chip (CPU + FPGA). This heterogeneous system effectively improves overall computing power, enabling the rapid solution of complex control algorithms on embedded platforms. The direct solution of continuous-time optimal control problems primarily involves two stages: integration, i.e., solving ordinary differential equations (ODEs), and optimization. Because ODEs involve mathematical expressions, which require significant computational resources compared to standard addition and multiplication operations and are not suitable for pipeline operations, implementing integration on FPGAs is not advisable. To this end, the present invention separates the operators of the biped robot NMPC model optimization solution algorithm, and the specific implementation involves operations such as division, square root, trigonometric function, and matrix construction, which are deployed to the CPU for implementation. Considering that the iterative solution of the linear equation system can be effectively mapped to the hardware circuit, this part is deployed to the FPGA to achieve rapid solution of the optimization process. In addition, the solution algorithm adopts the primal-dual interior point method, and utilizes the sparsity and symmetry of the KKT matrix to reduce the storage capacity of temporary data in the controller. A scheduling algorithm is used in matrix-vector multiplication to reduce the data access dependency of multiplication and accumulation operations. These technical combinations provide an important technical foundation and support for the low-power, real-time solution of the biped robot NMPC algorithm on embedded chips, and will show great potential and value in various practical application scenarios.

[0003] Bipedal robots are legged robots that utilize two legs for locomotion. Most traditional robots based on wheels or tracks are typically unable to navigate complex terrain as freely as humans. In contrast, bipedal robots, with their two legs, possess the advantages of human morphology, resulting in unparalleled advantages in fields such as service and military. Furthermore, in inertial motion space, the tips of their legs maintain discontinuous contact with the ground, enabling them to adapt to rugged and complex terrain, such as uneven terrain and those filled with small obstacles. This allows them to traverse terrain more effectively than wheeled and tracked mobile robots. The emergence of bipedal robots has opened new possibilities for the development of robotics and further expanded the scope of robotic applications. They are currently widely used in various industries, including manufacturing, services, military, and healthcare. As research in bipedal robotics deepens, they are expected to play an increasingly important role in replacing human labor in complex and hazardous environments.

[0004] Many strategies are currently available for bipedal robot motion control, such as the zero moment point (ZMP) or the spring-loaded inverted pendulum (SLIP) model. Both approaches have successfully maintained stable bipedal robot motion. The hybrid zero dynamics control framework primarily utilizes input-output linearization, allowing dynamic walking in underactuated bipedal robots. Nonlinear model predictive control (NMPC) offers significant advantages in dynamic motion, as the controller can stabilize the system by predicting the optimal input. Force-based NMPC control has been introduced into bipedal robots, enabling them to perform a variety of dynamic gaits and maintain robustness over rough terrain. However, achieving effective dynamic motion requires the robot to adjust its state in real time based on sensor data to control its walking. This approach places high demands on the robot hardware and is difficult to apply. This is largely due to the fact that NMPC requires the online solution of a nonlinear programming problem. Solving nonlinear equations requires iteratively solving the system's differential equations, resulting in a heavy online real-time computational burden and poor real-time performance, making it difficult to apply to practical fast dynamic systems. To address the problem of poor real-time solution, we consider using the parallel processing capability of FPGA on heterogeneous chips to achieve fast NMPC solution, thereby improving the real-time performance of its online calculation and realizing stable motion control of the bipedal robot.

[0005] To improve the solution speed of NMPC models, several strategies for implementing NMPC algorithms on FPGAs have emerged. A Chinese invention patent discloses a method for implementing an FPGA hardware-accelerated controller for nonlinear predictive control (Xu Fang, Mei Qin, Li Zongli, Ji Dongdong, Chen Hong. "A FPGA Hardware-Accelerated Controller for Nonlinear Predictive Control and Its Accelerated Implementation Method," Application No. 201610418142.9, Application Date: June 13, 2016). This invention establishes a predictive control model for a wheeled mobile robot (WMR) to solve an optimization problem while satisfying the WMR model's trajectory constraints. It then utilizes a particle swarm optimization (PSO) algorithm to solve the nonlinear programming problem. This approach effectively combines the parallel computing structure of FPGAs with the parallel computing characteristics of the PSO algorithm, improving the fast computing capability of the NMPC algorithm and meeting the real-time requirements of the controller. However, the PSO algorithm is highly sensitive to parameters and can easily get stuck in local optimal solutions. Numerical solutions to NMPC problems often involve complex functions, which may have different underlying mathematical operations, varying evaluation complexity, and irregular structures. Irregularity limits the reuse of computational logic and the acceleration of parallelization, and it requires a large amount of computing resources, making it unsuitable for pipeline operations. Furthermore, a Chinese invention patent discloses a trajectory tracking control method for autonomous vehicles based on MPC and FPGA (Tang Hao, Tang Xiaoming, Yu Zhaojin, and Li Wei, "A Trajectory Tracking Control Method for Autonomous Vehicles Based on MPC and FPGA," Application Number: CN202211578769.2, Application Date: December 5, 2022). This invention deploys the algorithm on the heterogeneous ZYNQ-7000 chip, solving the model predictive control algorithm optimization problem using the operator splitting quadratic programming (OSQP) algorithm. This improves the computational efficiency of the problem and reduces the complexity of the algorithm's implementation on actual autonomous vehicles. However, this method does not flexibly distribute the algorithm workload between software and hardware to balance computing resource usage and performance. The complex matrix construction and matrix decomposition algorithms involved in the solution process typically involve numerous nonlinear operators, making them difficult to directly map to hardware and consuming significant chip resources. Summary of the Invention

[0006] In view of the problems in the prior art, the present invention provides an FPGA heterogeneous acceleration method for a nonlinear model predictive controller of a biped robot.

[0007] The present invention provides an FPGA heterogeneous acceleration method for a bipedal robot nonlinear model predictive controller, establishes a bipedal robot dynamics model, establishes an NMPC model based on the bipedal robot dynamics model, and discretizes the NMPC model using a direct transcription method to obtain a quadratic programming form by discrete integral intervals. The quadratic programming form of the NMPC model is iteratively solved, and the single iteration process is completed: The primal-dual interior point method solves the linear equations constructed based on the quadratic programming form using the KKT condition. If the preset number of iterations is not reached, the symmetric linear equations are solved using the minres algorithm. If the preset number of iterations is reached, the solution is output. In the process of solving the symmetric linear equation system by the Minres algorithm, if the residual satisfies the preset conditions, the parameters of the primal-dual interior point method are updated and the next round of iteration is started; otherwise, the process of solving the symmetric linear equation system by the Minres algorithm is repeated; Among them, the process of solving the linear equation system constructed based on the quadratic programming form using the KKT condition by the primal-dual interior point method is configured to be executed by the CPU; the process of solving the symmetric linear equation system by the minres algorithm is configured to be executed by the FPGA.

[0008] Preferably, the process of updating the parameters of the primal-dual interior point method is configured to be executed by a CPU.

[0009] Preferably, the biped robot dynamic model is:

[0010] in, , represents the robot body roll angle, is the pitch angle, is the yaw angle. represents the center of mass position of the robot, represents the center of mass velocity, represents the angular velocity, is the acceleration due to gravity, The variables to be solved for NMPC optimization represent the force and torque between the robot's foot and the ground, where , , are the forces and moments acting on the foot, Same thing. The position vector of the robot's foot, Indicates quality, is the moment of inertia of the robot in the center of mass coordinate system, represents the skew-symmetric matrix obtained by subtracting the center of mass and foot positions, given , then the skew-symmetric matrix is .

[0011] , ,

[0012] , .

[0013] Preferably, the NMPC model is: The NMPC problem can be written in the following standard form:

[0014] Constraints , is an inequality constraint, which represents the range of contact force and torque between the foot and the ground. Direction no more than , The direction and torque do not exceed the maximum values ​​supported by the structure and motor. ,in , which determines that the optimal control input of the swing leg is the zero vector. is the desired state, generated by the leg support state planner. Forecast duration. is the state weight matrix. is the input weight matrix.

[0015] Preferably, the NMPC model is converted into a quadratic programming form:

[0016] in, Its harmony denote the horizontal length and sampling time respectively, is the middle vector of the trapezoidal integrator, , ,but ,in , Represents a discrete interval, and after discretization and addition, the following form can be obtained:

[0017] in , is the number of discrete points of the direct transcription method, and similarly the inequality constraint , equality constraints Discretize into N intervals, add constraints on state variables and input torque, and obtain the constraint matrix.

[0018] Based on the different computational characteristics of the solution operators in the bipedal robot NMPC model, this application deploys the logically reusable Minres algorithm in the solution process to the FPGA computing platform and deploys the nonlinear operations to the CPU to fully utilize the computing power characteristics of heterogeneous chips. This takes into account the robot dynamics modeling, efficient optimization solution algorithm, and appropriate computing resource allocation to ensure the effectiveness and verification of algorithm acceleration. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1is a schematic diagram of the control architecture of the bipedal robot of the present invention; Figure 2 A simplified dynamic model of the biped robot of the present invention; Figure 3 This is a flow chart of the FPGA heterogeneous acceleration method for the biped robot nonlinear model predictive controller of the present invention; Figure 4 is a flow chart of the MINRES solver of the present invention; Figure 5 This is a flowchart of the processor-in-the-loop test of the FPGA heterogeneous acceleration method for the bipedal robot nonlinear model predictive controller of the present invention implemented with FPGA; Figure 6 This is a flow chart of the algorithm generation process using SPLIT and Protoip toolboxes in the present invention; Figure 7 Schematic diagram of computing resource allocation for the FPGA heterogeneous acceleration method of the biped robot nonlinear model predictive controller of the present invention. DETAILED DESCRIPTION

[0020] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. In this specification, the size ratios in the drawings do not represent the actual size ratios, but are only used to reflect the relative positional relationship and connection relationship between the various components. Components with the same name or the same number represent similar or identical structures and are only for illustrative purposes.

[0021] Bipedal robot control architecture Figure 1 As shown. The robot receives the navigation path required by the upper layer instruction, generates the desired state through the planner according to the instruction and the surrounding environment, and the desired state forms a column vector, including the desired body center of mass position , required body center of mass velocity , the required rotation matrix and the angular velocity required for the robot body Then the accelerator is solved by NMPC to obtain the ground force, and the joint torque is obtained by the WBC controller. . When the robot is in the swing phase, the legs do not exert ground contact force and are therefore not controlled by the force and torque based NMPC. In order to control the position of the legs and feet in each gait cycle, the required foot trajectory is controlled in Cartesian space by a swing phase planner. Finally, the robot evaluates its own state based on the sensor data and continues the next control cycle, thus completing a motion control cycle of the bipedal robot. The algorithm acceleration object of the embodiment is the bipedal robot NMPC controller. The present invention deploys the core component NMPC solution module on the heterogeneous chip ZYNQ, that is, the algorithm solution acceleration task is completed by allocating relevant algorithms on the CPU and FPGA.

[0022] The NMPC accelerator algorithm flow is as follows Figure 3 As shown, the process is divided into the following steps: Step 1: Build a dynamic model of the bipedal robot In order to absorb the frequent and strong impacts during dynamic movements, bipedal robots usually use lightweight limbs and links. This makes the weight and rotational inertia of each link very small compared to the weight and rotational inertia of the body. Therefore, the influence of the leg links on the robot dynamics can be ignored. Considering the number of contact points between each robot foot and the ground, the contact position, and the contact force and torque at each contact point, Figure 2 is a simplified model of the robot, which serves as the simplified dynamic design of this framework. The dynamic model of the model is:

[0023] in, , represents the robot body roll angle, is the pitch angle, is the yaw angle. represents the center of mass position of the robot, represents the center of mass velocity. Represents angular velocity. The variables to be solved for NMPC optimization represent the forces and moments between the robot's foot and the ground. The position vector of the robot's foot, Indicates quality, is the moment of inertia of the robot in the center of mass coordinate system, represents the skew-symmetric matrix obtained by subtracting the center of mass and foot positions, given , then the skew-symmetric matrix is .

[0024] , ,

[0025] , , is the acceleration due to gravity.

[0026] Step 2: Establish NMPC (nonlinear model predictive control) model In the prediction time The NMPC problem in can be written in the following standard form:

[0027] Constraints , is an inequality constraint that limits the contact force between the robot's foot and the ground in all directions, indicating the range of the contact force and torque between the foot and the ground. Direction no more than , The directional torque shall not exceed the maximum value supported by the structure and motor respectively. ,in The equality constraint determines that the optimal control input from the NMPC controller is the zero vector of the swing leg. is the desired state, generated by the leg support state planner. is the state weight matrix. is the input weight matrix.

[0028] Step 3: Discretize the integral interval using direct transcription method The NMPC model is discretized and transformed into:

[0029] in, Its harmony denote the horizontal length and sampling time respectively, is the middle vector of the trapezoidal integrator, , ,but ,in , Represents a discrete interval. This method discretizes the prediction time period. The initial state vector and input vector for each time period are independent of other time periods and are used as optimization variables. The ODE equation can be used to represent the state changes in each time period, and the integral area is then represented by a trapezoidal integrator. To this end, the above formula can be expressed as follows:

[0030] in , is the number of discrete points of the direct transcription method, and similarly the inequality constraint , equality constraints Discretize into N intervals, add constraints on state variables and input torque, and obtain the constraint matrix.

[0031] Step 4: Primal-dual interior point method solution The quadratic programming problem is transformed into an unconstrained optimization problem by introducing new free variables through the Lagrangian function, and the KKT condition is used to construct a linear equation system:

[0032] in is a sparse symmetric matrix. Next, the iterative solution process is carried out. The specific calculation method is as follows: 1 Set the initial value iteration parameter ,in is the vector to be solved, is the equality constraint Lagrange multiplier vector, is the inequality constraint Lagrange multiplier vector, is the relaxation vector, ,parameter , number of iterations ,calculate , in , , , , and They are and A diagonal matrix consisting of .

[0033]

[0034]

[0035] in

[0036] Step 5: Minres algorithm solves linear equations Use Figure 4 The algorithm flow shown is performed in FPGA The MINRES algorithm is based on the Lanczos kernel and performs recursive solution. Each iteration requires only two scalar divisions and two scalar square root calculations, which can be effectively mapped to the hardware. The residual error during the iteration is set to .

[0037] Step 6: Update iterative parameters to obtain the final solution After obtaining the solution to the linear equation, the original dual interior point method parameters need to be updated: , , ,

[0038] The optimal input parameters of the biped robot NMPC controller can be obtained after meeting the number of iterations.

[0039] The present invention uses the Xilinx Zynq-7000 hardware platform to implement model predictive control algorithm acceleration. The chip ARM core is mainly responsible for collecting sensor data, generating the desired state, constructing the KKT matrix and storing it in COO format. Then, the data is transferred from the memory to the logic end of the chip through the AXI interface. The logic end implements the minres algorithm. After the matrix iterative calculation is completed, the result is fed back to the ARM core for processing. Figure 5 The platform shown here verifies the acceleration effect of the actual algorithm. The PC calls Ethernet through MATLAB to send status data and control instructions to the ARM side. The ARM side receives the data and performs nonlinear operations to construct a sparse matrix. It then passes the data to the FPGA and receives the solution results of the Minres algorithm to update the iteration parameters. After meeting the number of iterations of the outer primal-dual interior point method, the optimal input variables for the solution can be obtained. At the same time, the platform records the overall algorithm calculation time to verify the acceleration effect. The results are shown in the following table:

[0040] The present invention uses the SPLIT toolbox and PROTOIP tool to quickly deploy the algorithm and adjust the workload between the CPU and FPGA to generate an NMPC algorithm acceleration project suitable for a bipedal robot model. The code flow generated using the SPLIT and PROTOIP tools is as follows: Figure 6 As shown in Figure 1, SPLIT is a code generation tool that uses operator splitting for embedded optimization. It supports code generation for CPUs, FPGAs, and heterogeneous platforms and provides a Matlab interface. PROTOIP is an open-source framework that abstracts many low-level FPGA design details and provides engineers with custom templates, scripts, example designs, and tutorials specifically tailored for embedded optimization applications. The detailed steps for code generation deployment are as follows: Step 1.1 Define state variables and input variables.

[0041] Step 1.2 Define the optimization objective equation Step 1.3 Define a set of ordinary differential equations to represent the state changes of the model Step 1.4 Define the equality constraints using slack variables (functions of x, u, and s). Step 1.5 Use constants to define some inequality constraints Step 2.1 Select a solution algorithm Step 2.2 Use the corresponding MATLAB interface of SPLIT Step 2.3 Modify iteration parameters and termination conditions Step 2.4 Adjust the algorithm deployment between CPU and FPGA Step 3.1 Use the MATLAB interface of PROIOIP Step 3.2 Code Generation and Debugging Step 3.3 Deploy to the hardware platform To improve the comprehensive computing power of heterogeneous chips, such as Figure 7 As shown in Figure 2, the computational work is distributed between the general-purpose CPU and the FPGA. The primal-dual interior point algorithm solves the NLP problem by incorporating inequality constraints into the objective function using a logarithmic barrier function scaled by the barrier parameter. and The problem is that the problem is often repetitively constructed, and these often have irregular structures. Furthermore, the Hessian matrix must be approximated using the Gauss-Newton method. In these cases, the matrix is ​​not conducive to effective acceleration. Therefore, the solution was considered to be implemented on a CPU. Given the iterative nature of linear equation solvers, which facilitates reuse of computational logic, the entire linear system solver was accelerated in hardware.

[0042] The SPLIT tool generates C code and acceleration instructions (pipelining, loop unrolling, etc.) suitable for high-level synthesis (HLS). For the FPGA implementation in this project, only internal on-chip memory is used for data storage, and external memory is used for input / output data transfer. When consecutive iterations are independent of each other, vector-to-vector operations within the algorithm can often be accelerated by pipelined iterations, which can be achieved using pipeline instructions. However, for some vector operations with read-write data dependencies, pipelining is not straightforward. Therefore, rewriting the C code to independently calculate the partial sums of vector elements first avoids data dependencies and creates an efficient pipeline. After the partial sums are completed, the final loop accumulates the partial sums. When performing sparse matrix-vector operations, the tool schedules the non-zero matrix elements on the CPU in a specific order, generating a COO format. This avoids data dependencies during operations and reduces FPGA resource usage.

[0043] Existing NMPC model solving methods place high demands on robot hardware. Realizing real-time robot state adjustments based on sensor data and achieving optimal dynamic motion is challenging. This invention combines robot dynamics modeling, efficient optimization algorithms, and appropriate computing resource allocation to ensure algorithm acceleration and verification. Key innovations include: 1. The prediction time domain of the biped robot NMPC model is discretized using the direct transcription method and converted into a quadratic programming form. The primal-dual interior point method is then used to greatly improve the solution efficiency and the KKT symmetric matrix is ​​constructed to reduce the memory required for calculation.

[0044] 2. Using the operator separation method, based on the different computational characteristics of the solution operators in the bipedal robot NMPC model, the logically reusable Minres algorithm in the solution process is deployed to the FPGA computing platform, and the nonlinear operations are deployed to the CPU for implementation, so as to fully utilize the computing power characteristics of heterogeneous chips.

[0045] 3. Use the SPLIT and PROIOIP toolboxes to quickly generate and deploy the code for the bipedal robot NMPC solution algorithm, reducing the difficulty of deploying the bipedal robot NMPC model to hardware and enhancing development efficiency.

[0046] The above content only describes the preferred embodiments of the present invention and does not limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made to the technical solution of the present invention by ordinary technicians in this field should fall within the scope of protection determined by the claims of the present invention.

Claims

1. A biped robot nonlinear model predictive controller FPGA heterogeneous acceleration method, characterized by: A bipedal robot dynamics model is established, and an NMPC model is established based on the bipedal robot dynamics model. The NMPC model is discretized using the direct transcription method to obtain a quadratic programming form by discrete integral intervals. The quadratic programming form of the NMPC model is iteratively solved, and the single iteration process is completed: The primal-dual interior point method solves the linear equations constructed based on the quadratic programming form using KKT conditions; If the preset number of iterations is not reached, the symmetric linear equations are solved using the minres algorithm. If the preset number of iterations is reached, the solution is output; In the process of solving the symmetric linear equation system by the Minres algorithm, if the residual satisfies the preset conditions, the parameters of the primal-dual interior point method are updated and the next round of iteration is started; otherwise, the process of solving the symmetric linear equation system by the Minres algorithm is repeated; The process of solving the linear equation system constructed by the primal-dual interior point method using KKT conditions based on the quadratic programming form is configured to be executed by the CPU; The process of solving symmetric linear equations by the minres algorithm is configured for FPGA execution.

2. The FPGA heterogeneous acceleration method for a bipedal robot nonlinear model predictive controller according to claim 1, characterized in that: The process of updating the parameters of the primal-dual interior point method is configured for CPU execution.

3. The FPGA heterogeneous acceleration method for a bipedal robot nonlinear model predictive controller according to claim 1, characterized in that: The dynamic model of the biped robot is: in, , represents the robot body roll angle, is the pitch angle, is the yaw angle. represents the center of mass position of the robot, represents the center of mass velocity, represents the angular velocity, is the acceleration due to gravity, The variables to be solved for NMPC optimization represent the force and torque between the robot's foot and the ground, where , , are the forces and moments acting on the foot, Similarly, The position vector of the robot's foot, Indicates quality, is the moment of inertia of the robot in the center of mass coordinate system, represents the skew-symmetric matrix obtained by subtracting the center of mass and foot positions, given , then the skew-symmetric matrix is , , , , ， The state space equation is: in The matrix can be obtained by It is calculated,using the average yaw value during the entire reference trajectory,as a constant matrix; The matrix can be based on Calculated, using the desired average yaw and foot position values.

4. The FPGA heterogeneous acceleration method for a bipedal robot nonlinear model predictive controller according to claim 1, characterized in that: The NMPC model is: The NMPC problem can be written in the following standard form: Constraints , is an inequality constraint, which represents the range of contact force and torque between the foot and the ground. Direction no more than , Direction, torque does not exceed the maximum value supported by the structure and motor, respectively. For equation c, it is a zero vector. is the desired state, generated by the leg support state planner, To predict the duration, is the state weight matrix, is the input weight matrix.

5. The FPGA heterogeneous acceleration method for a bipedal robot nonlinear model predictive controller according to claim 3, characterized in that: The continuous-time NMPC model is discretized into a nonlinear programming form using direct transcription: in, Its harmony denote the horizontal length and sampling time respectively, is the middle vector of the trapezoidal integrator, , ,but ,in , Represents a discrete interval, and after discretization and addition, the following standard quadratic programming form can be obtained: in , is the number of discrete points of the direct transcription method, and similarly the inequality constraint , equality constraints Discretize into N intervals, add constraints on state variables and input torque, and obtain the constraint matrix.

Images