Generalized udwadia-kalaba control method and apparatus for variable formation constrained unmanned ground swarms

Abstract

The application discloses a generalized Udwadia-Kalaba control method and device for a variable formation constraint unmanned ground cluster, relates to the technical field of multi-unmanned vehicle cooperative control, and comprises the following steps: based on Udwadia-Kalaba constraint dynamics, a generalized acceleration of the cluster is designed as and a dynamic model is established as wherein represents a cluster inertia matrix, represents a combination vector of total generalized control inputs of all unmanned vehicles, and is split according to vehicles; in the formula, is total generalized control input applied to the unmanned vehicle; and and are equivalent control forces in the inertial coordinate system and the direction respectively; is an equivalent heading control moment around the vertical axis; a bottom layer driving control module of the unmanned vehicle is sent to generate driving instructions to realize vehicle control. The application enables the motion direction formed by the reference trajectory, the adjacent vehicle and the obstacle state to directly enter the solving process of the total generalized control input, so that the lag of the actual heading of the vehicle relative to the target motion direction and repeated correction are reduced.

Description

Technical Field

[0001] This invention relates to the technical field of multi-unmanned vehicle cooperative control, and in particular to a generalized Udwadia–Kalaba control method and device for variable formation constrained unmanned ground clusters. Background Technology

[0002] Existing multi-vehicle platooning control typically employs methods such as artificial potential field methods, leader-follower methods, consensus control, sliding mode control, robust control, or model predictive control to achieve trajectory tracking, formation maintenance, and obstacle avoidance. Artificial potential field methods can generate local motion directions based on the relative positions of the target, neighboring vehicles, and obstacles, offering advantages such as intuitive structure, low computational cost, and ease of online implementation. Leader-follower methods and consensus control are suitable for maintaining relative formations between vehicles. Model predictive control can explicitly consider state and input constraints within a finite prediction time domain. However, existing methods still have the following shortcomings: Some unmanned vehicle control methods based on artificial potential fields mainly use potential field information to generate planning direction or reference speed, but fail to further convert the direction into heading constraints and generalized control inputs that can be directly executed by the vehicle dynamics layer. This causes the vehicle's actual heading to fail to follow the reference trajectory or obstacle avoidance direction in a timely manner, resulting in steering lag, deviation of motion trajectory, or repeated correction of control commands. Summary of the Invention

[0003] To address the technical problem in existing technologies where the actual heading of a vehicle cannot promptly follow the reference trajectory or the direction of obstacle avoidance, this invention provides a generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms. The invention provides the following technical solution: Based on Udwadia–Kalaba constrained dynamics, the generalized acceleration of the cluster is designed as follows: ; In the formula, This represents the basic generalized acceleration that satisfies the heading acceleration constraint; This represents the right-hand vector of the cluster heading acceleration layer constraint. Indicates the first The target heading angular acceleration of the unmanned vehicle; This represents the negative first power of the cluster inertia matrix; The Moore-Penrose generalized inverse of the mass-weighted heading constraint matrix. Represents the identity matrix; Indicates the final distance adjustment auxiliary input; A represents the null space projection matrix of the heading constraint; A represents the heading constraint matrix of the swarm. Represent the Moore-Penrose generalized inverse of the cluster heading constraint matrix A; The autonomous vehicle is equivalent to a planar three-degree-of-freedom fully driven rigid body, and a dynamic model is established: ;in, Represents the cluster inertia matrix; This represents the combined vector of the total generalized control inputs of all autonomous vehicles; Breakdown by vehicle: ; In the formula, Indicates application to the first The total generalized control input of an autonomous vehicle; and These represent the equivalent control forces in the x and y directions of the inertial coordinate system, respectively. This represents the equivalent heading control moment about the vertical axis; Will Send to the The underlying drive control module of the autonomous vehicle, which controls... , and Vehicle control is achieved by converting the torque, speed, steering angle, or corresponding actuator drive commands into wheel torque, wheel speed, steering angle, or other parameters.

[0004] As a preferred embodiment of the generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms described in this invention, wherein: the final distance adjustment auxiliary input Designed as follows: ; In the formula, Let H represent the minimum norm solution that satisfies the vehicle distance homeomorphism dynamics, and let H represent the cluster vehicle distance control matrix. H represents the Moore-Penrose generalized inverse of the cluster vehicle distance control matrix H; h represents the homeomorphic control target vector of all controlled vehicle pairs; The null projection matrix representing the vehicle distance control matrix; This indicates the auxiliary target in the direction of the integrated artificial potential field.

[0005] As a preferred embodiment of the generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms described in this invention, wherein: the integrated artificial potential field direction auxiliary target The design method is as follows: Definition of the first The directional assistance target for the autonomous vehicle is: Then, let ; In the formula, Indicates the gain of planar velocity-assisted adjustment; and These represent the deviations between the vehicle's actual planar velocity and the velocity induced by the integrated artificial potential field; the third component is set to 0, indicating that the auxiliary target does not directly set the heading angular acceleration.

[0006] As a preferred embodiment of the generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms described in this invention, the method further includes: Regulation No. The heading angle of the autonomous vehicle satisfies a first-order following relationship: ;in, ; In the formula, Let be the actual heading angle of the i-th unmanned vehicle. Let be the actual heading angular velocity of the i-th unmanned vehicle. Indicates the first The expected heading angle of an autonomous vehicle. This represents the extraction matrix of the heading speed of a single vehicle; Indicates the heading angle following gain; Indicates the first The right-hand side of the heading speed layer constraint for an unmanned vehicle; Combine all vehicle heading speed layer constraints: ; ; In the formula, This represents the group heading constraint matrix, which is a constant matrix. Represents the right-hand vector of the cluster heading velocity layer constraint; for Find the time derivative: ; because It is a constant matrix. Therefore, we get: ; And the single-vehicle acceleration layer constraint is: ; In the formula, This represents the right-hand vector of the cluster heading acceleration layer constraint; Indicates the first The target heading angular acceleration of the unmanned vehicle; Indicates the right-hand term of the velocity layer. This represents the right-hand term of the acceleration layer; the two correspond to different constraint levels.

