A vehicle trajectory tracking model control method and device based on Lie group Hamiltonian dynamics and neural ordinary differential equations

Abstract

This invention belongs to the technical field of vehicle trajectory control. It discloses a model predictive control method and device for vehicle trajectory tracking based on Lie group Hamiltonian dynamics and the NND (Neural Regular Differential Equation). The steps are as follows: S1, describing the vehicle's attitude and motion using a Lie group; S2, establishing vehicle dynamics equations on the Lie group; S3, constructing the NND based on the obtained vehicle dynamics equations, integrating the derivatives of the generalized coordinates and generalized velocities to obtain predicted values ​​of the generalized coordinates and velocities, comparing the predicted and actual values ​​to calculate the mean square error, and then using it for backpropagation to update and optimize the parameters of the NND; S4, using a vehicle dynamics model trained based on the NND to describe the vehicle's state changes over time, and then using model predictive control for vehicle trajectory tracking control. This invention improves applicability.

Description

Technical Field

[0001] This invention belongs to the technical field of vehicle trajectory control, and more specifically, relates to a predictive control method and device for vehicle trajectory tracking model based on Lie group Hamiltonian dynamics and neural ordinary differential equations. Background Technology

[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 stringent. Traditional linear control methods, such as proportional-integral-derivative (PID) controllers, while simple and easy to implement, often exhibit limitations when facing complex road environments, particularly in situations requiring rapid response or precise control. In recent years, researchers have proposed various nonlinear control strategies, such as sliding mode control and model predictive control, to improve trajectory tracking quality. However, these methods typically come with high computational costs and present certain challenges in parameter tuning.

[0003] To achieve safe and reliable autonomous driving, accurate modeling of vehicle motion is essential, followed by the design of efficient control strategies. Traditional vehicle dynamics modeling methods typically employ differential equations, such as those based on Newtonian mechanics, and Lagrange / Hamiltonian equations, such as those based on analytical mechanics. These methods derive vehicle dynamic equations from physical laws, providing a picture of the dynamic changes within the vehicle system. Numerous models have been proposed to date, including 2-DOF, 3-DOF, and 7-DOF vehicle models. The 3-DOF model, considering only three degrees of freedom—longitudinal displacement, lateral displacement, and heading angle—has low computational complexity and high efficiency, effectively capturing the vehicle's fundamental dynamic characteristics. It exhibits good stability and robustness, particularly for low- and medium-speed operation, and numerous studies have used this model for vehicle parameter identification and trajectory planning control. Physics-based modeling and control methods demonstrate good stability and interpretability, achieving significant success in linear control applications. However, as the speed of autonomous vehicles increases, external disturbances become substantial, and the vehicle's dynamic characteristics become more complex, making it difficult to derive the vehicle's dynamic equations using traditional methods. Furthermore, these methods are not suitable for highly nonlinear systems because controllers based on simplified physical models cannot accurately capture vehicle dynamics, leading to decreased control performance or even loss of vehicle control.

[0004] In recent years, constructing vehicle dynamics models using artificial intelligence technologies such as neural networks has become a research hotspot. The main methods include MPC control based on Gaussian process regression, deep learning frameworks based on the Koopman operator, and data segmentation modeling methods based on deep neural networks (DNNs). Neural networks possess powerful nonlinear fitting capabilities, effectively fitting various nonlinear dynamic models. However, these methods lack interpretability, and model accuracy is affected by factors such as training time and network hyperparameters. Therefore, many researchers have attempted to combine physical principles with data-driven solutions, combining 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 method for autonomous vehicle modeling and controller optimization. While these methods enhance traditional physical model-based control, the neural network remains a black box, relying on the closed loop constructed by traditional methods for safety protection. This is mainly because the neural networks used in most solutions lack physical constraints and rationality.

[0005] The recently emerging field of neural ODEs offers a novel approach to handling continuous-time dynamical systems. These methods directly learn the state's changes over time through neural networks without pre-assuming specific functional forms, making them well-suited for complex systems that are difficult to describe using traditional analytical methods. Combining Neural ODEs with advanced control theory holds promise for further improving the performance of autonomous vehicles, particularly in handling uncertainties and external disturbances.

[0006] Furthermore, Lie group theory provides a powerful mathematical tool for handling problems such as rigid body motion and attitude estimation; this method can more intuitively express changes in vehicle attitude and helps 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 in describing changes in vehicle pose. 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] While some work has begun exploring the application of these technologies in vehicle control, a systematic approach is currently lacking that effectively integrates the advantages of Lie group analysis, Hamiltonian neural networks, and model predictive control to solve the high-precision trajectory tracking problem. MPC is an advanced control strategy that generates control inputs through an online optimization process, making it particularly suitable for handling constrained control systems. However, combining MPC with the aforementioned advanced technologies and ensuring the real-time performance of the entire system in actual operation remains a challenge.

