Robust control method for trajectory tracking of multiple unmanned vehicles based on nested dynamics modeling

Abstract

The present disclosure relates to a multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling, applied to a controller used for controlling a multi-unmanned vehicle system, the multi-unmanned vehicle system including a plurality of target vehicles to be controlled, the method comprising: obtaining vehicle information of each target vehicle; performing trajectory constraint processing on the vehicle information of each target vehicle based on a multi-level nested dynamics model, to obtain a corresponding control torque of each target vehicle, wherein the multi-level nested dynamics model is obtained by embedding kinematic constraint coupling between dynamics models corresponding to each control unit, and each control unit corresponds to at least one target vehicle; and outputting the corresponding control torque of each target vehicle to each target vehicle, so as to perform trajectory tracking control of each target vehicle through the corresponding control torque of each target vehicle. The embodiment of the present disclosure can reduce the control complexity of the controller for the multi-unmanned vehicle system and improve the flexibility and accuracy of the control.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of multi-body dynamics modeling and trajectory tracking control, and particularly relates to a multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling. BACKGROUND

[0002] A multi-body system refers to a system composed of multiple bodies connected to each other and having obvious relative motion between each other, such as engineering machinery, robots, spacecraft, vehicles, etc., which can be modeled as a multi-body system.

[0003] In related technologies, a multi-body system often adopts a mechanical system dynamics modeling method, such as the Newton-Euler method, the Lagrange method, etc., but this kind of modeling method has difficulty in operation or cannot obtain an analytical form of dynamics equation when calculating the dynamics model of a complex multi-body mechanical system, especially a multi-body system with a closed loop structure.

[0004] Therefore, the dynamics modeling method adopted in related technologies cannot be well applied to controller design, that is, the current controller has poor complexity, flexibility and accuracy for controlling a multi-body system. SUMMARY

[0005] Therefore, it is necessary to provide a multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling, which can improve the complexity, flexibility and accuracy of the controller for controlling a multi-body system.

[0006] In a first aspect, the present application provides a multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling, applied to a controller, wherein the controller is used to control a multi-unmanned vehicle system, the multi-unmanned vehicle system includes a plurality of target vehicles to be controlled, and the method comprises the following steps.

[0007] Obtaining vehicle information of each target vehicle;

[0008] Performing trajectory constraint processing on the vehicle information of each target vehicle based on a multi-level nested dynamics model, to obtain a control torque corresponding to each target vehicle, wherein the multi-level nested dynamics model is obtained by embedding a kinematic constraint coupling between dynamics models corresponding to each control unit, and each control unit corresponds to at least one target vehicle;

[0009] Outputting the control torque corresponding to each target vehicle to each target vehicle, to perform trajectory tracking control of each target vehicle through the control torque corresponding to each target vehicle.

[0010] In one of the embodiments, for an N-level nested dynamics model in the multi-level nested dynamics model, the N-level nested dynamics model corresponds to a first control unit and a second control unit, the first control unit corresponds to a dynamics model, and the second control unit corresponds to a dynamics model, and the N-level nested dynamics model is coupled by nesting a first kinematics constraint between the dynamics model corresponding to the first control unit and the dynamics model corresponding to the second control unit, the dynamics model corresponding to the first control unit is an N-1-level nested dynamics model, the second control unit includes at least one target vehicle, and the dynamics model of each target vehicle in the at least one target vehicle is coupled by nesting a second kinematics constraint to obtain the dynamics model corresponding to the second control unit, where N is an integer greater than 0 and less than or equal to K, and K is the total number of levels of the multi-level nested dynamics model.

[0011] In one of the embodiments, the multi-level nested dynamics model includes a generalized constraint force matrix of the multi-unmanned vehicle system, and the generalized constraint force matrix is a matrix equation established based on the Udwadia-Kalaba theory.

[0012] The trajectory constraint processing of the vehicle information of each target vehicle based on the multi-level nested dynamics model respectively obtains the control torque corresponding to each target vehicle, including:

[0013] Based on the generalized constraint force matrix of the multi-unmanned vehicle system and the vehicle information of each target vehicle, the control input of the multi-unmanned vehicle system is obtained.

[0014] Based on the control input and the conversion matrix of the generalized constraint force of each target vehicle, the control torque corresponding to each target vehicle is obtained.

[0015] In one of the embodiments, based on the generalized constraint force matrix of the multi-unmanned vehicle system and the vehicle information of each target vehicle, the control input of the multi-unmanned vehicle system is obtained, including:

[0016] For any target vehicle, the mass inertia matrix and the generalized force matrix of the target vehicle are determined according to the vehicle information of the target vehicle.

[0017] The mass inertia matrix of the multi-unmanned vehicle system is determined according to the mass inertia matrix of each target vehicle, and the generalized unconstrained force matrix of the multi-unmanned vehicle system is obtained according to the generalized force matrix of each target vehicle.

[0018] The kinematics constraint of the multi-unmanned vehicle system is obtained according to the vehicle information of each target vehicle.

[0019] The generalized unconstrained force matrix of the multi-unmanned vehicle system, the mass inertia matrix of the multi-unmanned vehicle system and the kinematic constraint of the multi-unmanned vehicle system are input into the generalized constrained force matrix of the multi-unmanned vehicle system for trajectory constraint, so as to obtain the control input of the multi-unmanned vehicle system.

[0020] In one of the embodiments, according to the vehicle information of the target vehicle, the mass inertia matrix and the generalized force matrix of the target vehicle are determined, including:

[0021] According to the mass of the target vehicle, the mass moment of inertia of the center of mass of the target vehicle and the vehicle width of the target vehicle, the mass inertia matrix of the target vehicle is determined;

[0022] According to the mass of the target vehicle and the two-dimensional plane position information of the target vehicle, the generalized unconstrained force matrix of the target vehicle is determined;

[0023] According to the two-dimensional plane position information of the target vehicle, the kinematic constraint of the target vehicle is determined;

[0024] According to the generalized unconstrained force matrix of the target vehicle, the mass inertia matrix of the target vehicle and the kinematic constraint of the target vehicle, the generalized force matrix of the target vehicle is obtained.

[0025] In one of the embodiments, the kinematic constraint includes a first constraint variable and a second constraint variable, and the method further includes:

[0026] Based on the two-dimensional plane position information of the target vehicle, a first correction value for the first constraint variable and a second correction value for the second constraint variable are determined;

[0027] Based on the first correction value, the first constraint variable is corrected to obtain the first constraint variable after correction;

[0028] Based on the second correction value, the second constraint variable is corrected to obtain the second constraint variable after correction.

[0029] In one of the embodiments, the kinematic constraint includes a constraint matrix, a constraint acceleration, a first constraint variable and a second constraint variable, and for the N-level nested dynamic model,

[0030] The constraint matrix is a matrix constructed based on the trajectory constraint, using the two-dimensional plane position information of each target vehicle included in the first control unit and the two-dimensional plane information of each target vehicle included in the second control unit;

[0031] The constraint acceleration is obtained based on the constraint acceleration nested in the dynamic model corresponding to the first control unit and the constraint acceleration nested in the dynamic model corresponding to the second control unit.

[0032] The first constraint variable is a one-dimensional column vector obtained based on the total number of the target vehicles included in the first control unit and the target vehicles included in the second control unit.

[0033] The second constraint variable is a one-dimensional column vector obtained based on the two-dimensional plane position information of the target vehicles included in the first control unit and the two-dimensional plane position information of the target vehicles included in the second control unit.

[0034] In a second aspect, the present application provides a multi-unmanned vehicle trajectory tracking robust control device based on nested dynamic modeling, applied to a controller, the controller being used to control a multi-unmanned vehicle system, the multi-unmanned vehicle system including a plurality of target vehicles to be controlled, and the device including:

[0035] An acquisition module is configured to acquire vehicle information of each target vehicle.

[0036] A constraint module is configured to perform trajectory constraint processing on the vehicle information of each target vehicle based on a multi-level nested dynamic model, to obtain a control torque corresponding to each target vehicle, wherein the multi-level nested dynamic model is obtained by embedding a kinematic constraint between dynamic models corresponding to each control unit, and each control unit corresponds to at least one target vehicle.

[0037] An output module is configured to output the control torque corresponding to each target vehicle to each target vehicle, respectively, to perform trajectory tracking control of each target vehicle through the control torque corresponding to each target vehicle.