[0007] As a preferred embodiment of the generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms described in this invention, wherein: the first Expected heading angle of an autonomous vehicle Designed as follows: ; In the formula, Represents the arctangent function in the four quadrants; and These represent its components in the x and y directions of the inertial coordinate system, respectively. when At that time, the desired heading angular velocity is: ; In the formula, This represents the expected rate of change of heading angle; and This represents the first-order time derivative of the two components of the synthetic induced vector; This represents a preset threshold to prevent the magnitude of the integrated induced vector from approaching zero; when At the same time, maintain the desired heading angle from the previous control cycle.

[0008] As a preferred embodiment of the generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms described in this invention, the method further includes: Constructing a reference trajectory for the first The planar motion induced vector generated by the autonomous vehicle : In the formula, Indicates the gain of the reference trajectory; This indicates the positional deviation from the vehicle's current position to the current reference attraction point; Other driverless cars The collaborative effect of the autonomous vehicles is as follows: ; In the formula, Indicates other driverless cars to the first The total cooperative induced vector generated by the autonomous vehicles; This indicates the gain when the vehicle is close to the lower limit region. This indicates the effect gain when the vehicle distance exceeds the upper limit area; Indicates the minimum safe distance between vehicles; This indicates the upper limit of the reference distance for vehicle cooperation; Indicates when Time to take Otherwise, a non-negative truncation operator that takes 0; Represents the relative position vector between vehicles; All obstacles to the first The environmental impact of driverless vehicles is as follows: ; In the formula, Indicates all obstacles to the first... The total environmental guidance vector generated by the autonomous vehicle; Indicates the gain due to the environmental effect of obstacles; Indicates the minimum permissible distance between the center of a vehicle and an obstacle; Indicates the obstacle detection distance; This represents the relative position vector from the center of the obstacle towards the vehicle; Adding the three types of effects together, we get: ; In the formula, Indicates the first The integrated artificial potential field induced vector of an unmanned vehicle.

[0009] As a preferred embodiment of the generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms described in this invention, the method further includes: constructing a one-sided logarithmic homeomorphic variable for vehicle distance and setting the target formation distance, specifically including: For any vehicle ,structure: ; In the formula, For the distance to the controlled vehicle pair set, Indicates vehicle to One-sided log-homeomorphic variables; This indicates the zero-point adjustment parameters of the vehicle pair; Indicates the actual distance between vehicles; Indicates the minimum safe distance between vehicles; This indicates the margin of vehicle distance relative to the minimum safety boundary; Represent the natural logarithm function; Satisfying the mapping: ; The initial state should satisfy ;when hour: , Therefore, ; In the formula, This represents the target formation distance corresponding to the zero point of the homeomorphic variable; When the distance between the vehicles approaches the minimum safety boundary, there is .

[0010] As a preferred embodiment of the generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms described in this invention, the method further includes: selecting vehicle pairs requiring distance adjustment based on formation configuration, communication topology, or mission requirements to form a set of distance-controlled vehicle pairs. ; In the formula, represents the set of controlled vehicle pairs; m represents the number of controlled vehicle pairs; Let s represent the controlled vehicle pair in group s. The neighboring vehicles used in the artificial potential field calculation can be determined based on the sensing range, while distance homeomorphism control only targets... The vehicles in the process are subject to implementation.

[0011] As a preferred embodiment of the generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms described in this invention, the method further includes: For any ,Regulation: ; Right now ; In the formula, and Let these represent the first and second time derivatives of the homeomorphic variables, respectively. This represents the homeomorphic variable damping gain, used to suppress oscillations during vehicle distance adjustment. Represents the homeomorphic variable recovery gain, used to drive... Approaching zero; from We can obtain: .

[0012] This invention also discloses a generalized Udwadia-Kalaba control device for variable formation constrained unmanned ground swarms, applied to the aforementioned generalized Udwadia-Kalaba control method for variable formation constrained unmanned ground swarms. The device includes:

[0013] A generalized acceleration building block for the cluster, based on Udwadia–Kalaba constrained dynamics, is used to design the cluster's generalized acceleration. ; The dynamics model building module equates the autonomous vehicle to a planar three-degree-of-freedom fully driven rigid body, and establishes a dynamics model: ;in, Represents the cluster inertia matrix; This represents the combined vector of the total generalized control inputs for all autonomous vehicles; Input module, used to input Segmented by vehicle ; In the formula, Indicates application to the first The total generalized control input of an autonomous vehicle; and Representing the inertial coordinate system , Equivalent control force in direction; This represents the equivalent heading control moment about the vertical axis; And the underlying driver control module, used to... , and Vehicle control is achieved by converting the torque, speed, steering angle, or corresponding actuator drive commands into wheel torque, wheel speed, steering angle, or other parameters.

[0014] The beneficial effects of this invention are: 1. This invention transforms the direction induced by the integrated artificial potential field into the heading and velocity layer constraints and acceleration layer constraints of the unmanned vehicle, so that the motion direction formed by the reference trajectory, neighboring vehicles and obstacle states can be directly entered into the solution process of the total generalized control input, thereby reducing the lag and repeated correction of the vehicle's actual heading relative to the target motion direction.

[0015] 2. The present invention obtains the vehicle distance adjustment auxiliary input within the zero space of the heading constraint, so that the heading constraint already satisfied is not changed when the vehicle adjusts the formation distance, thereby reducing the mutual interference between formation distance adjustment, reference trajectory following, and obstacle avoidance.

[0016] 3. This invention uses a one-sided logarithmic homeomorphism to describe the minimum safe distance between vehicles, so that the vehicle distance only needs to be kept above the minimum safe boundary, without having to be forced to a fixed distance in the long term, thereby reserving movement space for vehicles to avoid obstacles, pass through restricted areas, and adjust formation configuration.

