Trajectory tracking control method, system and medium for multi-unmanned vehicle cooperative transport system

Abstract

The present invention discloses a trajectory tracking control method, system and medium for a multi-unmanned vehicle cooperative transport system. The method of the present invention includes establishing unconstrained dynamic models of the three subsystems of the leading unmanned vehicle, the load and the following unmanned vehicle in the multi-unmanned vehicle cooperative transport system respectively; establishing constraint equations, converting them into second-order forms by derivation of time, and then introducing constraint equations of trajectory errors in zero-order and first-order forms to establish a total constraint equation of the system in matrix form; embedding the total constraint equation of the system into the unconstrained dynamic model to obtain a constraint dynamic model of the entire system of the multi-unmanned vehicle cooperative transport system, solving the constraint dynamic model to obtain the control torque of the leading unmanned vehicle and the following unmanned vehicle to realize the trajectory tracking of the multi-unmanned vehicle cooperative transport system. The present invention aims to overcome the problems of the complexity, low efficiency and insufficient control accuracy of the existing control methods, and has the advantages of simplicity, high efficiency and good control effect.

Description

Technical Field

[0001] The present invention relates to a multi-unmanned vehicle cooperative control technology, and in particular to a trajectory tracking control method, system and medium for a multi-unmanned vehicle cooperative transport system. Background Art

[0002] Unmanned vehicles are a common means of transportation. Since they do not require manual driving and are flexible and efficient, they have been widely used in factories, logistics and other scenarios. However, due to the size of unmanned vehicles, when carrying longer beam structures or similar components, a single unmanned vehicle cannot meet the conditions for transportation, and multiple unmanned vehicles are required to carry them together. A multi-unmanned vehicle collaborative transportation system refers to a system composed of two or more unmanned vehicles, which are used to carry the same material at the same time and travel autonomously and collaboratively. However, when carrying longer beam structures or similar components, the multi-unmanned vehicle collaborative transportation system involves multiple unmanned vehicles, which has problems such as complex control, low efficiency and insufficient control accuracy. Summary of the invention

[0003] Technical problem to be solved by the present invention: In view of the above-mentioned problems in the prior art, a trajectory tracking control method, system and medium for a multi-unmanned vehicle collaborative transport system are provided. The present invention aims to overcome the problems of complexity, low efficiency and insufficient control accuracy of the existing control methods for multi-unmanned vehicle collaborative transport systems.

[0004] In order to solve the above technical problems, the technical solution adopted by the present invention is:

[0005] A trajectory tracking control method for a multi-unmanned vehicle cooperative transport system, comprising:

[0006] S101, for the three subsystems of the leading unmanned vehicle, the load and the following unmanned vehicle in the multi-unmanned vehicle cooperative transport system, unconstrained dynamic models of the three subsystems are established respectively; constraint equations are established for the multi-unmanned vehicle cooperative transport system, the constraint equations are converted into second-order forms by taking the time derivative, and then the constraint equations of the trajectory error in zero-order form and first-order form are introduced, and the total constraint equation of the system in matrix form is established;

[0007] S102, embed the total constraint equation of the system in matrix form into the unconstrained dynamics model to obtain the constraint dynamics model of the entire multi-unmanned vehicle cooperative transport system, and solve the constraint dynamics model to obtain the control torque of the guiding unmanned vehicle and the following unmanned vehicle to achieve trajectory tracking of the multi-unmanned vehicle cooperative transport system.

[0008] Optionally, the function expression of the unconstrained dynamics model established for guiding the unmanned vehicle in step S101 is:

[0009]

[0010] In the above formula, q g is the state variable for guiding the unmanned vehicle, t is the time, is the state variable q g The second-order derivative of is the state variable q g The first derivative of M g (q g ,t) represents the mass / inertia matrix of the guided unmanned vehicle, represents the generalized force matrix guiding the unmanned vehicle, represents the generalized constraint matrix for guiding the unmanned vehicle, the subscript g represents guiding the unmanned vehicle, and:

[0011]

[0012]

[0013]

[0014]

[0015] Among them, m g To guide the quality of the unmanned vehicle, r g is the wheel radius of the guided unmanned vehicle, θ g To guide the azimuth of the unmanned vehicle, I g To guide the center of mass moment of inertia of the unmanned vehicle, l g To guide the half width of the unmanned vehicle, (x g ,y g ) is the center of mass coordinate of the unmanned vehicle on the two-dimensional plane, The x-axis component x of the center of mass coordinate of the unmanned vehicle is g The second-order derivative of The y-axis component y of the center of mass coordinate of the unmanned vehicle is g The second-order derivative of To guide the azimuth angle θ of the unmanned vehicle g The second-order derivative of is the x-axis component of the centroid coordinate x g The first derivative of is the y-axis component of the centroid coordinate g The first derivative of To guide the azimuth angle θ of the unmanned vehicle g The first derivative of grf To guide the driving torque of the right front wheel of the unmanned vehicle, u glf To guide the driving torque of the left front wheel of the unmanned vehicle, the function expression of the unconstrained dynamic model established for the load in step S101 is:

[0016]

[0017] In the above formula, q c is the state variable of the load, t is the time, is the state variable q c The second-order derivative of is the state variable q c The first derivative of M c (q c ,t) represents the mass / inertia matrix of the load, The generalized force matrix representing the load, represents the generalized constraint force matrix of the load, the subscript c represents the load, and has:

[0018]

[0019]

[0020]

[0021]

[0022] Among them, m c is the mass of the load, I c is the moment of inertia of the load’s center of mass, is the x-axis component of the load's centroid coordinate c The second-order derivative of is the y-axis component of the load's center of mass coordinate c The second-order derivative of is the load azimuth angle θ c The second derivative of (x c ,y c ) is the center of mass coordinate of the load on the two-dimensional plane; the function expression of the unconstrained dynamic model established for the following unmanned vehicle in step S101 is:

[0023]

[0024] In the above formula, q f is the state variable of the following unmanned vehicle, t is the time, is the state variable q f The second-order derivative of is the state variable q f The first derivative of M f (q f ,t) represents the mass / inertia matrix of the following unmanned vehicle, represents the generalized force matrix following the unmanned vehicle, represents the generalized constraint matrix of the following unmanned vehicle, the subscript f represents the following unmanned vehicle, and:

[0025]

[0026]

[0027]

[0028]