[0038] In one embodiment, for an N-level nested dynamic model in the multi-level nested dynamic model, the N-level nested dynamic model corresponds to a first control unit and a second control unit, the dynamic model corresponding to the first control unit and the dynamic model corresponding to the second control unit are coupled to obtain the N-level nested dynamic model by nesting a first kinematic constraint, the dynamic model corresponding to the first control unit is an N-1-level nested dynamic model, the second control unit includes at least one target vehicle, and the dynamic models of each target vehicle in the at least one target vehicle are coupled to obtain the dynamic model corresponding to the second control unit by nesting a second kinematic constraint, wherein N is an integer greater than 0 and less than or equal to K, and K is the total number of levels of the multi-level nested dynamic model.

[0039] In one of the embodiments, the multi-level nested dynamics model comprises a generalized constraint matrix of the multi-unmanned vehicle system, which is a matrix equation established based on the Udwadia-Kalaba theory;

[0040] The constraint module is further configured to:

[0041] obtain the control input of the multi-unmanned vehicle system based on the generalized constraint matrix of the multi-unmanned vehicle system and the vehicle information of each target vehicle;

[0042] obtain the control force corresponding to each target vehicle based on the control input and the conversion matrix of the generalized constraint force of each target vehicle.

[0043] In one of the embodiments, the constraint module is further configured to:

[0044] For any target vehicle, determine the mass inertia matrix and the generalized force matrix of the target vehicle according to the vehicle information of the target vehicle;

[0045] determine the mass inertia matrix of the multi-unmanned vehicle system according to the mass inertia matrix of each target vehicle, and obtain the generalized unconstrained force matrix of the multi-unmanned vehicle system according to the generalized force matrix of each target vehicle;

[0046] obtain the kinematic constraint of the multi-unmanned vehicle system according to the vehicle information of each target vehicle;

[0047] input the generalized unconstrained force matrix of the multi-unmanned vehicle system, the mass inertia matrix of the multi-unmanned vehicle system, and the kinematic constraint of the multi-unmanned vehicle system into the generalized constraint matrix of the multi-unmanned vehicle system for trajectory constraint to obtain the control input of the multi-unmanned vehicle system.

[0048] In one of the embodiments, the constraint module is further configured to:

[0049] determine the mass inertia matrix of the target vehicle according to the mass of the target vehicle, the mass center rotational inertia of the target vehicle, and the vehicle width of the target vehicle;

[0050] determine the generalized unconstrained force matrix of the target vehicle according to the mass of the target vehicle and the two-dimensional planar position information of the target vehicle;

[0051] determine the kinematic constraint of the target vehicle according to the two-dimensional planar position information of the target vehicle;

[0052] obtain the generalized force matrix of the target vehicle according to the generalized unconstrained force matrix of the target vehicle, the mass inertia matrix of the target vehicle, and the kinematic constraint of the target vehicle.

[0053] In one of the embodiments, the kinematic constraint includes a first constraint variable and a second constraint variable, and the device further includes:

[0054] a determining module configured to determine a first correction value for the first constraint variable and a second correction value for the second constraint variable based on the two-dimensional planar position information of the target vehicle;

[0055] a first correction module configured to correct the first constraint variable based on the first correction value to obtain a corrected first constraint variable;

[0056] a second correction module configured to correct the second constraint variable based on the second correction value to obtain a corrected second constraint variable.

[0057] In one of the embodiments, the kinematic constraint includes a constraint matrix, a constraint acceleration, a first constraint variable and a second constraint variable, and for the N-level nested kinematic model,

[0058] the constraint matrix is a matrix constructed based on trajectory constraints and the two-dimensional planar position information of each target vehicle included in the first control unit and the two-dimensional planar information of each target vehicle included in the second control unit;

[0059] the constraint acceleration is an acceleration constructed based on constraint accelerations of the kinematic models nested by the first control unit and constraint accelerations of the kinematic models nested by the second control unit;

[0060] the first constraint variable is a one-dimensional column vector obtained based on the total number of the target vehicles included in the first control unit and the target vehicles included in the second control unit;

[0061] the second constraint variable is a one-dimensional column vector constructed based on the two-dimensional planar position information of the target vehicles included in the first control unit and the two-dimensional planar position information of the target vehicles included in the second control unit.

[0062] In a third aspect, the present application further provides a computer device. The computer device includes a memory and a processor, the memory stores a computer program, and the processor executes the computer program to implement the steps of the method according to any one of the preceding aspects.

[0063] In a fourth aspect, the present application also provides a computer readable storage medium. The computer readable storage medium has stored thereon a computer program, and the computer program, when executed by a processor, implements the steps of the method according to any one of the preceding aspects.

[0064] In a fifth aspect, the present application also provides a computer program product. The computer program product comprises a computer program, and the computer program, when executed by a processor, implements the steps of the method according to any one of the preceding aspects.

[0065] The above-mentioned multi-unmanned vehicle trajectory tracking robust control method, device, computer device, storage medium and computer program product based on nested dynamics modeling, based on the multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling provided by the embodiments of the present application, the controller can obtain the vehicle information of each target vehicle, and based on the multi-level nested dynamics model, the vehicle information of each target vehicle is subjected to trajectory constraint processing to obtain the control torque corresponding to each target vehicle, respectively. The multi-level nested dynamics model is obtained by embedding the kinematic constraint coupling between the dynamics models corresponding to each control unit, and each control unit corresponds to at least one target vehicle. And respectively output the control torque corresponding to each target vehicle to each target vehicle, so as to control the trajectory tracking of each target vehicle through the control torque corresponding to each target vehicle. The multi-unmanned vehicle trajectory tracking robust control method, device, computer device, storage medium and computer program product based on nested dynamics modeling provided by the embodiments of the present application, the trajectory constraint is performed by using the multi-level nested dynamics model. Since the modeling process of the multi-level nested dynamics model is simple, clear and has strong self-adaptation ability, the control complexity of the controller for the multi-body system is reduced, and the flexibility and accuracy of the control are improved. BRIEF DESCRIPTION OF DRAWINGS

[0066] Figure 1 A flowchart of a multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0067] Figure 2a A flowchart of a multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0068] Figure 2b A flowchart of a multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0069] Figure 3 A flowchart of a multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0070] Figure 4Flowchart of a multi-vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0071] Figure 5 Flowchart of a multi-vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0072] Figure 6 Flowchart of a multi-vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0073] Figure 7 Flowchart of a multi-vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0074] Figure 8 Flowchart of a multi-vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0075] Figure 9 Flowchart of a multi-vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0076] Figure 10 Flowchart of a multi-vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0077] Figure 11 Flowchart of a multi-vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0078] Figure 12 Flowchart of a multi-vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0079] Figure 13 Flowchart of a multi-vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0080] Figure 14 Flowchart of a multi-vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0081] Figure 15 Flowchart of a multi-vehicle trajectory tracking robust control method based on nested dynamics modeling in an embodiment;

[0082] Figure 16 Structural block diagram of a multi-vehicle trajectory tracking robust control device based on nested dynamics modeling in an embodiment;

[0083] Figure 17 Internal structure diagram of a computer device in an embodiment. DETAILED DESCRIPTION

[0084] In order to make the purposes, technical solutions and advantages of the present application clearer, the present application is further described in detail below with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and not used to limit the present application.

[0085] A multi-body system refers to a system composed of multiple bodies connected to each other and having obvious relative motion between each other, such as engineering machinery, robots, spacecraft, vehicles, etc. can be modeled as a multi-body system. Among numerous dynamic modeling methods for complex multi-body systems, there are few methods for analytical modeling of multi-body systems. Analytical models of mechanical systems are very important for system control design, and the research on analytical modeling of complex multi-body mechanical systems is of great significance. However, the current existing dynamic modeling methods of mechanical systems, such as Newton-Euler method, Lagrange method, etc., are difficult to operate or cannot obtain analytical dynamic equations when obtaining the dynamic model of a complex multi-body mechanical system, especially a multi-body system with closed-loop structure, so that the model obtained by such dynamic modeling method cannot be well applied to controller design.

[0086] To solve the above technical problems, the embodiment of the present disclosure provides a multi-robot vehicle trajectory tracking robust control method based on nested dynamic modeling. For a multi-robot vehicle system, based on the Udwadia-Kalaba theory of constraint mechanical system modeling, using its hierarchical attribute, a new nested analytical modeling method is used for dynamic modeling, and it is used for modular multi-robot vehicle trajectory tracking robust controller design, which can simplify the control operation of the controller for the multi-robot vehicle system and improve the flexibility and accuracy of the controller for the multi-body system control.

[0087] In one embodiment, as shown in Figure 1 A multi-robot vehicle trajectory tracking robust control method based on nested dynamic modeling is provided, which is applied to a controller for controlling a multi-robot vehicle system, the multi-robot vehicle system including a plurality of target vehicles to be controlled. The controller can be implemented through a terminal, and it can also be implemented through a server or a system including a terminal and a server. The method includes the following steps:

[0088] In step 102, vehicle information of each target vehicle is obtained.

