Steering and torque distribution integrated control method for four-wheel independent drive vehicle based on nonlinear predictive control

Abstract

The invention provides a steering and torque distribution integrated control method for a four-wheel independent drive vehicle based on nonlinear predictive control, which comprises the following steps of: firstly, establishing a nonlinear state-space equation of the four-wheel independent drive vehicle, and establishing an optimal control problem based on the state-space equation; and then the optimal control problem of the original continuous time system is converted into a nonlinear optimization problem by using a forward Euler method and a multiple targeting method, and the singularity of the nonlinear optimization problem is effectively eliminated and the convexity of the nonlinear optimization problem is improved by adding a regular term in an optimization target. Finally, a moderate-scale sequential quadratic programming mode is designed, and compared with the prior art adopting hierarchical nonlinear predictive control, an open source solver IPOPT and the like, the control precision and the solving speed can be effectively improved.

Description

Technical Field

[0001] This invention belongs to the field of yaw motion control technology for vehicle trajectory tracking. Specifically, it involves using nonlinear predictive control technology to simultaneously calculate the steering and torque output of a four-wheel independently driven vehicle, thereby achieving integrated control technology for the steering and torque of the four-wheel independently driven vehicle. Background Technology

[0002] Currently, vehicle yaw motion control plays a crucial role in improving vehicle stability and safety in the field of autonomous driving. By independently controlling the torque of each of the four wheels, it can perform precise and complex driving maneuvers, making it particularly suitable for complex conditions such as high-speed driving and emergency obstacle avoidance. Most existing vehicle yaw motion control technologies either do not consider torque distribution or separate torque distribution from wheel angle control, while employing linearized vehicle models to simplify control. However, this approach suffers from poor model accuracy and performs poorly under high-speed and extreme conditions. Summary of the Invention

[0003] In view of this, and to address the technical problems existing in this field, the present invention provides an integrated control method for steering and torque distribution of a four-wheel independent drive vehicle based on nonlinear predictive control, specifically including the following steps:

[0004] Step 1: Design the optimal control problem for the yaw motion of the unmanned vehicle, and establish a dynamic model including the speed, sideslip angle, and yaw rate of the unmanned vehicle. The classic magic tire formula MF98 is used as the tire model.

[0005] Step 2: Using the forward Euler method and the multiple target method, the optimal control problem is processed into a nonlinear optimization problem, and the optimization objective, nonlinear equality constraints, and linear inequality constraints of the nonlinear optimization are defined.

[0006] Step 3: Add a regularization term to the optimization objective to eliminate the singularity of the nonlinear optimization problem;

[0007] Step 4: Offline generation of the objective function, objective gradient (i.e., Jacobi vector function), objective second gradient (i.e., Hessian matrix function), equality constraint function, and equality constraint gradient (i.e., Jacobi matrix function) to save online computation time.

[0008] Step 5: Perform online calculations to directly obtain the accurate Hessian matrix; or use the BFGS method to iteratively calculate an approximate Hessian matrix.

[0009] Step 6: Use the Hessian matrix obtained in Step 5 to define and solve the quadratic programming subproblem;

[0010] Step 7: Perform Armijo line search based on backstepping to obtain the search step size, and determine convergence based on the set convergence conditions; if convergence is achieved, output the optimization result; if convergence is not achieved, return to step 5.

[0011] Furthermore, step one specifically defines the optimal control problem in the following form:

[0012]

[0013] Among them, J ocp The objective function of the optimal control problem is given by the state variables x = [v, β, ω], where v, β, and ω represent the vehicle's speed, sideslip angle, yaw rate, and wheel angle, respectively. The control input is u = [δ, F]. xf1 ,F xf2 ,F xr1 ,F xr2 ], δ is the wheel rotation angle, F xf1 F xf2 F xr1 、,F xr2 Let be the longitudinal forces of the left and right front wheels and the rear wheels, respectively; t be the time variable; x0 be the initial state corresponding to the optimal control problem; and the superscript · represents the first derivative of the corresponding parameter. Also:

[0014] φ(x,u)=a1(vv des ) 2 +a2(ω-ω des ) 2 +a3β 2