[0029] Among them, m f To follow the mass of the unmanned vehicle, r f is the wheel radius of the following unmanned vehicle, θ f To follow the azimuth of the unmanned vehicle, I f is the moment of inertia of the center of mass of the unmanned vehicle, l f is half the width of the following unmanned vehicle, (x f ,y f ) is the coordinate of the center of mass of the unmanned vehicle on the two-dimensional plane. is the x-axis component of the center of mass coordinate of the following unmanned vehicle f The second-order derivative of is the y-axis component of the center of mass coordinate of the following unmanned vehicle f The second-order derivative of is the azimuth angle θ of the following unmanned vehicle f The second-order derivative of is the x-axis component of the centroid coordinate x f The first derivative of is the y-axis component of the centroid coordinate f The first derivative of is the azimuth angle θ of the following unmanned vehicle f The first derivative of frf is the driving torque of the right front wheel of the following unmanned vehicle, u flf is the driving torque of the left front wheel of the following unmanned vehicle.

[0030] Optionally, the constraint equations established for the multi-unmanned vehicle cooperative transport system in step S101 include the trajectory constraint of the leading unmanned vehicle, the trajectory constraint of the following unmanned vehicle, the integrity constraint between the leading unmanned vehicle and the following unmanned vehicle, and the geometric constraint between the leading unmanned vehicle, the load and the following unmanned vehicle, wherein the function expression of the trajectory constraint of the leading unmanned vehicle is shown as follows:

[0031] x g =r a cost,y g =r b cost,(16)

[0032] The function expression of following the trajectory constraint of the unmanned vehicle is as follows:

[0033]

[0034] The functional expression of the complete constraint between the leading unmanned vehicle and the following unmanned vehicle is as follows:

[0035] (x g -x f ) 2 +(y g -y f ) 2 =D 2 , (18)

[0036] The functional expression of the geometric constraints between the leading unmanned vehicle, the payload, and the following unmanned vehicle is as follows:

[0037] x g +x f -2x c =0,y g +y f -2y c =0, (19)

[0038] Among them, r a is the major semi-axis of the elliptical trajectory, r b is the minor semi-axis of the elliptical trajectory, D is the constant distance that needs to be maintained between the following unmanned vehicle and the leading unmanned vehicle, (x g ,y g ) is the center of mass coordinate of the unmanned vehicle on the two-dimensional plane, (x f ,y f ) is the center of mass coordinate of the following unmanned vehicle on the two-dimensional plane, and t is the time.

[0039] Optionally, after the constraint equation is converted into a second-order form in step S101, the second-order form of the constraint for guiding the unmanned vehicle trajectory is obtained as follows:

[0040]

[0041] The obtained second-order form of the constraint of following the unmanned vehicle trajectory is:

[0042]

[0043] The obtained second-order form of the complete constraint between the leading unmanned vehicle and the following unmanned vehicle is:

[0044]

[0045] The second-order form of the geometric constraints between the leading unmanned vehicle, the payload, and the following unmanned vehicle is obtained as:

[0046]

[0047] in, The x-axis component x of the center of mass coordinate of the unmanned vehicle is g The second-order derivative of The y-axis component y of the center of mass coordinate of the unmanned vehicle is g The second-order derivative of is the x-axis component of the centroid coordinate x g The first derivative of is the y-axis component of the centroid coordinate g The first derivative of is the x-axis component of the center of mass coordinate of the following unmanned vehicle f The second-order derivative of is the y-axis component of the center of mass coordinate of the following unmanned vehicle f The second-order derivative of is the x-axis component of the centroid coordinate x f The first derivative of is the y-axis component of the centroid coordinate f The first derivative of a is the major semi-axis of the elliptical trajectory, r b is the minor semi-axis of the ellipse trajectory.

[0048] Optionally, the constraint equations for introducing the trajectory error in zero-order form and first-order form in step S101 include:

[0049]

[0050] e1=x g -acost, (25)

[0051] e2=y g -bsint, (26)

[0052]

[0053] Among them, α and β are constants greater than 0, e is the zero-order form of the error between the actual trajectory and the ideal trajectory, is the first-order form of the error e between the actual trajectory and the ideal trajectory, is the second-order form of the error e between the actual trajectory and the ideal trajectory, e1 is the x-axis component of the trajectory error of the guided unmanned vehicle, e2 is the y-axis component of the trajectory error of the guided unmanned vehicle, and e3 is the trajectory error of the following unmanned vehicle. (x g ,y g ) is the center of mass coordinate of the unmanned vehicle on the two-dimensional plane, (x f ,y f ) is the coordinate of the center of mass of the following unmanned vehicle on the two-dimensional plane, t is the time, r a is the major semi-axis of the elliptical trajectory, r b is the minor semi-axis of the ellipse trajectory.

[0054] Optionally, the function expression of the system total constraint equation in matrix form established in step S101 is:

[0055]

[0056] In the above formula, q is the total state variable of the system, t is the time, is the second-order derivative of the total state variable q, is the first-order derivative of the total state variable q, represents an m×n-order constraint matrix, is an m-dimensional column vector with:

[0057]

[0058]

[0059]

[0060] Among them, (x g ,y g ) is the center of mass coordinate of the unmanned vehicle on the two-dimensional plane, (x f ,y f ) is the coordinate of the center of mass of the following unmanned vehicle on the two-dimensional plane, t is the time, r a is the major semi-axis of the elliptical trajectory, r b is the minor semi-axis of the ellipse trajectory, The x-axis component x of the center of mass coordinate of the unmanned vehicle is g The second-order derivative of The y-axis component y of the center of mass coordinate of the unmanned vehicle is g The second-order derivative of To guide the azimuth angle θ of the unmanned vehicle g The second-order derivative of is the x-axis component of the centroid coordinate x g The first derivative of is the y-axis component of the centroid coordinate g The first derivative of To guide the azimuth angle θ of the unmanned vehicle g The first derivative of is the x-axis component of the load's centroid coordinate c The second-order derivative of is the y-axis component of the load's center of mass coordinate c The second-order derivative of is the load azimuth angle θ c The second-order derivative of is the x-axis component of the center of mass coordinate of the following unmanned vehicle f The second-order derivative of is the y-axis component of the center of mass coordinate of the following unmanned vehiclef The second-order derivative of is the azimuth angle θ of the following unmanned vehicle f The second-order derivative of is the x-axis component of the centroid coordinate x f The first derivative of is the y-axis component of the centroid coordinate f The first derivative of is the azimuth angle θ of the following unmanned vehicle f The first-order derivative of , α and β are constants greater than 0, e is the zero-order form of the error between the actual trajectory and the ideal trajectory, is the first-order derivative of the error e between the actual trajectory and the ideal trajectory, is the second-order derivative of the error e between the actual trajectory and the ideal trajectory, e1 is the x-axis component of the trajectory error of the guided unmanned vehicle, e2 is the y-axis component of the trajectory error of the guided unmanned vehicle, and e3 is the trajectory error of the following unmanned vehicle. is the first-order derivative of the x-axis component e1 of the trajectory error of the unmanned vehicle, is the first-order derivative of the y-axis component e2 of the trajectory error of the unmanned vehicle, is the first-order derivative of the trajectory error e3 following the unmanned vehicle.