[0017] 4. This invention modifies the homeomorphic zero-point adjustment parameter. Set a fixed, periodically varying, or gradually varying target formation distance to maintain the same control calculation structure in different formation distance tasks, thereby reducing controller switching and the resulting distance abrupt changes.

[0018] 5. This invention expresses the minimum safety boundary between vehicles and the target formation distance in the same homomorphic variable, so that vehicles maintain the minimum safety distance while tracking the time-varying formation distance, reducing the switching and repetitive coordination between safety distance control and target distance tracking. Attached Figure Description

[0019] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the following description of the embodiments will be briefly introduced. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort. Wherein: Figure 1 This is a schematic diagram of the overall process of the generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground clusters proposed in this invention. Figure 2 This is a schematic diagram of the trajectory tracking and obstacle avoidance results under the fixed formation spacing condition in Example 2; Figure 3 This is a graph showing the distance changes between six groups of adjacent vehicles under the fixed spacing condition in Example 2; Figure 4 This is a diagram showing the actual motion trajectory of the six vehicles under the sinusoidal time-varying spacing condition in Example 2; Figure 5 This is a schematic diagram of the distance changes between six groups of adjacent vehicles under the sinusoidal time-varying spacing condition in Example 2; Figure 6 This is a diagram showing the actual motion trajectories of the six vehicles under the exponential convergence spacing condition in Example 2; Figure 7 This is a graph showing the changes in the distance between six adjacent vehicles under the exponential convergence spacing condition in Example 2. Detailed Implementation

[0020] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0021] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and those skilled in the art can make similar extensions without departing from the spirit of the invention. Therefore, the invention is not limited to the specific embodiments disclosed below.

[0022] Secondly, the term "one embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The phrase "in one embodiment" appearing in different places in this specification does not necessarily refer to the same embodiment, nor is it a single or selective embodiment that is mutually exclusive with other embodiments.

[0023] Example 1

[0024] This embodiment uses a cluster of six fully-driven, three-degree-of-freedom planar autonomous vehicles as the control object. Guided by a reference trajectory, the cluster of vehicles avoids three static circular obstacles and maintains six selected groups of adjacent vehicles at a minimum safe distance while approaching a fixed target formation distance. (Refer to...) Figure 1 This embodiment is implemented according to the following twelve steps. Specifically, it includes: Step 1: Establish dynamic models for single vehicles and autonomous vehicle clusters; Definition of the first The generalized coordinates, generalized velocity, and generalized acceleration of the autonomous vehicle are as follows: ; In the formula, Indicates the driverless vehicle's number; and They represent the first The lateral and longitudinal coordinates of the center of mass of the autonomous vehicle in the inertial coordinate system are both in meters. Indicates the first The longitudinal axis of the autonomous vehicle relative to the inertial coordinate system The heading angle of the axis, in rad; and Represents the planar velocity component, with units of m / s; This represents the angular velocity of heading, measured in rad / s. and Represents the planar acceleration component, with units of m / s². 2 ; This represents the angular acceleration of the heading, measured in rad / s. 2 ; superscript This indicates the matrix transpose.

[0025] No. The driverless car has an inertial coordinate system , The autonomous vehicle has two planar translational degrees of freedom and a yaw rotational degree of freedom about the vertical axis. The underlying drive control module converts the output of the upper-level controller into equivalent generalized forces in two translational directions and an equivalent generalized torque about the vertical axis. Therefore, the autonomous vehicle can be equivalently represented as a planar three-degree-of-freedom fully driven rigid body, and its dynamic model is as follows: ; in, ; In the formula, Indicates the first The inertial matrix of an autonomous vehicle; This indicates the vehicle's equivalent mass, expressed in kg. The yaw moment of inertia represents the vehicle's rotation about its vertical axis around its center of mass, expressed in kg·m. 2 ; Indicates application to the first The total generalized control input of an autonomous vehicle; and Representing the inertial coordinate system , Equivalent control force in direction, measured in N; It represents the equivalent heading control moment about the vertical axis, in N·m. This represents the diagonal matrix construction operator.

[0026] Combining all the autonomous vehicle variables in sequence, we get: ; ; ; In the formula, These represent the cluster generalized coordinates, cluster generalized velocity, and cluster generalized acceleration, respectively. Represents the cluster inertia matrix; This represents the combined vector of the total generalized control inputs for all autonomous vehicles. The model uses constant mass and constant heading rotational inertia in the inertial coordinate system, so the inertial matrix is ​​a constant diagonal matrix; vehicle rolling resistance, tire friction, actuator errors and other unmodeled factors are compensated by the underlying drive control module, or treated as model uncertainties and external disturbances.

[0027] In this embodiment, The six driverless vehicles use the same dynamic parameters: ; therefore, ; Step 2: Set the initial state of the autonomous vehicle, reference trajectory, reference attraction points, and obstacle environment; Generally, The initial planar positions of the autonomous vehicles are as follows: ; In the formula, This represents the initial planar position matrix of the autonomous vehicle; Indicates the first The initial planar position of the autonomous vehicle; and These represent the initial x-coordinate and initial y-coordinate, respectively, both in meters (m).

[0028] In the environment The combination of the center positions of the static obstacles is: ; In the formula, A matrix representing the positions of the obstacle centers; Indicates the first The center position of each obstacle; and The x and y coordinates of the center of the obstacle are respectively represented in meters. Indicates the obstacle number.

[0029] The reference trajectory is represented as a continuously differentiable function The reference attraction point's x-coordinate is set based on the autonomous vehicle swarm's task progress, current location, or preset operating patterns. Then the current reference attraction point and its velocity are: ; ; In the formula, Represents the reference trajectory function; This represents the trajectory advancement function set according to the task progress; This represents the current reference attraction point position vector, in meters (m). This indicates the velocity at the reference attraction point, in m / s. Indicates the reference trajectory function with respect to The first derivative of . For any given time, These are the specific two-dimensional coordinates on the reference trajectory; for the entire control process, It is a two-dimensional vector function that changes with time.

