Automobile trajectory tracking model control method and device based on Lie group Hamiltonian dynamics and Shenchang differential equation

Abstract

The invention belongs to the related technical field of vehicle trajectory control, and discloses a vehicle trajectory tracking model prediction control method and device based on Lie group Hamiltonian dynamics and Shenchang differential equation, and the method comprises the steps: S1, describing the posture and motion of a vehicle under the Lie group; s2, establishing a vehicle kinetic equation on the Lie group; s3, constructing a Shenchang differential equation based on the obtained vehicle kinetic equation, integrating the derivative of the generalized coordinate and the derivative of the generalized speed to obtain predicted values of the generalized coordinate and the generalized speed, and comparing the predicted values of the generalized coordinate and the generalized speed with actual values to calculate a mean square error, further performing back propagation to update and optimize parameters of the Shenchang differential equation; and S4, describing a change rule of a vehicle state along with time by adopting a vehicle dynamics model obtained by training based on a Shenchang differential equation, and then carrying out trajectory tracking control on the vehicle by adopting model prediction control. According to the invention, the applicability is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field related to vehicle trajectory control, and more specifically, relates to a vehicle trajectory tracking model predictive control method and device based on Lie group Hamiltonian dynamics and Neural ordinary differential equations. Background Art

[0002] With the development of intelligent transportation systems, especially the advancement of autonomous driving technology, the requirements for vehicle trajectory tracking performance are becoming increasingly higher. Traditional linear control methods, such as proportional-integral-derivative (PID) controllers, although simple and easy to implement, often show limitations when faced with complex road environments, especially when fast response or precise control is required. In recent years, researchers have proposed a variety of nonlinear control strategies, such as sliding mode control and model predictive control, in order to improve the quality of trajectory tracking. However, these methods are usually accompanied by high computational costs and have certain challenges in parameter adjustment.

[0003] In order to achieve safe and reliable autonomous driving, it is necessary to accurately model the motion of the vehicle and design an efficient control strategy based on this. Traditional vehicle dynamics modeling methods are usually differential equations represented by Newtonian mechanics and Lagrange equations / Hamiltonian equations represented by analytical mechanics. The vehicle dynamics equations are derived through physical laws, providing the change process of the vehicle dynamic system. Many models have been proposed so far, including 2-DOF vehicle models, 3-DOF vehicle models, 7-DOF vehicle models, etc. Among them, the 3-DOF vehicle model only considers three degrees of freedom: longitudinal displacement, lateral displacement and heading angle. It has low computational complexity and high computational efficiency, and can well capture the basic dynamic characteristics of the vehicle, especially for low-speed and medium-speed operations. It has good stability and robustness. Many literatures have conducted vehicle parameter identification and trajectory planning control based on this model. The physics-based modeling and control method has good stability and interpretability, and has achieved great success in linear control applications. However, when the driving speed of the autonomous vehicle increases, the external interference will become huge, and the dynamic characteristics of the vehicle will become more complex, so it is difficult to derive the dynamic equations of the vehicle using traditional methods. Furthermore, these approaches are not applicable to highly nonlinear systems because controllers based on simplified physical models cannot accurately capture vehicle dynamics, resulting in degraded control performance or even vehicle loss of control.

[0004] In recent years, the use of artificial intelligence technologies such as neural networks to build vehicle dynamics models has become a research hotspot. The main methods include MPC control methods based on Gaussian process regression, deep learning frameworks based on Koopman operators, and data segmentation modeling methods based on deep neural networks (DNNs). Neural networks have powerful nonlinear fitting capabilities and can fit various nonlinear dynamic models well. However, these methods lack interpretability, and the model accuracy is affected by factors such as training time and network hyperparameters. Therefore, many researchers have tried to combine physical principles with data-driven solutions to combine the advantages of both. Spielberg et al. first combined feedforward and feedback control based on physical models, using neural networks to predict lateral acceleration and yaw rate using a series of past states and inputs. Wang et al. proposed a data-driven autonomous vehicle modeling and controller optimization method. Although these methods enhance traditional physical model-based control, neural networks are still black boxes and rely on closed loops built by the original traditional methods as safety protection. The main reason is that the neural networks used in most solutions have no physical constraints and rationality.

[0005] The recently emerged Neural ODEs provide a novel approach to deal with continuous-time dynamical systems. This type of approach directly learns the law of state change over time through neural networks without assuming a specific functional form in advance, so it is very suitable for complex systems that are difficult to describe with traditional analytical methods. Combining Neural ODEs with advanced control theory is expected to further improve the performance of autonomous vehicles, especially in dealing with uncertainty and external disturbances.

[0006] In addition, Lie group theory provides a powerful mathematical tool for dealing with problems such as rigid body motion and attitude estimation; this method can more intuitively express changes in vehicle attitude and help simplify certain types of control problems. Specifically, the special Euclidean group SE(3) can be used to represent rigid body transformations in three-dimensional space, which is very useful when describing changes in vehicle posture. Applying Lie group theory to vehicle trajectory tracking can more naturally express changes in vehicle position and attitude, thereby improving the geometric consistency and computational efficiency of the model.

[0007] Although some work has begun to explore the application of the above technologies in vehicle control, there is still a lack of a systematic approach that can effectively integrate the advantages of Lie group analysis, Hamiltonian neural networks, and model predictive control to solve the problem of high-precision trajectory tracking. MPC is an advanced control strategy that generates control inputs through an online optimization process and is particularly suitable for handling control systems with constraints. However, how to combine MPC with the aforementioned advanced technologies and ensure the real-time performance of the entire system in actual operation remains a challenge.

