Redundant constraint-free MPC trajectory tracking control method for four-steering wheeled omni-directional vehicle

Abstract

The application provides a kind of redundancy solution constraint type MPC trajectory tracking control method of four steering wheel omni-directional vehicle, comprising the following steps: step S10, obtaining the reference trajectory of four steering wheel omni-directional vehicle;Step S20, construct three-dimensional state vehicle kinematics model;Step S30, linearization to three-dimensional state vehicle kinematics model;Step S40, according to the linearized three-dimensional state vehicle kinematics model and reference trajectory establishes MPC controller, and MPC controller is with the reference trajectory of omni-directional vehicle as reference information to the vehicle state information of four steering wheel omni-directional vehicle is rolled optimization, exports four steering wheel independent steering angle and wheel speed instruction, realizes trajectory tracking.The application reduces the complexity of solution by three-dimensional state modeling, and avoids control amount swing by redundancy solution constraint, significantly improves control real-time and stability, and provides a feasible engineering solution for trajectory tracking of overdrive four steering wheel omni-directional vehicle.

Description

Technical Field

[0001] This invention relates to the field of vehicle trajectory tracking, and in particular to a redundant unconstrained MPC trajectory tracking control method for an overdriven four-steering wheel omnidirectional vehicle. Background Technology

[0002] With the rapid development of mobile robots and autonomous driving technologies, omnidirectional mobile vehicles, due to their all-around flexible movement capabilities, are widely used in warehousing and logistics, intelligent manufacturing, and special operations. Among them, four-wheeled omnidirectional vehicles, with their independent steering and speed regulation characteristics of four wheels, have become a core research direction for omnidirectional mobile vehicles. Trajectory tracking control, as the core component of motion control for four-wheeled omnidirectional vehicles, directly determines the vehicle's motion accuracy, stability, and engineering feasibility. Designing a trajectory tracking control strategy that combines high precision, high stability, and real-time performance has significant engineering application value.

[0003] Currently, trajectory tracking control methods for four-wheel omnidirectional vehicles mainly include PID control, linear quadratic regulator (LQR), and model predictive control (MPC). While PID control and LQR methods have low computational complexity and good real-time performance, they struggle to handle the decoupling problem in multi-input systems, easily leading to insufficient accuracy in steering wheel coordinated control and failing to meet high-precision trajectory tracking requirements. MPC, due to its ability to achieve multi-variable, multi-constraint rolling time-domain optimization and robustness to system parameter disturbances, has become the mainstream method for trajectory tracking control of four-wheel omnidirectional vehicles. However, when traditional MPC methods are applied to four-wheel omnidirectional vehicles, the following key technical problems still exist: First, traditional MPCs are mostly built on high-dimensional state models that include linear and angular velocities, failing to simplify the model dimensions for the motion characteristics of a four-steering-wheel omnidirectional vehicle. This results in complex Jacobian matrix derivations and large numerical differential errors, increasing the computational load of optimization and reducing control accuracy. Second, traditional MPCs often employ fixed prediction time-domain strategies, making it difficult to balance trajectory tracking accuracy and real-time performance in complex scenarios. Fixed short time domains can lead to accumulated tracking errors, while fixed long time domains significantly increase the computation time of the OSQP sparse solver, failing to meet the requirements of engineering real-time control. Finally, four-steering-wheel omnidirectional vehicles have a redundant solution space for control inputs. Traditional MPCs only filter redundant solutions through simple control magnitude constraints, without combining steering angle change rate constraints and reference value deviation constraints. This can easily lead to sudden changes in steering wheel angles and vehicle instability, severely restricting the stability of trajectory tracking.

[0004] To address the aforementioned issues, some studies have attempted to employ nonlinear model predictive control (NMPC) to handle the nonlinear characteristics of the four-steering wheel system. While this can improve model accuracy to some extent, it still fails to resolve the computational complexity issues arising from high-dimensional state modeling and lacks a targeted redundant constraint solution strategy for overdrive configurations. Other studies have derived the Jacobian matrix using numerical differentiation, which simplifies the computational process, but the errors introduced by numerical differentiation reduce the accuracy of the linearized model, thus affecting the performance of MPC rolling optimization. In summary, how to eliminate the numerical errors in the Jacobian matrix derivation while simplifying the state model dimension of the four-steering wheel omnidirectional vehicle, and how to combine dynamic time-domain strategies and multi-level redundant constraint solution to solve the control variable oscillation problem under overdrive configurations, thereby achieving high-precision, high-stability, and high-real-time trajectory tracking control, has become a critical problem urgently needing to be solved in the engineering application of four-steering wheel omnidirectional vehicles. Summary of the Invention

[0005] To overcome the aforementioned technical problems, the purpose of this invention is to provide a redundant unconstrained MPC trajectory tracking control method for a four-wheeled omnidirectional vehicle. By simplifying the state model dimension, eliminating Jacobian matrix numerical errors, designing dynamic time-domain strategies and multi-level redundant unconstrained methods, and combining the efficient optimization capabilities of the OSQP sparse solver, this method achieves high-precision, high-stability, and high-real-time trajectory tracking control for the four-wheeled omnidirectional vehicle. It solves the problems of complex modeling, large errors, insufficient real-time performance, and unreasonable selection of redundant solutions in traditional MPC applications for four-wheeled omnidirectional vehicles, thereby promoting the practical application of overdrive four-wheeled omnidirectional vehicles.

