A method and device for controlling the travel path of a construction machine

Abstract

The application discloses an engineering machine driving path tracking control method and device, which can improve the prediction accuracy of the driving trajectory of a loader. The engineering machine driving path tracking control method comprises the following steps: constructing a nonlinear kinematics model of a loader based on simulation data of the loader; constructing a Koopman operator; linearizing the nonlinear kinematics model according to the Koopman operator to construct a whole vehicle dynamics prediction model; obtaining a deviation between a current state vector and a target state vector of the loader and a control increment between a current control vector and a target control vector; inputting the deviation and the control increment into the whole vehicle dynamics prediction model for calculation to generate an optimal prediction trajectory of the loader.

Description

Technical Field

[0001] This application relates to the field of path planning technology, specifically to a method and device for tracking and controlling the travel path of engineering machinery. Background Technology

[0002] The loader consists of two parts: a front body and a rear body, connected by a hinge point and a swing ring. A hydraulic actuator causes the front and rear bodies to deflect relative to each other, achieving steering. This structure reduces the turning radius of the wheeled loader, improves its maneuverability, and makes it adaptable to different working environments. However, this steering mechanism introduces highly nonlinear dynamics. Current dynamic models are based on the kinematics of passenger cars, but passenger cars move differently from loaders. Directly using passenger car kinematics models to predict the loader's trajectory will result in low prediction accuracy, impacting the loader's working efficiency. Summary of the Invention

[0003] To address the aforementioned technical problems, this application is proposed. Embodiments of this application provide a method and apparatus for tracking and controlling the travel path of construction machinery, which can improve the accuracy of predicting the travel trajectory of a loader.

[0004] According to one aspect of this application, a method for tracking and controlling the travel path of engineering machinery is provided, comprising: constructing a nonlinear kinematic model of the loader based on simulation data of the loader; constructing a Koopman operator; linearizing the nonlinear kinematic model according to the Koopman operator to construct a vehicle dynamics prediction model; obtaining the deviation between the current state vector and the target state vector of the loader, and the control increment between the current control vector and the target control vector; and inputting the deviation and the control increment into the vehicle dynamics prediction model for calculation to generate the optimal predicted trajectory of the loader.

[0005] In one embodiment, constructing the Koopman operator includes: constructing a Koopman operator applied to x. k+1 =f(x) k u k The space represented by ) is as follows: Where, x k+1 =f(x) k u k (x) represents a discrete-time nonlinear system rewritten from a nonlinear kinematic model; where K is the Koopman operator, ψ is a scalar function of the state vector and control vector, and h(x) is a vector vector of the state and control vector. k ,u k) is a function that propagates the input forward in time; and the Koopman operator is computed using recursive least squares; wherein the Koopman operator is used to define the observation function of the scalar function ψ in time.

[0006] In one embodiment, the Koopman operator is computed using recursive least squares, comprising: acquiring trajectory data at multiple time points, the trajectory data including a state vector and a control vector at one time point; determining the past and future values ​​of the observation function based on the trajectory data at multiple time points and the vector ψ of the observation function, wherein the relationship between the future value and the past value of the observation function is: Y f =KY p +ξ; where Y f Y represents the future value of the observed function, K is the Koopman operator, and Y... p Let ξ be the past value of the observation function, where ξ is the approximation error; based on the relationship between the future value and the past value of the observation function, K is determined by the least squares method to be the Koopman operator when the approximation error is minimized.

[0007] In one embodiment, the Koopman operator is: Among them, Y f Y represents the future value of the observed function. p These are past values ​​of the observed function. This is the pseudoinverse of Moore-Pensose.

[0008] In one embodiment, after constructing the Koopman operator, the engineering machinery driving path tracking control method further includes updating the Koopman operator based on a recursive least squares algorithm with a forgetting factor.

[0009] In one embodiment, updating the Koopman operator based on a recursive least squares algorithm with a forgetting factor includes: calculating an approximate value of the operator at the current time step based on the Koopman operator, wherein the approximate value of the operator at the current time step is: Among them, Y f Y represents the future value of the observed function. p For past values ​​of the observed function; Γ k =(YpYp T ) -1 Based on the correction vector, forgetting factor λ, and Γ k Calculate Γ k+1 Based on the trajectory data at the current moment and the trajectory data at the next moment, write the new future value of the observation function and the new past value of the observation function; and based on Γ k+1The Koopman operator is updated by the new future value of the observation function and the new past value of the observation function.

[0010] In one embodiment, the forgetting factor λ is: 0.9 < λ < 0.995.

[0011] In one embodiment, linearizing the nonlinear kinematic model according to the Koopman operator to construct a vehicle dynamics prediction model includes: determining an observation vector based on the nonlinear kinematic model and the Koopman operator; calculating the interaction relationship of the Koopman operator with the observation vector based on the observation vector and the Koopman operator; constructing a linear relationship between the observation vectors based on the interaction relationship; and constructing the vehicle dynamics prediction model based on the linear relationship.

[0012] In one embodiment, constructing a nonlinear kinematic model of the loader based on simulated data of the loader includes: establishing the driving kinematic equations of the loader; determining the simulated state vector and simulated control vector of the loader during the simulated task; selecting a state representation according to the direction of movement of the loader; and determining the nonlinear kinematic model according to the simulated state vector, the simulated control vector, and the driving kinematic equations.

