A trajectory tracking control method for unmanned tracked vehicle based on constraint following

Abstract

The application discloses a kind of unmanned tracked vehicle trajectory tracking control methods based on constraint following.First, the dynamics model of tracked vehicle is obtained using Euler-Lagrange equation and combined with kinematics model, and the coupled dynamics model of unmanned tracked vehicle is obtained.Then, based on U-K theory, the construction of servo constraint is carried out, and the problem is transformed into a kind of approximate constraint following by constructing constraint following error.Furthermore, the motion characteristics of controlled system are analyzed, the system uncertainty disturbance is identified, and the adaptive law is designed.Finally, guided by trajectory tracking, a robust controller is designed to form an adaptive robust control strategy, and its effectiveness is verified by examples.The application establishes a more accurate dynamics model of unmanned tracked vehicle, and solves the problem of unmanned tracked vehicle trajectory tracking control from the perspective of constraint following, reduces the interference of system uncertainty on motion control accuracy, and improves control stability.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of mechanical dynamics modeling and motion control, in particular to a trajectory tracking control method for unmanned tracked vehicle based on constraint following. BACKGROUND

[0002] At present, the research on the motion control of unmanned tracked vehicle is often only aimed at one of the kinematics or dynamics model, and lacks accurate model basis. In the aspect of trajectory tracking control of unmanned tracked vehicle, the control method for this kind of problem can only select a single control target at present, and once the control target changes, the whole control architecture needs to be restructured, so the control efficiency and flexibility are insufficient. In addition, since the system also has complex time-varying uncertain disturbance, it is necessary to develop a motion control method for trajectory tracking of unmanned tracked vehicle, which has stronger flexibility, higher precision and better resistance to time-varying uncertain disturbance. SUMMARY

[0003] The purpose of the present application is to provide a trajectory tracking control method for unmanned tracked vehicle based on constraint following.

[0004] The technical scheme for realizing the purpose of the present application is as follows: a trajectory tracking control method for unmanned tracked vehicle based on constraint following, comprising the following steps:

[0005] Step 1: obtaining the analytical dynamics model of the tracked vehicle through Euler-Lagrange equation, and then combining it with the kinematics model to obtain the coupled dynamics model of the unmanned tracked vehicle;

[0006] Step 2: establishing the trajectory tracking constraint mathematical model of the unmanned tracked vehicle, and converting the initial constraint into servo constraint to obtain the corresponding constraint matrix, constraint vector and constraint tracking error;

[0007] Step 3: based on the controlled unmanned tracked vehicle system dynamics model in step 1, determining a function that comprehensively describes the boundary value of system uncertainty, and combining the constraint tracking error constructed in step 2 to construct an adaptive law;

[0008] Step 4: based on the constraint matrix, constraint vector and servo constraint tracking error constructed in step 2, combining the adaptive law to construct an adaptive robust controller, and performing trajectory tracking control on the controlled system in step 1.

[0009] Preferably, in step 1, the coupled dynamics model of the controlled system is established based on the Euler-Lagrange modeling method combined with the kinematic characteristics of the tracked vehicle, and the specific method is as follows:

[0010] Based on the Euler-Lagrange modeling method, the dynamics model of the tracked vehicle is obtained as follows:

[0011]

[0012] where, is the uncertainty of the system, t represents time, q(t) ∈ R n is the coordinate of the tracked vehicle, and where, and represent the corresponding velocity and acceleration, respectively, i.e. For the sake of simplicity, we omit t in the following. M(q, δ, t) is the inertia matrix, is the Coriolis centrifugal force, G(q, δ, t) is the gravity, is the friction and other external disturbances, T d (t) is the system control input, A represents the constraint matrix of the system itself, and σ is the constraint vector of the system itself.

[0013] According to the kinematic characteristics of the tracked vehicle, its kinematic model is:

[0014]

[0015] where, is the forward direction angle, v is the forward linear velocity, and ω is the angular velocity of rotation around the center of mass. According to its kinematic model, we have i.e. Therefore, After that, we consider

[0016]

[0017] where, v L and v R are the linear velocities of the left and right tracks, respectively. The above two equations are combined to obtain