[0030] In this embodiment, the initial planar positions of the six unmanned vehicles are: ; Set up three static circular obstacles, namely Its center position and physical radius are: ; The five control points used to generate the reference trajectory are: ; Based on the control points, a fourth-order polynomial fit is performed to obtain: ; ; Therefore, the current reference attraction point in this embodiment is: ; Step 3: Calculate the relative state of the vehicles and determine the distance to the controlled vehicle pair; Define a two-dimensional position extraction matrix: ; By the The generalized coordinates of an autonomous vehicle are used to extract its planar position and planar velocity: ; In the formula, Represents a two-dimensional position extraction matrix; Indicates the first The planar position of the driverless vehicle, in meters; Indicates the first The planar velocity of an autonomous vehicle, expressed in m / s.

[0031] No. The number of driverless cars compared to the first The relative positions, relative speeds, and actual distances of the two driverless vehicles are as follows: ; ; ; In the formula, Indicates vehicle Relative to vehicles The relative position vector, in units of meters; This represents the relative velocity vector between the two vehicles, in m / s. This represents the Euclidean distance between the centers of gravity of the two vehicles, in meters (m). This represents the two-dimensional Euclidean norm.

[0032] No. The number of driverless cars compared to the first The relative positions of the obstacles and the distances to the vehicle obstacles are: ; ; In the formula, Indicates vehicle Compared to the first The relative position vectors of the centers of the obstacles, in meters; Indicates vehicle Center of mass to the first The distance between the centers of the obstacles, in meters.

[0033] In this embodiment, the distances to the three obstacles are as follows:

[0034] ; Based on formation configuration, communication topology, or mission requirements, select vehicle pairs that require distance adjustment to form a set of distance-controlled vehicle pairs: ; In the formula, This represents the set of controlled vehicle pairs at a distance; Indicates the number of controlled vehicles; Indicates the first A group of controlled vehicles. The neighboring vehicles used in the artificial potential field calculation can be determined based on the sensing range, while distance homeomorphic control only targets... The vehicles in the process are subject to implementation.

[0035] This embodiment uses a ring distance constrained topology: .

[0036] Step 4: Construct the induced vector of the synthetic artificial potential field and its time derivative; Based on the reference attraction point, the state of adjacent vehicles, and the state of obstacles, construct the first... The combined artificial potential field induced vector for the autonomous vehicle. The reference trajectory serves as: ; In the formula, Indicates the reference trajectory for the first The induced planar motion vector generated by an autonomous vehicle, in m / s; Indicates the gain of the reference trajectory, in units of ; This indicates the positional deviation from the vehicle's current position to the current reference attraction point, expressed in meters (m).

[0037] Other driverless cars The collaborative effect of the autonomous vehicles is as follows: ; In the formula, Indicates other driverless cars to the first The total cooperative guidance vector generated by the autonomous vehicles, in m / s; This indicates the gain when the vehicle is close to the lower limit region. This indicates the effect gain when the vehicle distance exceeds the upper limit area; This indicates the minimum safe distance between vehicles, expressed in meters (m). This indicates the upper limit of the reference distance for vehicle cooperation, in meters. Indicates when Time to take Otherwise, a non-negative truncation operator that takes 0; This represents the relative position vector between vehicles.

[0038] All obstacles to the first The environmental impact of driverless vehicles is as follows: ; In the formula, Indicates all obstacles to the first... The total environmental guidance vector generated by the autonomous vehicle, in m / s; Indicates the gain due to the environmental effect of obstacles; This represents the minimum permissible distance between the center of a vehicle obstacle and the center of the obstacle, in meters (m). This indicates the obstacle detection distance, in meters (m). This represents the relative position vector from the center of the obstacle to the vehicle. This includes obstacle radius, vehicle equivalent external dimensions, and preset safety margin.

[0039] when When, the effect of the corresponding obstacle is zero; when When, the corresponding obstacle's effect is activated; when At that time, the effect of the corresponding obstacle increases.

[0040] Adding the three types of effects together, we get: ; In the formula, Indicates the first The combined artificial potential field induced vector of an unmanned vehicle, in m / s; and They represent their positions in the inertial coordinate system, respectively. , Components in direction.

[0041] Find its time derivative: ; in, This represents the first-order time derivative of the synthesized induced vector, in m / s. 2 ; and Differentiate time term by term according to the above vehicle-cooperative action equation and obstacle-environment action equation. For any differentiable interval of action, the product derivative is as follows: ; ; Among them, static obstacles satisfy At the switching point of the truncation function, a continuous smooth approximation or one-sided derivative calculation is used.

[0042] In this embodiment, the parameters are set as follows: ; ; Step 5: Calculate the desired heading and its rate of change based on the induced vector of the integrated artificial potential field; Specifically, the expected heading angle is: ; In the formula, Indicates the first The expected heading angle of an autonomous vehicle, in rad; Represents the arctangent function in the four quadrants; and The integrated induced vector components from step four are used.

[0043] when At that time, the desired heading angular velocity is: ; In the formula, This represents the expected rate of change of heading angle, in rad / s. and This represents the first-order time derivative of the two components of the synthetic induced vector; This represents a preset threshold to prevent the magnitude of the integrated induced vector from approaching zero. When At the same time, maintain the desired heading angle from the previous control cycle.

[0044] Step Six: Construct the heading velocity layer constraints and its acceleration layer form; Regulation No. The heading angle of the autonomous vehicle satisfies a first-order following relationship; ; in, ; In the formula, This represents the extraction matrix of the heading speed of a single vehicle. Let be the actual heading angular velocity of the i-th unmanned vehicle, in rad / s; Let be the actual heading angle of the i-th unmanned vehicle, in rad; This indicates the heading angle following gain, in units of... ; Indicates the first The heading speed of an unmanned vehicle is a right-hand term constrained by a layer constraint, in rad / s.