[0061] Optionally, the function expression of the constraint dynamics model of the entire multi-unmanned vehicle cooperative transport system obtained in step S102 is:

[0062]

[0063] In the above formula, M represents the mass / inertia matrix of the total system, Q represents the generalized force matrix of the total system, A represents the constraint matrix of m×n order, and b is the m-dimensional column vector. is the second-order derivative of the total state variable q, and:

[0064]

[0065]

[0066] In the above formula, M g To guide the mass / inertia matrix M of the unmanned vehicle g (q g ,t),M c is the mass / inertia matrix M of the load c (q c ,t),M f is the mass / inertia matrix M of the following unmanned vehicle f (q f ,t),Q g The generalized force matrix for guiding the unmanned vehicle Qc is the generalized force matrix of the load Q f is the generalized force matrix of the following unmanned vehicle

[0067] Optionally, in step S102, the constraint dynamics model is solved to obtain the control torque of the leading unmanned vehicle and the following unmanned vehicle:

[0068]

[0069] In the above formula, u is the control torque, M represents the mass / inertia matrix of the total system, Q represents the generalized force matrix of the total system, A represents the constraint matrix of m×n order, and b is the m-dimensional column vector; and:

[0070] u=[u grf ,u glf ,u frf ,u flf ] Τ

[0071] In the above formula, u grf To guide the driving torque of the right front wheel of the unmanned vehicle, u glf To guide the driving torque of the left front wheel of the unmanned vehicle, u frf is the driving torque of the right front wheel of the following unmanned vehicle, u flf is the driving torque of the left front wheel of the following unmanned vehicle.

[0072] In addition, the present invention also provides a trajectory tracking control system for a multi-unmanned vehicle cooperative transport system, comprising a microprocessor and a memory connected to each other, wherein the microprocessor is programmed or configured to execute the trajectory tracking control method for the multi-unmanned vehicle cooperative transport system.

[0073] In addition, the present invention also provides a computer-readable storage medium, in which a computer program is stored. The computer program is used to be programmed or configured by a microprocessor to execute the trajectory tracking control method of the multi-unmanned vehicle cooperative transport system.

[0074] Compared with the prior art, the present invention mainly has the following advantages: the method of the present invention includes establishing unconstrained dynamic models of the three subsystems of the leading unmanned vehicle, the load and the following unmanned vehicle in the multi-unmanned vehicle cooperative transport system respectively; establishing a constraint equation, converting it into a second-order form by derivation of time, and then introducing the constraint equation of the trajectory error in the zero-order form and the first-order form to establish the total constraint equation of the system in matrix form; embedding the total constraint equation of the system in matrix form into the unconstrained dynamic model to obtain the constraint dynamic model of the entire system of the multi-unmanned vehicle cooperative transport system, and solving the constraint dynamic model to obtain the control torque of the leading unmanned vehicle and the following unmanned vehicle to realize the trajectory tracking of the multi-unmanned vehicle cooperative transport system. The present invention can overcome the problems of complexity, low efficiency and insufficient control accuracy of the existing control methods of the multi-unmanned vehicle cooperative transport system, and has the advantages of simplicity, high efficiency and good control effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] Figure 1 It is a schematic diagram of the structure of the dual-vehicle cooperative transport system in an embodiment of the present invention.

[0076] Figure 2 Schematic diagram of the basic flow of the method of the embodiment of the present invention.

[0077] Figure 3 The figure shows a comparison between the actual trajectory and the ideal trajectory for guiding the unmanned vehicle in an embodiment of the present invention.

[0078] Figure 4 1 is a comparison between the actual trajectory and the ideal trajectory of the load in the embodiment of the present invention.

[0079] Figure 5 The figure shows a comparison between the actual trajectory and the ideal trajectory of the unmanned vehicle in an embodiment of the present invention.

[0080] Figure 6 It is the error between the ideal trajectory and the actual trajectory of the unmanned vehicle in the embodiment of the present invention.

[0081] Figure 7 is the error between the ideal trajectory and the actual trajectory of the unmanned vehicle in the embodiment of the present invention.

[0082] Figure 8 The figure shows the comparison of the right front wheel driving torque between the leading unmanned vehicle and the following unmanned vehicle in the embodiment of the present invention.

[0083] Fig. 9 The figure shows the comparison of the right front wheel driving torque between the leading unmanned vehicle and the following unmanned vehicle in the embodiment of the present invention. DETAILED DESCRIPTION

[0084] Figure 1The schematic diagram of the structure of the dual-vehicle cooperative transport system of the present invention is shown, which is composed of a leading unmanned vehicle, a load and a following unmanned vehicle. The ideal trajectory represents a standard ellipse, the major semi-axis of the ellipse represents 10m, and the minor semi-axis represents 6m. Figure 1 It can be seen that (x g ,y g ) represents the coordinates of the center of mass of the guided unmanned vehicle on the two-dimensional plane (XOY), (x c ,y c ) represents the coordinates of the center of mass of the load on the two-dimensional plane, (x f ,y f ) represents the center of mass coordinates of the unmanned vehicle on the two-dimensional plane, θ g represents the azimuth of the guided unmanned vehicle, θ c represents the azimuth angle of the load, θ f represents the azimuth of the following unmanned vehicle, v g represents the speed of guiding the unmanned vehicle, v c Indicates the speed of the load, v f represents the speed of the following unmanned vehicle, 2l g represents the width of the guided unmanned vehicle, 2l f Indicates the width of the following unmanned vehicle.

[0085] like Figure 2 As shown, the trajectory tracking control method of the multi-unmanned vehicle cooperative transport system in this embodiment includes:

[0086] S101, for the three subsystems of the leading unmanned vehicle, the load and the following unmanned vehicle in the multi-unmanned vehicle cooperative transport system, unconstrained dynamic models of the three subsystems are established respectively; constraint equations are established for the multi-unmanned vehicle cooperative transport system, the constraint equations are converted into second-order forms by taking the time derivative, and then the constraint equations of the trajectory error in zero-order form and first-order form are introduced, and the total constraint equation of the system in matrix form is established;