[0008] In summary, while existing technologies have solved some of the trajectory tracking problems to a certain extent, they still lack a solution that can remain clear and concise in theory while effectively addressing various challenges in practical applications. Summary of the Invention

[0009] To address the aforementioned deficiencies or improvement needs of existing technologies, this 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 existing methods cannot be applied to complex and ever-changing road environments.

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

[0011] S1 describes the vehicle's attitude and motion under a Lie group and determines the vehicle's state parameters;

[0012] S2. Based on port-Hamiltonian mechanics and generalized coordinates and generalized velocities, establish the vehicle dynamics equations on the Lie group;

[0013] S3. Based on the obtained vehicle dynamics equations, construct the ordinary differential equations of the God. Apply the ordinary differential equation solver to integrate the derivatives of the generalized coordinates and the generalized velocity to obtain the predicted values ​​of the generalized coordinates and the generalized velocity. Compare the predicted values ​​of the generalized coordinates and the generalized velocity with the actual values ​​to calculate the mean square error, and then use it for backpropagation to update and optimize the parameters of the God ordinary differential equations.

[0014] S4 uses a vehicle dynamics model trained based on the constant differential equation to describe the change of vehicle state over time, and then uses model predictive control to perform trajectory tracking control of the vehicle.

[0015] Furthermore, 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 rotation matrix R of the vehicle's own coordinate system axes is determined; the vehicle's kinematic equations are determined by the linear velocity of the vehicle's own coordinate system relative to the world inertial coordinate system. and angular velocity The decision is that 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 port-Hamiltonian form as follows:

[0017]

[0018] In the formula, H(q,p) is the Hamiltonian function of the vehicle. M -1 V(q) and V(q) are the inverse of the mass matrix and the potential energy of the vehicle on the Lie group, respectively. M v (q) is related to 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 independent of the generalized coordinate q, and M(q) is a positive definite matrix; p v p ω These are the linear momentum and angular momentum of the vehicle, respectively. The generalized momentum is... The relationship between generalized momentum and generalized velocity is: D v (q, p) and D ω (q, p) correspond to linear momentum p respectively. 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) is related to linear velocity and angular velocity respectively, and the control input matrix B(q) = [b v (q) T b ω (q) T ] T ;u represents the control input force or torque.

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

[0020] Step 1: Based on the vehicle dynamics equations on the Lie group, create PINODEs and calculate the derivatives of the generalized coordinates and the generalized velocity.

[0021] Step 2: Based on the ordinary differential equation framework, apply the ordinary differential equation solver to PINODEs to select the initial values ​​of generalized coordinates and generalized velocities, and integrate the derivatives of the generalized coordinates and generalized velocities to obtain the predicted values ​​of the generalized coordinates and generalized velocities.

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

[0023] Furthermore, the construction steps of PINODEs are as follows:

[0024] (1) Based on the physical information of the mass matrix, energy dissipation matrix, potential energy and control input matrix, establish mass neural network, energy dissipation neural network, potential energy neural network and control input neural network respectively;

[0025] (2) Input 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; input the generalized coordinates and the generalized velocity into the energy dissipation neural network to obtain the energy dissipation matrix;

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

[0027] (4) Obtain the partial derivative of the Hamiltonian with respect to the generalized coordinates by automatically differentiating the Hamiltonian of the vehicle with respect to the generalized coordinates; obtain the partial derivative of the Hamiltonian with respect to the generalized momentum by automatically differentiating the Hamiltonian of the vehicle with respect to the generalized momentum.

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