[0006] This invention is achieved through the following technical solution: a redundant unconstrained MPC trajectory tracking control method for a four-steering-wheel omnidirectional vehicle, comprising the following steps: Step S10, obtaining the reference trajectory of the four-steering-wheel omnidirectional vehicle; Step S20, constructing a three-dimensional state vehicle kinematic model; Step S30, linearizing the three-dimensional state vehicle kinematic model; Step S40, establishing an MPC controller based on the linearized three-dimensional state vehicle kinematic model and the reference trajectory. The MPC controller uses the omnidirectional vehicle's reference trajectory as reference information to perform rolling optimization on the vehicle state information of the four-steering-wheel omnidirectional vehicle, and outputs independent steering angles and wheel speed commands for the four steering wheels to achieve trajectory tracking.

[0007] Furthermore, the reference trajectory in step S10 includes time, x-axis coordinate, y-axis coordinate, heading angle, angular velocity, and linear velocity.

[0008] Furthermore, in step S10, a linear interpolation algorithm is used to analyze the reference trajectory, and based on the MPC prediction time domain length, x-axis position reference sequence, y-axis position reference sequence, and heading angle reference sequence corresponding to the prediction step number are generated.

[0009] Furthermore, the three-dimensional state vehicle kinematic model in step S20 includes three state variables: x-axis position, y-axis position, and heading angle.

[0010] Furthermore, in step S30, the Jacobian matrix is ​​derived analytically to eliminate the error caused by numerical differentiation, thereby obtaining the linearized three-dimensional state vehicle kinematic model.

[0011] Furthermore, in step S40, the optimization problem in the MPC controller includes one or more of the following: fixed and dynamic dual-mode time-domain strategies, cost functions, and multi-level hard constraints. The MPC optimization problem is transformed into a standard quadratic form, and the rolling optimization is completed through the OSQP solver. The independent steering angle and wheel speed commands of the four steering wheels are output to achieve trajectory tracking.

[0012] Furthermore, the cost function includes position and heading angle tracking terms and control change penalty terms.

[0013] Furthermore, the multi-level hard constraints include steering angle change rate constraints, steering angle reference deviation constraints, wheel speed change rate constraints, and wheel speed amplitude constraints.

[0014] Furthermore, the constraint on the rate of change of steering angle is as follows: , in, T is the rate of change of the maximum steering angle of the steering wheel. s To control the cycle, Let i be the steering angle of the i-th steering wheel at step k. Let be the steering angle of the i-th steering wheel in the (k-1)-th step; Steering angle reference deviation constraint is , in, Let ε be the reference steering angle for the i-th steering wheel at step k, and let ε be the deviation threshold. Wheel speed change rate constraint is , in, The maximum rate of change of the steering wheel speed. Let i be the wheel speed of the i-th steering wheel at step k. Let be the wheel speed of the i-th steering wheel in the (k-1)-th step; Wheel speed amplitude constraint is , in, The maximum wheel speed of the steering wheel. i = 1, 2, 3, 4 represent the four steering wheels.

[0015] Furthermore, in step S40, the linearized three-dimensional state vehicle kinematic model is discretized using the forward Euler method, the cost function is transformed into a standard quadratic programming form, and the OSQP sparse solver is used for iterative solution. Finally, the first element of the optimal control sequence is used as the actual output to obtain the independent steering angle and wheel speed commands of the four steering wheels, thus completing the rolling optimized trajectory tracking.

[0016] The beneficial effects of this invention are: 1. It uses only x-axis position, y-axis position, and heading angle. 1. Three-dimensional state modeling significantly reduces model complexity, and numerical differential error is eliminated at its source by combining analytical Jacobian and regularized least squares, thus significantly improving the accuracy of the prediction model; 2. A fixed and dynamic dual-mode prediction time-domain strategy is proposed, which can adaptively balance tracking accuracy and computational real-time performance. Combined with the OSQP sparse solver, it meets the requirements of engineering real-time control; 3. A multi-level redundant hard constraint system is designed for overdrive characteristics, which can effectively suppress sudden changes in steering wheel angle and control variable oscillation, and significantly improve the stability and robustness of trajectory tracking. Attached Figure Description

[0017] Figure 1 This is a schematic diagram of the three-dimensional vehicle kinematics model of the present invention; Figure 2 This is a schematic diagram of the process of the present invention; Figure 3 This is a schematic diagram of the control system of the present invention; Figure 4 This is a simulation diagram of the state and control quantities of the present invention. Detailed Implementation

[0018] Embodiments of this application will now be described in more detail with reference to the accompanying drawings. While some embodiments of this application are shown in the drawings, it should be understood that this application can be implemented in various forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided to provide a more thorough and complete understanding of this application. It should be understood that the drawings and embodiments of this application are for illustrative purposes only and are not intended to limit the scope of protection of this application.

[0019] It should be understood that the steps described in the method embodiments of this application may be performed in different orders and / or in parallel. Furthermore, the method embodiments may include additional steps and / or omit the steps shown. The scope of this application is not limited in this respect.

[0020] The term "comprising" and its variations as used herein are open-ended inclusions, meaning "including but not limited to". The term "based on" means "at least partially based on". The term "one embodiment" means "at least one embodiment"; the term "another embodiment" means "at least one additional embodiment"; the term "some embodiments" means "at least some embodiments". Definitions of other terms will be given in the description below.

[0021] It should be noted that the terms "one" and "multiple" used in this application are illustrative rather than restrictive, and those skilled in the art should understand that, unless explicitly stated otherwise in the context, they should be understood as "one or more". "Multiple" should be understood as two or more.

[0022] This invention provides a redundant unconstrained MPC trajectory tracking control method for a four-steering wheel omnidirectional vehicle, comprising the following steps.

[0023] Step S10: Obtain the reference trajectory of the four-steering wheel omnidirectional vehicle, analyze and process the reference trajectory to generate position and heading angle reference sequences, and provide a benchmark for subsequent trajectory tracking control.

[0024] The reference trajectory includes time, x-axis coordinate, y-axis coordinate, heading angle, angular velocity, and linear velocity.