[0015]

[0016] The vehicle speed, sideslip angle, and yaw rate models are as follows:

[0017]

[0018] In the formula, mass and m are both the mass and inertial of the vehicle. z F is the angular inertia of the vehicle about the z-axis center. yf1 F yf2 F yr1 、,F yr2 These represent the lateral forces on the left and right front wheels and the rear wheels, respectively. f l r These are the longitudinal distances from the vehicle's center of gravity to the front and rear axles, respectively. y This refers to the vehicle's lateral dimensions;

[0019] Lateral tire force F y Calculated based on the following formula:

[0020] F y =μF z D sin(C arctan(sB-E(sB-arctan(sB))))

[0021] Where B, C, D, and E are parameters of the Magic Tire formula, μ is the maximum ground adhesion coefficient, and F... z is the vertical load on the tire, and s is the lateral slip ratio of the tire;

[0022] The lateral slip ratio of the four wheels is expressed as follows:

[0023]

[0024] Furthermore, in step two, the forward Euler method and the multiple-target method are used to obtain the following nonlinear optimization problem:

[0025]

[0026] In the formula, J nlp Let be the objective function of the nonlinear optimization problem, k represent the current time, Δt be the time interval between sampling points in the multiple shooting method, lbx and ubx are the upper and lower bounds of the state variables, and lbu and ubu are the upper and lower bounds of the control input variables.

[0027] Simplifying the above equation, we get:

[0028] Minimize f nlp (r)

[0029] stf eq (r)=0

[0030] lbr≤r≤ubr

[0031] Among them, f eq (r) is a column vector function mapping composed of the above nonlinear programming equality constraints, and the specific form of variable r is:

[0032] r = [x(0)] T ,u(0) T ,x(1) T ,u(1) T ,...,x(N-1) T ,u(N-1) T ,x(N) T ] T

[0033] In the formula, N is the number of time sub-intervals.

[0034] Furthermore, step three involves adding regularization terms of the following form to the optimization objective:

[0035]

[0036] Where ζ1 and ζ2 are both coefficients.

[0037] Furthermore, in step four, the open-source automatic differentiation library Casadi is used to generate the symbolic expression of the objective function for optimization offline, and the other functions are calculated in turn. Finally, the code generation function is used to generate a .c format file.

[0038] Furthermore, in step five, the approximate Hessian matrix is ​​calculated by performing the following iterative process of the BFGS method:

[0039]

[0040] Where α is the previous search step size, Δr is the previous search direction, r is the previous solution, B is the previous Hessian matrix or approximate Hessian matrix, and r new For the current solution, B new For the new approximate Hessian matrix; the Lagrange function is defined as:

[0041]

[0042] Where λ is a Lagrange multiplier, λ new Its current value.

[0043] Furthermore, the specific process of solving the quadratic programming subproblem in step six includes:

[0044] First, define the set of equality constraints ε and the set of inequality constraints I for the quadratic programming subproblem, and define them respectively:

[0045]

[0046] Then, define a i ,i∈ε represents A eq In the i-th line, define a i ,i∈I represents A ineq The i-th row;

[0047] For the valid set obtained in the previous iteration First, solve the following problem:

[0048]

[0049] In the formula, d is the solution to the problem. To obtain a feasible solution

[0050] Define I = I1 ∪ I2, and we get the following two sets of inequality constraints:

[0051]

[0052] Then, with To find a feasible initial solution, the following problem is solved using the effective set method:

[0053]

[0054] In the formula, M is a sufficiently large penalty parameter used to reduce the infeasibility of the problem. When the problem is feasible and M is sufficiently large, the solution to the quadratic programming problem is a feasible solution to the original problem.

[0055] In each iteration of the effective set method, a quadratic programming problem with only equality constraints is computed, denoted as:

[0056]

[0057] in, This is the current valid set;

[0058] Using the previously obtained approximate Hessian matrix, the following explicit solution is obtained:

[0059]

[0060] Where A, b are row vectors A matrix concatenated column-wise, λ sub The Lagrange multipliers are the optimal solutions to the quadratic programming subproblem under the current effective set;