[0029] (6) Invert the mass matrix, then calculate the automatic differential of the inverse of the mass matrix with respect to the generalized coordinates to obtain the derivative of the inverse of the mass matrix with respect to the generalized coordinates. Multiply this derivative value with the derivative value of the generalized coordinates to obtain the derivative of the inverse of the mass matrix. Based on the relationship between generalized momentum and generalized velocity, substitute the derivatives of the mass matrix, generalized momentum, inverse of the mass matrix, and generalized momentum obtained by multiplying the mass matrix and generalized velocity into the relationship between generalized momentum and generalized velocity to obtain the derivative of generalized velocity, and then obtain the normal differential equation.

[0030] Integrate the constant differential equation of the god to obtain a forward solution:

[0031]

[0032] In the formula, θ represents the neural network parameters; N(x,t,θ) is the neural network for fitting the function f(x).

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

[0034]

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

[0036] (1) Based on the vehicle dynamics model obtained by training the Lie group Hamiltonian constant differential equation, describe the change law of vehicle state with time, and calculate the predicted values ​​of generalized velocity 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 that includes the tracking error between the actual trajectory and the desired trajectory and the penalty term for the control input;

[0039]

[0040] In the formula, These are predicted values ​​of vehicle system state variables. It is the predictive control variable, x r Q is the reference signal, and R is the weighting coefficient for the state deviation and the control input penalty, respectively.

[0041] (4) Select external force constraints and velocity constraints as the optimization constraints;

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

[0043] (5) In each sampling period, the 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 performs 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 storing machine-executable instructions, which, when called and executed by a processor, cause 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 summary, compared with the prior art, the vehicle trajectory tracking model predictive control method and device based on Lie group Hamiltonian dynamics and neural ordinary differential equations provided by the present invention have the following advantages:

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

[0049] 2. By using Lie group structures to accurately describe vehicle attitude changes, including variations in position, heading angle, pitch angle, and roll angle, a dynamic model within a Hamiltonian framework capable of naturally handling rotational and translational maneuvers is constructed. Accurate path tracking is achieved by combining the established high-precision vehicle dynamics model with model predictive control. This comprehensive approach is particularly suitable for autonomous vehicles, maintaining stability and accuracy in complex and changing road environments.

[0050] 3. Based on the Hamiltonian dynamics equations of vehicles under Lie groups, the Lie group Hamiltonian neural network ordinary differential equation method is used to learn the dynamics of vehicles. The aim is to capture the regularity in the data more easily by embedding physical information, respect the laws of physics, thereby improving the accuracy, reliability and interpretability of predictions, improving the generalization of the model, and simplifying the neural network structure.

[0051] 4. After learning the vehicle dynamics based on the Lie group Hamiltonian constant differential equations, model predictive control (MPC) is employed for vehicle trajectory tracking control. This aims to better integrate the vehicle dynamics model learned from the Lie group Hamiltonian constant differential equations, thereby improving the model and enhancing the method's adaptability and robustness. Simultaneously, MPC can consider the effects of multiple inputs (such as steering angle, acceleration / deceleration, etc.) and optimize their coordination to achieve optimal trajectory tracking performance. Attached Figure Description

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

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

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

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

[0056] Figure 5 This is a flowchart of a predictive control method for vehicle trajectory tracking based on Lie group Hamiltonian dynamics and neural ordinary differential equations provided in an embodiment of the present invention;

[0057] Figure 6 It is the training loss curve;

[0058] Figure 7 It is the predicted loss curve. Detailed Implementation

[0059] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0060] This invention provides a model predictive control method for vehicle trajectory tracking based on Lie group Hamiltonian dynamics and neural network ordinary differential equations (PINODEs). This method combines Lie group Hamiltonian dynamics, PINODEs (physical information embedded in PINODEs), and model predictive control (MPC). It fully utilizes the geometric advantages provided by Lie group theory while leveraging Hamiltonian neural networks to build an accurate dynamic model and employs an MPC strategy to execute trajectory tracking control. By using Lie group structures to accurately describe vehicle attitude changes, including variations in position, heading angle, pitch angle, and roll angle, a dynamic model within a Hamiltonian framework capable of naturally handling rotational and translational maneuvers is constructed. By combining the established high-precision vehicle dynamic model with model predictive control, accurate path tracking is achieved. This comprehensive approach is particularly suitable for autonomous vehicles, maintaining stability and accuracy in complex and changing road environments.