[0025] A linear interpolation algorithm is used to analyze the reference trajectory. Based on the MPC prediction time domain length, x-axis position reference sequence, y-axis position reference sequence, and heading angle reference sequence corresponding to the prediction steps are generated to ensure the continuity and smoothness of the reference sequence. The length of the reference sequence is dynamically matched with the subsequent dynamically adjusted prediction steps.

[0026] Step S20: Construct a three-dimensional state vehicle kinematic model.

[0027] The three-dimensional vehicle kinematic model includes only three state variables: x-axis position, y-axis position, and heading angle. This simplifies the model dimensions to reduce the complexity of optimization while ensuring the model accurately represents the vehicle's motion characteristics.

[0028] This step specifically includes: Let the state variables be: Where x represents the x-axis position of the four-wheel omnidirectional vehicle in the world coordinate system; y represents the y-axis position in the world coordinate system; This refers to the vehicle's heading angle.

[0029] The control variables are the independent steering angle and wheel speed of the four steering wheels, namely: .in, These are the steering angles of the four steering wheels; These are the wheel speeds of the four steering wheels.

[0030] Establish speed constraints for a single steering wheel: .in, Let represent the installation position of the i-th wheel in the vehicle body coordinate system.

[0031] The constraints of the four steering wheels are uniformly written in matrix form: .

[0032] in: , , .

[0033] Since the four-steering wheel system is an overdrive system, there are four steering wheel velocity constraint equations, but only three vehicle body motion parameters need to be solved, forming an overdetermined system of equations. Direct solution can easily lead to matrix singularities, unstable solutions, or even no solutions. Therefore, regularized least squares method is used to solve for the vehicle body velocity, minimizing constraint errors while ensuring the stability and uniqueness of the solution. .

[0034] in, is the regularization coefficient used to prevent matrix singularities; I is a 3×3 identity matrix.

[0035] Transform the vehicle's velocity to the world coordinate system to obtain a three-dimensional vehicle kinematic model: .in, , Let ω be the velocity along the x and y axes of the vehicle body coordinate system, and ω be the angular velocity.

[0036] That is, the state-space equation: .in, The vehicle kinematics state transition function has the same form as the three-dimensional state vehicle kinematics established above, and it represents the mapping relationship between the rate of change of vehicle position and heading angle, vehicle speed, and steering wheel control quantity.

[0037] Step S30: Linearize the three-dimensional vehicle kinematic model.

[0038] An analytical method is used to derive the Jacobian matrix to obtain a linearized three-dimensional state vehicle kinematic model, thereby replacing the traditional numerical differentiation method, eliminating the errors caused by numerical differentiation, improving the accuracy of the linearized model, and providing a reliable prediction basis for MPC rolling optimization.

[0039] Specifically, the state Jacobian matrix A and the control Jacobian matrix B are derived analytically.

[0040] The state Jacobian matrix A is a 3×3 matrix that characterizes the influence of state variables on the rate of change of the state. .in, It is a state transition function. It is a three-dimensional state vector. It is a control vector. Its specific form is determined by the state variables in the three-dimensional vehicle kinematics model. Taking the partial derivative, we get: .

[0041] The control Jacobian matrix B is a 3×8 matrix, representing the influence of the control variables on the rate of change of the state. The partial derivative is divided into a steering angle partial derivative and a wheel speed partial derivative. The wheel speed partial derivative is determined by a regularized least squares mapping matrix. The partial derivative of the steering angle is obtained directly by differentiating the steering wheel mapping relationship and combining it with the transformation relationship between the world coordinate system and the vehicle coordinate system, ultimately ensuring that the control Jacobian matrix is ​​completely consistent with the vehicle motion model.

[0042] Step S40: Based on the linearized three-dimensional state vehicle kinematic model and reference trajectory, establish an MPC controller. The MPC controller uses the reference trajectory of the omnidirectional vehicle as reference information to perform rolling optimization on the vehicle state information of the four-steering wheel omnidirectional vehicle, and outputs independent steering angle and wheel speed commands for the four steering wheels to achieve trajectory tracking.

[0043] In this step, the optimization problem in the MPC controller includes one or more of the following: fixed and dynamic dual-mode time-domain strategies, cost functions, and multi-level hard constraints. The MPC optimization problem is transformed into a standard quadratic form, and the rolling optimization is completed through the OSQP solver. The independent steering angle and wheel speed commands of the four steering wheels are output to achieve trajectory tracking.

[0044] The specific implementation logic of the time-domain strategy is divided into fixed mode and dynamic mode: In fixed mode, the number of predicted time-domain steps is fixed to a preset value within 1 to 50 steps, and the parameters are deemed invalid if they exceed the range; In dynamic mode, the number of time-domain steps at the end of the trajectory decreases synchronously with the remaining steps, taking into account both tracking accuracy and real-time performance.

[0045] The cost function includes position and heading angle tracking terms and control change penalty terms; the multi-level hard constraints include steering angle change rate constraints, steering angle reference deviation constraints, wheel speed change rate constraints and wheel speed amplitude constraints, in order to screen engineering feasible solutions in the overdrive redundancy solution space, take into account both trajectory tracking accuracy and control smoothness, and avoid sudden changes in steering wheel angle.

[0046] It adopts two modes: fixed time domain and dynamic time domain. The prediction step number N can be dynamically adjusted according to the tracking error of the reference trajectory. When the remaining prediction step number is less than the set prediction step number N, the prediction step number is dynamically reduced according to the remaining step number.

[0047] To minimize position and heading angle tracking errors and control smoothness, a quadratic cost function is constructed: .