[0045] Combine all vehicle heading speed layer constraints: ; ; In the formula, This represents the group heading constraint matrix, which is a constant matrix. This represents the right-hand vector of the cluster heading velocity layer constraint.

[0046] right Find the time derivative: ; because It is a constant matrix. ,therefore, ; And the single-vehicle acceleration layer constraint is: ; In the formula, This represents the right-hand vector of the cluster heading acceleration layer constraint. Let be the actual angular acceleration of the i-th unmanned vehicle, in rad / s. 2 ; Indicates the first The target heading angular acceleration of the unmanned vehicle, in rad / s. 2 . Indicates the right-hand term of the velocity layer. This represents the right-hand term of the acceleration layer; the two correspond to different constraint levels.

[0047] In this embodiment, ,therefore , .

[0048] Step 7: Construct a one-sided logarithmic homeomorphic variable for vehicle distance and set the target formation distance; Since the minimum safe distance between vehicles is essentially a one-sided constraint, it only requires that the actual distance between vehicles remain above the safe boundary. If the vehicle distance is forced to a fixed equation in the long term, even if the vehicles are already within the safe range, it will still restrict the relative movement between vehicles, potentially compressing the space required for vehicles to avoid obstacles, pass through restricted areas, or adjust formation configurations. Therefore, this invention uses a one-sided logarithmic homeomorphism to describe the minimum safe distance between vehicles, so that the vehicle distance only needs to be kept above the minimum safe boundary, without needing to be forced to a fixed distance in the long term. This preserves the space for vehicles to avoid obstacles, pass through restricted areas, and adjust formation configurations.

[0049] Furthermore, some control methods handle maintaining the minimum safe distance between vehicles and tracking the target formation distance separately. When the target formation distance changes over time or approaches the minimum safe boundary, it is necessary to switch or coordinate between multiple distance control targets, which may cause the vehicle distance to overshoot or even approach the minimum safe boundary for a short time.

[0050] To this end, the present invention expresses the minimum safety boundary between vehicles and the target formation distance in the same homeomorphic variable, so that vehicles maintain the minimum safety distance while tracking the time-varying formation distance, thereby reducing the switching and repetitive coordination between safety distance control and target distance tracking.

[0051] Specifically: For any ,structure: ; In the formula, Indicates vehicle to The unilateral log-homeomorphic variable is a dimensionless quantity; This represents the zero-point adjustment parameter of the vehicle pair, in meters. -1 ; This indicates the actual distance between vehicles, in meters (m). Indicates the minimum safe distance between vehicles; This indicates the margin of vehicle distance relative to the minimum safety boundary; This represents the natural logarithm function.

[0052] The following satisfy the mapping relationship: , The initial state should satisfy .when hour: ; therefore, ; In the formula, This represents the target formation distance corresponding to the zero point of the homeomorphic variable, in meters. This target distance is not an additional fixed distance equality constraint, but rather the corresponding position of the zero point of the homeomorphic variable in the actual distance variable.

[0053] definition: ; In the formula, and Let these represent the first and second time derivatives of the homeomorphic parameters, respectively, with units of and . and .

[0054] In this embodiment, for all set up: ; therefore, ; Step 8: Establish the relationship between vehicle distance, homeomorphic variables, and cluster generalized acceleration; Depend on Take the first derivative with respect to the vehicle distance: ; In the formula, ; This represents the first-order rate of change of vehicle distance, in m / s. This indicates that the two vehicles are gradually moving away from each other. This indicates that the two vehicles are gradually approaching each other.

[0055] The gradient of the vehicle distance with respect to the cluster's generalized coordinates is: ; In the formula, Indicates the vehicle distance gradient; Located in the vehicle The corresponding three generalized coordinate positions; Located in the vehicle The corresponding position; the element at the corresponding position for the other vehicles is zero.

[0056] Differentiate again: ; In the formula, This represents the second-order rate of change of vehicle distance, in m / s. 2 ; This represents the direct contribution of the cluster's generalized acceleration to the second-order rate of change of distance; This represents the geometric term resulting from the change in the direction of the relative velocity of the vehicles, with units of m / s. 2 .

[0057] The first derivative of a homeomorphic variable is: ; Differentiate again: ; In the formula, and Let these represent the first and second time derivatives of the homeomorphic variables, respectively, with units of seconds. -1 and s -2 In the second derivative of a homeomorphic variable, only It includes the generalized acceleration of the cluster to be determined, and the other terms can be calculated from the current vehicle state and homeomorphic parameters.

[0058] In this embodiment, m, and ,therefore, ; ; Step 9: Define the second-order stable dynamics of homeomorphic variables; For any ,Regulation: ; Right now ; In the formula, Represents the homeomorphic variable damping gain, in units of s. -1 This is used to suppress oscillations during vehicle distance adjustment. Represents the homeomorphic variable recovery gain, in seconds. -2 Used to drive Approaching zero. From We can obtain: ; In this embodiment, , ,therefore, ; Step 10: Calculate the basic acceleration constrained by the heading acceleration constraint; According to step six The basic acceleration satisfying the heading constraint is obtained using Udwadia–Kalaba constrained dynamics: ; In the formula, This represents the basic generalized acceleration that satisfies the heading acceleration constraint; This represents the negative first power of the cluster inertia matrix; Represents the Moore-Ponros generalized inverse of the mass-weighted heading constraint matrix; superscript This represents the Moore-Penrose generalized inverse.