[0013] According to another aspect of this application, a driving path tracking control device for construction machinery is provided, comprising: a first construction module for constructing a nonlinear kinematic model of the loader based on simulation data of the loader; a second construction module for constructing a Koopman operator; a linearization module for linearizing the nonlinear kinematic model according to the Koopman operator to construct a vehicle dynamics prediction model; an acquisition module for acquiring the deviation between the current state vector and the target state vector of the loader, and the control increment between the current control vector and the target control vector; and a calculation module for inputting the deviation and the control increment into the vehicle dynamics prediction model for calculation to generate the optimal predicted trajectory of the loader.

[0014] The engineering machinery travel path tracking control method and device provided in this application linearizes a nonlinear kinematic model based on Koopman theory to construct a vehicle dynamics prediction model. The nonlinear kinematic model can adapt to the structure of the loader, and linearizing the nonlinear kinematic model can solve the control problem of the nonlinear system. The linearized model can obtain the system's control input through model predictive control, thus achieving system control. Combining Koopman theory and model predictive control, a linear model predictive control is designed for the nonlinear system, thereby predicting the loader's trajectory. The accuracy of the model predictive control is improved through control increments and deviations to achieve more precise control and response, thereby generating the optimal predicted trajectory of the loader. Attached Figure Description

[0015] The above and other objects, features, and advantages of this application will become more apparent from the more detailed description of the embodiments of this application in conjunction with the accompanying drawings. The drawings are provided to further illustrate the embodiments of this application and form part of the specification. They are used together with the embodiments of this application to explain this application and do not constitute a limitation thereof. In the drawings, the same reference numerals generally represent the same components or steps.

[0016] Figure 1 This is a system schematic diagram of a model prediction trajectory tracking control system provided in an exemplary embodiment of this application.

[0017] Figure 2 This is a flowchart illustrating an exemplary embodiment of the engineering machinery travel path tracking and control method provided in this application.

[0018] Figure 3 This is a flowchart illustrating a method for tracking and controlling the travel path of engineering machinery provided in another exemplary embodiment of this application.

[0019] Figure 4 This is a flowchart illustrating a method for tracking and controlling the travel path of engineering machinery provided in another exemplary embodiment of this application.

[0020] Figure 5 This is a flowchart illustrating a method for tracking and controlling the travel path of engineering machinery provided in another exemplary embodiment of this application.

[0021] Figure 6 This is a flowchart illustrating a method for tracking and controlling the travel path of engineering machinery provided in another exemplary embodiment of this application.

[0022] Figure 7 This is a schematic diagram of the two-wheel kinematics of a loader provided in an exemplary embodiment of this application.

[0023] Figure 8This is a schematic diagram of the structure of an engineering machinery travel path tracking and control device provided in an exemplary embodiment of this application.

[0024] Figure 9 This is a schematic diagram of the structure of a construction machinery driving path tracking control device provided in another exemplary embodiment of this application.

[0025] Figure 10 This is a structural diagram of an electronic device provided in an exemplary embodiment of this application. Detailed Implementation

[0026] Hereinafter, exemplary embodiments according to this application will be described in detail with reference to the accompanying drawings. Obviously, the described embodiments are merely some embodiments of this application, and not all embodiments of this application. It should be understood that this application is not limited to the exemplary embodiments described herein.

[0027] Application Overview

[0028] The loader consists of two parts: a front body and a rear body, connected by a hinge point and a swing ring. A hydraulic actuator causes the front and rear bodies to deflect relative to each other, achieving steering. During steering, there is no relative deflection between the wheels and the frame, thus eliminating the need for components such as the steering drive axle. This structural form differs from traditional passenger car structures, reducing the turning radius of the wheeled loader. However, current loader steering mechanisms introduce highly nonlinear dynamics. Current dynamic models are based on the kinematics of passenger cars, which cannot adequately address the motion problems of loaders. When using these models to predict the loader's trajectory, the error is significant, making it difficult to obtain an optimal trajectory. This introduces additional complexity to the trajectory tracking and control problem and differs considerably from autonomous driving in passenger cars.

[0029] The engineering machinery travel path tracking control method and device provided in this application introduces Koopman identification theory to linearize the strongly nonlinear model. Essentially, a finite-dimensional nonlinear system can be upgraded to an infinite-dimensional linear system, thus preserving information from the original system. Based on the linearized model constructed under Koopman theory, the system's control input can be obtained through Model Predictive Control (MPC), thereby achieving system control. Furthermore, the prediction accuracy of the model is improved by updating it online, using a Recursive Least Square-Forget Factor (RLS-FF) algorithm with a forgetting factor. The combination of RLS, Koopman theory, and MPC is referred to here as the KR-MPC controller. This method constructs a data-driven controller that can solve the control problem of nonlinear systems, and while ensuring control accuracy, it significantly reduces computation time compared to Nonlinear Model Predictive Control (NMPC). In addition, a nonlinear kinematic model of the loader is constructed, and the loader's travel trajectory is predicted based on this model, improving the prediction accuracy and obtaining the optimal travel trajectory of the loader.

[0030] In addition to loaders, the construction machinery travel path tracking control method and device provided in this application can also be applied to other construction machinery with similar structures to loaders.

[0031] Exemplary System