[0089] In the embodiments of the present disclosure, the vehicle information of the target vehicle can include current two-dimensional plane position information (for example, two-dimensional plane coordinates (x, y), azimuth angle θ, etc.), mass, wheel radius, mass center moment of inertia, body width of the target vehicle, and the like. The embodiments of the present disclosure can obtain the corresponding vehicle information from a database for recording data information of multiple unmanned systems, or can obtain the vehicle information of the target vehicle by interacting with the vehicle terminal of each target vehicle. The embodiments of the present disclosure do not make specific limitations on the way of obtaining the vehicle information of the target vehicle, and any way of obtaining the vehicle information is applicable to the embodiments of the present disclosure.

[0090] In step 104, the vehicle information of each target vehicle is subjected to trajectory constraint processing based on a multi-level nested dynamics model, and the control torque corresponding to each target vehicle is obtained, wherein the multi-level nested dynamics model is obtained by embedding the kinematic constraint coupling between the dynamics models corresponding to each control unit, and each control unit corresponds to at least one target vehicle.

[0091] In step 106, the control torque corresponding to each target vehicle is output to each target vehicle respectively, so as to perform trajectory tracking control of each target vehicle through the control torque corresponding to each target vehicle.

[0092] In the embodiments of the present disclosure, the idea of nested constraint modeling is adopted to model the multi-level nested dynamics model, which simplifies the dynamics modeling process of the complex multi-unmanned vehicle system. The multi-unmanned vehicle system can be divided into control units, and each control unit can correspond to at least one target vehicle. The multi-level nested dynamics model takes the multiple target vehicles corresponding to the previous level dynamics model as the control unit of the current level nested dynamics model, and embeds the dynamics model corresponding to the new control unit through the kinematic constraint coupling based on the previous level nested dynamics model. Therefore, when the multi-vehicle configuration changes, only the constraint kinematic model and the dynamics model of the changed control unit need to be added or reduced, which is simpler and clearer than the traditional dynamics modeling process, and the self-adaptation ability of the model is significantly enhanced.

[0093] After obtaining the vehicle information of each target vehicle, the multi-level nested dynamics model can be used to process the vehicle information of each target vehicle, and then the control torque corresponding to each target vehicle is obtained. For example, the multi-unmanned vehicle system includes target vehicle 1, target vehicle 2, target vehicle 3, and target vehicle 4, and the control torque u1 of the target vehicle 1, the control torque u2 of the target vehicle 2, the control torque u3 of the target vehicle 3, and the control torque u4 of the target vehicle 4 can be obtained.

[0094] After obtaining the control torque corresponding to each target vehicle, the corresponding control torque can be output to each target vehicle respectively to realize the trajectory tracking control of the target vehicle.

[0095] Based on the multi-robot vehicle trajectory tracking robust control method based on nested dynamics modeling provided in the embodiments of the present disclosure, after obtaining the vehicle information of each target vehicle, the controller can perform trajectory constraint processing on the vehicle information of each target vehicle based on the multi-level nested dynamics model to obtain the control torque corresponding to each target vehicle, respectively. The multi-level nested dynamics model is obtained by embedding the kinematic constraint coupling between the dynamics models of each control unit. Each control unit corresponds to at least one target vehicle. The control torque corresponding to each target vehicle is output to each target vehicle, respectively, to perform trajectory tracking control of each target vehicle through the control torque corresponding to each target vehicle. The multi-robot vehicle trajectory tracking robust control method based on nested dynamics modeling provided in the embodiments of the present disclosure uses a multi-level nested dynamics model for trajectory constraint. Since the modeling process of the multi-level nested dynamics model is simple, clear, and has strong self-adaptation ability, the control complexity of the controller for the multi-body system is reduced, and the flexibility and accuracy of the control are improved.

[0096] In an exemplary embodiment, for an N-level nested dynamics model in the multi-level nested dynamics model, the N-level nested dynamics model corresponds to a first control unit and a second control unit, the dynamics model corresponding to the first control unit and the dynamics model corresponding to the second control unit are coupled to obtain the N-level nested dynamics model by nesting a first kinematic constraint, the dynamics model corresponding to the first control unit is an N-1-level nested dynamics model, the second control unit includes at least one target vehicle, and the dynamics models of each target vehicle in the at least one target vehicle are coupled to obtain the dynamics model corresponding to the second control unit by nesting a second kinematic constraint, wherein N is an integer greater than 0 and less than or equal to K, and K is the total number of levels of the multi-level nested dynamics model.

[0097] In the embodiments of the present disclosure, the multi-level nested dynamics model can be a K-level nested dynamics model, and the N-level nested dynamics model can be any level of nested dynamics model in the modeling process. Referring to FIGS. 1 to 3, in the modeling process of the multi-level nested dynamics model, the constraint dynamics model of the target vehicle can be established (for example, modeled by using the Newton-Euler dynamics method), wherein the constraint part can be represented by the Udwadia-Kalaba equation. Based on the established dynamics model of each target vehicle, the dynamics model of the first target vehicle and the dynamics model of the second target vehicle are coupled by the kinematic constraint between the first target vehicle and the second target vehicle to establish a one-level nested dynamics model. At this time, the first target vehicle is the first control unit corresponding to the one-level nested dynamics model, and the second target vehicle is the second control unit corresponding to the one-level nested dynamics model. Figure 2a Figure 2b

[0098] ​​On the basis of establishing the first-level nested dynamics model, the first target vehicle and the second target vehicle are taken as the first control unit corresponding to the second-level nested dynamics model, the third target vehicle and the fourth target vehicle are taken as the second control unit corresponding to the second-level nested dynamics model, the dynamics model corresponding to the second control unit can be established based on the kinematic constraint between the third target vehicle and the fourth target vehicle (that is, the second kinematic constraint), and the dynamics model corresponding to the first control unit and the dynamics model corresponding to the second control unit of the second-level nested dynamics model are coupled based on the kinematic constraint between the first control unit and the second control unit of the second-level nested dynamics model (that is, the first kinematic constraint), so that the second-level nested dynamics model is established. …, and so on, and then the multi-level nested dynamics model is established.

[0099] It should be noted that the above is only an example of modeling the multi-level nested dynamics model in the embodiment of the present disclosure, and in fact, each control unit corresponds to at least one target vehicle, that is, the number of the second control unit is not specifically limited in the embodiment of the present disclosure.

[0100] By using the multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling provided in the embodiment of the present disclosure, the dynamics model corresponding to the new control unit is coupled and embedded through the kinematic constraint, so that when the multi-vehicle configuration changes, only the constraint kinematic model and the dynamics model of the changed control unit need to be added or reduced, which is simpler and clearer than the traditional dynamics modeling process, and the self-adaptation ability of the model is significantly enhanced.

[0101] In an exemplary embodiment, the kinematic constraint includes a constraint matrix, a constraint acceleration, a first constraint variable, and a second constraint variable. For an N-level nested dynamics model, the constraint matrix is based on trajectory constraints, and is constructed by using two-dimensional plane position information of each target vehicle included in the first control unit and two-dimensional plane information of each target vehicle included in the second control unit;

[0102] The constraint acceleration is based on the constraint acceleration nested in the dynamics model corresponding to the first control unit and the constraint acceleration nested in the dynamics model corresponding to the second control unit, and is constructed as follows:

[0103] The first constraint variable is a one-dimensional column vector based on the total number of target vehicles included in the first control unit and target vehicles included in the second control unit.

[0104] The second constraint variable is a one-dimensional column vector constructed based on two-dimensional plane position information of target vehicles included in the first control unit and two-dimensional plane position information of target vehicles included in the second control unit.

[0105] In the embodiments of the present disclosure, the kinematic constraint can include a constraint matrix, a constraint acceleration, a first constraint variable, and a second constraint variable.

[0106] Still taking the example shown in Figure 2a and Figure 2b as an example to illustrate the embodiments of the present disclosure. A single target vehicle is modeled using the Newton-Euler dynamics method, and is simplified and arranged into the following matrix form, refer to formulas (one) to (four).

[0107]

[0108] wherein

[0109]

[0110]

[0111]

[0112] wherein q1 is a state variable of a first target vehicle (hereinafter referred to as unmanned vehicle 1), x1 and y1 are the mass center positions (two-dimensional plane position information) of the unmanned vehicle 1 in a two-dimensional plane, θ1 is the azimuth angle of the unmanned vehicle 1, m1 is the mass of the unmanned vehicle 1, I1 is the mass center rotational inertia of the unmanned vehicle 1, l1 is 1 / 2 of the vehicle width of the unmanned vehicle 1, r1 is the wheel radius of the unmanned vehicle 1, M1(q1, t) represents the mass / inertia matrix (hereinafter can be referred to as M1) of the unmanned vehicle 1, represents the generalized force matrix (hereinafter can be referred to as Q1) of the unmanned vehicle 1, represents the generalized unconstrained force matrix (hereinafter can be referred to as Q 11 ) of the unmanned vehicle 1, represents the generalized constraint force matrix (hereinafter can be referred to as Q1 C ) of the unmanned vehicle 1, R1 represents the conversion matrix of the generalized constraint force of the unmanned vehicle 1, u1 represents the input torque of the unmanned vehicle 1, u 1lf represents the input torque of the left front wheel of the unmanned vehicle 1, u 1rf represents the input torque of the right front wheel of the unmanned vehicle 1.