[0087] S102, embed the total constraint equation of the system in matrix form into the unconstrained dynamics model to obtain the constraint dynamics model of the entire multi-unmanned vehicle cooperative transport system, and solve the constraint dynamics model to obtain the control torque of the guiding unmanned vehicle and the following unmanned vehicle to achieve trajectory tracking of the multi-unmanned vehicle cooperative transport system.

[0088] In this embodiment, the function expression of the unconstrained dynamics model established for guiding the unmanned vehicle in step S101 is:

[0089]

[0090] In the above formula, q g is the state variable for guiding the unmanned vehicle, t is the time, is the state variable q gThe second-order derivative of is the state variable q g The first derivative of M g (q g ,t) represents the mass / inertia matrix of the guided unmanned vehicle, represents the generalized force matrix guiding the unmanned vehicle, represents the generalized constraint matrix for guiding the unmanned vehicle, the subscript g represents guiding the unmanned vehicle, and:

[0091]

[0092]

[0093]

[0094]

[0095] Among them, m g To guide the quality of the unmanned vehicle, r g is the wheel radius of the guided unmanned vehicle, θ g To guide the azimuth of the unmanned vehicle, I g To guide the center of mass moment of inertia of the unmanned vehicle, l g To guide the half width of the unmanned vehicle, (x g ,y g ) is the center of mass coordinate of the unmanned vehicle on the two-dimensional plane, The x-axis component x of the center of mass coordinate of the unmanned vehicle is g The second-order derivative of The y-axis component y of the center of mass coordinate of the unmanned vehicle is g The second-order derivative of To guide the azimuth angle θ of the unmanned vehicle g The second-order derivative of is the x-axis component of the centroid coordinate x g The first derivative of is the y-axis component of the centroid coordinate g The first derivative of To guide the azimuth angle θ of the unmanned vehicle g The first derivative of grf To guide the driving torque of the right front wheel of the unmanned vehicle, u glf To guide the driving torque of the left front wheel of the unmanned vehicle, the function expression of the unconstrained dynamic model established for the load in step S101 is:

[0096]

[0097] In the above formula, q c is the state variable of the load, t is the time, is the state variable qc The second-order derivative of is the state variable q c The first derivative of M c (q c ,t) represents the mass / inertia matrix of the load, The generalized force matrix representing the load, represents the generalized constraint force matrix of the load, the subscript c represents the load, and has:

[0098]

[0099]

[0100]

[0101]

[0102] Among them, m c is the mass of the load, I c is the moment of inertia of the load’s center of mass, is the x-axis component of the load's centroid coordinate c The second-order derivative of is the y-axis component of the load's center of mass coordinate c The second-order derivative of is the load azimuth angle θ c The second derivative of (x c ,y c ) is the center of mass coordinate of the load on the two-dimensional plane; the function expression of the unconstrained dynamic model established for the following unmanned vehicle in step S101 is:

[0103]

[0104] In the above formula, q f is the state variable of the following unmanned vehicle, t is the time, is the state variable q f The second-order derivative of is the state variable q f The first derivative of M f (q f ,t) represents the mass / inertia matrix of the following unmanned vehicle, represents the generalized force matrix following the unmanned vehicle, represents the generalized constraint matrix of the following unmanned vehicle, the subscript f represents the following unmanned vehicle, and:

[0105]

[0106]

[0107]

[0108]

[0109] Among them, m f To follow the mass of the unmanned vehicle, r f is the wheel radius of the following unmanned vehicle, θ f To follow the azimuth of the unmanned vehicle, I f is the moment of inertia of the center of mass of the unmanned vehicle, l f is half the width of the following unmanned vehicle, (x f ,y f ) is the coordinate of the center of mass of the unmanned vehicle on the two-dimensional plane. is the x-axis component of the center of mass coordinate of the following unmanned vehicle f The second-order derivative of is the y-axis component of the center of mass coordinate of the following unmanned vehicle f The second-order derivative of is the azimuth angle θ of the following unmanned vehicle f The second-order derivative of is the x-axis component of the centroid coordinate x f The first derivative of is the y-axis component of the centroid coordinate f The first derivative of is the azimuth angle θ of the following unmanned vehicle f The first derivative of frf is the driving torque of the right front wheel of the following unmanned vehicle, u flf is the driving torque of the left front wheel of the following unmanned vehicle. In step S101 of this embodiment, for the three subsystems of the leading unmanned vehicle, the load and the following unmanned vehicle in the multi-unmanned vehicle cooperative transport system, when the unconstrained dynamic models of the three subsystems are established respectively: the leading unmanned vehicle is modeled by the Newton-Euler dynamics method, and it is organized into a matrix form, as shown in the above formula (1); the load is modeled by the Lagrangian dynamics method, without considering the potential energy, and it is organized into a matrix form, as shown in the above formula (6); the leading unmanned vehicle is modeled by the Newton-Euler dynamics method, and it is organized into a matrix form, as shown in the above formula (11).

[0110] The constraint equations established for the multi-unmanned vehicle cooperative transport system in step S101 include some or all of the motion constraints, geometric constraints, complete constraints, and non-complete constraints, and more required constraints can be expanded as needed. Specifically, the constraint equations established for the multi-unmanned vehicle cooperative transport system in step S101 of this embodiment include the trajectory constraints of the leading unmanned vehicle, the trajectory constraints of the following unmanned vehicle, the complete constraints between the leading unmanned vehicle and the following unmanned vehicle, and the geometric constraints between the leading unmanned vehicle, the load, and the following unmanned vehicle, wherein the function expression of the trajectory constraints of the leading unmanned vehicle is shown as follows:

[0111] x g =r a cost,y g =r b cost,(16)

[0112] The function expression of following the trajectory constraint of the unmanned vehicle is as follows:

[0113]

[0114] The functional expression of the complete constraint between the leading unmanned vehicle and the following unmanned vehicle is as follows:

[0115] (x g -x f ) 2 +(y g -y f ) 2 =D 2 , (18)

[0116] The function expression of the geometric constraints between the leading unmanned vehicle, the payload and the following unmanned vehicle (the center of mass of the payload is always in the middle of the center of mass of the leading unmanned vehicle and the center of mass of the following unmanned vehicle) is as follows:

[0117] x g +x f -2x c =0,y g +y f -2y c =0, (19)

[0118] Among them, r a is the major semi-axis of the elliptical trajectory, r b is the minor semi-axis of the elliptical trajectory, D is the constant distance that needs to be maintained between the following unmanned vehicle and the leading unmanned vehicle, (x g ,y g ) is the center of mass coordinate of the unmanned vehicle on the two-dimensional plane, (x f ,y f ) is the center of mass coordinate of the following unmanned vehicle on the two-dimensional plane, and t is the time.