[0008] In summary, although the existing technology has solved some trajectory tracking problems to a certain extent, it still lacks a solution that is both clear and concise in theory and can effectively cope with various challenges in practical applications. Summary of the invention

[0009] In view of the above defects or improvement needs of the prior art, the present invention provides a vehicle trajectory tracking model predictive control method and device based on Lie group Hamiltonian dynamics and Neural ordinary differential equations, which aims to solve the problem that the existing methods are not applicable to complex and changeable road environments.

[0010] To achieve the above object, according to one aspect of the present invention, a vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations is provided, and the method comprises the following steps:

[0011] S1, describes the posture and motion of the vehicle under the Lie group and determines the vehicle state parameters;

[0012] S2, based on port-Hamiltonian mechanics and generalized coordinates and generalized velocity, establish the vehicle dynamics equations on Lie groups;

[0013] S3, constructing a neural ordinary differential equation based on the obtained vehicle dynamics equation, applying an ordinary differential equation solver to integrate the derivatives of the generalized coordinates and the derivatives of the generalized velocity to obtain predicted values ​​of the generalized coordinates and the generalized velocity, comparing the predicted values ​​of the generalized coordinates and the generalized velocity with the actual values ​​to calculate a mean square error, which is then used for back propagation to update and optimize the parameters of the neural ordinary differential equation;

[0014] S4, uses the vehicle dynamics model obtained by training based on Neural Ordinary Differential Equations to describe the change of vehicle state over time, and then uses model predictive control to perform vehicle trajectory tracking control.

[0015] Furthermore, the posture of the vehicle's own coordinate system xOy in the world inertial coordinate system XOY is determined by the translation position of the vehicle's center of mass The kinematic equation of the vehicle is determined by the linear velocity of the vehicle's own coordinate system relative to the world inertial coordinate system. and angular velocity Determine, the generalized speed is The generalized coordinates are q=(p c ,R).

[0016] Furthermore, the vehicle dynamics equations on the Lie group are expressed in the Port-Hamiltonian form as:

[0017]

[0018] Where H(q,p) is the Hamiltonian function of the vehicle, M -1 (q) and V(q) are the inverse matrix and potential energy of the vehicle’s mass matrix on the Lie group, respectively; M v (q) is related to the vehicle mass, while M ω (q) is related to the vehicle's moment of inertia. The vehicle's mass and moment of inertia are usually constants and are independent of the generalized coordinate q, and M(q) is a positive definite matrix; p v , p ω are the linear momentum and angular momentum of the vehicle respectively, and the generalized momentum is The relationship between generalized momentum and generalized velocity is D v (q, p) and D ω (q, p) respectively correspond to the linear momentum p v and angular momentum p ω , the energy dissipation matrix is D(q,p) represents all uncertainties in the vehicle system; b v (q), b ω (q) are related to the linear velocity and angular velocity respectively, and the control input matrix B(q) = [b v (q) T b ω (q) T ] T ; u is the control input force or torque.

[0019] Furthermore, step S3 includes the following sub-steps:

[0020] Step 1: Create PINADEs based on the vehicle dynamics equations on the Lie group, and calculate the derivatives of the generalized coordinates and the derivatives of the generalized velocity;

[0021] Step 2: According to the ordinary differential equation framework, the ordinary differential equation solver is applied to PINADEs to select the initial values ​​of the generalized coordinates and generalized velocities, and the derivatives of the generalized coordinates and the derivatives of the generalized velocities are integrated to obtain the predicted values ​​of the generalized coordinates and the generalized velocities;

[0022] Step 3: Compare the predicted and actual values ​​of the generalized coordinates and generalized velocities to calculate the mean square error, which is then used for backpropagation to update and optimize the PINADEs parameters.

[0023] Furthermore, the construction steps of PINADEs are:

[0024] (1) According to the physical information of mass matrix, energy dissipation matrix, potential energy and control input matrix, a mass neural network, an energy dissipation neural network, a potential energy neural network and a control input neural network are established respectively;

[0025] (2) Inputting the generalized coordinates into the mass neural network, the control input neural network, and the potential energy neural network to obtain the mass matrix, the potential energy matrix, and the control input matrix, respectively; inputting the generalized coordinates and the generalized velocity into the energy dissipation neural network to obtain the energy dissipation matrix;

[0026] (3) Substituting the mass matrix and potential energy into the vehicle's Hamiltonian function, and inputting the generalized momentum obtained by multiplying the mass matrix and the generalized velocity to obtain the vehicle's Hamiltonian;

[0027] (4) Automatically differentiating the Hamiltonian of the vehicle with respect to the generalized coordinates to obtain the partial differential of the Hamiltonian with respect to the generalized coordinates; Automatically differentiating the Hamiltonian of the vehicle with respect to the generalized momentum to obtain the partial differential of the Hamiltonian with respect to the generalized momentum;

[0028] (5) Substituting the energy dissipation matrix, the control input matrix, the partial differential of the Hamiltonian with respect to the generalized coordinates, the partial differential of the Hamiltonian with respect to the generalized momentum, the driving force, the generalized coordinates, and the generalized momentum into the vehicle dynamics equation on the Lie group, and outputting the derivative values ​​of the generalized coordinates and the derivative values ​​of the generalized momentum respectively;