[0059] Cluster generalized acceleration is represented as: ; In the formula, Represents the identity matrix; This indicates the auxiliary input to be requested; Let A represent the null-space projection matrix of the heading constraint. Let A represent the cluster heading constraint matrix, which is obtained by combining the heading velocity extraction matrices of all unmanned vehicles. It is used to extract the heading angular velocities of each vehicle in the generalized velocity of the cluster. Since A is a constant matrix, after taking the time derivative of the heading velocity layer constraint, the same matrix A is used to establish the cluster heading acceleration layer constraint. The Moore-Ponros generalized inverse of the cluster heading constraint matrix A is used to construct... ;in, This represents the null-space projection matrix of the heading constraint, ensuring that the auxiliary input projected through this matrix does not alter the already satisfied heading acceleration constraints. (Superscript "...") " represents the Moore-Penrose generalized inverse, i.e. Describe the Moore-Penrose generalized inverse of A, which is " " is the superscript symbol for the generalized inverse of a matrix and does not represent ordinary addition operations.

[0060] Verification shows that: ; Therefore, auxiliary input The projection will not change the already satisfied heading acceleration constraint. In this embodiment, , , .

[0061] Step 11: Convert the vehicle distance homeomorphism into an auxiliary input equation and obtain the auxiliary input; Step 10: ; Substituting the second derivative of the homeomorphic variable from step eight: ; Make it satisfy step nine The results are as follows: ; Define the necessary matrix quantities: ; ; In the formula, Indicates auxiliary input for vehicle The action coefficient of the homeomorphic second-order dynamics; This represents the second-order control variable of the vehicle to the target homeomorphism, in units of s. -2 Therefore, a single controlled vehicle edge satisfies: ; All Controlled vehicle side-by-side combination: ; get ; In the formula, Represents the distance control matrix for clustered vehicles; This represents the homeomorphic control target vector of all controlled vehicle edges. When When the auxiliary input equations are compatible; when When the row order is full, this condition is automatically met.

[0062] Because some multi-vehicle control methods separately set up control modules for heading adjustment, inter-vehicle distance adjustment, formation maintenance, and obstacle avoidance, the simultaneous control effects of different modules may cancel each other out or repeatedly correct each other, causing heading fluctuations, inter-vehicle distance oscillations, or frequent switching between maintaining formation and avoiding obstacles. Therefore, this invention calculates the auxiliary input for vehicle distance adjustment within the heading constraint null space, ensuring that adjusting the formation distance does not change the already satisfied heading constraints, thereby reducing the mutual interference between formation distance adjustment, reference trajectory following, and obstacle avoidance. In this embodiment, the auxiliary input is: ; In the formula, Indicates the final distance adjustment auxiliary input; This represents the minimum norm solution that satisfies the vehicle distance homeomorphism dynamics; The null projection matrix representing the vehicle distance control matrix; The directional auxiliary target of the integrated artificial potential field is represented. Here, H represents the cluster vehicle distance control matrix, which is formed by combining the auxiliary input coefficients of each controlled vehicle pair row by row, and is used to represent the overall relationship between the cluster auxiliary input and the second-order dynamics of the homeomorphic variables of all controlled vehicle pairs. Denotes the Moore-Ponros generalized inverse of the cluster vehicle distance control matrix H, where This represents the minimum norm solution that satisfies the vehicle distance homeomorphism dynamic equation. Let represent the null space projection matrix of the vehicle distance control matrix H. h represents the homeomorphic control target vector for all controlled vehicle pairs, formed by sequentially combining the second-order homeomorphic control variables corresponding to each controlled vehicle pair, and together with the cluster vehicle distance control matrix H, constitutes the auxiliary input equation Hμ=h. (Superscript "Hμ" is used to indicate the superscript "Hμ" in the original text.) "This represents the Moore-Penrose generalized inverse, Let H be the Mohs-Penrose generalized inverse.

[0063] In addition, the The directional assistance target for the autonomous vehicle is: ; In the formula, This indicates the plane velocity-assisted adjustment gain, measured in seconds (s). -1 ; and These represent the deviations between the vehicle's actual planar velocity and the velocity induced by the integrated artificial potential field; the third component is set to 0, indicating that the auxiliary target does not directly set the heading angular acceleration.

[0064] Verification ; Therefore, the first term realizes the vehicle's distance homeomorphic dynamics, while the second term does not change the distance homeomorphic dynamics and utilizes the remaining degrees of freedom to make the vehicle's planar velocity continue to approach the induced vector of the integrated artificial potential field.

[0065] In this embodiment, , ,therefore , , ,and .

[0066] The final generalized acceleration of the cluster is: ; Step 12: Obtain and apply the total generalized control input to each autonomous vehicle; according to ,get: ; In the formula, This represents the total generalized control input of the autonomous vehicle cluster. It is then broken down by vehicle: ; Will Send to the The underlying drive control module of the autonomous vehicle, which controls... , and This is converted into drive commands for wheel torque, wheel speed, steering angle, or corresponding actuators. and Used to change the speed of the vehicle's planar motion. Used to change the vehicle's heading, enabling the unmanned vehicle to adjust its heading and speed according to the integrated guidance direction, and to maintain a safe distance between vehicles, track the set formation spacing, and avoid obstacles during movement.

[0067] After adopting the above method, the actual movement trajectory of the six vehicles under the fixed formation spacing condition is as follows: Figure 2 As shown, the distance changes between six groups of adjacent vehicles are as follows: Figure 3 As shown, six unmanned vehicles navigated around three obstacles along a reference trajectory. The distance between the six controlled vehicles remained above the minimum safe distance of 2.0 m and approached the fixed target distance of 4.0 m.

[0068] This embodiment also discloses a generalized Udwadia-Kalaba control device for variable formation constrained unmanned ground swarms. This device includes: a generalized acceleration construction module for the swarm, a dynamics model construction module, an input module, and a low-level drive control module. Specifically: A generalized acceleration building block for the cluster, based on Udwadia–Kalaba constrained dynamics, is used to design the cluster's generalized acceleration. The dynamics model building module is used to equate the autonomous vehicle to a planar three-degree-of-freedom fully driven rigid body and establish a dynamics model: The input module is used to input... Segmented by vehicle ; and the underlying driver control module is used to... , and Vehicle control is achieved by converting the torque, speed, steering angle, or corresponding actuator drive commands into wheel torque, wheel speed, steering angle, or other parameters.