[0032] To address the highly nonlinear problem in the loader dynamics model, this exemplary embodiment constructs a linearized loader dynamics state-space equation using Koopman theory. Considering lateral position deviation and heading angle deviation during operation, and using acceleration and articulation angle as control inputs, and vehicle speed, lateral position deviation, and heading angle deviation as control outputs, a vehicle dynamics prediction model is established. With acceleration, articulation angle, and vehicle speed as constraints, the objective function is transformed into a quadratic programming problem. Based on the linearized model constructed under Koopman theory, the system's control input can be obtained through Model Predictive Control (MPC), thus achieving system control. To address the issue of low model accuracy, this application employs the RLS-FF algorithm to improve the model's prediction accuracy. Simultaneously, an update mechanism is used to determine whether to update the model. In summary, based on the Koopman operator and RLS modeling, an MPC control system is established that meets the operational requirements of the loader, providing a model prediction trajectory tracking control system. Figure 1This is a system schematic diagram of a model prediction trajectory tracking control system provided in an exemplary embodiment of this application, such as... Figure 1 As shown, r k e k Input to Model PredictControl (MPC) 41, e k This represents the deviation between the state and its reference, used to correct the model predictive control 41, and outputs u from the MPC. k The data is then transmitted to the controlled object 42 (Plant) and the model update 43 (Model Update). The controlled object 42 outputs y. k and x k y k and e k Feedback can be sent to the model predictive control 41 to make small adjustments or increments to the control signal, which can be used to gradually adjust the system's state or output to achieve more precise control and response. k and u k The input is fed into model update 43 to generate the updated K, where K represents the Koopman operator. Model update 43 uses the RLS-FF algorithm to improve the model's prediction accuracy. k Indicates the target state, e k x represents the deviation of the state from its reference. k Let represent the state vector, uk represent the input vector at time k, and yk represent the correction vector.

[0033] Exemplary methods

[0034] Figure 2 This is a flowchart illustrating an exemplary embodiment of the engineering machinery travel path tracking and control method provided in this application, as shown below. Figure 2 As shown, the method for tracking and controlling the travel path of construction machinery includes:

[0035] Step 100: Construct a nonlinear kinematic model of the loader based on the loader's simulation data.

[0036] Loaders primarily operate at low speeds, resulting in minimal slippage. Therefore, the slippage effect of the loader can be ignored. In other words, the direction of the loader's front (rear) vehicle velocity is consistent with its heading. The nonlinear kinematic model of the loader can be described by the following equations:

[0037]

[0038] In the formula, (x f ,y f (x) indicates the position of the center point of the front wheel axle. r ,y r) represents the position of the rear axle center point, θ is the rear vehicle heading angle, γ is the articulation angle, which is the difference between the heading angles of the front and rear vehicles, and L f L represents the distance from the hinge point to the front axle. r The distance v represents the distance from the hinge point to the rear axle. f The velocity v of the front vehicle body is expressed as... r This indicates the speed of the rear of the vehicle.

[0039] Step 200: Construct the Koopman operator.

[0040] Given the nonlinear kinematic model of the loader, once the rear vehicle heading angle and articulation angle are known, the loader's state vector and control vector can be represented using the nonlinear kinematic model. After obtaining the state vector and control vector, the nonlinear kinematic model can be rewritten as a discrete-time nonlinear system: x k+1 =f(x) k u k Based on discrete-time nonlinear systems, the Koopman operator is constructed. The core idea of ​​the Koopman operator formula is to represent the final state variables of a dynamical system as functions. Specifically, the Koopman operator maps one function to another, describing the changes of state variables in the phase space. The Koopman operator can transform the evolution of state variables over time into the evolution of functions. Therefore, the Koopman operator can describe the linear evolution of observable state variables in high-dimensional space of a nonlinear system, transforming nonlinear problems into linear problems.

[0041] Step 300: Linearize the nonlinear kinematic model using the Koopman operator to construct a vehicle dynamics prediction model.

[0042] The nonlinear kinematic model is linearized based on Koopman theory. Considering the lateral position deviation and heading angle deviation of the loader during driving, the vehicle dynamics prediction model is established with acceleration and articulation angle as control inputs and vehicle speed, lateral position deviation and heading angle deviation as control outputs.

[0043] Step 400: Obtain the deviation between the current state vector and the target state vector of the loader, as well as the control increment between the current control vector and the target control vector.

[0044] Bias refers to the systematic error or deviation from the true value when a predictive model predicts input data. Control increment refers to a small adjustment or increment made to the control signal in a control system based on the current control signal and controller output. This incremental adjustment can be used to gradually adjust the system's state or output to achieve more precise control and response. By feeding back bias and control increment, the influence on the predictive model can be further adjusted, including improving system response speed, control accuracy, stability, and model adaptability.

[0045] Step 500: Input the deviation and control increment into the vehicle dynamics prediction model for calculation to generate the optimal predicted trajectory of the loader.

[0046] Minimum form of unbiased performance metric:

[0047]

[0048] In the formula, e k+i =r k+i -x k+i The deviation between the state and its reference (the deviation between the current state vector and the target state vector) is Δu. k+i N is the prediction step size. Matrix T is positive semi-definite, and R is positive definite.

[0049] For linear predictive control, a linear time-invariant (LTI) model is used. This model requires non-incremental input values; therefore, past values ​​of the input are used to enhance the state, resulting in:

[0050]

[0051] Using this model and cost function, the Optimal Control Problem (OCP) is expressed as:

[0052] To obtain the optimal driving trajectory of the loader.