[0029] (6) Invert the mass matrix, and then calculate the automatic differentiation of the inverse matrix of the mass matrix with respect to the generalized coordinates to obtain the derivative of the inverse matrix of the mass matrix with respect to the generalized coordinates. The derivative value is multiplied with the derivative value of the generalized coordinates to obtain the derivative of the inverse matrix of the mass matrix. According to the relationship between generalized momentum and generalized velocity, substitute the mass matrix, the derivative of the generalized momentum, the derivative of the inverse matrix of the mass matrix, and the generalized momentum obtained by multiplying the mass matrix and the generalized velocity into the relationship between the generalized momentum and the generalized velocity to obtain the derivative of the generalized velocity, and then obtain the neural ordinary differential equation.

[0030] Integrate the Neural ODE to solve it forward:

[0031]

[0032] Where θ is the neural network parameter; N(x, t, θ) is the neural network fitting function f(x).

[0033] Furthermore, the loss of the Lie group Hamiltonian ordinary differential equation is divided into the loss of the rotation matrix geometric distance, the loss of the generalized velocity and the loss of the translation position. The total loss is:

[0034]

[0035] Furthermore, step S4 includes the following sub-steps:

[0036] (1) Based on the vehicle dynamics model trained by the Lie group Hamiltonian ordinary differential equation, describe the change of vehicle state over time and calculate the predicted values ​​of generalized speed and generalized coordinates;

[0037] (2) Select the prediction time domain T p , control time domain T c and sampling period T s ;

[0038] (3) Based on the desired trajectory, define a cost function including the tracking error between the actual trajectory and the desired trajectory and a penalty term for the control input;

[0039]

[0040] In the formula, is the predicted value of the vehicle system state quantity, is the predicted control quantity, x r is the reference signal, Q and R are the weight coefficients for the state deviation and control input penalty respectively;

[0041] (4) Selecting external force constraints and speed constraints as optimization constraints;

[0042] F min <F<F max , 0<v x <40,-20<v y <20,-2<ω<2

[0043] (5) In each sampling period, a sequential quadratic programming method is used to find the control sequence that minimizes the cost function;

[0044] (6) Take the first control action from the optimized control sequence and apply it to the actual controller, and then wait for the next sampling period to repeat.

[0045] The present invention also provides a vehicle trajectory tracking model predictive control system based on Lie group Hamiltonian dynamics and Neural ordinary differential equations. The system includes a memory and a processor. The memory stores a computer program. When the processor executes the computer program, it executes the vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations as described above.

[0046] The present invention also provides a computer-readable storage medium, which stores machine-executable instructions. When the machine-executable instructions are called and executed by a processor, the machine-executable instructions prompt the processor to implement the vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations as described above.

[0047] In general, compared with the prior art, the above technical solutions conceived by the present invention mainly have the following beneficial effects:

[0048] 1. The method combines Lie group Hamiltonian dynamics, neural ordinary differential equations (physical information embedded in neural ordinary differential equations, PINADEs) and model predictive control (MPC). It can fully utilize the geometric advantages provided by Lie group theory, use Hamiltonian neural networks to establish an accurate dynamic model, and use MPC strategy to perform trajectory tracking control, thereby improving accuracy and applicability.

[0049] 2. By using Lie group structures to accurately describe the vehicle's posture changes, including changes in position, heading angle, pitch angle, and roll angle, a dynamic model in a Hamiltonian framework that can naturally handle rotation and translation operations is constructed. By combining the established high-precision vehicle dynamics model and model predictive control to achieve accurate path tracking, this comprehensive approach is particularly suitable for autonomous vehicles and can maintain stability and accuracy in complex and changing road environments.

[0050] 3. Based on the vehicle Hamiltonian dynamics equation under the Lie group, the Lie group Hamiltonian ordinary differential equation method is used to learn the dynamics of the vehicle. It aims to embed physical information, capture the patterns in the data more easily, respect the laws of physics, thereby improving the accuracy, reliability and interpretability of the prediction, improving the generalization of the model, and simplifying the neural network structure.

[0051] 4. After learning the dynamics of the vehicle based on the Lie group Hamiltonian ordinary differential equation, the model predictive control method is used to perform vehicle trajectory tracking control, which aims to better integrate with the vehicle dynamics model learned by the Lie group Hamiltonian ordinary differential equation, and can improve the model and enhance the adaptability and robustness of the method. At the same time, MPC can simultaneously consider the influence of multiple inputs (such as steering angle, acceleration / deceleration, etc.) and optimize the coordination between them to achieve the best trajectory tracking performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 is a schematic diagram of the vehicle model;

[0053] Figure 2 is a schematic diagram of a quality neural network;

[0054] Figure 3 It is a schematic diagram of Lie group Hamiltonian neural network ordinary differential equation;

[0055] Figure 4is a schematic diagram of the model predictive control framework;

[0056] Figure 5 It is a flow chart of a vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations provided by an embodiment of the present invention;

[0057] Figure 6 is the training loss curve;

[0058] Figure 7 is the predicted loss curve. DETAILED DESCRIPTION

[0059] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0060] The present invention provides a vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural Ordinary Differential Equations. The method combines Lie group Hamiltonian dynamics, Neural Ordinary Differential Equations (physical information embedded in Neural Ordinary Differential Equations, PINADEs) with Model Predictive Control (MPC). While making full use of the geometric advantages provided by Lie group theory, it can use Hamiltonian neural networks to establish an accurate dynamic model and use MPC strategies to perform trajectory tracking control. By using Lie group structures to accurately describe the posture transformation of the vehicle, including changes in position, heading angle, pitch angle, and roll angle, a dynamic model in a Hamiltonian framework that can naturally handle rotation and translation operations is constructed. Accurate path tracking is achieved by combining the established high-precision vehicle dynamics model and model predictive control. This comprehensive method is particularly suitable for autonomous driving vehicles and can maintain stability and accuracy in complex and changing road environments.