[0048] Where Q is the position and heading angle tracking weight matrix, R Δ A penalty matrix for control variables is used to suppress abrupt changes in steering angle and wheel speed. The cost function achieves trajectory tracking by tracking state variables and smooths the trajectory by penalizing changes in control variables. This, combined with subsequent hard constraints, filters out overdrive redundant solutions.

[0049] To address the characteristics of overdriven systems, a multi-level hard constraint mechanism is constructed: .

[0050] The constraint is the rate of change of steering angle, where, T is the rate of change of the maximum steering angle of the steering wheel. s To control the cycle, Let i be the steering angle of the i-th steering wheel at step k. Let be the steering angle of the i-th steering wheel at step k-1.

[0051] For steering angle reference deviation constraints, where, The reference steering angle is the i-th steering wheel at the k-th step. The reference steering angle is determined by inverse kinematics solution of the desired vehicle motion parameters obtained by the reference trajectory analysis. ε is the deviation threshold. In this application, ε=0.1rad is selected to limit the fluctuation range of the steering wheel angle near the reference value and ensure the smoothness of the steering wheel motion. Other values ​​can also be selected as needed.

[0052] For the wheel speed change rate constraint, where, The maximum rate of change of the steering wheel speed. Let i be the wheel speed of the i-th steering wheel at step k. Let be the wheel speed of the i-th steering wheel at step k-1.

[0053] For wheel speed amplitude constraints, where, This represents the maximum wheel speed of the steering wheel.

[0054] In the above constraints, i=1, 2, 3, 4 represent the four steering wheels.

[0055] By constraining the rate of change of steering angle, the reference deviation of steering angle, the rate of change of wheel speed, and the amplitude of wheel speed, stable screening of redundant solutions is achieved, avoiding control variable oscillation. The reference deviation of steering angle is used to limit the steering wheel angle to within the reference steering angle. Nearby fluctuation range This ensures that the movement of the steering wheel is within the physically feasible range.

[0056] The state equations are discretized using the forward Euler method: .

[0057] Transform the cost function into a standard quadratic programming form: Among them, augmented matrix , For three-dimensional state variables, Let P be the control variable at time k, P be the quadratic coefficient matrix, and q be the linear coefficient vector. The optimal control sequence obtained after solving is: N is the control time domain length, and the first element of the optimal control sequence is used as the actual control input of the system.

[0058] Iterative solution is performed using the OSQP sparse solver, with a maximum number of iterations set to 5000 and a convergence accuracy of [value missing]. The solution efficiency is guaranteed by sparse matrix storage and LDLT decomposition, and the single-step solution time is controlled within 4ms.

[0059] The first element of the optimal control sequence is used as the actual output to obtain the independent steering angle and wheel speed commands for the four steering wheels, thus completing the rolling optimized trajectory tracking.

[0060] The method of the invention will be described in detail below with reference to specific embodiments.

[0061] The parameter settings for the four-wheel omnidirectional vehicle are shown in the table below. parameter value Wheelbase L / m 0.5 Wheelbase W / m 0.6 Maximum steering angle change rate of steering wheel / (rad / s) π / 2 Steering angle reference deviation constraint ε / rad 0.1 maximum speed of steering wheel / (m / s) 2 <![CDATA[Maximum rotation speed change rate of steering wheel / (m / s 2 )]]> 2 Regularization coefficient λ <![CDATA[1×10 -3 ]]> Predicting time domain N 12 <![CDATA[Control period T s / s]]> 0.01 State weight matrix Q diag(180,180,200) Control quantity change rate weight matrix diag(80,80,80,80,50,50,50,50) OSQP maximum number of iterations 5000 OSQP convergence accuracy <![CDATA[1×10 -8 ]]> The method of the present invention specifically includes the following steps.

[0062] Step S10: Obtain the reference trajectory of the four-wheel omnidirectional vehicle.

[0063] Step S20: Construct a three-dimensional state vehicle kinematic model.

[0064] like Figure 1 As shown, this invention defines two coordinate systems: the world coordinate system (inertial coordinate system) and the vehicle body coordinate system (following coordinate system). World coordinate system O-XY: O is the starting point of the trajectory, the X-axis is horizontal forward, and the Y-axis is horizontal to the right, used to describe the absolute position and attitude of the vehicle; Vehicle coordinate system o-xy: The origin o is the vehicle's center of mass, the x-axis moves forward longitudinally along the vehicle body, and the y-axis moves and rotates laterally to the right along the vehicle body; Coordinate transformation relationship: Let the vehicle heading angle be... (The angle between the x-axis of the vehicle body coordinate system and the x-axis of the world coordinate system, with counterclockwise being positive), then the transformation matrix of any point between the world coordinate system and the vehicle body coordinate system is: The transformation relationship between world coordinate system coordinates (X,Y) and vehicle body coordinate system coordinates (x,y) is: [X,Y]T =R( [x,y] T .

[0065] Each steering wheel of the four-steering-wheel omnidirectional vehicle can be independently steered and its speed adjusted. The motion of a single steering wheel can be decomposed into "rotation around its own axis" and "translation + rotation with the vehicle body". Based on the installation position of the steering wheel, the speed constraint of a single steering wheel can be derived.