[0061] The method mainly includes the following steps:

[0062] S1 describes the vehicle's attitude and motion under a Lie group and determines the vehicle's state parameters.

[0063] Vehicle state parameters include generalized coordinates and generalized velocity.

[0064] Please see Figure 1 Vehicle mass m, moment of inertia I about the z-axis z Vehicle control input force: longitudinal force F along the x-axis x Lateral force F along the y-axis yYaw torque M about the z-axis z Vehicle state parameters include: longitudinal displacement x along the x-axis, lateral displacement y along the y-axis, vertical displacement z along the z-axis, and yaw angle about the z-axis. and the vehicle's longitudinal velocity along the x-axis Lateral velocity along the y-axis Vertical velocity along the z-axis angular velocity about 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 orientation rotation matrix R of the vehicle's own coordinate system axes determines:

[0066]

[0067] The vehicle's kinematic equations are derived from the linear velocity of the vehicle's own coordinate system relative to the world inertial coordinate system. and angular velocity

[0068] The decision is made using the vehicle's own coordinates. Therefore, the generalized speed is... The generalized coordinates are q = (p c ,R).

[0069] S2. Based on port-Hamiltonian mechanics and generalized coordinates and generalized velocities, establish the vehicle dynamics equations on the Lie group.

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

[0071]

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

[0073] in M v (q) is related to vehicle mass, while M ω 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. Furthermore, M(q) is a positive definite matrix.

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

[0075] D v (q, p) and D ω (q, p) correspond to linear momentum p respectively. v and angular momentum p ω The energy dissipation matrix is D(q,p) represents all the uncertainties that may occur in the vehicle system, including the friction between the tires and the ground, the nonlinear dynamic effects between the vehicle system, the external forces that disturb the motion, sensor errors, and model errors. The vehicle's energy dissipation matrix D(q,p) is usually related to the generalized coordinate q and the generalized velocity ζ, and D(q,p) is a positive semi-definite matrix that only affects the generalized momentum p.

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

[0077] S3. Based on the obtained vehicle dynamics equations, construct the NSD ordinary differential equations. Apply the ordinary differential equation solver to integrate the derivatives of the generalized coordinates and the generalized velocity to obtain the predicted values ​​of the generalized coordinates and the generalized velocity. Compare 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 NSD ordinary differential equations.

[0078] In this embodiment, the vehicle dynamics are learned by using the Hamiltonian equation of vehicle dynamics under the Lie group and the Lie group Hamiltonian neural ordinary differential equation method. The aim is to capture the regularity in the data more easily by embedding physical information, respect the laws of physics, thereby improving the accuracy, reliability and interpretability of prediction, improving the generalization of the model, and simplifying the neural network structure.

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

[0080] Step 1: Based on the vehicle dynamics equations on the Lie group, create PINODEs and calculate the derivatives of the generalized coordinates and the generalized velocity.

[0081] Step 2: Based on the ordinary differential equation framework, apply the ordinary differential equation solver to PINODEs to select the initial values ​​of generalized coordinates and generalized velocities, and integrate the derivatives of the generalized coordinates and generalized velocities to obtain the predicted values ​​of the generalized coordinates and generalized velocities.

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

[0083] Please see Figure 2 and Figure 3 The construction steps for PINODEs are as follows:

[0084] (1) Based on the physical information of the mass matrix, energy dissipation matrix, potential energy and control input matrix, establish mass neural network, energy dissipation neural network, potential energy neural network and control input neural network respectively.