[0061] The method mainly comprises the following steps:

[0062] S1, describes the posture and motion of the vehicle under the Lie group and determines the vehicle state parameters.

[0063] The vehicle state parameters include generalized coordinates and generalized speed.

[0064] See also Figure 1 , vehicle mass m, moment of inertia about the z axis I z Vehicle control input force: longitudinal force F along the x-axis x , lateral force F along the y-axis y, the yaw torque M around the z-axis z The vehicle state parameters include: the longitudinal displacement x along the x-axis, the lateral displacement y along the y-axis, the vertical displacement z along the z-axis, the yaw angle around the z-axis and the longitudinal velocity of the vehicle along the x-axis Lateral velocity along the y axis Vertical velocity along the z axis Angular velocity around the z-axis Etc. The distances a and b from the front and rear axles to the vehicle's center of gravity.

[0065] The attitude of the vehicle's own coordinate system xOy in the world inertial coordinate system XOY is determined by the translation position of the vehicle's center of mass The direction of the vehicle's own coordinate system axis is determined by the rotation matrix R:

[0066]

[0067] The kinematic equation of the vehicle is given by the linear velocity of the vehicle's own coordinate system relative to the world inertial coordinate system and angular velocity

[0068] is determined by the vehicle's own coordinates. Therefore, the generalized velocity is The generalized coordinates are q=(p c ,R).

[0069] S2, the vehicle dynamics equations on Lie groups are established based on port-Hamiltonian mechanics and generalized coordinates and generalized velocities.

[0070] The vehicle dynamics equations on the Lie group are expressed in the Port-Hamiltonian form as:

[0071]

[0072] Where H(q,p) is the Hamiltonian function of the vehicle, M -1 (q) and V(q) are the inverse matrix and potential energy of the vehicle's mass matrix on the Lie group, respectively. The potential energy in the present invention is a constant. When the vehicle's own coordinate system is the same as the mass center, the mass matrix is ​​in block diagonal form:

[0073] in M v (q) is related to the vehicle mass, while M ω (q) is related to the vehicle's moment of inertia. The vehicle's mass and moment of inertia are usually constants and are independent of the generalized coordinate q, and M(q) is a positive definite matrix.

[0074] p v , p ωare the linear momentum and angular momentum of the vehicle respectively, and the generalized momentum is The relationship between generalized momentum and generalized velocity is

[0075] D v (q, p) and D ω (q, p) respectively correspond to the linear momentum p v and angular momentum p ω , the energy dissipation matrix is D(q,p) represents all possible uncertainties in the vehicle system, including friction between the tire and the ground, nonlinear dynamic effects between vehicle systems, external forces that interfere with motion, sensor errors, and model errors. The energy dissipation matrix D(q,p) of the vehicle is usually related to the generalized coordinates q and the generalized velocity ζ, and D(q,p) is a semi-positive matrix that only affects the generalized momentum p.

[0076] b v (q), b ω (q) are related to the linear velocity and angular velocity respectively, and the control input matrix B(q) = [b v (q) T b ω (q) T ] T . u is the control input force or torque.

[0077] S3, constructing a neural ordinary differential equation based on the obtained vehicle dynamics equation, applying an ordinary differential equation solver to integrate the derivatives of the generalized coordinates and the derivatives of the generalized velocity to obtain predicted values ​​of the generalized coordinates and the generalized velocity, comparing the predicted values ​​of the generalized coordinates and the generalized velocity with the actual values ​​to calculate the mean square error, which is then used for backpropagation to update and optimize the parameters of the neural ordinary differential equation.

[0078] In this embodiment, according to the vehicle Hamiltonian dynamics equation under the Lie group, the Lie group Hamiltonian neural ordinary differential equation method is used to learn the dynamics of the vehicle, which aims to embed physical information to more easily capture the patterns in the data and respect the laws of physics, thereby improving the accuracy, reliability and interpretability of the prediction, improving the generalization of the model, and simplifying the neural network structure.

[0079] Step S3 includes the following sub-steps:

[0080] Step 1: Create PINADEs based on the vehicle dynamics equations on the Lie group, and calculate the derivatives of the generalized coordinates and the derivatives of the generalized velocity;

[0081] Step 2: According to the ordinary differential equation framework, the ordinary differential equation solver is applied to PINADEs to select the initial values ​​of the generalized coordinates and generalized velocities, and the derivatives of the generalized coordinates and the derivatives of the generalized velocities are integrated to obtain the predicted values ​​of the generalized coordinates and the generalized velocities;

[0082] Step 3: Compare the predicted and actual values ​​of the generalized coordinates and generalized velocities to calculate the mean square error, which is then used for backpropagation to update and optimize the PINADEs parameters. The data loss includes the error of the rotation matrix, the error of the generalized velocity, and the error of the translation position.

[0083] See also Figure 2 and Figure 3 , the construction steps of PINADEs are:

[0084] (1) According to the physical information of mass matrix, energy dissipation matrix, potential energy and control input matrix, the mass neural network, energy dissipation neural network, potential energy neural network and control input neural network are established respectively.