[0119] In this embodiment, after the constraint equation is converted into a second-order form in step S101, the second-order form of the trajectory constraint for guiding the unmanned vehicle is obtained as follows:

[0120]

[0121] The obtained second-order form of the constraint of following the unmanned vehicle trajectory is:

[0122]

[0123] The obtained second-order form of the complete constraint between the leading unmanned vehicle and the following unmanned vehicle is:

[0124]

[0125] The second-order form of the geometric constraints between the leading unmanned vehicle, the payload, and the following unmanned vehicle is obtained as:

[0126]

[0127] in, The x-axis component x of the center of mass coordinate of the unmanned vehicle is g The second-order derivative of The y-axis component y of the center of mass coordinate of the unmanned vehicle is g The second-order derivative of is the x-axis component of the centroid coordinate x g The first derivative of is the y-axis component of the centroid coordinate g The first derivative of is the x-axis component of the center of mass coordinate of the following unmanned vehicle f The second-order derivative of is the y-axis component of the center of mass coordinate of the following unmanned vehicle f The second-order derivative of is the x-axis component of the centroid coordinate x f The first derivative of is the y-axis component of the centroid coordinate f The first derivative of a is the major semi-axis of the elliptical trajectory, r b is the minor semi-axis of the ellipse trajectory.

[0128] In this embodiment, the constraint equations for introducing the zero-order form and the first-order form of the trajectory error in step S101 include:

[0129]

[0130] e1=x g -acost, (25)

[0131] e2=y g -bsint, (26)

[0132]

[0133] Among them, α and β are constants greater than 0, e is the zero-order form of the error between the actual trajectory and the ideal trajectory, is the first-order form of the error e between the actual trajectory and the ideal trajectory, is the second-order form of the error e between the actual trajectory and the ideal trajectory, e1 is the x-axis component of the trajectory error of the guided unmanned vehicle, e2 is the y-axis component of the trajectory error of the guided unmanned vehicle, and e3 is the trajectory error of the following unmanned vehicle. (x g ,y g ) is the center of mass coordinate of the unmanned vehicle on the two-dimensional plane, (x f ,y f ) is the coordinate of the center of mass of the following unmanned vehicle on the two-dimensional plane, t is the time, r a is the major semi-axis of the elliptical trajectory, r b is the short semi-axis of the elliptical trajectory. In order to obtain better control effect, an improved control method is introduced in this embodiment, and the zero-order form and the first-order form are introduced to modify the system constraints. Specifically, in this embodiment, the improved control method proposed by Hancheol Cho and Firdaus E.Udwadia is shown in formula (24). This method is simple and accurate and can be well applied to analytical dynamics. Since the initial conditions may not meet the requirements of the ideal trajectory, in order to improve the control effect, the error e between the actual trajectory and the ideal trajectory is introduced here, and e1, e2 and e3 are the trajectory errors of the guiding unmanned vehicle and the following unmanned vehicle, as shown in formulas (24) to (27).

[0134] In this embodiment, the function expression of the system total constraint equation in matrix form established in step S101 is:

[0135]

[0136] In the above formula, q is the total state variable of the system, t is the time, is the second-order derivative of the total state variable q, is the first-order derivative of the total state variable q, represents an m×n-order constraint matrix, is an m-dimensional column vector with:

[0137]

[0138]

[0139]

[0140] Among them, (x g ,y g ) is the center of mass coordinate of the unmanned vehicle on the two-dimensional plane, (x f ,y f ) is the coordinate of the center of mass of the following unmanned vehicle on the two-dimensional plane, t is the time, r a is the major semi-axis of the elliptical trajectory, r b is the minor semi-axis of the ellipse trajectory, The x-axis component x of the center of mass coordinate of the unmanned vehicle is g The second-order derivative of The y-axis component y of the center of mass coordinate of the unmanned vehicle is g The second-order derivative of To guide the azimuth angle θ of the unmanned vehicle g The second-order derivative of is the x-axis component of the centroid coordinate x g The first derivative of is the y-axis component of the centroid coordinate g The first derivative of To guide the azimuth angle θ of the unmanned vehicle g The first derivative of is the x-axis component of the load's centroid coordinate c The second-order derivative of is the y-axis component of the load's center of mass coordinate c The second-order derivative of is the load azimuth angle θ c The second-order derivative of is the x-axis component of the center of mass coordinate of the following unmanned vehicle f The second-order derivative of is the y-axis component of the center of mass coordinate of the following unmanned vehicle f The second-order derivative of is the azimuth angle θ of the following unmanned vehicle f The second-order derivative of is the x-axis component of the centroid coordinate x f The first derivative of is the y-axis component of the centroid coordinate f The first derivative of is the azimuth angle θ of the following unmanned vehicle f The first-order derivative of , α and β are constants greater than 0, e is the zero-order form of the error between the actual trajectory and the ideal trajectory, is the first-order derivative of the error e between the actual trajectory and the ideal trajectory, is the second-order derivative of the error e between the actual trajectory and the ideal trajectory, e1 is the x-axis component of the trajectory error of the guided unmanned vehicle, e2 is the y-axis component of the trajectory error of the guided unmanned vehicle, and e3 is the trajectory error of the following unmanned vehicle. is the first-order derivative of the x-axis component e1 of the trajectory error of the unmanned vehicle, is the first-order derivative of the y-axis component e2 of the trajectory error of the unmanned vehicle, is the first-order derivative of the trajectory error e3 following the unmanned vehicle.

[0141] In this embodiment, the function expression of the constraint dynamics model of the entire multi-unmanned vehicle cooperative transport system obtained in step S102 is:

[0142]

[0143] In the above formula, M represents the mass / inertia matrix of the total system, Q represents the generalized force matrix of the total system, A represents the constraint matrix of m×n order, and b is the m-dimensional column vector. is the second-order derivative of the total state variable q, and:

[0144]

[0145]

[0146] In the above formula, M g To guide the mass / inertia matrix M of the unmanned vehicle g (q g ,t),M c is the mass / inertia matrix M of the load c (q c ,t),M f is the mass / inertia matrix M of the following unmanned vehicle f (q f ,t),Q g The generalized force matrix for guiding the unmanned vehicle Q c is the generalized force matrix of the load Q f is the generalized force matrix of the following unmanned vehicle