[0113] In the embodiments of the present disclosure, only trajectory constraints are involved, constraint equations can be established, and the constraint equations can be converted into first-order and second-order forms, which can be referred to in the following formulas (five) to (fourteen).

[0114] x1 2 +y1 2 =V 2 Formula (five)

[0115] Derivation with respect to t, and arrangement into first-order and second-order forms

[0116]

[0117]

[0118] The constraint equation can be rewritten in the following matrix form

[0119]

[0120]

[0121] wherein

[0122] A1(q1, t) = [x1 y10] Equation (Ten)

[0123]

[0124]

[0125] c1(q1, t) = [0] Equation (Thirteen)

[0126]

[0127] wherein A1(q1, t) represents the constraint matrix of the first target vehicle of the unmanned vehicle 1 (hereinafter can be referred to as A1), c1(q1, t) represents the first constraint variable of the unmanned vehicle 1 (hereinafter can be referred to as c1), represents the second constraint variable of the unmanned vehicle 1 (hereinafter can be referred to as b1), represents the speed of the unmanned vehicle 1, represents the constraint acceleration of the unmanned vehicle 1, and V represents the trajectory radius.

[0128] The constraint acceleration of the unmanned vehicle 1 is The generalized constraint force matrix is expressed by the Udwadia-Kalaba equation, which can be referred to in the following Equation (Fifteen).

[0129]

[0130] At this time, the constraint acceleration of the unmanned vehicle 1 is The generalized force matrix can be expressed as the following Equation (Sixteen).

[0131]

[0132] On the basis of establishing the dynamics model of each target vehicle, through the kinematics constraint between each target vehicle, a one-level nested dynamics model can be established. For example, the dynamics model of the second target vehicle (hereinafter referred to as unmanned vehicle 2) containing constraints can be established by referring to the modeling method of the aforementioned unmanned vehicle 1. For details, refer to Equations (Seventeen) to (Twenty).

[0133]

[0134] in

[0135]

[0136]

[0137]

[0138] q2 represents the state variable of autonomous vehicle 2, x2 and y2 represent the positions of the center of mass of autonomous vehicle 2 on the two-dimensional plane (two-dimensional plane position information), θ2 represents the azimuth angle of autonomous vehicle 2, m2 represents the mass of autonomous vehicle 2, I2 represents the moment of inertia of the center of mass of autonomous vehicle 2, l2 represents half the width of autonomous vehicle 2, r2 represents the wheel radius of autonomous vehicle 2, and M2(q2,t) represents the mass / inertia matrix of autonomous vehicle 2 (hereinafter referred to as M2). Let Q2 represent the generalized force matrix of driverless car 2. The generalized unconstrained force matrix of driverless car 2 (hereinafter referred to as Q) 21 ), The generalized constraint matrix of driverless car 2 (hereinafter referred to as...) R2 represents the transformation matrix of the generalized constraint force of autonomous vehicle 2, u2 represents the input torque of autonomous vehicle 2, and u 2lf u represents the input torque of the left front wheel of driverless car 2. 2rf This represents the input torque of the right front wheel of driverless vehicle 2.

[0139] The driverless car 2 The generalized constraint matrix is ​​represented by the Udwadia-Kalaba equation, which yields the following formulas (XXI) to (XXIII).

[0140]

[0141] in,

[0142] Formula (XXII) A2(q2,t)=[x2y20]

[0143]

[0144] A2(q2,t) represents the constraint matrix of driverless car 2 (hereinafter referred to as A2). This is the second constraint variable corresponding to driverless car 2 (hereinafter referred to as b2).

[0145] Driverless car 2 The generalized force matrix can be expressed as the following formula (XXIV).

[0146]

[0147] Similarly, the constraint equation between two unmanned vehicles is designed, which only involves its geometric constraint, and the constraint equation is converted into first-order and second-order forms, as shown in the following formula (twenty-five).

[0148] (x1-x2) 2 +(y1-y2) 2 =L 2 Formula (twenty-five)

[0149] wherein L is the distance between the centers of adjacent unmanned vehicles. Taking the derivative of t, and after being arranged into first-order and second-order forms, the following formula (twenty-six) and formula (twenty-seven) are obtained.

[0150]

[0151]

[0152] The constraint equation is rewritten in the following matrix form, and formula (twenty-eight) and formula (thirty-three) are obtained.

[0153]

[0154]

[0155] wherein,

[0156]

[0157]

[0158]

[0159]

[0160] wherein, represents the constraint matrix of unmanned vehicle 1 and unmanned vehicle 2 in the first-level nested dynamics model (hereinafter can be referred to as ), represents the first constraint variable of unmanned vehicle 1 and unmanned vehicle 2 in the first-level nested dynamics model (hereinafter can be referred to as ), represents the second constraint variable of unmanned vehicle 1 and unmanned vehicle 2 in the first-level nested dynamics model (hereinafter can be referred to as ).

[0161] Through the dynamics constraint of unmanned vehicle 1 and unmanned vehicle 2, the first-level nested dynamics model with constraints can be established, and the following formula (thirty-four) to formula (thirty-seven) can be obtained.

[0162]

[0163] wherein,

[0164]

[0165]

[0166]

[0167] is the state variable of the primary nested dynamics model, represents the mass / inertia matrix of the primary nested dynamics model (hereinafter can be referred to as ), represents the generalized force matrix of the primary nested dynamics model (hereinafter can be referred to as ), represents the generalized unconstrained force matrix of the primary nested dynamics model (hereinafter can be referred to as ), represents the generalized constraint force matrix of the primary nested dynamics model (hereinafter can be referred to as ).

[0168] The generalized constraint force matrix of the primary nested dynamics model can be expressed by the Udwadia-Kalaba equation, and formulas (thirty-eight) to (forty) can be obtained.

[0169]

[0170] wherein,

[0171]

[0172]

[0173] The generalized force matrix of the primary nested dynamics model can be expressed as the following formula (forty-one).

[0174]

[0175] Thus, the primary nested dynamics model is obtained. On the basis of the primary nested dynamics model, a secondary nested dynamics model can be further established.

[0176] For example, the first control unit corresponding to the two-level nested dynamics model can be the first unmanned vehicle 1 and the second unmanned vehicle 2 corresponding to the one-level nested dynamics model, that is, the dynamics model corresponding to the first control unit is the one-level nested dynamics model, and the second control unit can include a third target vehicle (hereinafter referred to as unmanned vehicle 3) and a fourth target vehicle (hereinafter referred to as unmanned vehicle 4). The dynamics model corresponding to the second control unit is the dynamics model obtained by kinematically constraining the dynamics model corresponding to the unmanned vehicle 3 and the dynamics model corresponding to the unmanned vehicle 4. In fact, the second control unit can also include one unmanned vehicle and multiple unmanned vehicles, and the specific modeling process is similar to the process of including two unmanned vehicles, and the specific modeling process is similar to the process of including two unmanned vehicles. Reference can be made, and the present embodiment of the present disclosure will not be repeated here.

[0177] Referring to the foregoing process of establishing the dynamics model corresponding to the first control unit, the constrained dynamics model (hereinafter referred to as one-level nested dynamics model 2) of the second control unit can be obtained. The following formulas (forty-two) to (forty-five) are obtained.

[0178]

[0179]

[0180]

[0181]

[0182]

[0183] is the state variable of the one-level nested dynamics model 2, is the mass / inertia matrix (hereinafter referred to as M2) of the one-level nested dynamics model 2, M3(q3, t) represents the mass / inertia matrix (which can be referred to as M3) of the unmanned vehicle 3, and M4(q4, t) represents the mass / inertia matrix (which can be referred to as M4) of the unmanned vehicle 4, is the generalized force matrix (hereinafter referred to as Q2) of the one-level nested dynamics model 2, is the generalized unconstrained force matrix (hereinafter referred to as Q2u) of the one-level nested dynamics model 2, is the generalized constraint force matrix (hereinafter referred to as Q2c) of the one-level nested dynamics model 2, is the generalized force matrix (which can be referred to as Q3) of the unmanned vehicle 3, is the generalized force matrix (which can be referred to as Q4) of the unmanned vehicle 4, is the constraint acceleration of the unmanned vehicle 3,​​​​ This represents the constrained acceleration of driverless car 4. Wherein, and For the specific calculation process, please refer to and Therefore, M3 and M4 can be referred to as M1 and M2, and Q3 and Q4 can be referred to as Q1 and Q2. This will not be elaborated further in the embodiments disclosed herein.