[0085] According to the mass matrix is ​​positive definite, and M v (q) and M ω (q) is a constant, a mass neural network M(q,θ) is established with input as generalized coordinates and output as mass matrix using a fully connected network, and two network parameters initialized as vehicle mass and one network parameter initialized as vehicle moment of inertia are added to the mass neural network. At the same time, to ensure the positive definiteness of the mass matrix, a small constant ε is added to the diagonal elements of the output in the forward calculation of the mass neural network. According to the characteristics that the energy dissipation matrix is ​​semi-positive definite and that the energy dissipation of the vehicle is related to the generalized coordinates and generalized speed of the vehicle, an energy dissipation neural network D(q,p,θ) is established with input as generalized coordinates and generalized speed and output as energy dissipation matrix using a fully connected network. Potential energy is fitted using a fully connected network, and a potential energy neural network V(q,θ) is established with input as generalized coordinates and output as potential energy, and a new network parameter initialized as vehicle potential energy is added. The control input matrix is ​​fitted using a fully connected network, and a control input neural network is established with input as generalized coordinates and output as control input matrix.

[0086] The mass matrix M(q) is positive definite. According to Cholesky decomposition, the mass matrix can be decomposed into the product of the lower triangular matrix L(q,θ) with non-negative diagonal elements and the transpose of L(q,θ), where θ is the neural network parameter. Therefore, the number of output nodes of the mass neural network can be determined to be 21. Since the mass matrix of the vehicle can be divided into M related to the mass v (q) and M related to the moment of inertia ω (q), and M v (q), M ω(q) is a fixed value not related to the generalized coordinate q, and these values ​​can be iteratively optimized as part of the quality neural network parameters θ instead of being considered as output nodes. v (q), M ω The part of (q) is 0, so the number of output nodes of the quality neural network can be further reduced to 0. In order to ensure that the quality neural network is positive, a small constant ε is added to the diagonal elements, as shown in the following formula

[0087] M(q,θ)=L(q,θ)L T (q,θ)+ε

[0088] The energy dissipation matrix of the vehicle is a comprehensive indicator of the dynamic behavior of the system, which is related to the generalized coordinates and generalized speed of the vehicle. Therefore, an energy dissipation neural network with generalized coordinates and generalized speed as input can be established. Because the generalized momentum p is related to the generalized speed, the input of the energy dissipation neural network D(q, p, θ) can convert the generalized momentum into the generalized speed. Since the generalized momentum cannot be measured directly, the present invention establishes an energy dissipation neural network with generalized coordinates and generalized speed as input. According to the energy dissipation matrix being semi-positive definite, the structure of the energy dissipation neural network can refer to the mass neural network and be decomposed into a lower triangular matrix L with non-negative diagonal elements according to Cholesky. D (q, θ) and L D The product of the transpose of (q, θ), so the number of output nodes of the energy dissipation neural network is 21.

[0089] The potential energy of the present invention is a certain value, so a network parameter is created to fit the potential energy, resulting in the potential energy neural network output node being 0. The dimension of the control input matrix is ​​6 rows and 3 columns, so the control input neural network output node is 18.

[0090] (2) The generalized coordinates are input into the mass neural network, the control input neural network, and the potential energy neural network to obtain the mass matrix, potential energy, and control input matrix, respectively; the generalized coordinates and generalized velocity are input into the energy dissipation neural network to obtain the energy dissipation matrix.

[0091] (3) Substitute the mass matrix and potential energy into the vehicle’s Hamiltonian function, and input the generalized momentum obtained by multiplying the mass matrix and the generalized velocity to obtain the vehicle’s Hamiltonian.

[0092] (4) Automatically differentiating the vehicle's Hamiltonian with respect to the generalized coordinates to obtain the partial differential of the Hamiltonian with respect to the generalized coordinates; and automatically differentiating the vehicle's Hamiltonian with respect to the generalized momentum to obtain the partial differential of the Hamiltonian with respect to the generalized momentum.

[0093] (5) Substitute the energy dissipation matrix, the control input matrix, the partial differential of the Hamiltonian with respect to the generalized coordinates, the partial differential of the Hamiltonian with respect to the generalized momentum, the driving force, the generalized coordinates, and the generalized momentum into the vehicle dynamics equation on the Lie group, and output the derivative values ​​of the generalized coordinates and the derivative values ​​of the generalized momentum, respectively.

[0094] (6) Invert the mass matrix, and then calculate the automatic differentiation of the inverse matrix of the mass matrix with respect to the generalized coordinates to obtain the derivative of the inverse matrix of the mass matrix with respect to the generalized coordinates. The derivative value is multiplied with the derivative value of the generalized coordinates to obtain the derivative of the inverse matrix of the mass matrix. According to the relationship between generalized momentum and generalized velocity, substitute the mass matrix, the derivative of the generalized momentum, the derivative of the inverse matrix of the mass matrix, and the generalized momentum obtained by multiplying the mass matrix and the generalized velocity into the relationship between the generalized momentum and the generalized velocity to obtain the derivative of the generalized velocity, and then obtain the neural ordinary differential equation.

[0095] According to the form of the Neural Ordinary Differential Equation, the Hamiltonian equation is integrated to obtain a forward solution.

[0096]

[0097] Where θ is the neural network parameter; N(x, t, θ) is the neural network fitting function f(x).

[0098] The loss of Lie group Hamiltonian ordinary differential equation can be divided into the loss of rotation matrix geometric distance, the loss of generalized velocity and the loss of translation position. The total loss is:

[0099]