[0018]

[0019] where, r is the radius of the tracked vehicle drive wheel, and D is the center distance between the two tracks, Assume

[0020]

[0021] From the above equations, we have Taking the derivative, we obtain where,

[0022]

[0023] The coupled dynamics model of the unmanned tracked vehicle can be obtained as:

[0024]

[0025] where,

[0026]

[0027]

[0028] For the problem of uncertainty processing of the system, the model is decomposed, and the dynamics model is decomposed into a nominal part and an uncertain part:

[0029]

[0030]

[0031] wherein, is the nominal part, is the uncertain part, for the convenience of subsequent controller design, the following definitions are made:

[0032]

[0033]

[0034]

[0035] wherein (·) -1 represents the inverse matrix, and I is the unit matrix.

[0036] Preferably, in step 2, a mathematical model of track tracking constraint of the unmanned tracked vehicle is established, and the initial constraint is converted into a servo constraint to obtain a corresponding constraint matrix, a constraint vector and a constraint tracking error. The specific method is as follows:

[0037] Define the error function:

[0038]

[0039] wherein, x, y, are the position coordinates of the current tracked vehicle, x d , d , are the expected position coordinates, each of the above is a function of time t, and is second-order continuous and derivable.

[0040] Derivation is performed to obtain a mathematical model corresponding to the first-order servo constraint:

[0041]

[0042] wherein, l i =[l1l2 l3], wherein each term is an arbitrary constant greater than 0. The second-order derivative is obtained to obtain a mathematical model corresponding to the second-order servo constraint:

[0043]

[0044] wherein, According to the definition in step 2, the constraint matrix and the constraint vector are obtained;

[0045] Constraint matrix:

[0046]

[0047] Constraint vector:

[0048]

[0049] Thus, the constraint tracking error is obtained:

[0050]

[0051] wherein,

[0052] Preferably, in step 3, based on the controlled system dynamics model in step 1, a function that comprehensively describes the uncertainty bound of the system is determined, and the constraint tracking error constructed in step 2 is combined to construct an adaptive law, and the specific method is:

[0053] Based on the controlled system dynamics model in step 1, the uncertainty thereof is analyzed, the general form of the uncertainty parameter δ is determined, and the following inequality is used for scaling transformation to obtain a function П(·) that comprehensively describes the uncertainty bound of the system:

[0054]

[0055] wherein,

[0056]

[0057]

[0058] wherein, α is the uncertainty disturbance in the system, ν is the control gain, p is a unit matrix, is the nominal part, is the uncertain part, and ρ>-1 is a constant.

[0059] Based on the trajectory tracking servo constraint tracking error λ constructed in step 2 and the function Π(·) constructed in this step, an adaptive law that can self-evaluate the uncertainty variable α is constructed:

[0060]

[0061] wherein, is the real-time estimated value of α, β1, β2∈R, β1, β2>0 are design parameters.

[0062] Preferably, in step 4, based on the constraint matrix, the constraint vector and the servo constraint tracking error constructed in step 2, combined with the adaptive law designed in step 3, the adaptive robust controller is constructed to perform trajectory control on the controlled system in step 1, and the specific method is as follows:

[0063] For the unmanned tracked vehicle system in step 1, based on the constraint matrix and the constraint vector constructed in step 3, and the constructed servo constraint tracking error, combined with the adaptive law a robust controller is constructed:

[0064]

[0065] wherein,

[0066]

[0067] wherein,

[0068]

[0069]

[0070] wherein respectively represent a part of the control input torque T d (t), and ξ>0 is a constant, so as to satisfy the servo constraint of step 2.

[0071] The application also provides a computer device, which comprises a memory, a processor and a computer program stored in the memory and executable on the processor, and the processor implements the method for performing the constraint following based trajectory tracking control of the unmanned tracked vehicle when executing the computer program.

[0072] The application also provides a computer readable storage medium, which stores a computer program, and the computer program is executable on the processor to implement the method for performing the constraint following based trajectory tracking control of the unmanned tracked vehicle.