[0066] Let the steering angle of the i-th steering wheel be δ. i (The angle between the steering wheel's rotation direction and the x-axis of the vehicle's coordinate system, with counterclockwise being positive), the wheel speed is v. i (The linear velocity of the steering wheel along its own rotation direction, with forward being positive); The velocity component of the i-th steering wheel as it moves with the vehicle body: the average velocity of the vehicle body is (v x ,v y (The translational velocity in the x and y directions in the vehicle's coordinate system), the angular velocity of the vehicle's rotation is ω (the angular velocity about the center of mass, positive counterclockwise), then the linear velocity component of the steering wheel as it rotates with the vehicle is (-ωy). i ,ωx i )(x i y i (The mounting position of the steering wheel in the vehicle coordinate system). The combined velocity constraint for a single steering wheel: The actual velocity of the steering wheel (along its own steering direction) is equal to "the projection of the velocity along the vehicle body onto the steering wheel's steering direction", that is: .

[0067] After expansion, the velocity constraint equations for a single steering wheel are obtained (final form): .

[0068] The above formula represents the speed constraint for a single steering wheel, thus defining the steering angle δ of the steering wheel. i Wheel speed v i With vehicle body motion parameters (v) x ,v y The relationship between ω and ω lays the foundation for the subsequent establishment of the overall model.

[0069] The four-steering-wheel omnidirectional vehicle is an overdrive system (4 steering wheels, 3 vehicle motion parameters). The velocity constraint equations for the 4 steering wheels are redundant, making it impossible to directly solve for the vehicle motion parameters (v). x ,v y Therefore, regularized least squares is used to solve the problem. The complete derivation is as follows: By simultaneously solving the velocity constraint equations of the four steering wheels, a matrix form can be constructed: .

[0070] in, It is a 4×3 constraint matrix (determined by the steering wheel angle and installation position). This is the vector of vehicle body motion parameters. This is the wheel speed vector of the steering wheel.

[0071] The specific derivation and construction process: For each steering wheel, extract the formula from , , The coefficients yield a 2x3 submatrix, and the four steering wheels together form an 8x3 matrix. After simplification (merging redundant terms), a 4x3 matrix is ​​obtained. .

[0072] Each row corresponds to the speed constraint coefficient of a steering wheel. Redundant terms have been eliminated during the derivation process to ensure the validity of the matrix.

[0073] Solving using regularized least squares: Because Given a 4×3 matrix (number of rows > number of columns), singular solutions may exist (leading to solution failure). Therefore, a regularization term λI is introduced (λ is the regularization coefficient, and I is a 3×3 identity matrix), constructing a regularized least squares objective function: .

[0074] The physical meaning of the objective function is to minimize the steering wheel speed constraint error, while suppressing excessive fluctuations in vehicle motion parameters and avoiding singularities in the solution. Differentiate the above equation to find the optimal solution. Find the partial derivative and set it equal to 0. The derivation process is as follows: , After sorting, we get: .

[0075] Therefore, vehicle body motion parameters The optimal solution is: .

[0076] The selection of the regularization coefficient λ is based on the motion characteristics of a four-steering-wheel omnidirectional vehicle, and through multiple simulation tests, λ = 1 × 10⁻⁶ is chosen. -3 This effectively avoids matrix singularities without affecting the solution accuracy (when λ > 1 × 10⁻⁶). -2 When λ < 1 × 10⁻⁶, the solution accuracy decreases. -4 (At that time, singular values ​​cannot be avoided). Vehicle state variable definition: .

[0077] Where x and y are the positions in the world coordinate system. The heading angle is the three-dimensional state vector. Vehicle speed conversion to world coordinate system: based on coordinate transformation matrix R( ), vehicle translational speed (v) x ,v y The velocity components transformed to the world coordinate system are: .

[0078] In the above formula, velocity is the time derivative of position, which is obtained by differentiating the position transformation relationship between the world coordinate system and the vehicle coordinate system and combining the chain rule; The relationship between angular velocity and heading angle is as follows: the vehicle's angular velocity ω is the heading angle. The time derivative, i.e.: .

[0079] Combining the above equations yields the three-dimensional state model (state-space equations) of the four-steering-wheel omnidirectional vehicle: .in, , Let ω be the velocity along the x and y axes of the vehicle body coordinate system, and ω be the angular velocity.

[0080] The above three-dimensional state model can be used to calculate the forward kinematics of the vehicle: Given the vehicle's current position (x, y) and heading angle (φ), combined with its translational velocity (v) in the vehicle's coordinate system... x v y The vehicle's position x, y and heading angle φ in world coordinates are updated through coordinate transformation and integration calculation, and the angular velocity ω is obtained in real time. This enables the vehicle's motion state to be updated and fed back in real time, providing closed-loop state support for MPC trajectory tracking control.

[0081] The solution is obtained from the wheel speeds and steering angles of the four steering wheels using regularized least squares: Among them, A veh The wheel motion mapping matrix is ​​4×3, λ=1×10 -3 b is the regularization coefficient. veh The wheel velocity vector is 4×1, v body This is the vehicle's velocity vector.

[0082] Step S30: Linearize the three-dimensional vehicle kinematic model.

[0083] The core of MPC control is to linearize the nonlinear state model. The key to linearization is solving for the state Jacobian matrix A and the control Jacobian matrix B, where: State Jacobian matrix A: (3×3 matrix) representing state variables rate of change of state The impact; Controlling the Jacobian matrix B: (3×8 matrix), representing control variables rate of change of state The impact; The linearized state-space equations are as follows: ,in, This refers to the current state and control variables.

[0084] Element-by-element derivation of the state Jacobian matrix A: Rate of change of state Changes in state According to the definition of the state Jacobian matrix... ,right , , Regarding respectively , , Find the partial derivative: ,right Taking the partial derivative gives ,right , The partial derivative is 0; ,right Taking the partial derivative gives ,right , The partial derivative is 0; Therefore, the final form of state Jacobi A is: .

[0085] Element-by-element derivation of the control Jacobian matrix B: Rate of change of state Control vector According to the definition of the control Jacobian matrix ,right , , Regarding each steering wheel separately and Find the partial derivative: According to the chain rule, it can be decomposed into the product of the coordinate transformation Jacobian matrix and the steering wheel constraint Jacobian matrix: .

[0086] From the velocity transformation relationship between the world coordinate system and the vehicle coordinate system, the coordinate transformation Jacobian matrix can be obtained: .

[0087] Vehicle speed Solved using regularized least squares: .

[0088] in, The constraint matrix is ​​4×3 for the steering wheel. This is the wheel velocity vector.

[0089] wheel speed Partial derivatives (last four columns): .

[0090] in, For the first Unit vectors, therefore That is, the j-th column of the pseudo-inverse matrix J.

[0091] Steering wheel constraint relationship Both sides regarding Differentiate: Summarized as follows: .

[0092] in, To regularize pseudo-inverse, Only the The behavior is non-zero, and the element in this row is: .

[0093] Let intermediate variables ,but: .

[0094] Substituting the above results into the chain rule, we obtain each column of the controlling Jacobian matrix B: For steering angle Partial derivatives (first 4 columns): .

[0095] wheel speed Partial derivatives (last 4 columns): .

[0096] in, For regularized pseudo-inverse matrix The Line number Column elements.

[0097] Therefore, the complete form of the control Jacobian matrix B is: .

[0098] Step S40: Based on the linearized three-dimensional state vehicle kinematic model and reference trajectory, establish an MPC controller. The MPC controller uses the reference trajectory of the omnidirectional vehicle as reference information to perform rolling optimization on the vehicle state information of the four-steering wheel omnidirectional vehicle, and outputs independent steering angle and wheel speed commands for the four steering wheels to achieve trajectory tracking.

[0099] Constructing the MPC cost function and constraints: The core objective of the cost function in this invention is to "minimize the trajectory tracking error and suppress the sudden change in control quantity". Combining the overdrive characteristics of a four-wheel omnidirectional vehicle, a quadratic cost function is constructed.

[0100] The detailed derivation process is as follows: The overall form of the cost function adopts rolling time-domain optimization, with the prediction time domain being N. The cost function is the sum of the tracking error term and the control smoothing term in the prediction time domain, i.e.: .

[0101] in, Let k be the tracking error term. This is the smoothing term for the control quantity at step k.

[0102] The specific form of the state variable tracking term is: .

[0103] Wherein, the predicted state is The reference state is ; , which is a 3×3 state tracking weight matrix; = =180, which is the position tracking weight to ensure that the position error is controlled within a small range; =200, which is greater than the position weight, prioritizes ensuring the accuracy of heading angle tracking and avoids the wheels deviating from the trajectory direction.

[0104] The four-steering-wheel omnidirectional vehicle is an overdrive system, resulting in a redundant solution space for the control variables. Without constraints on the changes in control variables, problems such as sudden changes in steering wheel angles and drastic fluctuations in wheel speed can easily occur, leading to vehicle instability and decreased tracking performance. Therefore, a control variable smoothing term needs to be introduced to suppress sudden changes in control variables. The change in control variables is defined as: ( For the control quantity in step k, (This refers to the control quantity at step k-1). The control quantity smoothing term adopts a weighted quadratic form, namely: .

[0105] in, , which is an 8×8 control change weight matrix; =80, which is the weight of the change in steering angle. The steering angle has a significant impact on vehicle stability, so it has a large weight. =50, which is the weight of wheel speed change. The impact of sudden wheel speed changes is small, and the weight is less than that of steering angle.

[0106] The final form of the cost function is: .

[0107] The constraints of this invention address the overdrive characteristics and mechanical constraints of a four-steering-wheel omnidirectional vehicle, constructing multi-level hard constraints. Since the control input dimension of the overdrive system is greater than the system's degrees of freedom, mathematically there are infinitely many redundant solutions satisfying the motion equations. While these solutions satisfy the mathematical relationships of the vehicle's motion equations, some are physically infeasible and engineeringally unusable. Direct application of these solutions would lead to steering angle conflicts, motion incoordination, and instability in the four steering wheels. Therefore, it is necessary to filter physically feasible and engineering-usable control quantities from the redundant solution space through constraints to ensure the feasibility of control commands (consistent with steering wheel mechanical performance) and system stability. Each type of constraint is specifically as follows: Constraint 1: Steering angle change rate constraint There is an upper limit to the rate of change of steering angle of the steering wheel actuator. =π / 2rad / s), if the rate of change of the steering angle exceeds this upper limit, the actuator will fail to operate and mechanical wear will occur. Therefore, the change in steering angle within a single-step control cycle is constrained. The single-step time is the control cycle (T). s =0.01s), therefore the maximum steering angle in a single step is The specific formula is as follows: .

[0108] in, Let i be the steering angle of the i-th steering wheel at step k. Let be the steering angle of the i-th steering wheel at step k-1.

[0109] Constraint 2: Steering angle reference deviation constraint The steering angle of a four-steering-wheel omnidirectional vehicle has redundant solutions. If the steering angle deviates too far from the reference steering angle, it will cause incoordination between the steering wheels, affecting the vehicle's omnidirectional performance. Therefore, the deviation between the steering angle and the reference steering angle is constrained. Let the deviation threshold be ε, then the constraint form is: .

[0110] in, This is the reference steering angle for the i-th steering wheel at the k-th step; the reference steering angle is the desired vehicle speed obtained from the reference trajectory analysis. , and desired angular velocity The expression is determined by inverse kinematics: .

[0111] in, , Let be the installation coordinates of the i-th steering wheel in the vehicle coordinate system.

[0112] Constraint 3: Wheel speed change rate constraint The rate of change of the steering wheel speed has an upper limit. =2m / s 2 If the wheel speed change rate exceeds this upper limit, the motor will be overloaded. Therefore, the single-step change of wheel speed is constrained. The derivation process is consistent with the constraint on the steering angle change rate. The specific formula is as follows: .

[0113] in, Let be the linear velocity of the i-th steering wheel at step k. Let be the steering angle of the i-th steering wheel at step k-1.

[0114] Constraint 4: Wheel speed amplitude constraint The maximum wheel speed of the steering wheel is v max =2m / s. Exceeding this limit will cause the steering wheel to slip and lose control. Therefore, the wheel speed amplitude is directly constrained. .

[0115] Where i = 1, 2, 3, 4 represent four steering wheels.

[0116] State model discretization and OSQP solution: The following OSQP standard quadratic form transformation process is illustrated by taking the control time domain N=2 as an example. For any control time domain N, it can be extended by similar block matrix cyclic splicing, and the core construction logic remains unchanged.

[0117] The state model of this invention is a nonlinear continuous model. MPC optimization requires a discretized model. Therefore, the forward Euler method is used to discretize the continuous state space equations. The discretization period is the control period. The derivation process is as follows: The continuous state model is: .

[0118] The forward Euler discretization formula is: .

[0119] Substituting into the linearized model, we obtain the discretized state-space equations: .

[0120] Among them, A d =I+T s A (3×3 discrete state matrix), B d =T s B is a 3×8 discrete control matrix, and I is a 3×3 identity matrix.

[0121] The standard quadratic programming form of the OSQP solver is: .

[0122] Where P is the quadratic coefficient matrix (symmetric positive semi-definite), q is the linear coefficient vector, and A osqp Let l be the constraint matrix. osqp u osqp These are the upper and lower bound vectors for the constraints, respectively.

[0123] The MPC optimization problem is transformed into the standard form described above. The detailed transformation process is as follows: Step 1: Define the optimization variable U (the decision variable for MPC optimization).

[0124] .in, Let N be the control variable at step k, and N be the prediction time domain length.

[0125] Step 2: Transform the cost function into an OSQP quadratic form.

[0126] Expanding the MPC cost function, substituting it into the discretized state equation, and combining it with the definition of the optimization variable U, we derive: .

[0127] The specific derivation process of P and q is as follows: The eight-dimensional control quantity of the present invention With 4 steering angles and 4 wheel speeds Composition. Since the weights of each steering angle and each wheel speed are consistent, the weight matrix of the control change penalty term can be represented as a block diagonal matrix. ,in , These are the weights for the changes in steering angle and wheel speed, respectively. It is a 4×4 identity matrix.

[0128] Define augmented vectors ,in It is a three-dimensional state vector. In the quadratic form, P is the block matrix of the quadratic coefficients, and q is the block vector of the linear coefficients.

[0129] The specific form of the corresponding quadratic term matrix P is: .

[0130] in, It is a 3×3 state weight matrix, corresponding to the state tracking cost; It is the 8×8 diagonal control weight matrix defined above, corresponding to the control quantity smoothing cost.

[0131] The specific form of the corresponding linear term vector q is: .

[0132] in, .

[0133] Step 3: Convert the constraints to OSQP constraint form.

[0134] The four types of constraints in MPC (steering angle change rate, steering angle reference deviation, wheel speed change rate, and wheel speed amplitude) and the discretized state constraints are uniformly transformed into the OSQP standard linear inequality constraint form: .

[0135] Among them, l osqp u osqp To constrain the upper and lower bound vectors, the dimension is consistent with the total number of constraints, and is determined by the specific parameters of each type of constraint.

[0136] The corresponding l osqp The format is: .

[0137] The corresponding u osqp The format is: .

[0138] Corresponding A osqp The format is: .

[0139] OSQP solution process and control output: (e.g.) Figure 2 As shown, the control flow of this invention starts by obtaining the reference trajectory of the four-steering wheel omnidirectional vehicle, and then proceeds to construct a three-dimensional state vehicle kinematic model, linearize the three-dimensional state kinematic model, establish a redundant unconstrained MPC controller and perform rolling optimization in combination with a dynamic time-domain strategy, and finally output the independent steering angle and wheel speed commands of the four steering wheels through the OSQP solver to achieve trajectory tracking.

[0140] Step 1: Obtain the current vehicle status (Collected by sensors) and reference state sequence (k=0,1,...,N-1, provided by the reference trajectory generator); Step 2: Based on the current state and reference state Calculate the initial tracking error ; Step 3: Based on the discretized state A d Control Matrix B d The predicted state sequence in the prediction time domain is obtained recursively. ; Step 4: Transform the MPC cost function and constraints into the standard OSQP form, and construct sparse matrices P and A.osqp and vectors q and l osqp u osqp ; Step 5: Call the OSQP solver and input P, ​​q, and A. osqp l osqp u osqp And solve for the parameters to obtain the optimal optimization variables. ; Step 6: The control output logic (rolling time-domain optimization core) selects only the first element of the optimal control sequence. As the actual control command at the current moment, it is output to the four steering wheel actuators, and the rest of the control quantities are discarded; Step 7: Enter the next control cycle and repeat steps 1 to 6 to achieve rolling time-domain optimization and ensure the real-time and continuous nature of trajectory tracking.

[0141] Verification of solution efficiency and tracking accuracy Solution time verification: The average solution time for single-step OSQP is 1 to 3 ms, which is less than the control cycle (T). s =0.01s), meeting real-time requirements. Compared with traditional nonlinear optimization solvers, the OSQP sparse solver significantly improves computational efficiency while ensuring accuracy, and can better adapt to the real-time control needs of four-wheel omnidirectional vehicles.