[0100] S4, uses the vehicle dynamics model obtained by training based on Neural Ordinary Differential Equations to describe the change of vehicle state over time, and then uses model predictive control to perform vehicle trajectory tracking control.

[0101] After learning the dynamics of the vehicle based on the Lie Group Hamiltonian Ordinary Differential Equation, the model predictive control method is used to perform vehicle trajectory tracking control, which aims to better integrate the vehicle dynamics model learned by the Lie Group Hamiltonian Ordinary Differential Equation, and can improve the model and enhance the adaptability and robustness of the method. MPC can simultaneously consider the influence of multiple inputs (such as steering angle, acceleration / deceleration, etc.) and optimize the coordination between them to achieve the best trajectory tracking performance.

[0102] See also Figure 4 , step S4 includes the following sub-steps:

[0103] (1) Based on the vehicle dynamics model trained by the Lie group Hamiltonian ordinary differential equation, describe the change of vehicle state over time and calculate the predicted values ​​of generalized speed and generalized coordinates;

[0104] (2) Select the prediction time domain T p , control time domain T c and sampling period T s ;

[0105] (3) Based on the desired trajectory, define a cost function including the tracking error between the actual trajectory and the desired trajectory and a penalty term for the control input;

[0106]

[0107] In the formula, is the predicted value of the vehicle system state quantity, is the predicted control quantity, x r is the reference signal, Q and R are the weight coefficients for the state deviation and control input penalty respectively.

[0108] (4) Selecting external force constraints and speed constraints as optimization constraints;

[0109] F min <F<F max , 0<v x <40,-20<v y <20,-2<ω<2

[0110] (5) In each sampling period, a sequential quadratic programming method is used to find the control sequence that minimizes the cost function;

[0111] (6) Take the first control action from the optimized control sequence and apply it to the actual controller, then wait for the next sampling period to repeat the above process.

[0112] MPC works in a rolling optimization mode, that is, it only executes one control action at a time, then re-evaluates the current state and optimizes again. This method enables MPC to adapt to the ever-changing working environment and adjust the control strategy in time.

[0113] The present invention is further described in detail below with reference to specific embodiments.

[0114] See also Figure 5 , the embodiment of the present invention includes the following steps:

[0115] S1, data needs to be collected to obtain a data set for PIONDEs training. The specific steps are as follows:

[0116] (1) Collecting vehicle state parameters and vehicle control input force during the vehicle's motion. When collecting data, the control frequency (collection control input force frequency) is required to be lower than the vehicle state parameter sampling frequency. In the embodiment of the present invention, the vehicle state parameter sampling frequency is selected to be 500 Hz and the control frequency is selected to be 20 Hz.

[0117] (2) The data is preprocessed by first-order filtering to remove outliers and convert the vehicle state parameters into a Lie group representation (see Table 1). This yields a data set for training using the Lie group Hamiltonian ordinary differential equation. 70% of the data set is used as a training set and 30% as a test set.

[0118] Table 1 General expressions and expressions under Lie groups

[0119]

[0120] S2, after obtaining the data set, starts training PINADEs to learn vehicle dynamics. The specific steps are as follows:

[0121] Step 1: Create PINADEs based on the vehicle dynamics equations on the Lie group and output the derivatives of the generalized coordinates and the derivatives of the generalized velocity;

[0122] Step 2: According to the ordinary differential equation framework, the Runge-Kutta fourth-order ordinary differential equation solver is applied to PINADEs, the vehicle state data at time 0 in the data set is selected as the initial value, the derivatives of the generalized coordinates and the derivatives of the generalized velocity are integrated from time 0 to 0.01, the time interval is 0.002s, and the predicted values ​​of the generalized coordinates and generalized velocity are output;

[0123] Step 3: Compare the predicted and actual values ​​of the generalized coordinates and generalized velocities, calculate the mean square error, and use it for backpropagation to update and optimize the PINADEs parameters. The data loss formula includes the error of the rotation matrix, the error of the generalized velocity, and the error of the translation position. The final training loss is as follows: Figure 6 shown.

[0124] Step 4: Substitute the test set for test verification, and predict the loss as follows Figure 7 As shown in Table 2, the maximum loss is 0.118, which meets the required accuracy, and the parameters of the trained quality neural network, energy dissipation neural network, control input neural network, and potential energy neural network, such as the number of network layers and the number of nodes, are saved. Table 2 shows the training results of PINADEs.

[0125] The specific steps of creating the PINADEs are as follows:

[0126] (1) According to the physical information of the mass matrix, energy dissipation matrix, potential energy, and control input matrix, a mass neural network, an energy dissipation neural network, a potential energy neural network, and a control input neural network are established respectively; for the mass neural network, the input nodes are designed to be 12 and the output nodes are 0, and two updateable mass parameters of 1000 are initialized as the 1st row and 1st column elements and the 2nd row and 2nd column elements of the output mass matrix respectively, and an updateable moment of inertia parameter of 300 is used as the 6th row and 6th column element of the output mass matrix (assuming that the vehicle mass of the embodiment of the present invention is 1000kg and the moment of inertia is 300kg.m2), and their gradient tracking is enabled, and ε=10-3 is selected; for the energy dissipation neural network, the input nodes are designed to be 18 and the output nodes are 21; for the potential energy neural network, the input nodes are designed to be 12 and the output nodes are 0, and an updateable potential energy parameter of 5000 is initialized as the output of the network, and its gradient tracking is enabled; for the control input neural network, the input nodes are designed to be 12 and the output nodes are 18.