[0073] Compared with the prior art, the present application has the following significant features: a) the dynamic model of the unmanned tracked vehicle is obtained by Euler-Lagrange equation, and then the dynamic model of the unmanned tracked vehicle is combined with the kinematic model to obtain the coupled dynamic model of the unmanned tracked vehicle, so that the obtained model is more accurate and can better reflect the real motion state; b) the expected trajectory tracking target is converted into a servo constraint, the trajectory tracking control problem of the unmanned tracked vehicle is converted into a kind of approximate constraint following control problem by constructing the constraint tracking error as the control object of the controller design, and c) the adaptive law is designed for the system uncertainty, and the adaptive robust control strategy is constructed, so that the three seemingly different constraints are met at the same time, which can not only make the controlled system resist time-varying uncertainty interference, but also only need to reconstruct the servo constraint when the control target changes, without the need to redesign the controller, and has strong flexibility and accuracy. BRIEF DESCRIPTION OF DRAWINGS

[0074] Figure 1 The constraint following error λ1, λ2, λ3 of the application control method.

[0075] Figure 2 The real-time motion trajectory of the unmanned tracked vehicle under the action of the controller. DETAILED DESCRIPTION

[0076] Step 1, based on the Euler-Lagrange modeling method, the kinematic characteristics of the tracked vehicle are combined to establish the coupled dynamic model of the controlled system, and the specific method is as follows:

[0077] Based on the Euler-Lagrange modeling method, the dynamic model of the tracked vehicle is obtained as follows:

[0078]

[0079] wherein, is the uncertainty of the system, t represents time, q(t) ∈ R n is the coordinate of the tracked vehicle, and And respectively represent the corresponding speed and acceleration, that is, In order to write simply, t is omitted. M(q, δ, t) is the inertia matrix, is the Coriolis centrifugal force, G(q, δ, t) is the gravity, is the friction and other external disturbances, T d (t) is the system control input, A represents the constraint matrix of the system itself, and σ is the constraint vector of the system itself.

[0080] According to the kinematic characteristics of the tracked vehicle, the kinematic model is as follows:

[0081]

[0082] where, is the forward direction angle, v is the forward linear velocity, and ω is the angular velocity of rotation around the center of mass. According to its kinematic model, we have i.e. Therefore, After that, consider

[0083]

[0084] where, v L , v R are the linear velocities of the left and right tracks, respectively. The above two equations are combined to obtain

[0085]

[0086] where, r is the radius of the track vehicle drive wheel, and D is the center distance between the two tracks, Assume

[0087]

[0088] From the above equations, we have Taking the derivative, we get where,

[0089]

[0090] Then the coupled dynamics model of the unmanned tracked vehicle can be obtained as:

[0091]

[0092] where,

[0093]

[0094]

[0095] To deal with the uncertainty of the system, the model is decomposed into a nominal part and an uncertain part:

[0096]

[0097]

[0098] where, is the nominal part, is the uncertain part.

[0099] Based on the coupled dynamics model of the unmanned tracked vehicle, the expected motion characteristics of the system, such as position, velocity, trajectory, etc., are analyzed, and the characteristics are mathematically abstracted to provide a standard form for the subsequent step 2, and a first-order servo constraint that can describe the motion characteristics is constructed and written in matrix form:

[0100]

[0101] where B = [B li ] m×n is the constraint matrix, a = [a1a2...a m ] T is the constraint vector, B li and c l is first-order derivable, then the servo constraint is differentiated to obtain the second-order servo constraint as follows:

[0102]

[0103] where c = [c1c2...c m ] T is the constraint vector.

[0104] The constraint tracking error λ is constructed as the control object of the subsequent controller design:

[0105]

[0106] where λ = [λ1λ2...λ m ] T .

[0107] Step 2, the tracked vehicle trajectory tracking constraint mathematical model is established, and the initial constraint is converted into a servo constraint to obtain the corresponding constraint matrix, constraint vector and constraint tracking error. The specific method is as follows:

[0108] Define the error function:

[0109]

[0110] where x, y, are the current tracked vehicle position coordinates, x d , y d , are the expected position coordinates, all of which are functions of time t and are second-order continuous and derivable.

[0111] Differentiate to obtain the mathematical model of the corresponding first-order servo constraint:

[0112]