[0142] Figure 3 The redundant unconstrained MPC trajectory tracking control closed-loop structure block diagram provided by this invention illustrates the complete closed-loop control process of a four-steering-wheel omnidirectional vehicle from reference trajectory input to control output. In the diagram, the reference trajectory and the vehicle's current state are input to the MPC controller. The controller internally optimizes the solution based on a linear time-varying MPC model, objective function, and constraints, outputting steering wheel control quantities (steering angle, wheel speed). These steering wheel control quantities are then processed through forward kinematics calculations and combined with dynamic predictive time-domain updates to update the vehicle state and feed it back to the next control cycle, forming real-time closed-loop trajectory tracking.

[0143] Figure 4 Simulation diagrams of state and control quantities provided for this invention. (From...) Figure 4 visible, Figure 4 (a) shows the vehicle trajectory tracking results. Under a typical omnidirectional vehicle motion trajectory, the control method of the present invention achieves high-precision tracking of the reference trajectory. Figure 4 (b) and (c) are the position tracking curves in the X and Y directions, respectively. The actual values ​​are in extremely good agreement with the reference values, which verifies the accurate tracking capability of the present invention in the position dimension. Figure 4 (d) is the heading angle tracking curve. The actual heading angle can quickly follow the reference heading angle, with a rapid response and no obvious lag. Figure 4(e) is the heading angle tracking error curve. The maximum tracking error does not exceed 0.004 rad, and the tracking accuracy meets the requirements of engineering applications. Figure 4 (f) shows the position error curves in the X and Y directions. The errors in both directions do not exceed 0.006m, and the single-direction error fluctuation is extremely small. This verifies the high precision and stability of the tracking control from a multi-dimensional perspective. Figure 4 (g) shows the position tracking error (Euclidean distance) curve. The maximum tracking error does not exceed 0.006m, which verifies the accuracy of trajectory tracking from a global perspective. Figure 4 (h) and Figure 4 (i) are the steering wheel angle and wheel speed curves, respectively. The steering angle and wheel speed output curves of the four steering wheels are smooth, with no obvious abrupt changes or overshoot, which effectively suppresses control jitter and improves vehicle driving stability.