[0184] The first-level nested dynamics model 2 The generalized constraint matrix is ​​represented by the Udwadia-Kalaba equation, which yields equations (46) to (48).

[0185]

[0186] in,

[0187]

[0188]

[0189] The constraint matrix representing the relationship between driverless car 3 and driverless car 4 (hereinafter referred to as...) ),

[0190] It is the second constraint variable between autonomous vehicle 3 and autonomous vehicle 4 (hereinafter referred to as...). x3 and y3 are the centroid positions of unmanned vehicle 3 on the two-dimensional plane (two-dimensional plane position information), and x4 and y4 are the centroid positions of unmanned vehicle 4 on the two-dimensional plane (two-dimensional plane position information).

[0191] That is, the first-level nested dynamics model 2 The generalized force matrix can be expressed as formula (49).

[0192]

[0193] Design the constraint equations between the two first control units and the second control unit, and transform the constraint equations into first-order and second-order forms to obtain the following formulas (50) to (52).

[0194] (x1-x2) 2 +(y1-y2) 2 =L 2 Formula(fifty)

[0195] (x2-x3) 2 +(y2-y3) 2 =L 2 Formula (51)

[0196] (x3-x4) 2 +(y3-y4) 2 = L 2 Equation (Fifty-two)

[0197] Taking the derivative of t, and arranging into first and second order forms, the following Equations (Fifty-three) through (Fifty-eight) can be obtained.

[0198]

[0199]

[0200]

[0201]

[0202]

[0203]

[0204] Rewriting the constraint equation into the following matrix form, the following Equations (Fifty-nine) through (Sixty-four) can be obtained.

[0205]

[0206]

[0207]

[0208]

[0209]

[0210]

[0211] wherein, wherein, represents the constraint matrix between the first control unit and the second control unit in the two-level nested kinetic model (which can be referred to as ), represents the first constraint variable between the first control unit and the second control unit in the two-level nested kinetic model (which can be referred to as ), represents the second constraint variable between the first control unit and the second control unit in the two-level nested kinetic model (which can be referred to as ), represents the constraint acceleration in the two-level nested kinetic model.

[0212] Based on the kinematic constraints of the first control unit and the second control unit, a two-level nested dynamic model is obtained by coupling, referring to the following formulas (65) to (68).

[0213]

[0214] in,

[0215]

[0216]

[0217]

[0218] For the state variables of the second-order nested dynamics model, The mass / inertia matrix of the second-order nested dynamics model (which can be simply referred to as...) ), The generalized force matrix representing a second-order nested dynamics model (which can be simply referred to as...) ), The generalized unconstrained force matrix representing a second-order nested dynamics model (which can be simply referred to as...) ), The generalized constraint matrix representing a second-order nested dynamics model (which can be simply referred to as...) ).

[0219] The second-level nested dynamic model The generalized constraint matrix is ​​represented by the Udwadia-Kalaba equation, which yields the following formula (69).

[0220]

[0221] in,

[0222]

[0223]

[0224] Second-order nested dynamics model The generalized force matrix can be expressed as the following formula (70).

[0225]

[0226] Thus, the generalized force matrix of the second-level nested dynamics model is obtained, and the second-level nested dynamics model is obtained, and so on, and the generalized force matrix of the multi-level nested dynamics model can be obtained, that is, the multi-level nested dynamics model is obtained.

[0227] By adopting the multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling provided in the embodiments of the present disclosure, through kinematic constraints, a new dynamics model corresponding to a control unit is coupled and embedded on the basis of the front-stage nested dynamics model to obtain a multi-stage nested dynamics model. When the multi-vehicle configuration changes, only the constraint kinematic model and the dynamics model of the changed control unit need to be added or reduced. Compared with the traditional dynamics modeling process, the process is simpler and clearer, and the self-adaptability of the model is significantly enhanced.

[0228] In an example embodiment, the multi-stage nested dynamics model includes a generalized constraint force matrix of the multi-unmanned vehicle system, and the generalized constraint force matrix is a matrix equation established based on the Udwadia-Kalaba theory. Figure 3 As shown in FIG. 1, in step 104, the vehicle information of each target vehicle is subjected to trajectory constraint processing based on the multi-stage nested dynamics model to obtain the control torque corresponding to each target vehicle, including:

[0229] In step 302, based on the generalized constraint force matrix of the multi-unmanned vehicle system and the vehicle information of each target vehicle, the control input of the multi-unmanned vehicle system is obtained.

[0230] In step 304, based on the control input and the conversion matrix of the generalized constraint force of each target vehicle, the control torque corresponding to each target vehicle is obtained.

[0231] In the embodiments of the present disclosure, based on the obtained multi-stage nested dynamics model, the vehicle information of each target vehicle can be input into the generalized constraint force matrix of the multi-unmanned vehicle system to obtain the control input of the multi-unmanned vehicle system. Since the control input is obtained from the conversion matrix of the generalized constraint force of each target vehicle and the control torque corresponding to each target vehicle, after obtaining the conversion matrix of the generalized constraint force of each target vehicle and the control input of the multi-unmanned vehicle system, the control torque corresponding to each target vehicle can be obtained.

[0232] For example, as shown in formula (four), the conversion matrix of the generalized constraint force of the target vehicle can be obtained according to the following formula (seventy-one).

[0233]

[0234] wherein i is the identifier of the unmanned vehicle, R i represents the conversion matrix of the generalized constraint force of the unmanned vehicle i, θ i represents the azimuth angle of the unmanned vehicle i. Based on the azimuth angle of each unmanned vehicle and the wheel radius of the unmanned vehicle, the conversion matrix of the generalized constraint force of each unmanned vehicle can be obtained, and based on the conversion matrix of the generalized constraint force of each target vehicle and the control input of the multi-unmanned vehicle system, the control torque corresponding to each target vehicle can be obtained.

[0235] In an exemplary embodiment, referring to FIG. 3, Figure 4 In step 302, based on the generalized constraint force matrix of the multi-unmanned vehicle system and the vehicle information of each target vehicle, the control input of the multi-unmanned vehicle system is obtained, including:

[0236] In step 402, for any target vehicle, the mass inertia matrix and the generalized force matrix of the target vehicle are determined according to the vehicle information of the target vehicle.

[0237] In step 404, the mass inertia matrix of the multi-unmanned vehicle system is determined according to the mass inertia matrix of each target vehicle, and the generalized unconstrained force matrix of the multi-unmanned vehicle system is obtained according to the generalized force matrix of each target vehicle.

[0238] In step 406, the kinematic constraint of the multi-unmanned vehicle system is obtained according to the vehicle information of each target vehicle.

[0239] In step 408, the generalized unconstrained force matrix of the multi-unmanned vehicle system, the mass inertia matrix of the multi-unmanned vehicle system, and the kinematic constraint of the multi-unmanned vehicle system are input into the generalized constraint force matrix of the multi-unmanned vehicle system for trajectory constraint, and the control input of the multi-unmanned vehicle system is obtained.

[0240] In an exemplary embodiment, referring to FIG. 4, Figure 5 In step 402, the mass inertia matrix and the generalized force matrix of the target vehicle are determined according to the vehicle information of the target vehicle, which can include:

[0241] In step 502, the mass inertia matrix of the target vehicle is determined according to the mass of the target vehicle, the mass moment of inertia of the center of mass of the target vehicle, and the vehicle width of the target vehicle.

[0242] In step 504, the generalized unconstrained force matrix of the target vehicle is determined according to the mass of the target vehicle and the two-dimensional plane position information of the target vehicle.

[0243] In step 506, the kinematic constraint of the target vehicle is determined according to the two-dimensional plane position information of the target vehicle.

[0244] In step 508, the generalized force matrix of the target vehicle is obtained according to the generalized unconstrained force matrix of the target vehicle, the mass inertia matrix of the target vehicle, and the kinematic constraint of the target vehicle.

[0245] In the embodiments of the present disclosure, the vehicle information of the target vehicle can include the mass of the target vehicle, the mass moment of inertia of the target vehicle, the vehicle width of the target vehicle, and two-dimensional plane position information of the target vehicle. The mass moment of inertia matrix of the target vehicle can be determined according to the mass of the target vehicle, the mass moment of inertia of the target vehicle, and the vehicle width of the target vehicle. The specific calculation process can refer to formula (two), and the determination process will not be described here in the embodiments of the present disclosure.