[0127] (2) Input the generalized coordinate q, and output the mass matrix, potential energy, and control input matrix through the mass neural network, potential energy neural network, and control input neural network; input the generalized coordinate q and generalized velocity ζ, and output the energy dissipation matrix through the energy dissipation neural network;

[0128] (3) The generalized momentum is obtained by multiplying the mass matrix and the generalized velocity. The generalized momentum is combined with the mass matrix and the potential energy and substituted into the Hamiltonian function of the vehicle to obtain the Hamiltonian of the vehicle.

[0129] (4) The vehicle's Hamiltonian is solved for partial differentials of generalized coordinates and generalized momentum by automatic differentiation, and the partial differentials of the Hamiltonian with respect to the generalized coordinates and the partial differentials of the Hamiltonian with respect to the generalized momentum are obtained;

[0130] (5) Substituting the energy dissipation matrix, the control input matrix, the partial differential of the Hamiltonian with respect to the generalized coordinates, the partial differential of the Hamiltonian with respect to the generalized momentum, the control input force, the generalized coordinates and the generalized momentum into the vehicle dynamics equations on the Lie group, the derivatives of the generalized coordinates and the derivatives of the generalized momentum are obtained;

[0131] (6) Inverting the mass matrix, and then calculating the automatic differentiation of the inverse matrix of the mass matrix with respect to the generalized coordinates to obtain the derivative of the inverse matrix of the mass matrix with respect to the generalized coordinates, and multiplying the derivative value with the derivative value of the generalized coordinates to obtain the derivative of the inverse matrix of the mass matrix; according to the relationship between the generalized momentum and the generalized velocity, substituting the mass matrix, the derivative of the generalized momentum, the derivative of the inverse matrix of the mass matrix, and the generalized momentum obtained by multiplying the mass matrix and the generalized velocity into the relationship between the generalized momentum and the generalized velocity, to obtain the derivative of the generalized velocity;

[0132] Table 2 PINADEs training results

[0133]

[0134] S3, after training and verification, the network parameters of the Lie group Hamiltonian ordinary differential equation are obtained, that is, the vehicle dynamics equation on the Lie group is obtained, and then the model predictive control method is used for trajectory tracking. The specific implementation steps are:

[0135] (1) Substitute the verified parameters into the vehicle dynamics equation on the Lie group as the prediction model in MPC. Integrate the dynamics equation to predict the state change of the vehicle in the future.

[0136] (2) According to actual needs, set the prediction time domain to 2s and the control time domain to 0.1s;

[0137] (3) Calculate the difference between the desired trajectory and the actual trajectory based on the quadratic form, add a penalty term to the control input, and add the two terms to get the total cost;

[0138] (4) Based on the physical constraints, in each sampling period, the quadratic programming method is used to solve the optimization problem in the finite time domain and find the control sequence that minimizes the cost function;

[0139] (5) extracting the first control action from the optimized control sequence, converting it into a specific control command and sending it to the actuator;

[0140] (6) After executing the control action, the vehicle status continues to be monitored and the above MPC control process is repeated to form a closed-loop control system. By continuously adjusting the control input, the vehicle is ensured to travel along the desired trajectory.

[0141] The present invention also provides a vehicle trajectory tracking model predictive control system based on Lie group Hamiltonian dynamics and Neural ordinary differential equations. The system includes a memory and a processor. The memory stores a computer program. When the processor executes the computer program, it executes the vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations as described above.

[0142] The present invention also provides a computer-readable storage medium, which stores machine-executable instructions. When the machine-executable instructions are called and executed by a processor, the machine-executable instructions prompt the processor to implement the vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations as described above.

[0143] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations, characterized in that: The method comprises the following steps: S1, describes the posture and motion of the vehicle under the Lie group and determines the vehicle state parameters; S2, based on port-Hamiltonian mechanics and generalized coordinates and generalized velocity, establish the vehicle dynamics equations on Lie groups; S3, constructing a neural ordinary differential equation based on the obtained vehicle dynamics equation, applying an ordinary differential equation solver to integrate the derivatives of the generalized coordinates and the derivatives of the generalized velocity to obtain predicted values ​​of the generalized coordinates and the generalized velocity, comparing the predicted values ​​of the generalized coordinates and the generalized velocity with the actual values ​​to calculate a mean square error, which is then used for back propagation to update and optimize the parameters of the neural ordinary differential equation; S4, uses the vehicle dynamics model obtained by training based on Neural Ordinary Differential Equations to describe the change of vehicle state over time, and then uses model predictive control to perform vehicle trajectory tracking control.

2. The vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations as claimed in claim 1, characterized in that: The attitude of the vehicle's own coordinate system xOy in the world inertial coordinate system XOY is determined by the translation position of the vehicle's center of mass The kinematic equation of the vehicle is determined by the linear velocity of the vehicle's own coordinate system relative to the world inertial coordinate system. and angular velocity Determine, the generalized speed is The generalized coordinates are q=(p c ,R).

3. The vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Godly ordinary differential equations as claimed in claim 2, characterized in that: The vehicle dynamics equations on the Lie group are expressed in the Port-Hamiltonian form as: Where H(q,p) is the Hamiltonian function of the vehicle, V(q) are the inverse matrix and potential energy of the vehicle’s mass matrix on the Lie group; M v (q) is related to the vehicle mass, while M ω (q) is related to the vehicle's moment of inertia. The vehicle's mass and moment of inertia are usually constants and are independent of the generalized coordinate q, and M(q) is a positive definite matrix; p v , p ω are the linear momentum and angular momentum of the vehicle respectively, and the generalized momentum is The relationship between generalized momentum and generalized velocity is D v (q, p) and D ω (q, p) respectively correspond to the linear momentum p v and angular momentum p ω , the energy dissipation matrix is D(q,p) represents all uncertainties in the vehicle system; b v (q), b ω (q) are related to the linear velocity and angular velocity respectively, and the control input matrix B(q) = [b v (q) T b ω (q) T ] T ; u is the control input force or torque.