[0085] Since the mass matrix is ​​positive definite, and M v (q) and M ω Given that q is a constant, a mass neural network M(q,θ) is established using a fully connected network, with generalized coordinates as input and a mass matrix as output. Two network parameters initialized to vehicle mass and one initialized to vehicle moment of inertia are added to the mass neural network. To ensure the positive definiteness of the mass matrix, a small constant ε is added to the diagonal elements of the output during the forward computation of the mass neural network. Based on the fact that the energy dissipation matrix is ​​semi-positive definite and that vehicle energy dissipation is related to the vehicle's generalized coordinates and generalized velocity, an energy dissipation neural network D(q,p,θ) is established using a fully connected network, with generalized coordinates and generalized velocity as input and an energy dissipation matrix as output. Potential energy is fitted using a fully connected network, establishing a potential energy neural network V(q,θ) with generalized coordinates as input and potential energy as output, with one network parameter initialized to vehicle potential energy. The control input matrix is ​​fitted using a fully connected network, establishing a control input neural network with generalized coordinates as input and a control input matrix as output.

[0086] The mass matrix M(q) is positive definite. According to Cholesky decomposition, the mass matrix can be decomposed into the product of a lower triangular matrix L(q,θ) with non-negative diagonal elements and the transpose of L(q,θ), where θ is a neural network parameter. Therefore, the number of output nodes of the mass neural network can be determined to be 21. Furthermore, since the vehicle's mass matrix can be divided into mass-related components M... v (q) and M related to the moment of inertia ω (q), and M v (q), M ω(q) are constant values ​​independent of the generalized coordinate q. These values ​​can be used as part of the parameters θ of the quality neural network for iterative optimization, rather than being considered as output nodes. Furthermore, because M(q) divided by M... v (q), M ω Since the part of (q) is 0, the number of output nodes of the quality neural network can be further reduced to 0. To ensure that the quality neural network is positive definite, a small constant ε is added to the diagonal elements, as shown in the following equation.

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

[0088] The energy dissipation matrix of a vehicle is a comprehensive indicator of the system's dynamic behavior, related to the vehicle's generalized coordinates and generalized velocity. Therefore, an energy dissipation neural network can be established with generalized coordinates and velocity as inputs. Since generalized momentum *p* is related to generalized velocity, the input to the energy dissipation neural network D(q,p,θ) can be converted into generalized velocity. Because generalized momentum cannot be directly measured, this invention establishes an energy dissipation neural network with generalized coordinates and velocity as inputs. Furthermore, based on the fact that the energy dissipation matrix is ​​semi-positive definite, the structure of the energy dissipation neural network can be referenced from the mass neural network, decomposed according to Cholesky into a lower triangular matrix L with non-negative diagonal elements. D (q, θ) and L D The product of (q, θ) transposes, therefore the number of output nodes of the energy dissipation neural network is 21.

[0089] The potential energy in this invention is a fixed value; therefore, a network parameter is created to fit this potential energy, resulting in 0 output nodes for the potential energy neural network. The control input matrix has a dimension of 6 rows and 3 columns, thus controlling the output nodes of the input neural network to be 18.

[0090] (2) Input 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; input the generalized coordinates and the generalized velocity 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) Obtain the partial derivative of the Hamiltonian with respect to the generalized coordinates by automatically differentiating the Hamiltonian of the vehicle with respect to the generalized coordinates; obtain the partial derivative of the Hamiltonian with respect to the generalized momentum by automatically differentiating the Hamiltonian of the vehicle with respect to the generalized momentum.

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

[0094] (6) Invert the mass matrix, then calculate the automatic differential of the inverse of the mass matrix with respect to the generalized coordinates to obtain the derivative of the inverse of the mass matrix with respect to the generalized coordinates. Multiply this derivative value with the derivative value of the generalized coordinates to obtain the derivative of the inverse of the mass matrix. Based on the relationship between generalized momentum and generalized velocity, substitute the derivatives of the mass matrix, generalized momentum, inverse of the mass matrix, and generalized momentum obtained by multiplying the mass matrix and generalized velocity into the relationship between generalized momentum and generalized velocity to obtain the derivative of generalized velocity, and then obtain the normal differential equation.

[0095] Based on the form of the ordinary differential equation, the Hamiltonian equation is integrated to obtain a forward solution.

[0096]

[0097] In the formula, θ represents the neural network parameters; N(x,t,θ) is the neural network for fitting the function f(x).

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

[0099]

[0100] S4 uses a vehicle dynamics model trained based on the constant differential equation to describe the change of vehicle state over time, and then uses model predictive control to perform trajectory tracking control of the vehicle.