[0246] After obtaining the mass of the target vehicle and the two-dimensional plane position information of the target vehicle, the generalized unconstrained force matrix of the target vehicle can be determined according to the mass of the target vehicle and the two-dimensional plane position information of the target vehicle by formula (four). The generalized unconstrained force matrix of the target vehicle can be obtained by using the following formula (seventy-two).

[0247]

[0248] Although formula (seventy-two) corresponds to unmanned vehicle 1, in fact, when each target vehicle determines the generalized unconstrained force matrix, it can refer to the formula (seventy-two), and the vehicle information of each target vehicle is substituted into the formula (seventy-two) to determine the generalized unconstrained force matrix of the target vehicle.

[0249] Further, the kinematic constraints of the target vehicle can be determined based on the two-dimensional plane position information of the target vehicle. The specific process can refer to formula (ten) to formula (fourteen), and the present disclosure will not be described here.

[0250] After obtaining the generalized unconstrained force matrix of the target vehicle, the mass moment of inertia matrix of the target vehicle, and the kinematic constraints of the target vehicle, the generalized force matrix of the target vehicle can be obtained according to the generalized unconstrained force matrix of the target vehicle, the mass moment of inertia matrix of the target vehicle, and the kinematic constraints of the target vehicle by referring to formula (sixteen).

[0251] After obtaining the mass moment of inertia matrix and the generalized force matrix of each target vehicle, the mass moment of inertia matrix of the multi-unmanned vehicle system can be constructed according to the mass moment of inertia matrix of the target vehicle. For example, when the multi-unmanned system includes a first control unit (unmanned vehicle 1 and unmanned vehicle 2), a second control unit (unmanned vehicle 3 and unmanned vehicle 4), after obtaining the mass moment of inertia matrix M1 of the unmanned vehicle 1, the mass moment of inertia matrix M2 of the unmanned vehicle 2, the mass moment of inertia matrix M3 of the unmanned vehicle 3, and the mass moment of inertia matrix M4 of the unmanned vehicle 4, the mass moment of inertia matrix of the first control unit can be obtained based on the mass moment of inertia matrix M1 of the unmanned vehicle 1 and the mass moment of inertia matrix M2 of the unmanned vehicle 2. The mass moment of inertia matrix of the second control unit is obtained based on the mass moment of inertia matrix M3 of the unmanned vehicle 3 and the mass moment of inertia matrix M4 of the unmanned vehicle 4. The mass moment of inertia matrix of the multi-unmanned vehicle system is obtained based on the mass moment of inertia matrix of the first control unit and the mass moment of inertia matrix of the second control unit.

[0252] As can be seen from equation (68), the generalized unconstrained force matrix of the multi-level nested dynamics model is constructed from the generalized force matrix of the first control unit and the generalized force matrix of the second control unit, so the generalized force matrix of each control unit can be constructed according to the generalized force matrix of each target vehicle, and then the generalized unconstrained force matrix of the multi-level nested dynamics model can be constructed according to the generalized force matrix of each control unit.

[0253] After the generalized unconstrained force matrix of the multi-level nested dynamics model is constructed, the kinematic constraints of the multi-unmanned vehicle system can be obtained based on the vehicle information of each target vehicle. For example, equations (61) to (64) can be used to obtain each kinematic constraint of the multi-unmanned vehicle system. Each kinematic constraint of the multi-unmanned vehicle system, the generalized unconstrained force matrix of the multi-unmanned vehicle system, and the mass inertia matrix of the multi-unmanned vehicle system are input into the generalized constraint force matrix of the multi-unmanned vehicle system (refer to the generalized constraint force matrix shown in equation (69)) to perform trajectory constraint to obtain the control input of the multi-unmanned vehicle system.

[0254] For example, in order to eliminate the error caused by the initial condition (for example, the starting position of the multi-unmanned vehicle is different), an error elimination matrix is deployed in the controller in the embodiment of the present disclosure. For details, refer to equation (73) below.

[0255]

[0256] wherein,

[0257]

[0258]

[0259] wherein, τ represents the control input, a is a constant greater than zero, represents the generalized constraint force matrix of the multi-level nested dynamics model, represents the error elimination matrix of the controller, Q C represents the generalized constraint force matrix of the multi-level nested dynamics model, M represents the mass inertia matrix of the multi-unmanned vehicle system, A represents the constraint matrix of the multi-unmanned vehicle system, b represents the second constraint variable of the multi-unmanned vehicle system, Q represents the generalized unconstrained force matrix of the multi-level nested dynamics model, represents the speed of the multi-unmanned vehicle system, and c represents the first constraint variable of the multi-unmanned vehicle system.

[0260] In the embodiments of the present disclosure, the control moments of the unmanned vehicles can be obtained by using the Udwadia-Kalaba theory. Still taking the multi-unmanned vehicle system including the four unmanned vehicles as an example, the relationship between the control moment of each unmanned vehicle and the conversion matrix of the generalized constraint force of each unmanned vehicle can be shown in the following formula (seventy-six) and formula (seventy-seven).

[0261]

[0262]

[0263] wherein,

[0264]

[0265]

[0266]

[0267]

[0268] wherein, R represents the conversion matrix of the generalized constraint force of the multi-unmanned vehicle system, u represents the control moment of the multi-unmanned vehicle system, R1, R2, R3, R4 represent the conversion matrix of the generalized constraint force of the unmanned vehicles 1, 2, 3, 4, and u1, u2, u3, u4 represent the control input of the unmanned vehicles 1, 2, 3, 4.

[0269] In an exemplary embodiment, the kinematic constraint includes a first constraint variable and a second constraint variable, and the method further includes:

[0270] determining a first correction value for the first constraint variable and a second correction value for the second constraint variable based on the two-dimensional planar position information of the target vehicle;

[0271] correcting the first constraint variable based on the first correction value to obtain a corrected first constraint variable;

[0272] correcting the second constraint variable based on the second correction value to obtain a corrected second constraint variable.

[0273] In the embodiments of the present disclosure, the first constraint variable and the second constraint variable of the target vehicle are determined as an example. Exemplarily, the motion constraint can be corrected by introducing the zero-order form and the first-order form of the motion constraint. That is, the first constraint variable and the second constraint variable can be determined first, and then the first correction value corresponding to the first constraint variable and the second correction value corresponding to the second constraint variable are determined by using the zero-order form and the first-order form, and the first constraint variable and the second constraint variable are modified by using the first correction value and the second correction value respectively, to obtain the modified first constraint variable and the modified second constraint variable.

[0274] For the first constraint variable and the second constraint variable, the zero-order forms are as follows respectively:

[0275]

[0276] e2 = (x1-x2) 2 +(y1-y2) 2 -L 2

[0277] The first-order forms are as follows respectively:

[0278]

[0279]

[0280] Wherein, e1 and are the first revised values, wherein e1 is the first revised value corresponding to the first constraint variable corresponding to the target vehicle, is the first revised value corresponding to the first constraint variable corresponding to the control unit. e2 and are the second revised values, wherein e2 is the second revised value corresponding to the second constraint variable corresponding to the target vehicle, is the second revised value corresponding to the second constraint variable corresponding to the control unit.

[0281] Therefore, in order to improve the accuracy of control, the first constraint variable and the second constraint variable are revised to obtain the revised first constraint variable and the revised second constraint variable.

[0282] Δc1(q1,t) = c1-s1e1

[0283]

[0284]

[0285]

[0286] Wherein, s1, s2 are constants greater than zero, is the new c1(q1,t) obtained by revising the zero-order form and the first-order form of the introduced constraint, The rest of the constraint equations are revised in this way, which will not be described here in the embodiment of the disclosure.

[0287] The model deployed in the controller in the embodiment of the disclosure can specifically refer to the following formula (seventy-seven) and formula (seventy-eight).

[0288]

[0289]

[0290] wherein, is the corrected is the corrected