[0113] where, li = [l1 l2 l3], where each is an arbitrary constant greater than 0. Taking the second derivative gives the mathematical model of the second order servo constraint:

[0114]

[0115] where, According to the definition in step 2, the constraint matrix and the constraint vector are obtained;

[0116] Constraint matrix:

[0117]

[0118] Constraint vector:

[0119]

[0120] Thus, the constraint following error is obtained:

[0121]

[0122] where,

[0123] Step 3, based on the controlled unmanned tracked vehicle system dynamics model in step 1, determine the function that comprehensively describes the system uncertainty boundary

[0124]

[0125] where,

[0126]

[0127]

[0128] where, α is the uncertainty disturbance in the system, v is the control gain, p is the unit matrix, is the nominal part, is the uncertain part, ρ > -1 is a constant.

[0129] Based on the trajectory tracking servo constraint tracking error λ constructed in step 2, and the function Π(·) constructed in this step, an adaptive law is constructed that can self-evaluate the uncertainty variable α:

[0130]

[0131] where, is the real-time estimated value of α, β1, β2 ∈ R, β1, β2 > 0 are design parameters.

[0132] Step 4, for the unmanned tracked vehicle system of step 1, based on the constraint matrix and constraint vector constructed in step 3, and the constructed servo constraint tracking error, combined with the adaptive law Construct a robust controller:

[0133] Wherein,

[0134]

[0135] Wherein,

[0136]

[0137]

[0138] Wherein Respectively represent a part of the control input torque , ξ>0 is a constant, so as to satisfy the servo constraint of step 2.

[0139] The embodiment also provides a computer device, including a memory, a processor and a computer program stored in the memory and executable on the processor, and the processor implements the method for performing the constraint following based unmanned tracked vehicle trajectory tracking control when executing the computer program.

[0140] The embodiment also provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to implement the method for performing the constraint following based unmanned tracked vehicle trajectory tracking control.

[0141] The hardware structure of the computer device can include a processor, a memory, an input / output interface, a communication interface and a bus. The processor, the memory, the input / output interface and the communication interface are connected with each other through the bus.

[0142] The processor can be implemented in the form of a general-purpose CPU, a microprocessor, an application-specific integrated circuit or one or more integrated circuits, and is used to execute related programs to implement the technical solutions provided by the embodiments of the present application.

[0143] The memory can be implemented in the form of a ROM, a RAM, a static storage device, a dynamic storage device and the like. The memory can store an operating system and other application programs, and when the technical solutions provided by the embodiments of the present application are implemented by software or firmware, the related program codes are saved in the memory and executed by the processor.

[0144] Input / output interfaces are used to connect input / output modules to enable information input and output. Input / output modules can be configured as components within a device or connected externally to provide corresponding functions. Input devices may include keyboards, mice, touchscreens, microphones, various sensors, etc., while output devices may include displays, speakers, vibrators, indicator lights, etc.

[0145] The communication interface is used to connect the communication module to enable communication and interaction between this device and other devices. The communication module can communicate via wired means (such as USB, Ethernet cable, etc.) or wireless means (such as mobile network, WIFI, Bluetooth, etc.).

[0146] A bus is a pathway that transmits information between various components of a device, such as processors, memory, input / output interfaces, and communication interfaces.

[0147] The computer-readable medium includes both permanent and non-permanent, removable and non-removable media, and information storage can be achieved by any method or technology. Information can be computer-readable instructions, data structures, program modules, or other data. Examples of computer storage media include, but are not limited to, phase-change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, CD-ROM, digital versatile optical disc (DVD) or other optical storage, magnetic tape, magnetic magnetic disk storage or other magnetic storage devices, or any other non-transfer medium that can be used to store information accessible by a computing device.

[0148] Example 1

[0149] To verify the effectiveness of the present invention, adaptive robust control is applied to the following unmanned tracked vehicle trajectory tracking control problem. The control object and objective are as follows:

[0150] (1). The controlled object is an unmanned tracked vehicle;

[0151] (2). The control objective is for the unmanned tracked vehicle to control the circular trajectory x. 2 +y 2 =81 for trajectory tracking.