[0101] After learning vehicle dynamics from Lie group Hamiltonian constant differential equations, model predictive control (MPC) is employed for vehicle trajectory tracking control. This aims to better integrate the vehicle dynamics model learned from the Lie group Hamiltonian constant differential equations, thereby improving the model and enhancing the method's adaptability and robustness. MPC can simultaneously consider the effects of multiple inputs (such as steering angle, acceleration / deceleration, etc.) and optimize their coordination to achieve optimal trajectory tracking performance.

[0102] Please see Figure 4 Step S4 includes the following sub-steps:

[0103] (1) Based on the vehicle dynamics model obtained by training the Lie group Hamiltonian constant differential equation, describe the change law of vehicle state with time, and calculate the predicted values ​​of generalized velocity 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 that includes the tracking error between the actual trajectory and the desired trajectory and the penalty term for the control input;

[0106]

[0107] In the formula, These are predicted values ​​of vehicle system state variables. It is the predictive control variable, x r Q is the reference signal, and R is the weighting coefficient for the state deviation and the control input penalty, respectively.

[0108] (4) Select external force constraints and velocity constraints as the optimization constraints;

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

[0110] (5) In each sampling period, the 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, and then wait for the next sampling period to repeat the above process.

[0112] MPC works by employing a rolling optimization approach, meaning it executes a control action only once at a time, then re-evaluates the current state and optimizes again. This approach allows MPC to adapt to constantly changing working environments and adjust its control strategies promptly.

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

[0114] Please see Figure 5 The embodiments of the present invention include the following steps:

[0115] S1, data needs to be collected to obtain the dataset for training PINADEs. The specific steps are as follows:

[0116] (1) Collect vehicle state parameters and vehicle control input force during the vehicle's movement. When collecting data, the control frequency (frequency for collecting control input force) is required to be lower than the sampling frequency of the vehicle state parameters. In this embodiment of the invention, the sampling frequency of the vehicle state parameters is selected as 500Hz and the control frequency is 20Hz.

[0117] (2) Perform first-order filtering and other preprocessing on the data to remove outliers and convert the vehicle state parameters into a Lie group representation (see Table 1) to obtain a dataset trained using the Hamiltonian ordinary differential equation of the Lie group. 70% of the dataset is used as the training set and 30% as the test set.

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

[0119]

[0120] S2. After obtaining the dataset, begin training the PINODEs to learn vehicle dynamics. The specific steps are as follows:

[0121] Step 1: Based on the vehicle dynamics equations on the Lie group, create PINODEs and output the derivatives of the generalized coordinates and the generalized velocities;

[0122] Step 2: Based on the ordinary differential equation framework, apply the Runge-Kutta fourth-order ordinary differential equation solver to PINODEs, select the vehicle state data at time 0 in the dataset as the initial value, integrate the derivatives of the generalized coordinates and the generalized velocity within the time interval from 0 to 0.01, with a time interval of 0.002s, and output the predicted values ​​of the generalized coordinates and the generalized velocity.

[0123] Step 3: Compare the predicted and actual values ​​of the generalized coordinates and generalized velocity, calculate the mean squared error, and use it for backpropagation to update and optimize the PINODEs 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 As shown.

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

[0125] The specific steps for creating the PINODEs are as follows:

[0126] (1) Based on the physical information of the mass matrix, energy dissipation matrix, potential energy, and control input matrix, establish a mass neural network, an energy dissipation neural network, a potential energy neural network, and a control input neural network respectively. For the mass neural network, design 12 input nodes and 0 output nodes, and initialize two updatable mass parameters of 1000 as the elements of the first row and first column and the second row and second column of the output mass matrix, respectively, and one updatable moment of inertia parameter of 300 as the element of the sixth row and sixth column of the output mass matrix (assuming that the vehicle mass in this embodiment is 1000kg and the moment of inertia is 300kg.m2), enable gradient tracking, and select ε = 10-3. For the energy dissipation neural network, design 18 input nodes and 21 output nodes. For the potential energy neural network, design 12 input nodes and 0 output nodes, initialize one updatable potential energy parameter of 5000 as the network output, and enable gradient tracking. For the control input neural network, design 12 input nodes and 18 output nodes.

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

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