[0291] With the multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling provided by the embodiments of the present disclosure, referring to FIG. 6, under the condition of an initial error of 500 mm, the error accuracy of the actual trajectory and the ideal trajectory can be controlled within (-5*10-3, 5*10-3). In FIG. 6, the solid line represents the actual trajectory of the unmanned vehicle, and the dashed line represents the ideal trajectory of the unmanned vehicle. Figures 6 to 15 Figures 6 to 9 In FIG. 6, the solid line represents the error between the ideal trajectory and the actual trajectory of the unmanned vehicle 1, and the dashed line represents the error between the ideal trajectory and the actual trajectory of the unmanned vehicle 2. The initial error is set to 500 mm. Figure 10 In FIG. 6, the solid line represents the error between the ideal trajectory and the actual trajectory of the unmanned vehicle 1, and the dashed line represents the error between the ideal trajectory and the actual trajectory of the unmanned vehicle 2. The initial error is set to 500 mm. Figure 11 In FIG. 6, the solid line represents the error between the ideal trajectory and the actual trajectory of the unmanned vehicle 1, and the dashed line represents the error between the ideal trajectory and the actual trajectory of the unmanned vehicle 2. The initial error is set to 500 mm. Figure 12 In FIG. 6, the solid line represents the right front wheel driving torque of the unmanned vehicle 1, and the dashed line represents the right front wheel driving torque of the unmanned vehicle 2. Figure 13 In FIG. 6, the solid line represents the right front wheel driving torque of the unmanned vehicle 1, and the dashed line represents the right front wheel driving torque of the unmanned vehicle 2. Figure 14 In FIG. 6, the solid line represents the right front wheel driving torque of the unmanned vehicle 1, and the dashed line represents the right front wheel driving torque of the unmanned vehicle 2. Figure 15 In FIG. 6, the solid line represents the right front wheel driving torque of the unmanned vehicle 1, and the dashed line represents the right front wheel driving torque of the unmanned vehicle 2.

[0292] It should be understood that, although each step in the flowchart involved in each of the above-described embodiments is shown in sequence according to the direction of the arrow, these steps are not necessarily executed in sequence according to the direction of the arrow. Unless otherwise specified herein, the execution of these steps is not strictly limited in sequence, and these steps can be executed in other sequences. Moreover, at least part of the steps in the flowchart involved in each of the above-described embodiments can include multiple steps or stages, which are not necessarily executed at the same time, but can be executed at different times, and the execution sequence of these steps or stages is not necessarily sequential, but can be executed in rotation or alternation with at least part of other steps or stages in other steps.

[0293] ​Based on the same inventive concept, the embodiments of the present application also provide a device for implementing the above-mentioned multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling. The implementation scheme for solving the problem provided by the device is similar to the implementation scheme described in the above method, so the specific limitations in one or more embodiments of the device for multi-unmanned vehicle trajectory tracking robust control based on nested dynamics modeling provided below can be referred to the limitations of the method for multi-unmanned vehicle trajectory tracking robust control based on nested dynamics modeling in the above, which will not be repeated here.

[0294] In one embodiment, as shown in Figure 16 a device for multi-unmanned vehicle trajectory tracking robust control based on nested dynamics modeling is provided, applied to a controller for controlling a multi-unmanned vehicle system including a plurality of target vehicles to be controlled, comprising: an acquisition module 1602, a constraint module 1604 and an output module 1606, wherein:

[0295] The acquisition module 1602 is configured to acquire vehicle information of each target vehicle.

[0296] The constraint module 1604 is configured to perform trajectory constraint processing on the vehicle information of each target vehicle based on a multi-level nested dynamics model to obtain a control torque corresponding to each target vehicle, wherein the multi-level nested dynamics model is obtained by embedding a kinematic constraint coupling between dynamics models corresponding to each control unit, and each control unit corresponds to at least one target vehicle.

[0297] The output module 1606 is configured to output the control torque corresponding to each target vehicle to each target vehicle respectively, so as to perform trajectory tracking control of each target vehicle through the control torque corresponding to each target vehicle.

[0298] By using the device for multi-unmanned vehicle trajectory tracking robust control based on nested dynamics modeling provided by the embodiments of the present application, the multi-level nested dynamics model is used for trajectory constraint, and because the modeling process of the multi-level nested dynamics model is simple, clear and has strong self-adaptation ability, the control complexity of the controller for the multi-body system is reduced, and the flexibility and accuracy of the control are improved.

[0299] In one of the embodiments, for an N-level nested dynamics model in the multi-level nested dynamics model, the N-level nested dynamics model corresponds to a first control unit and a second control unit, the dynamics model corresponding to the first control unit and the dynamics model corresponding to the second control unit are coupled to obtain the N-level nested dynamics model through a nested first kinematics constraint, the dynamics model corresponding to the first control unit is an (N-1)-level nested dynamics model, the second control unit includes at least one target vehicle, and the dynamics models of each of the at least one target vehicle are coupled to obtain the dynamics model corresponding to the second control unit through a nested second kinematics constraint, where N is an integer greater than 0 and less than or equal to K, and K is the total number of levels of the multi-level nested dynamics model.

[0300] In one of the embodiments, the multi-level nested dynamics model includes a generalized constraint force matrix of the multi-unmanned vehicle system, and the generalized constraint force matrix is a matrix equation established based on the Udwadia-Kalaba theory; the constraint module 1604 is further configured to:

[0301] obtain control input of the multi-unmanned vehicle system based on the generalized constraint force matrix of the multi-unmanned vehicle system and vehicle information of each target vehicle;

[0302] obtain control force corresponding to each target vehicle based on the control input and a conversion matrix of the generalized constraint force.

[0303] In one of the embodiments, the constraint module 1604 is further configured to:

[0304] for any target vehicle, determine a mass inertia matrix and a generalized force matrix of the target vehicle according to vehicle information of the target vehicle;

[0305] determine a mass inertia matrix of the multi-unmanned vehicle system according to the mass inertia matrix of each target vehicle, and obtain a generalized unconstrained force matrix of the multi-unmanned vehicle system according to the generalized force matrix of each target vehicle;

[0306] obtain a kinematics constraint of the multi-unmanned vehicle system according to the vehicle information of each target vehicle;

[0307] input the generalized unconstrained force matrix of the multi-unmanned vehicle system, the mass inertia matrix of the multi-unmanned vehicle system, and the kinematics constraint of the multi-unmanned vehicle system into the generalized constraint force matrix of the multi-unmanned vehicle system for trajectory constraint to obtain control input of the multi-unmanned vehicle system.

[0308] In one of the embodiments, the constraint module 1604 is further configured to:

[0309] determine a mass inertia matrix of the target vehicle according to a mass of the target vehicle, a mass moment of inertia of the target vehicle, and a vehicle width of the target vehicle;

[0310] determine a generalized unconstrained force matrix of the target vehicle according to the mass of the target vehicle and two-dimensional planar position information of the target vehicle;

[0311] determine kinematic constraints of the target vehicle according to the two-dimensional planar position information of the target vehicle;

[0312] obtain a generalized force matrix of the target vehicle according to the generalized unconstrained force matrix of the target vehicle, the mass inertia matrix of the target vehicle, and the kinematic constraints of the target vehicle.

[0313] In one of the embodiments, the kinematic constraints include a first constraint variable and a second constraint variable, and the device further includes:

[0314] a determination module configured to determine a first correction value for the first constraint variable and a second correction value for the second constraint variable based on the two-dimensional planar position information of the target vehicle;

[0315] a first correction module configured to correct the first constraint variable based on the first correction value to obtain a corrected first constraint variable;

[0316] a second correction module configured to correct the second constraint variable based on the second correction value to obtain a corrected second constraint variable.

[0317] In one of the embodiments, the kinematic constraints include a constraint matrix, a constraint acceleration, a first constraint variable, and a second constraint variable, and for the N-level nested dynamics model,

[0318] the constraint matrix is a matrix constructed based on trajectory constraints and the two-dimensional planar position information of the target vehicle included in the first control unit and the two-dimensional planar position information of the target vehicle included in the second control unit;

[0319] the constraint acceleration is an acceleration constructed based on constraint accelerations of the dynamics model nested by the first control unit and constraint accelerations of the dynamics model nested by the second control unit;

[0320] the first constraint variable is a one-dimensional column vector obtained based on a total number of the target vehicles included in the first control unit and the target vehicles included in the second control unit;

[0321] The second constraint variable is a one-dimensional column vector constructed based on the two-dimensional plane position information of the target vehicle included in the first control unit and the two-dimensional plane position information of the target vehicle included in the second control unit.

[0322] The modules in the multi-unmanned vehicle trajectory tracking robust control device based on nested dynamics modeling can be implemented by software, hardware, or a combination thereof, in whole or in part. The modules can be embedded in or independent of a processor in a computer device in hardware form, or stored in a memory in the computer device in software form, so that the processor can call and execute the operations corresponding to the modules.

[0323] In one embodiment, a computer device, which can be a terminal, is provided, and an internal structure diagram of the computer device can be as shown in Figure 17 The computer device includes a processor, a memory, a communication interface, a display screen, and an input device connected through a system bus. The processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for running the operating system and the computer program in the non-volatile storage medium. The communication interface of the computer device is configured to perform wired or wireless communication with an external terminal. The wireless communication can be achieved by WIFI, mobile cellular network, NFC (near field communication), or other technologies. The computer program is executed by the processor to implement a multi-unmanned vehicle trajectory tracking robust control method based on nested dynamics modeling. The display screen of the computer device can be a liquid crystal display screen or an electronic ink display screen. The input device of the computer device can be a touch layer overlaid on the display screen, or a key, trackball, or touchpad arranged on the housing of the computer device, or an external keyboard, touchpad, or mouse, etc.