[0061] Finally, after the quadratic programming subproblem converges, the following update formula is obtained:

[0062] Δr=d

[0063] Δλ=λ sub -λ.

[0064] Furthermore, the specific method for performing the line search in step seven is as follows:

[0065] The result Δr of the kth iteration k As the search direction, we seek a suitable α such that it satisfies:

[0066]

[0067] The convergence condition is set as follows:

[0068] Δr k ≤erro max Or iter ≥ iter max

[0069] Among them, erro maxIt is the maximum allowable error, iter, iter max These represent the current iteration number and the maximum allowed iteration number, respectively.

[0070] The integrated control method for steering and torque distribution of a four-wheel independently driven vehicle based on nonlinear predictive control provided by the present invention first establishes the nonlinear state-space equations of the four-wheel independently driven vehicle and then establishes the optimal control problem based on these equations. Next, the optimal control problem of the original continuous-time system is transformed into a nonlinear optimization problem using the forward Euler method and the multiple-shot method. By adding a regularization term to the optimization objective, the singularity of the nonlinear optimization problem is effectively eliminated and its convexity is improved. Finally, the present invention designs a moderately sized sequential quadratic programming approach, which, compared with existing technologies using hierarchical nonlinear predictive control and open-source solvers such as IPOPT, can effectively improve control accuracy and solution speed. Attached Figure Description

[0071] Figure 1 This provides the overall framework for the method provided by the present invention;

[0072] Figure 2 To compare the effects before and after regularization;

[0073] Figure 3 This is a simulation error comparison between an example of the present invention and an existing hierarchical nonlinear predictive control method;

[0074] Figure 4 This is an example of the present invention and a comparison of the simulation computation time with the open-source solver IPOPT. Detailed Implementation

[0075] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. 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.

[0076] The integrated control method for steering and torque distribution of four-wheel independent drive vehicles based on nonlinear predictive control provided by this invention has the following overall framework: Figure 1 As shown, the method specifically includes the following steps:

[0077] Step 1: Design the optimal control problem for the yaw motion of the unmanned vehicle, and establish a dynamic model including the speed, sideslip angle, and yaw rate of the unmanned vehicle. The classic magic tire formula MF98 is used as the tire model.

[0078] Step 2: Using the forward Euler method and the multiple target method, the optimal control problem is processed into a nonlinear optimization problem, and the optimization objective, nonlinear equality constraints, and linear inequality constraints of the nonlinear optimization are defined.

[0079] Step 3: Add a regularization term to the optimization objective to eliminate the singularity of the nonlinear optimization problem;

[0080] Step 4: Offline generation of the objective function, objective gradient (i.e., Jacobi vector function), objective second gradient (i.e., Hessian matrix function), equality constraint function, and equality constraint gradient (i.e., Jacobi matrix function) to save online computation time.

[0081] Step 5: Perform online calculations to directly obtain the accurate Hessian matrix; or use the BFGS method to iteratively calculate an approximate Hessian matrix.

[0082] Step 6: Use the Hessian matrix obtained in Step 5 to define and solve the quadratic programming subproblem;

[0083] Step 7: Perform Armijo line search based on backstepping to obtain the search step size, and determine convergence based on the set convergence conditions; if convergence is achieved, output the optimization result; if convergence is not achieved, return to step 5.

[0084] In a preferred embodiment of the present invention, step one specifically defines the following type of optimal control problem:

[0085]

[0086] Among them, J ocp The objective function of the optimal control problem is given by the state variables x = [v, β, ω], where v, β, and ω represent the vehicle's speed, sideslip angle, yaw rate, and wheel angle, respectively. The control input is u = [δ, F]. xf1 ,F xf2 ,F xr1 ,F xr2 ], δ is the wheel rotation angle, F xf1 F xf2 F xr1 、,F xr2 Let be the longitudinal forces of the left and right front wheels and the rear wheels, respectively; t be the time variable; a0 be the initial state corresponding to the optimal control problem; and the superscript · represents the first derivative of the corresponding parameter. Furthermore:

[0087] φ(x,u)=a1(vv des ) 2 +a2(ω-ω des ) 2 +a3β 2

[0088]

[0089] The vehicle speed, sideslip angle, and yaw rate models are as follows:

[0090]

[0091] In the formula, mass and m are both the mass and inertial of the vehicle. z F is the angular inertia of the vehicle about the z-axis center. yf1 F yf2 F yr1 、,F yr2 These represent the lateral forces on the left and right front wheels and the rear wheels, respectively. f l r These are the longitudinal distances from the vehicle's center of gravity to the front and rear axles, respectively. y This refers to the vehicle's lateral dimensions;

[0092] Lateral tire force F y Calculated based on the following formula:

[0093] F y =μF z D sin(C arctan(sB-E(sB-arctan(sB))))

[0094] Where B, C, D, and E are parameters of the Magic Tire formula, μ is the maximum ground adhesion coefficient, and F... z is the vertical load on the tire, and s is the lateral slip ratio of the tire;

[0095] The lateral slip ratio of the four wheels is expressed as follows:

[0096]

[0097] In a preferred embodiment of the present invention, the following nonlinear optimization problem is obtained in step two using the forward Euler method and the multiple target method:

[0098]

[0099] In the formula, J nlp Let be the objective function of the nonlinear optimization problem, k represent the current time, Δt be the time interval between sampling points in the multiple shooting method, lbx and ubx are the upper and lower bounds of the state variables, and lbu and ubu are the upper and lower bounds of the control input variables.

[0100] Simplifying the above equation, we get:

[0101] Minimize f nlp (r)

[0102] stf eq (r)=0

[0103] lbr≤r≤ubr

[0104] Among them, f eq (r) is a column vector function mapping composed of the above nonlinear programming equality constraints, and the specific form of variable r is:

[0105] r = [x(0)] T ,u(0) T ,x(1) T ,u(1) T ,...,x(N-1) T ,u(N-1) T ,x(N) T ] T

[0106] In the formula, N is the number of time sub-intervals.

[0107] In a preferred embodiment of the present invention, step three specifically adds a regularization term of the following form to the optimization objective:

[0108]

[0109] Where ζ1 and ζ2 are both coefficients.

[0110] In a preferred embodiment of the present invention, step four specifically utilizes the open-source automatic differentiation library casadi to generate the symbolic expression of the optimization objective function offline, calculates other functions in sequence, and finally uses the code generation function to generate a .c format file.

[0111] In a preferred embodiment of the present invention, step five specifically calculates the approximate Hessian matrix by performing the following iterative process of the BFGS method:

[0112]

[0113] Where α is the previous search step size, Δr is the previous search direction, r is the previous solution, B is the previous Hessian matrix or approximate Hessian matrix, and r new For the current solution, B new For the new approximate Hessian matrix; the Lagrange function is defined as:

[0114]

[0115] Where λ is a Lagrange multiplier, λ new Its current value.

[0116] In a preferred embodiment of the present invention, the specific process of solving the quadratic programming subproblem in step six includes:

[0117] First, define the set of equality constraints ε and the set of inequality constraints I for the quadratic programming subproblem, and define them respectively:

[0118]

[0119] Then, define a i ,i∈ε represents A eq In the i-th line, define a i ,i∈I represents A ineq The i-th row;

[0120] For the valid set obtained in the previous iteration First, solve the following problem:

[0121]

[0122] In the formula, d is the solution to the problem. To obtain a feasible solution

[0123] Define I = I1 ∪ I2, and we get the following two sets of inequality constraints:

[0124]

[0125] Then, with To find a feasible initial solution, the following problem is solved using the effective set method:

[0126]

[0127] In the formula, M is a sufficiently large penalty parameter used to reduce the infeasibility of the problem. When the problem is feasible and M is sufficiently large, the solution to the quadratic programming problem is a feasible solution to the original problem.

[0128] In each iteration of the effective set method, a quadratic programming problem with only equality constraints is computed, denoted as:

[0129]

[0130] in, This is the current valid set;

[0131] Using the previously obtained approximate Hessian matrix, the following explicit solution is obtained:

[0132]