[0147] In this embodiment, the control torque of the leading unmanned vehicle and the following unmanned vehicle is obtained by solving the constraint dynamics model in step S102:

[0148]

[0149] In the above formula, u is the control torque, M represents the mass / inertia matrix of the total system, Q represents the generalized force matrix of the total system, A represents the constraint matrix of m×n order, and b is the m-dimensional column vector; and:

[0150] u=[u grf ,u glf ,u frf ,u flf ] Τ

[0151] In the above formula, u grf To guide the driving torque of the right front wheel of the unmanned vehicle, u glf To guide the driving torque of the left front wheel of the unmanned vehicle, u frf is the driving torque of the right front wheel of the following unmanned vehicle, u flfIt is the driving torque of the left front wheel of the following unmanned vehicle. On the basis of embedding the modified constraint equation into the unconstrained dynamics model of the entire cooperative transport system to establish the constrained dynamics model of the entire system, the control torque of the unmanned vehicle can be solved by using the Udwadia-Kalaba theory. The control torque of the unmanned vehicle can be solved by using the Udwadia-Kalaba theory. For details, please refer to the reference: Kalaba, Robert, and Firdaus Udwadia. "Analytical dynamics with constraint forces that do work in virtual displacements." Applied mathematics and computation 121.2-3 (2001): 211-217. This embodiment only involves the application of the above method, not the improvement of the above method, so its implementation details are not described in detail here.

[0152] In order to verify the trajectory tracking control method of the multi-unmanned vehicle cooperative transport system of this embodiment, the following is a comparison between the results obtained by using the trajectory tracking control method of the multi-unmanned vehicle cooperative transport system of this embodiment and the ideal results.

[0153] Figure 3 In order to guide the unmanned vehicle, the actual trajectory and the ideal trajectory (ellipse) obtained by the trajectory tracking control method of the multi-unmanned vehicle cooperative transport system of this embodiment are compared, where the solid line represents the actual trajectory obtained by the guiding unmanned vehicle using the trajectory tracking control method of the multi-unmanned vehicle cooperative transport system of this embodiment, and the dotted line represents the ideal trajectory of the guiding unmanned vehicle. Figure 4 The figure compares the actual trajectory and the ideal trajectory (ellipse) of the load obtained by the trajectory tracking control method of the multi-unmanned vehicle cooperative transport system of this embodiment, wherein the solid line represents the actual trajectory of the load obtained by the trajectory tracking control method of the multi-unmanned vehicle cooperative transport system of this embodiment, and the dotted line represents the ideal trajectory of the load. Figure 5 The actual trajectory and the ideal trajectory (ellipse) obtained by following the unmanned vehicle using the trajectory tracking control method of the multi-unmanned vehicle cooperative transport system of this embodiment are compared. The solid line represents the actual trajectory obtained by following the unmanned vehicle using the trajectory tracking control method of the multi-unmanned vehicle cooperative transport system of this embodiment, and the dotted line represents the ideal trajectory of the following unmanned vehicle. Figure 3 to Figure 5 It can be seen that after adopting the trajectory tracking control method of the multi-unmanned vehicle cooperative transport system of this embodiment, the actual trajectories of the guiding unmanned vehicle, the load, and the following unmanned vehicles can all be made close to the ideal trajectory.

[0154] Figure 6 The error σ between the ideal trajectory and the actual trajectory of the unmanned vehicle is g , the initial error is set to 10mm; Figure 7 is the error σ between the ideal trajectory and the actual trajectory of the unmanned vehicle f , the initial error is set to 10mm. See 6 and Figure 7 It can be seen that at the beginning, the error between the ideal trajectory and the actual trajectory of the guiding unmanned vehicle and the following unmanned vehicle is large, but as time goes by, the error between their ideal trajectory and the actual trajectory gradually approaches 0.

[0155] Figure 8 To guide the right front wheel driving torque of the unmanned vehicle and the following unmanned vehicle, the solid line represents the right front wheel driving torque of the guiding unmanned vehicle, and the dotted line represents the right front wheel driving torque of the following unmanned vehicle. Fig. 9 The right front wheel driving torque of the leading unmanned vehicle and the following unmanned vehicle, the solid line represents the right front wheel driving torque of the leading unmanned vehicle, and the dotted line represents the right front wheel driving torque of the following unmanned vehicle. Figure 8 and Fig. 9 It can be seen that the driving torque of the trajectory tracking control method for the multi-unmanned vehicle cooperative transport system using the present embodiment is periodic between -35Nm and 35Nm, and except for slight fluctuations at the turning points, the overall performance is smooth. It can be seen that the driving torque of the trajectory tracking control method for the multi-unmanned vehicle cooperative transport system using the present embodiment meets actual expectations.

[0156] In summary, this embodiment records a trajectory tracking control design method for a collaborative transport system with two unmanned vehicles, in which the unmanned vehicle is front-wheel drive and the motion trajectory is an elliptical trajectory, including: establishing an unconstrained dynamic model of the three subsystems of the guiding unmanned vehicle, the load and the following unmanned vehicle; designing the constraint equations of the entire collaborative transport system, including but not limited to motion constraints, geometric constraints, complete constraints and incomplete constraints, and converting the constraint equations into second-order form; in order to obtain better control effects, an improved control method is introduced, and the zero-order form and the first-order form are introduced to modify the system constraints; the modified constraint equations are embedded into the unconstrained dynamic model of the entire collaborative transport system, and a constrained dynamic model of the entire system is established, and the control torque of the unmanned vehicle is obtained by using the Udwadia-Kalaba theory. The method of this embodiment has the advantages of being simple, efficient and having good control effects. Under the condition of an initial error of 10mm, the error accuracy between the actual trajectory and the ideal trajectory can be controlled within (-1*10 2 ,1*10 2 )Inside.

[0157] In addition, this embodiment also provides a trajectory tracking control system for a multi-unmanned vehicle cooperative transport system, including a microprocessor and a memory connected to each other, characterized in that the microprocessor is programmed or configured to execute the aforementioned multi-unmanned vehicle cooperative transport system trajectory tracking control method.

[0158] In addition, this embodiment also provides a computer-readable storage medium, in which a computer program is stored, characterized in that the computer program is used to be programmed or configured by a microprocessor to execute the aforementioned multi-unmanned vehicle collaborative transport system trajectory tracking control method.