[0053] The engineering machinery travel path tracking control method provided in this application linearizes a nonlinear kinematic model based on Koopman theory to construct a vehicle dynamics prediction model. The nonlinear kinematic model can adapt to the structure of the loader, and linearizing the nonlinear kinematic model can solve the control problem of the nonlinear system. The linearized model can obtain the system's control input through model predictive control, thus achieving system control. Combining Koopman theory and model predictive control, a linear model predictive control is designed for the nonlinear system, thereby predicting the loader's trajectory. The accuracy of the model predictive control is improved by controlling increments and deviations to achieve more precise control and response, ultimately generating the optimal predicted trajectory of the loader.

[0054] In one embodiment, step 200 may include: constructing a Koopman operator acting on x k+1 =f(x) k u k The space represented is as follows:

[0055] Where, x k+1 =f(x) k u k (x) represents a discrete-time nonlinear system rewritten from a nonlinear kinematic model; where K is the Koopman operator, ψ is a scalar function of the state vector and control vector, and h(x) is a vector vector of the state and control vector. k ,u k ψ is a function that propagates the input forward in time. The Koopman operator is computed using recursive least squares. The Koopman operator defines the time-varying observation function of the scalar function ψ.

[0056] Apply the Koopman operator to x k+1 =f(x) k u k The space represented is Where K is the Koopman operator, ψ is a scalar function of the state vector and the control vector, and h(x) k ,u k The input is a function that propagates forward in time. Therefore, the Koopman operator defines the evolution of the scalar function ψ over time, also represented as an observable function. The core idea of ​​the Koopman operator formula is to represent the final state variables of a dynamical system as functions. Specifically, the Koopman operator maps one function to another, describing the changes of state variables in the phase space. Therefore, the Koopman operator can describe the linear evolution of observable state variables of a nonlinear system in a high-dimensional space, and can transform nonlinear problems into linear problems.

[0057] Calculating K using the recursive least squares method is called extended dynamic mode decomposition:

[0058]

[0059] In one embodiment, the Koopman operator is computed using recursive least squares, including: acquiring trajectory data at multiple time points, where each trajectory data includes a state vector and a control vector at a single time point. Based on the trajectory data at multiple time points and the vector ψ of the observation function, the past and future values ​​of the observation function can be determined. The relationship between the future and past values ​​of the observation function is expressed as: Y f =KY p +ξ; where Y f Y represents the future value of the observed function, K is the Koopman operator, and Y... p Let ξ be the past values ​​of the observation function, and ξ be the approximation error. Based on the relationship between the future values ​​and past values ​​of the observation function, K is determined as the Koopman operator by using the least squares method to minimize the approximation error.

[0060] For strongly nonlinear models, Koopman identification theory can be used to linearize them. Essentially, a finite-dimensional nonlinear system can be elevated to an infinite-dimensional linear system, thus preserving information about the original system. However, computing an infinite-dimensional Koopman operator is impossible. Therefore, the operator is approximated in finite dimensions using Extended Dynamic Mode Decomposition (DMD). Based on the EDMD method, global linearization of the nonlinear system can be achieved through data-driven methods, including control inputs and other information about the nonlinear system.

[0061] Therefore, calculating K using the recursive least squares method is called extended dynamic mode decomposition:

[0062]

[0063] The status and input trajectory can be collected in the following ways:

[0064]

[0065] Where (x) i ,u i x is a tuple consisting of the state and input at time i. i It represents the state at time i, u i is the input at time i, and the scalar value P is the number of samples taken.

[0066] By using the vector Ψ of the observable functions, the following matrix can be obtained:

[0067]

[0068] Where Y p and Y f These are the past and future values ​​of an observable function, linked by the following equation:

[0069] Y f =KY p +ξ,

[0070] Where ξ represents the approximation error. Using the least squares method, we find a matrix K that minimizes ξ, i.e.:

[0071]

[0072] At this point, let the K with the smallest ξ be determined as the constructed Koopman operator.

[0073] In one embodiment, the Koopman operator is: Among them, Y f Y represents the future value of the observed function. p These are past values ​​of the observed function. This is the Moore-Pensose pseudoinverse.

[0074] In time series analysis or signal processing, an observation function refers to the numerical value obtained by observing or measuring a system or process at a given time point. The future value of an observation function refers to the value predicted or estimated based on existing observations and models for the system's state or numerical value at future time points. The past value of an observation function, also obtained by observing or measuring a system or process at a given time point, is used to construct time series models. By analyzing past observation data, characteristics, trends, periodicity, and seasonality of the system or process can be revealed. The past value of the observation function and existing models can be used to predict future observations. In other words, the future value of an observation function can include the future values ​​of the state vector and control vector, and the past value of an observation function can include the past values ​​of the state vector and control vector. Based on the state vector and control vector at past time points, the state vector and control vector at future time points can be analyzed and predicted.

[0075] We use the least squares method to find matrix K that minimizes ξ, i.e.:

[0076] This optimization problem has an analytical solution, namely in, This is the Moore-Penrose pseudoinverse. The Moore-Penrose pseudoinverse is a generalized inverse of a matrix used to find the least-squares solution to a system of linear equations.

[0077] Figure 3 This is a flowchart illustrating a method for tracking and controlling the travel path of construction machinery provided in another exemplary embodiment of this application, as shown below. Figure 3 As shown, after step 200 above, the engineering machinery travel path tracking control method may further include:

[0078] Step 600: Update the Koopman operator based on the recursive least squares algorithm with forgetting factor.