[0129] (4) The Hamiltonian of the vehicle is obtained by solving the partial derivatives of the generalized coordinates and the generalized momentum according to the automatic differentiation, so as to obtain the partial derivatives of the Hamiltonian with respect to the generalized coordinates and the partial derivatives of the Hamiltonian with respect to the generalized momentum.

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

[0131] (6) Invert the mass matrix, then calculate the automatic differential of the inverse of the mass matrix with respect to the generalized coordinates to obtain the derivative of the inverse of the mass matrix with respect to the generalized coordinates. Multiply this derivative value with the derivative value of the generalized coordinates to obtain the derivative of the inverse of the mass matrix. Based on the relationship between generalized momentum and generalized velocity, substitute the derivatives of the mass matrix, generalized momentum, inverse of the mass matrix, and generalized momentum obtained by multiplying the mass matrix and generalized velocity into the relationship between generalized momentum and generalized velocity to obtain the derivative of the generalized velocity.

[0132] Table 2 Training Results of PINODEs

[0133]

[0134] S3, after training and verification, yields the network parameters of the Hamiltonian constant differential equation of the Lie group, which is the vehicle dynamics equation on the Lie group. Next, model predictive control is used for trajectory tracking. The specific implementation steps are as follows:

[0135] (1) Substitute the verified parameters into the vehicle dynamics equation on the Lie group as the prediction model in MPC. By integrating the dynamics equation, the state changes of the vehicle in the future can be predicted.

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

[0137] (3) Based on the quadratic form, calculate the difference between the expected trajectory and the actual trajectory, add a penalty term to the control input, and add the two to obtain the total cost;

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

[0139] (5) Extract the first control action from the optimized control sequence and convert it into a specific control command to send to the actuator;

[0140] (6) After executing the control action, continue to monitor the vehicle status and repeat the above MPC control process to form a closed-loop control system. By continuously adjusting the control input, ensure that the vehicle travels 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 performs 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 storing machine-executable instructions, which, when called and executed by a processor, cause 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] Those skilled in the art will readily understand that the above description is merely 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 within the scope of protection of the present invention.

Claims