[0159] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present application may take the form of a computer program product implemented on one or more computer-readable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program codes. The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, may be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the functions in the process. Figure 1 A process or multiple processes and / or boxes Figure 1 These computer program instructions can also be stored in a computer-readable memory that can guide a computer or other programmable data processing device to work in a specific way, so that the instructions stored in the computer-readable memory produce a product including an instruction device, which implements the functions specified in the process. Figure 1 A process or multiple processes and / or boxes Figure 1 These computer program instructions can also be loaded onto a computer or other programmable data processing device, so that a series of operation steps are executed on the computer or other programmable device to produce a computer-implemented process, so that the instructions executed on the computer or other programmable device provide for implementing the process in the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0160] The above is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should also be regarded as the protection scope of the present invention.

Claims

1. A trajectory tracking control method for a multi-unmanned vehicle cooperative transport system, characterized in that: include: S101, for the three subsystems of the leading unmanned vehicle, the load and the following unmanned vehicle in the multi-unmanned vehicle cooperative transport system, unconstrained dynamic models of the three subsystems are established respectively; constraint equations are established for the multi-unmanned vehicle cooperative transport system, the constraint equations are converted into second-order forms by taking the time derivative, and then the constraint equations of the trajectory error in zero-order form and first-order form are introduced, and the total constraint equation of the system in matrix form is established; S102, embedding the total constraint equation of the system in matrix form into the unconstrained dynamics model to obtain the constraint dynamics model of the entire multi-unmanned vehicle cooperative transport system, and solving the constraint dynamics model to obtain the control torque of the leading unmanned vehicle and the following unmanned vehicle to achieve trajectory tracking of the multi-unmanned vehicle cooperative transport system; The function expression of the unconstrained dynamic model established for guiding the unmanned vehicle in step S101 is: ，（1） In the above formula, q g To guide the state variables of the unmanned vehicle, t For time, is a state variable q g The second-order derivative of is a state variable q g The first derivative of represents the mass / inertia matrix guiding the unmanned vehicle, represents the generalized force matrix guiding the unmanned vehicle, represents the generalized constraint matrix for guiding the unmanned vehicle, the subscript g represents guiding the unmanned vehicle, and: ，（2） ，（3） ，（4） ，（5） in, m g To guide the quality of unmanned vehicles, r g To guide the wheel radius of the unmanned vehicle, θ g To guide the azimuth of the unmanned vehicle, I g To guide the center of mass moment of inertia of the unmanned vehicle, l g To guide the unmanned vehicle with half the width, ( x g , y g ) is the center of mass coordinate of the unmanned vehicle on the two-dimensional plane, To guide the center of mass coordinates of the unmanned vehicle x Axis Component x g The second-order derivative of To guide the center of mass coordinates of the unmanned vehicle y Axis Component y g The second-order derivative of To guide the azimuth of the unmanned vehicle θ g The second-order derivative of is the centroid coordinate x Axis Component x g The first derivative of is the centroid coordinate y Axis Component y g The first derivative of To guide the azimuth of the unmanned vehicle θ g The first derivative of To guide the driving torque of the right front wheel of the unmanned vehicle, To guide the driving torque of the left front wheel of the unmanned vehicle, the function expression of the unconstrained dynamic model established for the load in step S101 is: ，（6） In the above formula, q c is the state variable of the load, t For time, is a state variable q c The second-order derivative of is a state variable q c The first derivative of represents the mass / inertia matrix of the load, The generalized force matrix representing the load, represents the generalized constraint force matrix of the load, the subscript c represents the load, and there is: ，（7） ，（8） ，（9） ，（10） in, m c is the mass of the load, is the moment of inertia of the load’s center of mass, is the mass center coordinate of the load x Axis Component x c The second-order derivative of is the mass center coordinate of the load y Axis Component y c The second-order derivative of is the load azimuth θ c The second derivative of x c , y c ) is the center of mass coordinate of the load on the two-dimensional plane; the function expression of the unconstrained dynamic model established for the following unmanned vehicle in step S101 is: ，（11） In the above formula, q f is the state variable following the unmanned vehicle, t For time, is a state variable q f The second-order derivative of is a state variable q f The first derivative of represents the mass / inertia matrix of the following unmanned vehicle, represents the generalized force matrix following the unmanned vehicle, represents the generalized constraint matrix following the unmanned vehicle, subscript f Indicates following the unmanned vehicle, and has: ，（12） ，（13） ，（14） ，（15） in, m f To follow the quality of the unmanned vehicle, r f To follow the wheel radius of the unmanned vehicle, θ f To follow the azimuth of the unmanned vehicle, I f To follow the center of mass moment of inertia of the unmanned vehicle, l f is half the width of the following unmanned vehicle, ( x f , y f ) is the coordinate of the center of mass of the unmanned vehicle on the two-dimensional plane. The center of mass coordinates of the following unmanned vehicle x Axis Component x f The second-order derivative of The center of mass coordinates of the following unmanned vehicle y Axis Component y f The second-order derivative of The azimuth of the following unmanned vehicle θ f The second-order derivative of is the centroid coordinate x Axis Component x f The first derivative of is the centroid coordinate y Axis Component y f The first derivative of The azimuth of the following unmanned vehicle θ f The first derivative of To follow the driving torque of the right front wheel of the unmanned vehicle, is the driving torque of the left front wheel of the following unmanned vehicle.

2. The trajectory tracking control method of a multi-unmanned vehicle cooperative transport system according to claim 1, characterized in that: The constraint equations established for the multi-unmanned vehicle cooperative transport system in step S101 include the trajectory constraint of the leading unmanned vehicle, the trajectory constraint of the following unmanned vehicle, the integrity constraint between the leading unmanned vehicle and the following unmanned vehicle, and the geometric constraint between the leading unmanned vehicle, the load and the following unmanned vehicle. The function expression of the trajectory constraint of the leading unmanned vehicle is shown as follows: ， ，（16） The function expression of following the trajectory constraint of the unmanned vehicle is as follows: ，（17） The functional expression of the complete constraint between the leading unmanned vehicle and the following unmanned vehicle is as follows: ，（18） The functional expression of the geometric constraints between the leading unmanned vehicle, the payload, and the following unmanned vehicle is as follows: ， ，（19） in, r a is the major semi-axis of the ellipse trajectory, r b is the minor semi-axis of the ellipse trajectory, D To maintain a constant distance between the following unmanned vehicle and the leading unmanned vehicle, ( x g , y g ) is the center of mass coordinate of the unmanned vehicle on the two-dimensional plane, ( x f , y f ) is the coordinate of the center of mass of the unmanned vehicle on the two-dimensional plane. t For time.