[0079] The performance of model predictive control depends on the prediction accuracy of the vehicle dynamics prediction model. Therefore, the prediction accuracy of the vehicle dynamics prediction model can be improved by updating the model online, i.e., using the Recursive Least Square-Forget Factor (RLS-FF) algorithm. The RLS-FF algorithm is a method for online estimation of linear regression model parameters. This algorithm recursively updates the parameter estimates, simultaneously considering the weights of the latest and past observations to adapt to changes in the data stream. Combining the RLS-FF algorithm with the forgetting factor, Koopman theory, and model predictive control, a KR-MPC controller can be constructed to solve control problems of nonlinear systems, and with less computation time than nonlinear model predictive controllers while maintaining control accuracy.

[0080] After constructing the Koopman operator (step 200), it can be determined whether the Koopman operator needs to be updated. If it does not need to be updated, step 300 can be executed directly. If the Koopman operator needs to be updated, step 600 should be executed first, followed by step 300.

[0081] Figure 4 This is a flowchart illustrating a method for tracking and controlling the travel path of construction machinery provided in another exemplary embodiment of this application, as shown below. Figure 4 As shown, step 600 above may include:

[0082] Step 610: Calculate the approximate value of the operator at the current time step based on the Koopman operator. The approximate value of the operator at the current time step is:

[0083] Among them, Y f Y represents the future value of the observed function. p For past values ​​of the observed function; Γ k=(YpYp T ) -1 ;

[0084] Step 620: Based on the correction vector, forgetting factor λ, and Γ k Calculate Γ k+1 .

[0085] Step 630: Based on the trajectory data at the current moment and the trajectory data at the next moment, write the new future value and the new past value of the observation function.

[0086] Step 640: Based on Γ k+1 The Koopman operator is updated with the new future value and the new past value of the observation function.

[0087] Consider the data for the Koopman operator from 1 to k as follows:

[0088] Among them, Γ k =(YpYp T ) -1 And K k This is an approximation of the Koopman operator at time k. Now, at the next time step, there is a new pair of data points (x... k ,u k ) and (x k+1 ,u k+1 This can be used to update the Koopman operator, rewriting matrix Y to take into account the new data points in the next time step. p and Y f :

[0089]

[0090] Here, Ψk and Ψk+1 represent the evaluation of mapping Ψ at the new data points. Now, the update approximation k+1 of the Koopman operator is given by:

[0091]

[0092] Among them, Γ k+1 It can be rewritten as:

[0093]

[0094]

[0095] Through unfolding We can obtain:

[0096]

[0097] pass Notice Kk Γ k -1 =Y f Y p T Therefore, we can conclude that:

[0098]

[0099] Will and Substitution From this, we can obtain:

[0100]

[0101] Where, γ k The correction vector is the equation involving three main components: the initial value of the Koopman operator, the prediction error, and the correction vector.

[0102] Mode The requirement is that matrix Γ is calculated for k+1 each time an update is performed. k+1 However, this calculation requires matrix reversal, which can be avoided using the following equation:

[0103]

[0104] K k+1 =K k +(Ψ k+1 -KΨ k )γ k ,

[0105]

[0106] Here, λ is the forgetting factor, which can take values ​​between 0.9 < λ < 0.995. It represents a trade-off between interference suppression and faster tracking of changing parameters. If λ = 1, there is no forgetting factor. The core idea of ​​the recursive least squares algorithm with a forgetting factor is to consider the weight of past observations when updating parameter estimates. By introducing a forgetting factor, the algorithm can assign higher weights to the latest observations and lower weights to past observations, thus adapting to changes in the data stream.

[0107] After constructing the Koopman operator (step 200), it can be determined whether the Koopman operator needs to be updated. If it does not need to be updated, step 300 can be executed directly. If the Koopman operator needs to be updated, steps 610-640 should be executed first, and then step 300 should be executed.

[0108] Figure 5This is a flowchart illustrating a method for tracking and controlling the travel path of construction machinery provided in another exemplary embodiment of this application, as shown below. Figure 5 As shown, step 300 above may include:

[0109] Step 310: Determine the observation vector based on the nonlinear kinematic model and the Koopman operator.

[0110] Step 320: Calculate the relationship between the Koopman operator and the observation vector based on the observation vector and the Koopman operator.

[0111] Step 330: Construct a linear relationship between the observation vectors based on the interaction relationship.

[0112] Step 340: Construct a vehicle dynamics prediction model using linear relationships.

[0113] Model predictive control strategies are designed based on the approximate K of the Koopman operator. Assuming the approximate K of the Koopman operator in a discrete-time nonlinear system is obtained, its observation vector is:

[0114]

[0115] Where, ψ nh (x k ) T It is a nonlinear function of the h-vector value, where the n elements h depend on the state. The corresponding effect of K on the observation vector is:

[0116]

[0117] By defining z k =[x k T ψ nh (x k ) T The linear model can be obtained as follows:

[0118] z k+1 =Az k +Bu k ;

[0119] Here, A is the system matrix, and B is the input matrix. The idea behind KMPC is to use this predictive model to design a linear model predictive control for a nonlinear system.

[0120] Figure 6 This is a flowchart illustrating a method for tracking and controlling the travel path of construction machinery provided in another exemplary embodiment of this application, as shown below. Figure 6 As shown, step 100 above may include:

[0121] Step 110: Establish the kinematic equations for the loader's movement.

[0122] Step 120: Determine the simulation state vector and simulation control vector of the loader during the simulation task.

[0123] Step 130: Select the state representation according to the loader's direction of movement, and determine the nonlinear kinematic model based on the simulated state vector, simulated control vector, and driving kinematic equations.

[0124] Considering that wheel loaders mainly operate at low speeds, the slip effect is relatively small. Therefore, the tire slip angle and the lateral forces acting on the tires were not taken into account. Figure 7 This is a schematic diagram of the two-wheel kinematics of a loader provided in an exemplary embodiment of this application, as shown below. Figure 7 As shown, each axle, which consists of two wheels, is replaced by a unique wheel, (x f ,y f (x) indicates the position of the center point of the front wheel axle. r ,y r ) represents the position of the rear axle center point, θ is the rear vehicle heading angle, γ is the articulation angle, which is the difference between the heading angles of the front and rear vehicles, and L f L represents the distance from the hinge point to the front axle. r The distance v represents the distance from the hinge point to the rear axle. f The velocity v of the front vehicle body is expressed as... r This represents the velocity of the rear vehicle body. Assuming the vehicle's velocity and acceleration are very small, and neglecting the slip effect, the directions of the velocities of the front and rear vehicle bodies are consistent with their headings. The loader's kinematics (nonlinear kinematic model) can be described by the following equations:

[0125]

[0126] In the formula, (x f ,y f (x) indicates the position of the center point of the front wheel axle. r ,y r ) represents the position of the rear axle center point, θ is the rear vehicle heading angle, γ is the articulation angle, which is the difference between the heading angles of the front and rear vehicles, and L f L represents the distance from the hinge point to the front axle. r The distance v represents the distance from the hinge point to the rear axle. f The velocity v of the front vehicle body is expressed as... r This indicates the speed of the rear of the vehicle.

[0127] After calculating the rear vehicle's heading angle and articulation angle using the equations, the loader's state can be fully described by the positions of the front and rear vehicles. Therefore, the state vector and control vector can be represented as follows:

[0128]

[0129] In the formula, x is the state vector, which is the current state of the loader, such as the position of the rear axle center point, the rear body heading angle, and the articulation angle; or the position of the front axle center point, the rear body heading angle, and the articulation angle. u is the control vector, which is the control quantity that the loader needs to control at the moment, and also the control input of the loader at the next moment. For example, the loader's body speed and the angular velocity of the articulation angle (the loader's articulation angle can be controlled by controlling the angular velocity of the articulation angle). The control quantity that needs to be controlled at the moment is: the body speed and the angular velocity of the articulation angle, which is the control input of the loader at the next moment.

[0130] Once the state vector and control vector are determined, the nonlinear kinematic model can be rewritten as a discrete-time nonlinear system: x k+1 =f(x) k ,u k In the formula, x k+1 Let x be the state vector of the loader at time k+1; k Let u be the state vector of the loader at time k; k Let be the control vector of the loader at time k.

[0131] A discrete-time nonlinear system is a nonlinear control system described at discrete time points. In MPC (Model Predictive Control) algorithms, a model describing the dynamic behavior of the system is required. This model's role is to predict the system's future dynamics. Specifically, it should be able to predict the output at time k+1 based on the system's state at time k and the control input at time k. Here, the input at time k is used to control the system's output at time k+1, making it as close as possible to the expected value at time k+1. In this application, the dynamic behavior of the system is described by a nonlinear kinematic model. After being rewritten as a discrete-time nonlinear system, the future dynamics of the system can be predicted.

[0132] This exemplary method addresses the highly nonlinear problem of loader dynamics models. It constructs a linearized state-space equation for loader dynamics using Koopman theory, considering lateral position deviation and heading angle deviation during operation. Using acceleration and articulation angle as control inputs, and vehicle speed, lateral position deviation, and heading angle deviation as control outputs, a vehicle dynamics prediction model is established. With acceleration, articulation angle, and vehicle speed as constraints, the objective function is transformed into a quadratic programming problem. To address the issue of low model accuracy, the RLS-FF algorithm is employed to improve prediction accuracy. Simultaneously, an update mechanism is used to determine whether to update the model. In summary, an MPC control system based on the Koopman operator and RLS modeling is established to meet the operational requirements of the loader. This method constructs a KR-MPC data-driven controller, which can solve the control problem of nonlinear systems. Furthermore, while ensuring control accuracy, it saves more computation time than the Nonlinear Model Predictive Control (NMPC). In addition, a nonlinear kinematic model of the loader is constructed, and the loader's trajectory is predicted based on this model, matching the loader's motion mode based on its special structure. This improves the prediction accuracy of the loader's trajectory, obtaining the optimal trajectory and solving the trajectory tracking control problem.

[0133] Exemplary device

[0134] Figure 8 This is a schematic diagram of the structure of a construction machinery travel path tracking and control device provided in an exemplary embodiment of this application, as shown below. Figure 8 As shown, the engineering machinery driving path tracking control device 2 includes: a first construction module 21, used to construct a nonlinear kinematic model of the loader based on the loader's simulation data; a second construction module 22, used to construct a Koopman operator; a linearization module 23, used to linearize the nonlinear kinematic model according to the Koopman operator to construct a vehicle dynamics prediction model; an acquisition module 24, used to acquire the deviation between the loader's current state vector and the target state vector, as well as the control increment between the current control vector and the target control vector; and a calculation module 25, used to input the deviation and the control increment into the vehicle dynamics prediction model for calculation to generate the loader's optimal predicted trajectory.

[0135] In one embodiment, the second construction module 22 described above can be configured to: construct a Koopman operator acting on x k+1 =f(x) k u k The space represented is as follows: Where, x k+1=f(x) k u k (x) represents a discrete-time nonlinear system rewritten from a nonlinear kinematic model; where K is the Koopman operator, ψ is a scalar function of the state vector and control vector, and h(x) is a vector vector of the state and control vector. k ,u k ) is a function that propagates the input forward in time; and the Koopman operator is computed using recursive least squares; where the Koopman operator is used to define the time-observable function of the scalar function ψ.