[0324] Those skilled in the art can understand that Figure 17 The structure shown in the above

[0325] In one embodiment, a computer device is also provided, which includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps in the above method embodiments.

[0326] In an embodiment, a computer readable storage medium is provided, having stored thereon a computer program which, when executed by a processor, implements the steps of any of the above method embodiments.

[0327] In an embodiment, a computer program product is provided, comprising a computer program which, when executed by a processor, implements the steps of any of the above method embodiments.

[0328] It should be noted that the user information (including but not limited to user equipment information, user personal information, etc.) and data (including but not limited to data for analysis, stored data, displayed data, etc.) involved in the present application are all information and data authorized by the user or authorized by all parties.

[0329] It can be understood by those skilled in the art that all or part of the processes in the above-mentioned embodiments can be completed by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer readable storage medium. When the computer program is executed, it can include the processes of the above-mentioned embodiments. Any reference to memory, database or other medium used in the embodiments provided by the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical storage, high-density embedded non-volatile memory, resistive memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. As an illustration but not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM), etc. The database involved in the embodiments provided by the present application can include at least one of a relational database and a non-relational database. The non-relational database can include a distributed database based on a block chain, etc., without being limited thereto. The processor involved in the embodiments provided by the present application can be a general-purpose processor, a central processing unit, a graphics processing unit, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., without being limited thereto.

[0330] Any combination of the technical features in the above embodiments can be made. For the sake of brevity, the foregoing description has not described all possible combinations of the technical features in the above embodiments. However, as long as the combination of the technical features does not contradict, it should be considered within the scope of the present disclosure.

[0331] The above embodiments only express several implementation manners of the present application, and the description is relatively specific and detailed, but it should not be understood as a limitation on the patent scope of the present 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 present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the appended claims.

Claims

1. A robust control method for trajectory tracking of multiple unmanned vehicles based on nested dynamics modeling, characterized in that, The method is applied to a controller for controlling a multi-vehicle system, the multi-vehicle system including multiple target vehicles to be controlled, and includes: Obtain vehicle information for each of the target vehicles; Based on a multi-level nested dynamics model, the vehicle information of each target vehicle is subjected to trajectory constraint processing to obtain the control torque corresponding to each target vehicle. The multi-level nested dynamics model is obtained by embedding kinematic constraint coupling between the dynamics models corresponding to each control unit, and each control unit corresponds to at least one target vehicle. The control torque corresponding to each of the target vehicles is output to each of the target vehicles respectively, so as to perform trajectory tracking control of each of the target vehicles through the control torque corresponding to each of the target vehicles; Specifically, for the N-level nested dynamic model in the multi-level nested dynamic model, the N-level nested dynamic model corresponds to a first control unit and a second control unit. The dynamic model corresponding to the first control unit and the dynamic model corresponding to the second control unit are coupled to obtain the N-level nested dynamic model through nested first kinematic constraints. The dynamic model corresponding to the first control unit is an N-1 level nested dynamic model. The second control unit includes at least one target vehicle. The dynamic models of each target vehicle in the at least one target vehicle are coupled to obtain the dynamic model corresponding to the second control unit through nested second kinematic constraints. Here, N is an integer greater than 0 and less than or equal to K, and K is the total number of levels in the multi-level nested dynamic model. The dynamic model of the target vehicle is a dynamic model with constraints.

2. The method according to claim 1, characterized in that, The multi-level nested dynamics model includes the generalized constraint force matrix of the multi-unmanned vehicle system, which is a matrix equation established based on the Udwadia-Kalaba theory. The process of performing trajectory constraint processing on the vehicle information of each target vehicle based on a multi-level nested dynamic model to obtain the control torque corresponding to each target vehicle includes: Based on the generalized constraint matrix of the multi-unmanned vehicle system and the vehicle information of each target vehicle, the control input of the multi-unmanned vehicle system is obtained. Based on the control input and the transformation matrix of the generalized constraint force of each target vehicle, the control torque corresponding to each target vehicle is obtained.

3. The method according to claim 2, characterized in that, The control input of the multi-unmanned vehicle system is obtained based on the generalized constraint matrix of the multi-unmanned vehicle system and the vehicle information of each target vehicle, including: For any of the target vehicles, the mass inertia matrix and generalized force matrix of the target vehicle are determined based on the vehicle information of the target vehicle. Based on the mass inertia matrix of each target vehicle, the mass inertia matrix of the multi-unmanned vehicle system is determined, and based on the generalized force matrix of each target vehicle, the generalized unconstrained force matrix of the multi-unmanned vehicle system is obtained. Based on the vehicle information of each target vehicle, the kinematic constraints of the multi-unmanned vehicle system are obtained; The generalized unconstrained force matrix, the mass inertia matrix, and the kinematic constraints of the multi-unmanned vehicle system are input into the generalized constraint force matrix of the multi-unmanned vehicle system to perform trajectory constraints, thereby obtaining the control input of the multi-unmanned vehicle system.

4. The method according to claim 3, characterized in that, Based on the vehicle information of the target vehicle, determine the mass inertia matrix and generalized force matrix of the target vehicle, including: The mass-inertia matrix of the target vehicle is determined based on the mass of the target vehicle, the moment of inertia of the center of mass of the target vehicle, and the width of the target vehicle. Based on the mass of the target vehicle and the two-dimensional planar position information of the target vehicle, determine the generalized unconstrained force matrix of the target vehicle; Based on the two-dimensional planar position information of the target vehicle, determine the kinematic constraints of the target vehicle; The generalized force matrix of the target vehicle is obtained based on the generalized unconstrained force matrix, the mass inertia matrix of the target vehicle, and the kinematic constraints of the target vehicle.

5. The method according to claim 4, wherein the kinematic constraints include a first constraint variable and a second constraint variable, and the method further includes: Based on the two-dimensional planar position information of the target vehicle, a first correction value for the first constraint variable and a second correction value for the second constraint variable are determined. The first constraint variable is modified based on the first correction value to obtain the modified first constraint variable. The second constraint variable is modified based on the second correction value to obtain the modified second constraint variable.

6. The method according to claim 1, characterized in that, Kinematic constraints include constraint matrices, constraint accelerations, first constraint variables, and second constraint variables. For the aforementioned N-level nested dynamics model, The constraint matrix is ​​a matrix constructed based on trajectory constraints, using the two-dimensional planar position information of each target vehicle included in the first control unit and the two-dimensional planar information of each target vehicle included in the second control unit. The constraint acceleration is constructed based on the constraint acceleration nested in the dynamic model corresponding to the first control unit and the constraint acceleration nested in the dynamic model corresponding to the second control unit. The first constraint variable is a one-dimensional column vector obtained based on the total number of target vehicles included in the first control unit and the second control unit. The second constraint variable is a one-dimensional column vector constructed based on the two-dimensional planar position information of the target vehicle included in the first control unit and the two-dimensional planar position information of the target vehicle included in the second control unit.

7. A robust control device for tracking the trajectory of multiple unmanned vehicles based on nested dynamics modeling, characterized in that, The device is applied to a controller for controlling a multi-unmanned vehicle system, the multi-unmanned vehicle system including multiple target vehicles to be controlled, and includes: The acquisition module is used to acquire vehicle information of each of the target vehicles; The constraint module is used to perform trajectory constraint processing on the vehicle information of each target vehicle based on a multi-level nested dynamic model, and obtain the control torque corresponding to each target vehicle. The multi-level nested dynamic model is obtained by embedding kinematic constraint coupling between the dynamic models corresponding to each control unit, and each control unit corresponds to at least one target vehicle. The output module is used to output the control torque corresponding to each of the target vehicles to each of the target vehicles respectively, so as to perform trajectory tracking control of each of the target vehicles through the control torque corresponding to each of the target vehicles; Specifically, for the N-level nested dynamic model in the multi-level nested dynamic model, the N-level nested dynamic model corresponds to a first control unit and a second control unit. The dynamic model corresponding to the first control unit and the dynamic model corresponding to the second control unit are coupled to obtain the N-level nested dynamic model through nested first kinematic constraints. The dynamic model corresponding to the first control unit is an N-1 level nested dynamic model. The second control unit includes at least one target vehicle. The dynamic models of each target vehicle in the at least one target vehicle are coupled to obtain the dynamic model corresponding to the second control unit through nested second kinematic constraints. Here, N is an integer greater than 0 and less than or equal to K, and K is the total number of levels in the multi-level nested dynamic model. The dynamic model of the target vehicle is a dynamic model with constraints.

8. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the method according to any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1 to 6.

Images