3. The trajectory tracking control method of a multi-unmanned vehicle cooperative transport system according to claim 2 is characterized in that: After the constraint equation is converted into a second-order form in step S101, the second-order form of the trajectory constraint for guiding the unmanned vehicle is obtained as follows: ， ，（10） The obtained second-order form of the constraint of following the unmanned vehicle trajectory is: ，（21） The obtained second-order form of the complete constraint between the leading unmanned vehicle and the following unmanned vehicle is: ，（22） The second-order form of the geometric constraints between the leading unmanned vehicle, the payload, and the following unmanned vehicle is obtained as: ， ，（23） in, To guide the center of mass coordinates of the unmanned vehicle x Axis Component x g The second-order derivative of To guide the center of mass coordinates of the unmanned vehicle y Axis Component y g The second-order derivative of is the centroid coordinate x Axis Component x g The first derivative of is the centroid coordinate y Axis Component y g The first derivative of The center of mass coordinates of the following unmanned vehicle x Axis Component x f The second-order derivative of The center of mass coordinates of the following unmanned vehicle y Axis Component y f The second-order derivative of is the centroid coordinate x Axis Component x f The first derivative of is the centroid coordinate y Axis Component y f The first derivative of r a is the major semi-axis of the ellipse trajectory, r b is the minor semi-axis of the ellipse trajectory.

4. The trajectory tracking control method of a multi-unmanned vehicle cooperative transport system according to claim 3 is characterized in that: The constraint equations for introducing the trajectory error in zero-order form and first-order form in step S101 include: ，（24） ，（25） ，（26） ，（27） in, α , β is a constant greater than 0, e is the zero-order form of the error between the actual trajectory and the ideal trajectory, is the error between the actual trajectory and the ideal trajectory e The first-order form of is the error between the actual trajectory and the ideal trajectory e The second-order form of e 1 is the trajectory error of the guided unmanned vehicle x Axis component, e 2 is the trajectory error of the guided unmanned vehicle y Axis component, e 3 is the trajectory error of the unmanned vehicle, ( x g , y g ) is the center of mass coordinate of the unmanned vehicle on the two-dimensional plane, ( x f , y f ) is the coordinate of the center of mass of the unmanned vehicle on the two-dimensional plane. t For time.

5. The trajectory tracking control method of a multi-unmanned vehicle cooperative transport system according to claim 4 is characterized in that: The function expression of the system total constraint equation in matrix form established in step S101 is: ，（28） In the above formula, q is the total state variable of the system, t For time, is the total state variable q The second-order derivative of is the total state variable q The first derivative of represents an m×n-order constraint matrix, is an m-dimensional column vector with: ，（29） ，（30） ，（31） in,( x g , y g ) is the center of mass coordinate of the unmanned vehicle on the two-dimensional plane, ( x f , y f ) is the coordinate of the center of mass of the unmanned vehicle on the two-dimensional plane. t For time, r a is the major semi-axis of the ellipse trajectory, r b is the minor semi-axis of the ellipse trajectory, To guide the center of mass coordinates of the unmanned vehicle x Axis Component x g The second-order derivative of To guide the center of mass coordinates of the unmanned vehicle y Axis Component y g The second-order derivative of To guide the azimuth of the unmanned vehicle θ g The second-order derivative of is the centroid coordinate x Axis Component x g The first derivative of is the centroid coordinate y Axis Component y g The first derivative of To guide the azimuth of the unmanned vehicle θ g The first derivative of is the mass center coordinate of the load x Axis Component x c The second-order derivative of is the mass center coordinate of the load y Axis Component y c The second-order derivative of is the load azimuth θ c The second-order derivative of The center of mass coordinates of the following unmanned vehicle x Axis Component x f The second-order derivative of The center of mass coordinates of the following unmanned vehicle y Axis Component y f The second-order derivative of The azimuth of the following unmanned vehicle θ f The second-order derivative of is the centroid coordinate x Axis Component x f The first derivative of is the centroid coordinate y Axis Component y f The first derivative of The azimuth of the following unmanned vehicle θ f The first derivative of α , β is a constant greater than 0, e is the zero-order form of the error between the actual trajectory and the ideal trajectory, is the error between the actual trajectory and the ideal trajectory e The first derivative of is the error between the actual trajectory and the ideal trajectory e The second-order derivative of e 1 is the trajectory error of the guided unmanned vehicle x Axis component, e 2 is the trajectory error of the guided unmanned vehicle y Axis component, e 3 is the trajectory error of the unmanned vehicle. To guide the trajectory error of the unmanned vehicle x Axis Component e The first derivative of 1, To guide the trajectory error of the unmanned vehicle y Axis Component e The first derivative of 2, The trajectory error of the unmanned vehicle e The first derivative of 3.

6. The trajectory tracking control method of a multi-unmanned vehicle cooperative transport system according to claim 5 is characterized in that: The function expression of the constraint dynamics model of the whole system of the multi-unmanned vehicle cooperative transport system obtained in step S102 is: ，（32） In the above formula ，M represents the mass / inertia matrix of the total system, Q represents the generalized force matrix of the total system, A represents an m×n-order constraint matrix, is an m-dimensional column vector, is the total state variable q The second-order derivative of , and: ，（33） ，（34） In the above formula, M g The mass / inertia matrix for guiding the unmanned vehicle , M c is the mass / inertia matrix of the load , M f The mass / inertia matrix of the following unmanned vehicle , Q g Generalized force matrix for guiding the unmanned vehicle , Q c is the generalized force matrix of the load , Q f is the generalized force matrix of the following unmanned vehicle .

7. The trajectory tracking control method of a multi-unmanned vehicle cooperative transport system according to claim 6 is characterized in that: In step S102, the constraint dynamics model is solved to obtain the control torque of the leading unmanned vehicle and the following unmanned vehicle: In the above formula, To control the torque, M represents the mass / inertia matrix of the total system, Q represents the generalized force matrix of the total system, A represents an m×n-order constraint matrix, is an m-dimensional column vector; and: In the above formula, To guide the driving torque of the right front wheel of the unmanned vehicle, To guide the driving torque of the left front wheel of the unmanned vehicle, To follow the driving torque of the right front wheel of the unmanned vehicle, is the driving torque of the left front wheel of the following unmanned vehicle.

8. A trajectory tracking control system for a multi-unmanned vehicle cooperative transport system, comprising an interconnected microprocessor and a memory, characterized in that: The microprocessor is programmed or configured to execute the trajectory tracking control method for a multi-unmanned vehicle cooperative transport system as described in any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program stored therein, characterized in that: The computer program is used to be programmed or configured by a microprocessor to execute the trajectory tracking control method for a multi-unmanned vehicle cooperative transport system as described in any one of claims 1 to 7.

Images