[0069] It should be noted that the above modules can be functional modules or program modules, and can be implemented through software or hardware. For modules implemented through hardware, the above modules can reside in the same processor; or the above modules can be located in different processors in any combination.

[0070] The information interaction and execution process between the above modules are based on the same concept as the method embodiments of this application. They are devices corresponding to the generalized Udwadia-Kalaba control method for variable formation constrained unmanned ground clusters. All implementation methods in the above method embodiments are applicable to the embodiments of this device. For details on its specific functions and the resulting technical effects, please refer to the method embodiment section. It will not be repeated here.

[0071] Example 2

[0072] Some formation control methods based on fixed expected distance error require resetting the target distance function, error expression, or control parameters when switching from fixed formation to periodic or gradual pitch changes. This may increase control calculations during mission switching and cause sudden distance changes or discontinuous control commands during formation deployment, retraction, or periodic changes. Therefore, this embodiment addresses this by changing the homeomorphic zero-point adjustment parameters. Set a fixed, periodically varying, or gradually varying target formation distance to maintain the same control calculation structure in different formation distance tasks, thereby reducing controller switching and the resulting distance abrupt changes.

[0073] This embodiment uses the same unmanned vehicle dynamics model, initial position, obstacle environment, integrated artificial potential field induced vector construction method, heading constraints, distance homeomorphic variables, and total generalized control input solution method as Embodiment 1. The difference is that this embodiment uses periodically varying homeomorphic zero-point adjustment parameters. ; In the formula, Indicating vehicle pairs under periodically varying formation spacing conditions The homeomorphic zero-point adjustment parameter, in m. -1 ; Indicates control time, in seconds; This represents the angular frequency, measured in rad / s, corresponding to a variation period of 20 s.

[0074] Its first and second time derivatives are: ; ; Will , and Substituting the general formulas from steps seven, eight, and eleven of Example 1, we obtain the target formation distance:

[0075] After adopting the above control method, the actual movement trajectory of the six vehicles is as follows: Figure 4 As shown, the distance changes between six groups of adjacent vehicles are as follows: Figure 5 As shown, the vehicle distance can continuously track the periodically changing target formation distance and maintain a minimum safe distance of 2.0 m during pitch changes.

[0076] Example 3

[0077] This embodiment uses the same unmanned vehicle dynamics model, initial position, obstacle environment, integrated artificial potential field induced vector construction method, heading constraints, distance homeomorphic variables, and total generalized control input solution method as Embodiment 1. The difference is that this embodiment uses asymptotically varying homeomorphic zero-point adjustment parameters: ; In the formula, This indicates the vehicle pair under the condition of gradually changing formation spacing. The homeomorphic zero-point adjustment parameter, in m. -1 ; Represents the natural constant; 0.08s -1 This represents the exponential convergence rate.

[0078] Its first and second time derivatives are: ; ; Will , and Substituting the general formulas from steps seven, eight, and eleven of Example 1, we obtain the target formation distance: ; In the formula, the target formation distance is 5.5 m at the initial moment, decreases smoothly over time according to an exponential law, and eventually approaches 3.0 m. After adopting the above control method, the actual trajectory of the six vehicles is as follows: Figure 6 As shown, the distance changes between six groups of adjacent vehicles are as follows: Figure 7 As shown, the vehicle distance can smoothly track the gradually changing target formation distance and remain above the minimum safe distance throughout the pitch change process.

[0079] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms, characterized in that, Includes the following steps: Based on Udwadia–Kalaba constrained dynamics, the generalized acceleration of the cluster is designed as follows: ； In the formula, This represents the basic generalized acceleration that satisfies the heading acceleration constraint; This represents the right-hand vector of the cluster heading acceleration layer constraint. Indicates the first The target heading angular acceleration of the unmanned vehicle; This represents the negative first power of the cluster inertia matrix; The Moore-Penrose generalized inverse of the mass-weighted heading constraint matrix. Represents the identity matrix; Indicates the final distance adjustment auxiliary input; A represents the null space projection matrix of the heading constraint; A represents the heading constraint matrix of the swarm. Represent the Moore-Penrose generalized inverse of the cluster heading constraint matrix A; The autonomous vehicle is equivalent to a planar three-degree-of-freedom fully driven rigid body, and a dynamic model is established: ;in, Represents the cluster inertia matrix; This represents the combined vector of the total generalized control inputs of all autonomous vehicles; Breakdown by vehicle: ； In the formula, Indicates application to the first The total generalized control input of an autonomous vehicle; and These represent the equivalent control forces in the x and y directions of the inertial coordinate system, respectively. This represents the equivalent heading control moment about the vertical axis; Will Send to the The underlying drive control module of the autonomous vehicle, which controls... , and Vehicle control is achieved by converting the torque, speed, steering angle, or corresponding actuator drive commands into wheel torque, wheel speed, steering angle, or other parameters.

2. The generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms according to claim 1, characterized in that: The final distance adjustment auxiliary input Designed as follows: ； In the formula, Let H represent the minimum norm solution that satisfies the vehicle distance homeomorphism dynamics, and let H represent the cluster vehicle distance control matrix. H represents the Moore-Penrose generalized inverse of the cluster vehicle distance control matrix H; h represents the homeomorphic control target vector of all controlled vehicle pairs; The null projection matrix representing the vehicle distance control matrix; This indicates the auxiliary target in the direction of the integrated artificial potential field.

3. The generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms according to claim 2, characterized in that: The integrated artificial potential field direction auxiliary target The design method is as follows: Definition of the first The directional assistance target for the autonomous vehicle is: Then, let ; In the formula, Indicates the gain of planar velocity-assisted adjustment; and These represent the deviations between the vehicle's actual planar velocity and the velocity induced by the integrated artificial potential field; the third component is set to 0, indicating that the auxiliary target does not directly set the heading angular acceleration.