[0152] Step 1: Obtain the analytical dynamic model of the tracked vehicle using the Euler-Lagrange equation method.

[0153]

[0154] Subsequently, based on its connection to the kinematic equations of unmanned tracked vehicles...

[0155]

[0156] The fusion is obtained unmanned ground vehicle coupling dynamics model

[0157]

[0158] In view of the problem of system uncertainty processing, the model is decomposed, and the dynamics model is decomposed into nominal part and uncertain part:

[0159]

[0160]

[0161] Wherein, The nominal part is The uncertain part is.

[0162] Step 2: According to the control target, the expected trajectory is

[0163]

[0164] According to the first order servo constraint in step 2 above, we get:

[0165]

[0166] Wherein,

[0167]

[0168] Derivation of the first order constraint, we get:

[0169]

[0170] Wherein,

[0171]

[0172]

[0173] According to the above derivation, the system constraint following error is

[0174]

[0175] Step 3: Based on the controlled unmanned ground vehicle system dynamics model of step 1, determine the function of comprehensive description of system uncertainty boundary

[0176]

[0177] Wherein,

[0178]

[0179]

[0180] wherein, a is the uncertainty disturbance in the system, v is the control gain, p is the unit matrix, is the nominal part, is the uncertain part, and p > -1 is a constant.

[0181] Based on the trajectory tracking servo constraint tracking error λ constructed in step 2, and the function П(·) constructed in this step, an adaptive law capable of self-evaluating the uncertainty variable a is constructed:

[0182]

[0183] wherein,

[0184]

[0185] Therefore, there is

[0186]

[0187] Step 4: Based on the constraint matrix, constraint vector and servo constraint tracking error constructed in step 2, and combined with the adaptive law, an adaptive robust controller is constructed

[0188]

[0189] wherein,

[0190]

[0191]

[0192]

[0193] Then, the trajectory tracking control of the controlled system in step 1 is simulated by using Matlab, and the simulation results are shown in Figs. Figure 1 ,2.

[0194] Figure 1 The constraint following errors λ1, λ2, λ3 of the application of the control method of the application can be found, and the trajectory tracking adaptive robust control method of the unmanned tracked vehicle of the application can make the controlled system present the expected motion characteristics in a very short time, and the tracking error fluctuation range is very small after the system is stable, and the control precision is high. Figure 2The real-time motion trajectory of the unmanned tracked vehicle under the action of the controller is shown, and it can be seen from the comparison with the expected trajectory that the unmanned tracked vehicle can approximately track the expected trajectory under the action of the controller. Therefore, the application can quickly and effectively solve the problem of trajectory tracking control of the unmanned tracked vehicle.

[0195] The technical features of the above embodiments can be combined in any manner. In order to make the description simple, all possible combinations of the technical features in the above embodiments are not described, but as long as the combinations of the technical features do not contradict, they should be considered as the scope of the description.

[0196] The above embodiments only express several implementation manners of the application, and the description is relatively specific and detailed, but it should not be understood as a limitation on the patent scope of the application. It should be pointed out that for ordinary skilled persons in the art, several modifications and improvements can be made without departing from the concept of the application, and these all belong to the protection scope of the application. Therefore, the patent protection scope of the application should be subject to the appended claims.

Claims