1. A predictive control method for vehicle trajectory tracking model based on Lie group Hamiltonian dynamics and neural ordinary differential equations, characterized in that, The method includes the following steps: S1 describes the vehicle's attitude and motion under a Lie group and determines the vehicle's state parameters; S2. Based on port-Hamiltonian mechanics and generalized coordinates and generalized velocities, establish the vehicle dynamics equations on the Lie group; S3. Based on the obtained vehicle dynamics equations, construct the ordinary differential equations of the God. Apply the ordinary differential equation solver to integrate the derivatives of the generalized coordinates and the generalized velocity to obtain the predicted values ​​of the generalized coordinates and the generalized velocity. Compare the predicted values ​​of the generalized coordinates and the generalized velocity with the actual values ​​to calculate the mean square error, and then use it for backpropagation to update and optimize the parameters of the God ordinary differential equations. S4 uses a vehicle dynamics model trained based on the constant differential equations to describe the change of vehicle state over time, and then uses model predictive control to perform trajectory tracking control of the vehicle; Step S3 includes the following sub-steps: Step 1: Based on the vehicle dynamics equations on the Lie group, create PINODEs and calculate the derivatives of the generalized coordinates and the generalized velocity. Step 2: Based on the ordinary differential equation framework, apply the ordinary differential equation solver to PINODEs to select the initial values ​​of generalized coordinates and generalized velocities, and integrate the derivatives of the generalized coordinates and generalized velocities to obtain the predicted values ​​of the generalized coordinates and generalized velocities. Step 3: Compare the predicted and actual values ​​of generalized coordinates and generalized velocity to calculate the mean square error, which is then used for backpropagation to update and optimize the PINODEs parameters; The steps for constructing PINODEs are as follows: (1) Based on the physical information of the mass matrix, energy dissipation matrix, potential energy, and control input matrix, establish the mass neural network, energy dissipation neural network, potential energy neural network, and control input neural network respectively; (2) Input 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; input the generalized coordinates and the generalized velocity into the energy dissipation neural network to obtain the energy dissipation matrix; (3) Substitute the mass matrix and potential energy into the Hamiltonian function of the vehicle, and input the generalized momentum obtained by multiplying the mass matrix and the generalized velocity to obtain the Hamiltonian of the vehicle. (4) Obtain the partial derivative of the Hamiltonian with respect to the generalized coordinates by automatically differentiating the Hamiltonian of the vehicle with respect to the generalized coordinates; obtain the partial derivative of the Hamiltonian with respect to the generalized momentum by automatically differentiating the Hamiltonian of the vehicle with respect to the generalized momentum. (5) Substitute the energy dissipation matrix, control input matrix, partial derivative of Hamiltonian with respect to generalized coordinates, partial derivative of Hamiltonian with respect to generalized momentum, driving force, generalized coordinates and generalized momentum into the vehicle dynamics equations on the Lie group, and output the derivative values ​​of generalized coordinates and generalized momentum respectively. (6) Invert the mass matrix, then calculate the automatic differential of the inverse of the mass matrix with respect to the generalized coordinates to obtain the derivative of the inverse of the mass matrix with respect to the generalized coordinates. Multiply this derivative value with the derivative value of the generalized coordinates to obtain the derivative of the inverse of the mass matrix. Based on the relationship between generalized momentum and generalized velocity, substitute the derivatives of the mass matrix, generalized momentum, inverse of the mass matrix, and generalized momentum obtained by multiplying the mass matrix and generalized velocity into the relationship between generalized momentum and generalized velocity to obtain the derivative of generalized velocity, and then obtain the normal differential equation of the divine. Based on the form of the ordinary differential equation, the Hamiltonian equation is integrated to obtain a forward solution: In the formula, θ For neural network parameters; For the fitting function Neural networks; Step S4 includes the following sub-steps: (1) Based on the vehicle dynamics model obtained by training the Lie group Hamiltonian constant differential equation, describe the change law of vehicle state with time, and calculate the predicted values ​​of generalized velocity 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 that includes the tracking error between the actual trajectory and the desired trajectory and the penalty term for the control input; In the formula, These are predicted values ​​of vehicle system state variables. It is a predictive control quantity. x r It is a reference signal. Q, W These are the weighting coefficients for state deviation and control input penalty, respectively; (4) Select external force constraints and velocity constraints as the optimization constraints; ， ， ， ； (5) In each sampling period, the 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.

2. The vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and neural ordinary differential equations as described 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. Rotation matrix of the vehicle's own coordinate system axes R The vehicle's kinematic equations are determined by the linear velocity of its own coordinate system relative to the world inertial coordinate system. and angular velocity The decision is that the generalized speed is Generalized coordinates are q = ( p c , R ).

3. The vehicle trajectory tracking model predictive control method based on Lie group Hamiltonian dynamics and neural ordinary differential equations as described in claim 2, characterized in that: The vehicle dynamics equations on the Lie group are expressed in port-Hamiltonian form as follows: In the formula, H ( q , p ) is the Hamiltonian function of the vehicle. , M -1 ( q ), V ( q Let f(x) be the inverse of the mass matrix and the potential energy of the vehicle on the Lie group, respectively. , , It is related to vehicle quality, and Related to the vehicle's moment of inertia, the vehicle's mass and moment of inertia are usually constants, and are related to the generalized coordinate system. q Irrelevant, and M ( q ) is a positive definite matrix; , These are the linear momentum and angular momentum of the vehicle, respectively. The generalized momentum is... The relationship between generalized momentum and generalized velocity is: ; D v ( q , p )and D ω ( q , p ) respectively correspond to linear momentum and angular momentum The energy dissipation matrix is ; Represents all uncertainties in the vehicle system; , Related to linear velocity and angular velocity respectively, the control input matrix ; u To control the 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 described in claim 1, characterized in that: The loss of the Lie group Hamiltonian constant differential equation is divided into the loss of the geometric distance of the rotation matrix, the loss of the generalized velocity, and the loss of the translation position. The total loss is: 。 5. A predictive control system for vehicle trajectory tracking based on Lie group Hamiltonian dynamics and neural ordinary differential equations, characterized in that: The system includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it performs 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-4.

6. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores machine-executable instructions, which, when invoked and executed by a processor, cause 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-4.

Images