[0136] In one embodiment, the second construction module 22 described above can be configured to: acquire trajectory data at multiple time points, the trajectory data including a state vector and a control vector at one time point; and determine the past and future values ​​of the observation function based on the trajectory data at multiple time points and the vector ψ of the observation function, wherein the relationship between the future value and the past value of the observation function is: Y f =KY p +ξ; where Y f Y represents the future value of the observed function, K is the Koopman operator, and Y... p Let ξ be the past value of the observation function, where ξ is the approximation error. Based on the relationship between the future value and the past value of the observation function, K is the Koopman operator when the approximation error is minimized by the least squares method.

[0137] In one embodiment, the second building module 22 described above can be configured such that the Koopman operator is: Among them, Y f Y represents the future value of the observed function. p These are past values ​​of the observed function. This is the pseudoinverse of Moore-Pensose.

[0138] Figure 9 This is a schematic diagram of the structure of a construction machinery travel path tracking control device provided in another exemplary embodiment of this application, as shown below. Figure 9 As shown, the engineering machinery driving path tracking control device 2 can also be configured as: an update module 26, used to update the Koopman operator based on a recursive least squares algorithm with a forgetting factor.

[0139] In one embodiment, such as Figure 9 As shown, the update module 26 can also be configured as: a first update unit 261, which calculates an approximate value of the operator at the current time based on the Koopman operator, wherein the approximate value of the operator at the current time is: Among them, Y f Y represents the future value of the observed function. p For past values ​​of the observed function; Γ k =(YpYpT ) -1 The second update unit 262 is used to update the correction vector, forgetting factor λ, and Γ. k Calculate Γ k+1 The third update unit 263 is used to write new future values ​​and new past values ​​of the observation function based on the trajectory data at the current time and the trajectory data at the next time; and the fourth update unit 264 is used to update the observation function based on Γ. k+1 The Koopman operator is updated with the new future value and the new past value of the observation function.

[0140] In one embodiment, the engineering machinery travel path tracking control device 2 can also be configured such that the forgetting factor λ is 0.9 < λ < 0.995.

[0141] In one embodiment, such as Figure 9 As shown, the linearization module 23 can be configured as follows: a first linearization unit 231, used to determine the observation vector based on the nonlinear kinematic model and the Koopman operator; a second linearization unit 232, used to calculate the interaction relationship between the Koopman operator and the observation vector based on the observation vector and the Koopman operator; a third linearization unit 233, used to construct a linear relationship between the observation vectors based on the interaction relationship; and a fourth linearization unit 234, used to construct a vehicle dynamics prediction model based on the linear relationship.

[0142] In one embodiment, the first construction module 21 described above can be configured as: a building unit 211, used to build the kinematic equations of the loader; a vector determination unit 212, used to determine the simulated state vector and simulated control vector of the loader during the simulated task; and a model determination unit 213, used to select the state representation according to the movement direction of the loader, and determine the nonlinear kinematic model according to the simulated state vector, simulated control vector and kinematic equations of the loader.

[0143] Exemplary electronic devices

[0144] An electronic device includes: a processor; a memory for storing processor-executable instructions; and a processor for executing the engineering machinery travel path tracking control method described in the embodiments of this application.

[0145] Below, for reference Figure 10 This application describes an electronic device according to embodiments thereof. The electronic device may be either or both of a first device and a second device, or a standalone device independent of them, which may communicate with the first device and the second device to receive acquired input signals from them.

[0146] Figure 10A block diagram of an electronic device according to an embodiment of this application is illustrated.

[0147] like Figure 10 As shown, the electronic device 10 includes one or more processors 11 and memory 12.

[0148] The processor 11 may be a central processing unit (CPU) or other form of processing unit with data processing capabilities and / or instruction execution capabilities, and may control other components in the electronic device 10 to perform desired functions.

[0149] The memory 12 may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. The volatile memory may include, for example, random access memory (RAM) and / or cache memory. The non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and the processor 11 may execute the program instructions to implement the engineering machinery travel path tracking control method of the various embodiments of this application described above, and / or other desired functions. Various contents such as input signals, signal components, and noise components may also be stored in the computer-readable storage medium.

[0150] In one example, the electronic device 10 may also include an input device 13 and an output device 14, which are interconnected via a bus system and / or other forms of connection mechanism (not shown).

[0151] When the electronic device is a standalone device, the input device 13 can be a communication network connector for receiving the collected input signals from the first device and the second device.

[0152] In addition, the input device 13 may also include, for example, a keyboard, a mouse, etc.

[0153] The output device 14 can output various information to the outside, including determined distance information, direction information, etc. The output device 14 may include, for example, a display, a speaker, a printer, and a communication network and its connected remote output devices, etc.

[0154] Of course, for the sake of simplicity, Figure 10 Only some of the components of the electronic device 10 relevant to this application are shown in this illustration; components such as buses, input / output interfaces, etc., are omitted. In addition, the electronic device 10 may include any other suitable components depending on the specific application.