1. A method for trajectory tracking control of a constraint-following based unmanned tracked vehicle, characterized in that, Comprising the following steps: Step 1, obtain the analytical dynamics model of the tracked vehicle through Euler-Lagrange equation, then combine with its kinematics model to obtain the coupled dynamics model of the unmanned tracked vehicle; Step 2, establish the trajectory tracking constraint mathematical model of the unmanned tracked vehicle, and convert the initial constraint into servo constraint to obtain the corresponding constraint matrix, constraint vector and constraint tracking error; Step 3, based on the controlled unmanned tracked vehicle system dynamics model of step 1, determine the function that comprehensively describes the boundary value of system uncertainty, and combine the constraint tracking error constructed in step 2 to construct the adaptive law; Step 4, based on the constraint matrix, constraint vector and servo constraint tracking error constructed in step 2, combine the adaptive law to construct the adaptive robust controller, and perform trajectory tracking control on the controlled system of step 1; Step 1 specifically comprises: Based on Euler-Lagrange modeling method, the dynamics model of the tracked vehicle is obtained as follows: ； where, is the uncertainty parameter of the system, t represents time, is the coordinate of the tracked vehicle, and is the velocity of the tracked vehicle, is the acceleration of the tracked vehicle, respectively, , , is the inertia matrix, is the Coriolis centrifugal force, is the gravity, is the friction and other external disturbances, is the system control input, represents the constraint matrix of the system itself, is the constraint vector of the system itself; According to the kinematics characteristics of the tracked vehicle, the kinematics model is as follows: ； wherein, is the forward direction angle, is the forward linear velocity, is the angular velocity of rotation around the center of mass; from its kinematic model, we get i.e. thus, ; considering ； wherein, Vr, Vl are the linear velocities of the right and left tracks, respectively; ； wherein, R is the track vehicle drive wheel radius, L is the two side track center distance, , assuming ； From the above equation, , the derivative is where, ； Then the coupled dynamics model of the unmanned tracked vehicle is obtained as follows: ； wherein , ； For the problem of system uncertainty processing, the model is decomposed, and the dynamics model is decomposed into a nominal part and an uncertain part: ； wherein is a nominal part, is an uncertainty part, for the convenience of subsequent controller design, the following definitions are made: wherein denotes the inverse matrix, is the identity matrix.

2. The constraint-following based unmanned tracked vehicle trajectory tracking control method according to claim 1, characterized in that, In step 2, the trajectory tracking constraint mathematical model of the unmanned tracked vehicle is established, and the initial constraint is converted into a servo constraint to obtain the corresponding constraint matrix, constraint vector and constraint tracking error, and the specific method is as follows: Define the error function: ; wherein, are the position coordinates of the current tracked vehicle, are the desired position coordinates, each of the above being a function of time and being twice continuously differentiable. Derivation, get the corresponding first-order servo constraint mathematical model: Wherein, ; All are greater than 0, any constant; the second derivative to get the corresponding second-order servo constraint mathematical model: ; wherein, ; according to the definition in step 2, get the constraint matrix and constraint vector; Constraint matrix: Constraint vector: ; Thus, the constraint tracking error is obtained as: ; where, .

3. The constraint-following based unmanned tracked vehicle trajectory tracking control method according to claim 2, characterized in that, In step 3, based on the controlled system dynamics model of step 1, the function that comprehensively describes the boundary value of system uncertainty is determined, and the constraint tracking error constructed in step 2 is combined to construct the adaptive law, and the specific method is as follows: Based on the controlled system dynamics model of step 1, analyze its uncertainty, determine the uncertainty parameters of the general form, and through the following inequality scaling transformation, get a function that comprehensively describes the system uncertainty bound : ; wherein, ； ； wherein is an uncertainty disturbance in the system, is a control gain, is an identity matrix, is a nominal part, is an uncertainty part, is a constant; Trajectory tracking servo constrains tracking error based on step 2 constructed trajectory , and the function constructed in this step , construct an adaptive law that can self-assess uncertainty disturbance ​ ； wherein, is a real-time estimate of , , is a design parameter.

4. The constraint-following based unmanned tracked vehicle trajectory tracking control method according to claim 3, characterized in that, In step 4, based on the constraint matrix, constraint vector and servo constraint tracking error constructed in step 2, the adaptive law designed in step 3 is combined to construct the adaptive robust controller, and the trajectory control is performed on the controlled system of step 1, and the specific method is as follows: For the unmanned tracked vehicle system of step 1, based on the constraint matrix and constraint vector constructed in step 3, and the constructed servo constraint tracking error, combined with the adaptive law , the robust controller is constructed: wherein, wherein, wherein , i = 1, 2, 3, respectively represent a portion of the control input torque , and is a constant such that it satisfies the servo constraint of step 2.

5. A computer device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor executes the computer program to realize the method of any one of claims 1-4 for trajectory tracking control of the unmanned tracked vehicle based on constraint following.

6. A computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to realize the method of any one of claims 1-4 for trajectory tracking control of the unmanned tracked vehicle based on constraint following.

Images