[0144] Finally, it is necessary to note that the above content is only used to help understand the technical solution of the present invention and should not be construed as a limitation on the scope of protection of the present invention; any non-essential improvements and adjustments made by those skilled in the art based on the above content of the present invention are all within the scope of protection claimed by the present invention.

Claims

1. A redundant constraint-relaxed MPC trajectory tracking control method for a four-steering-wheel omni-directional vehicle, characterized in that, Includes the following steps: Step S10: Obtain the reference trajectory of the four-steering wheel omnidirectional vehicle; Step S20: Construct a three-dimensional state vehicle kinematic model; Step S30: Linearize the three-dimensional vehicle kinematic model; Step S40: Based on the linearized three-dimensional state vehicle kinematic model and reference trajectory, establish an MPC controller. The MPC controller uses the reference trajectory of the omnidirectional vehicle as reference information to perform rolling optimization on the vehicle state information of the four-steering wheel omnidirectional vehicle, and outputs independent steering angle and wheel speed commands for the four steering wheels to achieve trajectory tracking. In step S40, the optimization problem in the MPC controller includes one or more of the following: fixed and dynamic dual-mode time-domain strategies, cost functions, and multi-level hard constraints. The MPC optimization problem is transformed into a standard quadratic form, and the rolling optimization is completed through the OSQP solver. The independent steering angle and wheel speed commands of the four steering wheels are output to achieve trajectory tracking. The multi-level hard constraints include steering angle change rate constraints, steering angle reference deviation constraints, wheel speed change rate constraints, and wheel speed amplitude constraints. The steering angle rate of change constraint is , wherein, is the maximum steering angle change rate of the steering wheel, T s is the control period, is the steering angle of the i-th steering wheel at the k-th step, is the steering angle of the i-th steering wheel at the k-1-th step; The steering angle reference deviation constraint is , wherein, is the reference steering angle for the kth step and the ith rudder, and ε is a deviation threshold. Wheel speed rate of change constraint is , wherein, is the maximum wheel speed change rate of the steering wheel, is the wheel speed of the i-th wheel at the k-th step, is the wheel speed of the i-th wheel at the k-1-th step; Wheel speed amplitude constraint is , wherein, Vmax is the maximum wheel speed of the steering wheel, i = 1, 2, 3, 4, representing the four steering wheels respectively.