[0133] Where A, b are row vectors A matrix concatenated column-wise, λ subThe Lagrange multipliers are the optimal solutions to the quadratic programming subproblem under the current effective set;

[0134] Finally, after the quadratic programming subproblem converges, the following update formula is obtained:

[0135] Δr=d

[0136] Δλ=λ sub -λ.

[0137] In a preferred embodiment of the present invention, the specific method for performing line search in step seven is as follows:

[0138] The result Δr of the kth iteration k As the search direction, we seek a suitable α such that it satisfies:

[0139]

[0140] In practical implementation, α = 0.7 can be chosen. m , where m is the number of searches performed by the line search;

[0141] The convergence condition is set as follows:

[0142] Δr k ≤erro max Or iter ≥ iter max

[0143] Among them, erro max It is the maximum allowable error, iter, iter max These represent the current iteration number and the maximum allowed iteration number, respectively.

[0144] In an example based on this invention, simulation software is used to model the yaw motion control problem of an autonomous vehicle, and algorithm simulation is performed. Figure 2 The non-zero elements of the Hessian matrix before and after regularization are given. Figure 3 The simulation results and the error comparison with existing techniques employing hierarchical nonlinear predictive control algorithms are shown. Figure 4 The simulation comparison of the computation time of the present invention and the open-source solver IPOPT is shown. The figure clearly reflects the advantages of the present invention in terms of solution accuracy and time compared with the prior art.

[0145] It should be understood that the sequence number of each step in the embodiments of the present invention does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0146] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. An integrated control method for steering and torque distribution of a four-wheel independent drive vehicle based on nonlinear predictive control, characterized in that: Specifically, the following steps are included: Step 1: Design the optimal control problem for the yaw motion of the unmanned vehicle, and establish a dynamic model including the speed, sideslip angle, and yaw rate of the unmanned vehicle. The classic magic tire formula MF98 is used as the tire model. Step 2: Using the forward Euler method and the multiple target method, the optimal control problem is processed into a nonlinear optimization problem, and the optimization objective, nonlinear equality constraints, and linear inequality constraints of the nonlinear optimization are defined. Step 3: Add a regularization term to the optimization objective to eliminate the singularity of the nonlinear optimization problem; Step 4: Offline generation of the objective function, objective gradient (i.e., Jacobi vector function), objective second gradient (i.e., Hessian matrix function), equality constraint function, and equality constraint gradient (i.e., Jacobi matrix function) to save online computation time. Step 5: Perform online calculations to directly obtain the accurate Hessian matrix; or use the BFGS method to iteratively calculate an approximate Hessian matrix. Step 6: Use the Hessian matrix obtained in Step 5 to define and solve the quadratic programming subproblem; Step 7: Perform Armijo line search based on backstepping to obtain the search step size, and determine convergence based on the set convergence conditions; if convergence is achieved, output the optimization result; if convergence is not achieved, return to step 5.

2. The method as described in claim 1, characterized in that: Step one specifically defines the optimal control problem in the following form: x(0)=x0 Among them, J ocp The objective function of the optimal control problem is given by the state variables x = [v, β, ω], where v, β, and ω represent the vehicle's speed, sideslip angle, yaw rate, and wheel angle, respectively. The control input is u = [δ, F]. xf1 ,F xf2 ,F xr1 ,F xr2 ], δ is the wheel rotation angle, F xf1 F xf2 F xr1 、,F xr2 Let be the longitudinal forces of the left and right front wheels and the rear wheels, respectively; t be the time variable; a0 be the initial state corresponding to the optimal control problem; and the superscript · represents the first derivative of the corresponding parameter. Furthermore: φ(x,u)=a1(vv des ) 2 +a2(ω-ω des ) 2 +a3b 2 The vehicle speed, sideslip angle, and yaw rate models are as follows: In the formula, mass and m are both the mass and inertial of the vehicle. z F is the angular inertia of the vehicle about the z-axis center. yf1 F yf2 F yr1 、,F yr2 These represent the lateral forces on the left and right front wheels and the rear wheels, respectively. f l r These are the longitudinal distances from the vehicle's center of gravity to the front and rear axles, respectively. y This refers to the vehicle's lateral dimensions; Lateral tire force F y Calculated based on the following formula: F y =μF z Dsin(Carctan(sB-E(sB-arctan(sB)))) Where B, C, D, and E are parameters of the Magic Tire formula, μ is the maximum coefficient of adhesion to the ground, and F... z is the vertical load on the tire, and s is the lateral slip ratio of the tire; The lateral slip ratios of the four wheels are expressed as follows:

3. The method as described in claim 2, characterized in that: In step two, the forward Euler method and the multiple target method are used to obtain the following nonlinear optimization problem: x(0)=x0 lbx≤x(k)≤ubx lbu≤u(k-1)≤ubu In the formula, J nlp Let be the objective function of the nonlinear optimization problem, k represent the current time, Δt be the time interval between sampling points in the multiple shooting method, lbx and ubx are the upper and lower bounds of the state variables, and lbu and ubu are the upper and lower bounds of the control input variables. Simplifying the above equation, we get: minimize f nlp (r) s.t.f eq (r)=0 lbr≤r≤ubr Among them, f eq (r) is a column vector function mapping composed of the above nonlinear programming equality constraints, and the specific form of variable r is: r=[x(0) T ,u(0) T ,x(1) T ,u(1) T ,...,, x(N-1) T ,u(N-1) T ,x(N) T ] T In the formula, N is the number of time sub-intervals.

4. The method as described in claim 3, characterized in that: Step 3: Add the following regular expression to the optimization objective: Where ζ1 and ζ2 are both coefficients.

5. The method as described in claim 4, characterized in that: In step four, the open-source automatic differentiation library Casadi is used to generate the symbolic expression of the objective function for optimization offline, and the other functions are calculated in turn. Finally, the code generation function is used to generate a .c format file.

6. The method as described in claim 5, characterized in that: Step five specifically calculates the approximate Hessian matrix by performing the following iterative process of the BFGS method: s=αΔr z = θy + (1-θ)Bs Where α is the previous search step size, Δr is the previous search direction, r is the previous solution, b is the previous Hessian matrix or approximate Hessian matrix, I is the unit vector, and r new For the current solution, B new For the new approximate Hessian matrix; the Lagrange function is defined as: Where λ is a Lagrange multiplier, λ new Its current value.

7. The method as described in claim 6, characterized in that: The specific process of solving the quadratic programming subproblem in step six includes: First, define the set of equality constraints ε and the set of inequality constraints I for the quadratic programming subproblem, and define them respectively: b eq =f eq (r) Then, define a i ,i∈ε represents A eq In the i-th line, define a i ,i∈I represents A ineq The i-th row; For the valid set obtained in the previous iteration First, solve the following problem: In the formula, d is the solution to the problem. To obtain a feasible solution Define I = I1 ∪ I2, and we get the following two sets of inequality constraints: Then, with To find a feasible initial solution, the following problem is solved using the effective set method: s.t.a i d=b i ,i∈ε a i d≥b i ,i∈I1 v＞＝0 In the formula, M is a sufficiently large penalty parameter used to reduce the infeasibility of the problem. When the problem is feasible and M is sufficiently large, the solution to the quadratic programming problem is a feasible solution to the original problem. In each iteration of the effective set method, a quadratic programming problem with only equality constraints is computed, denoted as: in, This is the current valid set; Using the previously obtained approximate Hessian matrix, the following explicit solution is obtained: Where A, b are row vectors A matrix concatenated column-wise, λ sub The Lagrange multipliers are the optimal solutions to the quadratic programming subproblem under the current effective set; Finally, after the quadratic programming subproblem converges, the following update formula is obtained: Δr=d Dλ=λ sub -l.

8. The method as described in claim 7, characterized in that: The specific method for performing the line search in step seven is as follows: The result Δr of the kth iteration k As the search direction, we seek a suitable α such that it satisfies: The convergence condition is set as follows: Δr k ≤erro max Or iter ≥ iter max Among them, erro max It is the maximum allowable error, iter, iter max These represent the current iteration number and the maximum allowed iteration number, respectively.

Images