4. The vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations as claimed in claim 1, characterized in that: Step S3 includes the following sub-steps: Step 1: Create PINADEs based on the vehicle dynamics equations on the Lie group, and calculate the derivatives of the generalized coordinates and the derivatives of the generalized velocity; Step 2: According to the ordinary differential equation framework, the ordinary differential equation solver is applied to PINADEs to select the initial values ​​of the generalized coordinates and generalized velocities, and the derivatives of the generalized coordinates and the derivatives of the generalized velocities are integrated to obtain the predicted values ​​of the generalized coordinates and the generalized velocities; Step 3: Compare the predicted and actual values ​​of the generalized coordinates and generalized velocities to calculate the mean square error, which is then used for backpropagation to update and optimize the PINADEs parameters.

5. The vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations as claimed in claim 4, characterized in that: The construction steps of PINODEs are: (1) According to the physical information of mass matrix, energy dissipation matrix, potential energy and control input matrix, a mass neural network, an energy dissipation neural network, a potential energy neural network and a control input neural network are established respectively; (2) Inputting the generalized coordinates into the mass neural network, the control input neural network, and the potential energy neural network to obtain the mass matrix, the potential energy matrix, and the control input matrix, respectively; inputting the generalized coordinates and the generalized velocity into the energy dissipation neural network to obtain the energy dissipation matrix; (3) Substituting the mass matrix and potential energy into the vehicle's Hamiltonian function, and inputting the generalized momentum obtained by multiplying the mass matrix and the generalized velocity to obtain the vehicle's Hamiltonian; (4) Automatically differentiating the Hamiltonian of the vehicle with respect to the generalized coordinates to obtain the partial differential of the Hamiltonian with respect to the generalized coordinates; Automatically differentiating the Hamiltonian of the vehicle with respect to the generalized momentum to obtain the partial differential of the Hamiltonian with respect to the generalized momentum; (5) Substituting the energy dissipation matrix, the control input matrix, the partial differential of the Hamiltonian with respect to the generalized coordinates, the partial differential of the Hamiltonian with respect to the generalized momentum, the driving force, the generalized coordinates, and the generalized momentum into the vehicle dynamics equation on the Lie group, and outputting the derivative values ​​of the generalized coordinates and the derivative values ​​of the generalized momentum respectively; (6) Invert the mass matrix, and then calculate the automatic differentiation of the inverse matrix of the mass matrix with respect to the generalized coordinates to obtain the derivative of the inverse matrix of the mass matrix with respect to the generalized coordinates. The derivative value is multiplied with the derivative value of the generalized coordinates to obtain the derivative of the inverse matrix of the mass matrix. According to the relationship between generalized momentum and generalized velocity, substitute the mass matrix, the derivative of the generalized momentum, the derivative of the inverse matrix of the mass matrix, and the generalized momentum obtained by multiplying the mass matrix and the generalized velocity into the relationship between the generalized momentum and the generalized velocity to obtain the derivative of the generalized velocity, and then obtain the neural ordinary differential equation.

6. The vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations as claimed in claim 5, characterized in that: According to the form of the Neural Ordinary Differential Equation, the Hamiltonian equation is integrated to solve it forward: Where θ is the neural network parameter; N(x, t, θ) is the neural network fitting function f(x).

7. The vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations as claimed in claim 1, characterized in that: The loss of the Lie group Hamiltonian ordinary differential equation is divided into the loss of the rotation matrix geometric distance, the loss of the generalized velocity and the loss of the translation position. The total loss is:

8. The vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations as claimed in claim 1, characterized in that: Step S4 includes the following sub-steps: (1) Based on the vehicle dynamics model trained by the Lie group Hamiltonian ordinary differential equation, describe the change of vehicle state over time and calculate the predicted values ​​of generalized speed and generalized coordinates; (2) Select the prediction time domain T p , control time domain T c and sampling period T s ; (3) Based on the desired trajectory, define a cost function including the tracking error between the actual trajectory and the desired trajectory and a penalty term for the control input; In the formula, is the predicted value of the vehicle system state quantity, is the predicted control quantity, x r is the reference signal, Q and R are the weight coefficients for the state deviation and control input penalty respectively; (4) Selecting external force constraints and speed constraints as optimization constraints; F min <F<F max ,0<v x <40,-20<v y <20,-2<ω<2 (5) In each sampling period, a sequential quadratic programming method is used to find the control sequence that minimizes the cost function; (6) Take the first control action from the optimized control sequence and apply it to the actual controller, and then wait for the next sampling period to repeat.

9. A vehicle trajectory tracking model predictive control system based on Lie group Hamiltonian dynamics and Neural ordinary differential equations, characterized by: The system includes a memory and a processor, wherein the memory stores a computer program, and when the processor executes the computer program, the vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations as described in any one of claims 1 to 8 is executed.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores machine-executable instructions. When the machine-executable instructions are called and executed by the processor, the machine-executable instructions prompt the processor to implement the vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and Neural ordinary differential equations as described in any one of claims 1-8.

Images