[0155] The computer program product can be written in any combination of one or more programming languages ​​to perform the operations of the embodiments of this application. The programming languages ​​include object-oriented programming languages ​​such as Java and C++, as well as conventional procedural programming languages ​​such as C or similar languages. The program code can be executed entirely on the user's computing device, partially on the user's computing device, as a standalone software package, partially on the user's computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0156] A computer-readable storage medium stores a computer program for executing the engineering machinery travel path tracking control method described in the embodiments provided in this application.

[0157] The computer-readable storage medium may be any combination of one or more readable media. A readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may, for example, include, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatuses, or devices, or any combination thereof. More specific examples of readable storage media (a non-exhaustive list) include: electrical connections having one or more wires, portable disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0158] The above description has been given for purposes of illustration and description. Furthermore, this description is not intended to limit the embodiments of this application to the forms disclosed herein. Although numerous exemplary aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, alterations, additions, and sub-combinations thereof.

Claims

1. A method for tracking and controlling the travel path of engineering machinery, characterized in that, include: A nonlinear kinematic model of the loader was constructed based on simulation data of the loader. Constructing Koopman operators; The Koopman operator is updated based on the recursive least squares algorithm with a forgetting factor. The Koopman operator is updated using a recursive least squares algorithm with a forgetting factor, including: The approximate value of the operator at the current time step is calculated based on the Koopman operator, and the approximate value of the operator at the current time step is: , where Y f Y represents the future value of the observed function. p For past values ​​of the observed function; Γ k =(YpYp T ) −1 ; Based on the correction vector, forgetting factor λ, and Γ k Calculate Γ k+1 ; Based on the trajectory data at the current moment and the trajectory data at the next moment, write the new future value of the observation function and the new past value of the observation function; and Based on Γ k+1 The Koopman operator is updated by the new future value of the observation function and the new past value of the observation function. The nonlinear kinematic model is linearized using the Koopman operator to construct a vehicle dynamics prediction model. Obtain the deviation between the current state vector and the target state vector of the loader, as well as the control increment between the current control vector and the target control vector; The deviation and the control increment are input into the vehicle dynamics prediction model for calculation to generate the optimal predicted trajectory of the loader.

2. The method for tracking and controlling the travel path of engineering machinery according to claim 1, characterized in that, The construction of the Koopman operator includes: Constructing the Koopman operator to act on The space represented is as follows: ;in, Discrete-time nonlinear systems rewritten from nonlinear kinematic models; Where K is the Koopman operator, ψ is a scalar function of the state vector and the control vector, and h(x) k ,u k ) is a function that propagates the input forward in time; and Calculate the Koopman operator using recursive least squares; The Koopman operator is used to define the observation function of the scalar function ψ over time.

3. The method for tracking and controlling the travel path of construction machinery according to claim 2, characterized in that, The Koopman operator is computed using the recursive least squares method, including: Acquire trajectory data at multiple time points, wherein the trajectory data includes a state vector and a control vector at one time point; Based on trajectory data from multiple moments and the vector ψ of the observation function, the past and future values ​​of the observation function can be determined. The relationship between the future and past values ​​of the observation function is as follows: ； Among them, Y f Y represents the future value of the observed function, K is the Koopman operator, and Y... p Let ξ be the past value of the observed function, where ξ is the approximation error; Based on the relationship between the future value and the past value of the observation function, the K operator is determined by the least squares method to minimize the approximation error.

4. The method for tracking and controlling the travel path of construction machinery according to claim 3, characterized in that, The Koopman operator is: K=Y f AND p †; Among them, Y f Y represents the future value of the observed function. p ξ represents the past value of the observation function, and † represents the Moore-Penrose pseudoinverse.

5. The method for tracking and controlling the travel path of engineering machinery according to claim 1, characterized in that, The forgetting factor λ is: 0.9 < λ < 0.

995.

6. The method for tracking and controlling the travel path of engineering machinery according to claim 1, characterized in that, The nonlinear kinematic model is linearized using the Koopman operator to construct a vehicle dynamics prediction model, including: Based on the nonlinear kinematic model and the Koopman operator, the observation vector is determined. Calculate the relationship between the Koopman operator and the observed vector based on the observed vector; and Construct a linear relationship between the observation vectors based on the aforementioned interaction formula; and The vehicle dynamics prediction model is constructed based on the linear relationship.

7. The method for tracking and controlling the travel path of engineering machinery according to claim 1, characterized in that, A nonlinear kinematic model of the loader is constructed based on simulation data, including: Establish the kinematic equations of the loader; Determine the simulation state vector and simulation control vector of the loader during the simulated task; The state representation is selected based on the loader's direction of movement, and the nonlinear kinematic model is determined based on the simulated state vector, the simulated control vector, and the driving kinematic equations.

8. A travel path tracking and control device for construction machinery, applicable to the travel path tracking and control method for construction machinery described in claim 1, characterized in that, include: The first building module is used to build a nonlinear kinematic model of the loader based on the loader's simulation data; The second building block is used to construct Koopman operators; A linearization module is used to linearize the nonlinear kinematic model according to the Koopman operator in order to construct a vehicle dynamics prediction model; The acquisition module is used to acquire the deviation between the current state vector and the target state vector of the loader, as well as the control increment between the current control vector and the target control vector; The calculation module is used to input the deviation and the control increment into the vehicle dynamics prediction model for calculation, so as to generate the optimal predicted trajectory of the loader.

Images