4. The generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms according to claim 3, characterized in that: The method also includes: Regulation No. The heading angle of the autonomous vehicle satisfies a first-order following relationship: ;in, ; In the formula, Let be the actual heading angle of the i-th unmanned vehicle. Let be the actual heading angular velocity of the i-th unmanned vehicle. Indicates the first The expected heading angle of an autonomous vehicle. This represents the extraction matrix of the heading speed of a single vehicle; Indicates the heading angle following gain; Indicates the first The right-hand side of the heading speed layer constraint for an unmanned vehicle; Combine all vehicle heading speed layer constraints: ; ； In the formula, The constrained matrix for the heading and velocity layer of the swarm is a constant matrix. Represents the right-hand vector of the cluster heading velocity layer constraint; for Find the time derivative: ； because It is a constant matrix. Therefore, we get: ; And the single-vehicle acceleration layer constraint is: ； In the formula, This represents the right-hand vector of the cluster heading acceleration layer constraint; Indicates the first The target heading angle acceleration of an unmanned vehicle.

5. The generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms according to claim 1, characterized in that: The first Expected heading angle of an autonomous vehicle Designed as follows: ； In the formula, Represents the arctangent function in the four quadrants; and These represent its components in the x and y directions of the inertial coordinate system, respectively. when At that time, the desired heading angular velocity is: ； In the formula, This represents the expected rate of change of heading angle; and This represents the first-order time derivative of the two components of the synthetic induced vector; This represents a preset threshold to prevent the magnitude of the integrated induced vector from approaching zero; when At the same time, maintain the desired heading angle from the previous control cycle.

6. The generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms according to claim 5, characterized in that: The method also includes: Constructing a reference trajectory for the first The planar motion induced vector generated by the autonomous vehicle : In the formula, Indicates the gain of the reference trajectory; This indicates the positional deviation from the vehicle's current position to the current reference attraction point; Other driverless cars The collaborative effect of the autonomous vehicles is as follows: ； In the formula, Indicates other driverless cars to the first The total cooperative induced vector generated by the autonomous vehicles; This indicates the gain when the vehicle is close to the lower limit region. This indicates the effect gain when the vehicle distance exceeds the upper limit area; Indicates the minimum safe distance between vehicles; This indicates the upper limit of the reference distance for vehicle cooperation; Indicates when Time to take Otherwise, a non-negative truncation operator that takes 0; Represents the relative position vector between vehicles; All obstacles to the first The environmental impact of driverless vehicles is as follows: ； In the formula, Indicates all obstacles to the first... The total environmental guidance vector generated by the autonomous vehicle; Indicates the gain due to the environmental effect of obstacles; Indicates the minimum permissible distance between the center of a vehicle and an obstacle; Indicates the obstacle detection distance; This represents the relative position vector from the center of the obstacle towards the vehicle; Adding the three types of effects together, we get: ； In the formula, Indicates the first The integrated artificial potential field induced vector of an unmanned vehicle.

7. The generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms according to claim 1, characterized in that: The method also includes: constructing a one-sided log-homeomorphic variable for vehicle distance and setting the target formation distance, specifically including: For any vehicle ,structure: ； In the formula, For the distance to the controlled vehicle pair set, Indicates vehicle to One-sided log-homeomorphic variables; This indicates the zero-point adjustment parameters of the vehicle pair; Indicates the actual distance between vehicles; Indicates the minimum safe distance between vehicles; This indicates the margin of vehicle distance relative to the minimum safety boundary; Represent the natural logarithm function; Satisfying the mapping: ; The initial state should satisfy ;when hour: ， Therefore, ; In the formula, This represents the target formation distance corresponding to the zero point of the homeomorphic variable; When the distance between the vehicles approaches the minimum safety boundary, there is .

8. The generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms according to claim 7, characterized in that: The method also includes: selecting vehicle pairs that require distance adjustment based on formation configuration, communication topology, or mission requirements to form a set of distance-controlled vehicle pairs. ; In the formula, This represents the set of controlled vehicle pairs at a distance; Indicates the number of controlled vehicles; Indicates the first For controlled vehicle pairs, the neighboring vehicles used in artificial potential field calculations can be determined based on the sensing range, while distance homeomorphic control only targets... The vehicles in the process are subject to implementation.

9. The generalized Udwadia–Kalaba control method for variable formation constrained unmanned ground swarms according to claim 7, characterized in that: The method also includes: For any ,Regulation: ; Right now ; In the formula, and Let these represent the first and second time derivatives of the homeomorphic variables, respectively. This represents the homeomorphic variable damping gain, used to suppress oscillations during vehicle distance adjustment. Represents the homeomorphic variable recovery gain, used to drive... Approaching zero; from We can obtain: 。 10. A generalized Udwadia-Kalaba control device for variable formation constrained unmanned ground swarms, applied to the generalized Udwadia-Kalaba control method for variable formation constrained unmanned ground swarms as described in any one of claims 1-9, characterized in that: The device includes; A generalized acceleration building block for the cluster, based on Udwadia–Kalaba constrained dynamics, is used to design the cluster's generalized acceleration. ; The dynamics model building module equates the autonomous vehicle to a planar three-degree-of-freedom fully driven rigid body, and establishes a dynamics model: ;in, Represents the cluster inertia matrix; This represents the combined vector of the total generalized control inputs for all autonomous vehicles; Input module, used to input Segmented by vehicle ; In the formula, Indicates application to the first The total generalized control input of an autonomous vehicle; and These represent the equivalent control forces in the x and y directions of the inertial coordinate system, respectively. This represents the equivalent heading control moment about the vertical axis; And the underlying driver control module, used to... , and Vehicle control is achieved by converting the torque, speed, steering angle, or corresponding actuator drive commands into wheel torque, wheel speed, steering angle, or other parameters.

Images