2. The redundancy-free MPC trajectory tracking control method according to claim 1, characterized in that, The reference trajectory in step S10 includes time, x-axis coordinate, y-axis coordinate, heading angle, angular velocity, and linear velocity.

3. The redundant unconstrained MPC trajectory tracking control method according to claim 2, characterized in that, In step S10, a linear interpolation algorithm is used to analyze the reference trajectory. Based on the MPC prediction time domain length, x-axis position reference sequence, y-axis position reference sequence, and heading angle reference sequence corresponding to the prediction step number are generated.

4. The redundant unconstrained MPC trajectory tracking control method according to claim 1, characterized in that, The three-dimensional state vehicle kinematic model in step S20 includes three state variables: x-axis position, y-axis position, and heading angle.

5. The redundant unconstrained MPC trajectory tracking control method according to claim 1, characterized in that, In step S30, the Jacobian matrix is ​​derived analytically to eliminate the error caused by numerical differentiation, and a linearized three-dimensional state vehicle kinematic model is obtained.

6. The redundant unconstrained MPC trajectory tracking control method according to claim 1, characterized in that, The cost function includes position and heading angle tracking terms and control change penalty terms.

7. The redundant unconstrained MPC trajectory tracking control method according to claim 1, characterized in that, In step S40, the linearized three-dimensional state vehicle kinematic model is discretized using the forward Euler method, the cost function is transformed into a standard quadratic programming form, and the OSQP sparse solver is used for iterative solution. Finally, the first element of the optimal control sequence is used as the actual output to obtain the independent steering angle and wheel speed commands of the four steering wheels, thus completing the rolling optimized trajectory tracking.

Images