A multi-aquatic robot control method fusing artificial potential field and affine formation

Abstract

The application discloses a kind of multi-underwater robot control methods of fusing artificial potential field and affine formation, belong to robot control technical field, for the problem that underwater environment safety obstacle avoidance and formation keeping are difficult to consider, first, artificial potential field model is established to generate obstacle avoidance control law;Desired position of second order leader is generated based on first order leader attitude and time-varying parameter using affine transformation matrix, and follower generates affine formation control law according to fixed stress matrix;Then, collision risk is judged, if there is no risk, affine formation control law is directly executed;If there is risk, through null space projection method, obstacle avoidance is taken as high priority task, and affine formation is taken as low priority task, and the low priority control law is projected to the high priority null space to generate joint control law;Finally, it is converted into bottom execution instruction, the application guarantees obstacle avoidance safety, maintains affine configuration to the greatest extent, avoids control discontinuity and formation destruction, and improves multi-AUV cooperative maneuvering capability.

Description

Technical Field

[0001] This invention relates to the field of marine environmental monitoring, specifically a method for collaborative control of multiple underwater robots. Background Technology

[0002] With the increasing demands for tasks such as marine resource development, underwater mapping, environmental monitoring, seabed target search, and collaborative inspection, collaborative operations of multiple Autonomous Underwater Vehicles (AUVs) are gradually becoming an important technical means to improve mission efficiency, expand perception range, and enhance system reliability. Compared to single AUV operations, multi-AUV formations can achieve a larger operational coverage and stronger mission redundancy through distributed collaboration, and therefore have attracted widespread attention. However, existing traditional rigid formations usually rely on fixed distances, fixed angles, or fixed relative position constraints. When the formation passes through narrow channels, complex obstacle areas, or performs complex maneuvers, the fixed configuration is difficult to continuously adjust according to environmental constraints, easily leading to insufficient environmental adaptability. Furthermore, redesigning control parameters or adjacency relationships is often required during formation reconfiguration.

[0003] Affine formation control methods utilize affine transformations such as translation, rotation, scaling, and shearing on the nominal configuration, enabling continuous deformation of the formation while maintaining its overall structural relationships. This approach is suitable for scenarios involving narrow waterways, overall turning, and configuration reconfiguration. However, affine formation control alone struggles to directly address collision avoidance in complex obstacle environments. Artificial potential field methods, due to their simple mathematical form, intuitive physical meaning, and good real-time performance, are widely used for local obstacle avoidance control of underwater robots. They typically utilize the target's attraction and the obstacle's repulsion to generate control commands, guiding the robot towards the target and away from the obstacle. However, when directly applying artificial potential fields to multi-AUV formations, each AUV often avoids obstacles independently based on its local potential field, easily leading to individual robot dispersion and formation disruption.

[0004] Existing solutions that simply weight and superimpose obstacle avoidance control and formation control or hard-switch between them are insufficient to effectively coordinate the conflict between safe obstacle avoidance and formation maintenance, potentially leading to problems such as insufficient obstacle avoidance, discontinuous control, abrupt trajectory changes, or slow formation recovery. Therefore, to address the need for multiple AUVs to both safely avoid obstacles and maintain coordinated formation in complex underwater environments, there is an urgent need for a cooperative maneuver control method that can effectively integrate artificial potential field obstacle avoidance and affine formation control. This method should prioritize obstacle avoidance safety while maintaining or restoring a pre-defined affine formation configuration as much as possible, enabling safe, continuous, and coordinated maneuvering of multiple AUVs in complex obstacle environments.

[0005] CN116027796A A Multi-Autonomous Underwater Robot Formation Control System and Method

[0006] This invention discloses a multi-autonomous underwater robot (AUV) formation control system and method. The formation control system includes a host computer and an AUV formation system containing several AUVs. The host computer is located on a mother ship or on shore and is used to receive information sent by each AUV before formation begins, and to designate a lead AUV and follower AUVs based on the received information after the formation task begins. During the formation task execution, the host computer periodically sends formation control commands to the lead AUV. The AUV formation system communicates with the host computer, sending position information and receiving control commands from the host computer to complete the corresponding formation tasks. However, the use of a hard-switching scheme between formation control and obstacle avoidance control may lead to control discontinuity issues.

[0007] CN120103847A A Method for Underwater Robot Formation Control Based on Virtual Structure and Cost Equilibrium Method

[0008] This invention discloses an underwater robot formation control method based on virtual structure and cost equalization, including the following steps: designing a multi-formation library and importing it into the main AUV; establishing a body coordinate system centered on the main AUV and generating a three-dimensional virtual matrix, introducing virtual AUVs between the auxiliary AUVs and their corresponding virtual mass points; selecting the corresponding formation from the multi-formation library of the main AUV according to task requirements; calculating the cost of each member to the target point based on the selected formation and the current state of the formation members using a cost model, obtaining a cost matrix; allocating tasks to the target point using the cost equalization method; and reconstructing the formation by tracking the motion trajectory of the virtual AUVs from the auxiliary AUVs. However, the method does not consider the requirement of rigid formation constraints during obstacle avoidance, and formation changes are limited by the number of formations in the formation library.

[0009] In existing technologies, multi-AUV cooperative control methods are mostly concentrated on traditional rigid formations, lead-follow formations, or single obstacle avoidance control methods. A unified cooperative control scheme that takes into account adaptability to complex environments, formation configuration maintenance, and safe obstacle avoidance has not yet been fully formed. Traditional rigid formations usually rely on fixed distances, fixed angles, or fixed relative position constraints. In narrow channels, areas with dense obstacles, or complex maneuvers, it is difficult to continuously adjust the formation according to environmental constraints, which easily leads to problems such as poor maneuverability, insufficient adaptability, and the need to redesign control parameters and adjacency relationships when reconfiguring the formation.

[0010] While affine formation control can achieve continuous deformation of formation configuration through transformations such as translation, rotation, scaling, and shearing, enabling multiple AUVs to possess a certain degree of maneuverability, existing affine formation methods mostly focus on formation transformation and configuration maintenance, lacking an effective coordination mechanism with obstacle avoidance tasks. In the presence of static obstacles, dynamic obstacles, or collision risks between AUVs, the prioritization of formation constraints may lead to untimely obstacle avoidance responses. Meanwhile, although artificial potential field methods have advantages such as simplicity, good real-time performance, and suitability for local obstacle avoidance, when directly applied to multi-AUV formations, they easily cause each AUV to independently avoid obstacles based on its local potential field, resulting in individual dispersion, formation disruption, and degradation of cooperative relationships. Existing schemes that simply weight and superimpose obstacle avoidance control and formation control or hard-switch between them also struggle to effectively handle the priority conflict between safe obstacle avoidance and formation maintenance, potentially leading to insufficient obstacle avoidance, discontinuous control, abrupt trajectory changes, or slow formation recovery.

[0011] Therefore, there is an urgent need for a multi-AUV cooperative control method that can effectively integrate artificial potential field obstacle avoidance and affine formation control. Under the premise of prioritizing obstacle avoidance safety, it can maintain or restore the preset formation configuration to the greatest extent, thereby improving the safety, coordination and engineering application value of multi-AUV systems in complex underwater environments. Summary of the Invention

[0012] This invention aims to address the problem that existing multi-underwater robot formation control methods struggle to balance safe obstacle avoidance and formation maintenance in complex obstacle environments. To address shortcomings such as insufficient adaptability of rigid formations, the tendency for individual robots to disperse when using artificial potential fields for obstacle avoidance, and control conflicts and poor continuity when simply superimposing or switching between obstacle avoidance and formation control, this invention proposes a multi-underwater robot control method that integrates artificial potential fields and affine formations. This method utilizes artificial potential fields to generate obstacle avoidance control laws and coordinates them with affine formation control laws through a null space projection mechanism. While prioritizing obstacle avoidance safety, it enables multiple underwater robots to maintain or restore a preset affine formation configuration to the greatest extent possible, safely achieving coordinated maneuvers such as translation, rotation, scaling, and shearing within the formation. Furthermore, this invention improves the system's feasibility, scalability, and collaborative control capabilities in complex task environments by clearly designing the functional division and control interfaces of the first-order leader, second-order leader, and followers.

[0013] The specific technical solution is as follows: This invention provides a control method for multiple underwater robots that integrates artificial potential fields and affine formations, including the following steps:

[0014] S1. Establish an artificial potential field environment model and generate an obstacle avoidance control law consisting of target attraction and obstacle repulsion to guide each underwater robot away from the obstacle and towards the target:

[0015] Based on the sensors mounted on each AUV, the position information of obstacles and surrounding robots is acquired in real time, and an artificial potential field environment model is constructed in the motion space. This model includes the gravitational potential field generated by the target point and the repulsive potential field generated by the obstacle.

[0016] Design a gravitational potential field: The magnitude of the gravitational potential field is negatively correlated with the distance from the AUV to the target point. The magnitude of the gravity is the negative gradient of the gravitational potential field, and its direction points towards the target point, which is used to guide the AUV to approach the target.

[0017] Design of the repulsive potential field: The magnitude of the repulsive potential field is negatively correlated with the distance between the AUV and the obstacle. The closer the obstacle is to the AUV, the greater the potential energy; the farther the obstacle is, the smaller the potential energy. When the obstacle exceeds the maximum detection range of the AUV sensor, the repulsive potential energy is zero. At the same time, to prevent the AUV from being unable to stay at the target point due to the repulsive force caused by the obstacle being too close to the target point, a distance factor between the AUV and the navigation target point is introduced into the repulsive potential field function to solve the target unreachability problem. The magnitude of the repulsive force is the negative gradient of the repulsive potential field, and its direction is from the obstacle to the AUV.

[0018] Generate obstacle avoidance control law: Calculate the vector sum of the gravitational force from the target and the repulsive force from the obstacle on the AUV to obtain the resultant force. Based on this resultant force and the control period, calculate the velocity component and heading component of the AUV in the Cartesian coordinate system to avoid the obstacle, which are used as the output of the obstacle avoidance control law.

[0019] S2. Using the affine transformation matrix, the desired position of the second-order navigator is generated based on the real-time attitude and time-varying affine parameters of the first-order navigator. Each follower then generates an affine formation control law for tracking the nominal configuration based on a fixed stress matrix.

[0020] The system adopts a hierarchical navigation structure, which includes a first-order navigator, multiple second-order navigators, and multiple followers. The first-order navigator uses the artificial potential field method to track the preset reference track while avoiding obstacles, providing the entire formation system with real-time position and heading angle attitude references.

[0021] Generating the desired position of the second-order navigator: The affine transformation matrix is ​​decomposed into a rotation matrix, a scaling matrix, a shearing matrix, and a translation vector; based on the real-time position and heading angle of the first-order navigator, combined with the offset of the second-order navigator in the first-order navigator's coordinate system in the initial nominal configuration, and the pre-set time-varying scaling factor and shearing factor, the desired position of the second-order navigator is calculated; the second-order navigator uses an artificial potential field method to track this desired position and avoid obstacles, thereby ensuring that the real-time formation of the navigator group strictly maintains the affine transformation of the initial nominal configuration;

[0022] Affine Formation Control Law for Followers: The communication topology of multiple AUVs is abstracted as an undirected graph, and a scalar stress weight is assigned to each edge in the formation. If the resultant force of the stress weights of all adjacent edges of any node in the formation is zero under the position difference of the corresponding neighboring node, the formation is said to be in stress equilibrium. Each follower calculates the affine formation control quantity based on a fixed stress matrix, using the stress weight scalar and relative position information between itself and its neighboring nodes. Thus, it can automatically track the affine configuration generated in real time by the navigator without having to readjust the control parameters.

[0023] S3. Determine whether there are obstacles or collision risks between robots in each AUV:

[0024] Define the distance between AUVs and other AUVs, as well as the distance between an AUV and the nearest obstacle within its detection range, and preset a collision safety threshold;

[0025] A collision risk is determined to exist when the distance between an AUV and other AUVs or the distance between an AUV and the nearest obstacle is less than the safety threshold; otherwise, no collision risk is determined to exist.

[0026] S4. If there is no collision risk, the affine formation control law is directly executed to maintain the formation configuration; if there is a collision risk, the obstacle avoidance control law is fused as a high-priority task and the affine formation control law as a low-priority task using the null space projection method to generate a joint control law.

[0027] When no collision risk is determined, the system only executes the affine formation control law to maintain the formation configuration;

[0028] When a collision risk is determined, the artificial potential field collision avoidance task, which involves navigation safety, is identified as a high-priority task, while the affine formation control task is identified as a low-priority task.

[0029] To address the singularity problem in control, a virtual point is selected directly in front of the AUV as a control reference, and a kinematic transformation relationship is established between the coordinates of this virtual point and the AUV control input. This transformation matrix is ​​a full-rank invertible matrix.

[0030] Define the high-priority obstacle avoidance task function as the distance between the virtual point and the obstacle. Take its derivative and substitute it into the kinematic equation of the virtual point to obtain the Jacobian matrix of the high-priority obstacle avoidance task. Define the low-priority affine formation task variable as the coordinates of the virtual point. Take its derivative to obtain the Jacobian matrix of the low-priority affine formation task.

[0031] Based on the null space behavior formula, the pseudo-inverse matrices of the two Jacobian matrices are calculated respectively, and the null space projection matrix of the high-priority task vector is constructed. The low-priority affine formation control vector is projected onto the null space of the high-priority obstacle avoidance control vector, and the joint control law is calculated.

[0032] The desired velocity, expressed in Cartesian coordinates by the joint control law, is converted into the desired linear and angular velocities of the AUV through the inverse of the kinematic transformation matrix, which serve as the final output of the joint control law. This ensures that the AUV prioritizes obstacle avoidance maneuvers when encountering obstacles, while maintaining formation as much as possible under safe conditions.

[0033] S5. Convert the desired velocity and angular velocity output from the joint control law or affine formation control law into thrust and rudder angle commands for the AUV thrusters, enabling coordinated maneuver control of multiple underwater robots in complex obstacle environments:

[0034] When no collision risk is detected, the desired velocity and angular velocity output by the affine formation control law are directly converted into low-level commands; when a collision risk is detected, the desired velocity and angular velocity output by the joint control law are converted into low-level commands.

[0035] The bottom-level controller generates thrust and rudder angle control commands based on the desired speed and angular velocity, combined with the thruster model, servo model, or PID speed closed-loop controller. This decouples the upper-level formation obstacle avoidance decision-making from the lower-level execution control, thereby enabling cooperative maneuver control of multiple underwater robots in complex obstacle environments. The cooperative maneuver control method supports continuous deformation of the formation configuration through translation, rotation, scaling, and shearing, and is suitable for cooperative maneuver of multiple AUVs in narrow waterways, areas with dense obstacles, and dynamic mission scenarios.

[0036] The advantages of this invention are: it unifies and integrates artificial potential field obstacle avoidance control and affine formation control through a null-space projection mechanism, forming a comprehensive control law that balances obstacle avoidance safety and formation maintenance. Unlike existing methods that simply weight and superimpose or hard-switch obstacle avoidance and formation terms, this invention prioritizes the obstacle avoidance task, projecting affine formation control into the null space of the obstacle avoidance task. This maximizes formation maintenance and affine maneuvering without weakening obstacle avoidance performance. This method not only avoids problems such as individual robot dispersion, formation disruption, and control discontinuity during local obstacle avoidance for multiple underwater robots, but also supports translation, rotation, scaling, and shearing adjustments of formation configuration in complex obstacle environments. Furthermore, through a hierarchical cooperative control structure of first-order leader, second-order leader, and followers, it improves the adaptability and deployability of multi-underwater robot systems in narrow channels, densely obstacle-prone areas, and task-constrained scenarios. Attached Figure Description

[0037] Figure 1A schematic diagram of the resultant force acting on an AUV;

[0038] Figure 2 This is a diagram illustrating the control mode switching.

[0039] Figure 3 A schematic diagram of the joint control law generation based on null space projection;

[0040] Figure 4 This is the overall logic block diagram of the present invention. Detailed Implementation

[0041] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0042] 1) Overall Approach and System Components

[0043] This invention relates to a cooperative formation system composed of multiple underwater robots. The system includes a first-order leader, two or more second-order leaders, and multiple followers. The first-order leader uses an artificial potential field to track a reference trajectory while avoiding obstacles, and provides a real-time attitude reference for the entire formation system. The second-order leaders generate a desired position based on the first-order leader's attitude and time-varying affine parameters, and use the artificial potential field method to track this desired position and avoid obstacles. The followers generate an affine formation control law based on a fixed stress matrix to track this nominal configuration. When a collision risk arises, a null-space projection method is used to fuse the artificial potential field control law and the affine formation control law, projecting the affine formation control law onto the null space of the artificial potential field control law, thereby completing the cooperative maneuver of the entire formation.

[0044] The overall process of this invention includes: first, initializing the environment and formation to determine the initial nominal configuration, stress matrix, safety threshold, and reference trajectory; then, establishing an artificial potential field environment model to calculate the target attraction, obstacle repulsion, and corresponding obstacle avoidance velocities; next, the first-order leader generates the desired position of the second-order leader by combining attitude information and time-varying affine parameters; the followers generate affine formation control laws based on the stress matrix; the system further determines whether there are obstacles or collision risks between robots; if there are no collision risks, affine formation control is executed; if there are collision risks, a joint control law satisfying obstacle avoidance priority is generated for the followers through the null space projection method; finally, the underlying controller converts the desired velocity and angular velocity into underlying control commands such as thruster thrust and rudder angle.

[0045] 2) Potential field environment modeling

[0046] This invention utilizes sensors such as sonar and underwater acoustic communication devices mounted on each AUV to acquire the real-time positions of obstacles and surrounding robots. The navigation target point applies an gravitational potential field to the AUV, while obstacles apply a repulsive potential field to the AUV.

[0047] The gravitational potential field is designed as follows:

[0048]

[0049] in, Indicates the gravitational field gain coefficient; This represents the position coordinates of the underwater robot in the northeast-northeast coordinate system. and target location The distance between them , The magnitude of gravity is the negative gradient of the gravitational field, i.e., the gravitational potential field function. The direction of fastest decrease is expressed as:

[0050]

[0051] The repulsive potential field is another important component in the construction of the potential field function. When an obstacle is within the detection range of the robot's sensors, it will generate a repulsive force. The repulsive force function is affected by the distance between the robot and the obstacle; the farther the obstacle is from the robot, the smaller its potential energy; the closer the obstacle is to the robot, the greater its potential energy. When the robot's repulsive potential energy is zero, it indicates that the obstacle has moved out of the robot's detection range. The potential function of the repulsive potential field is expressed as:

[0052]

[0053] in, This is the repulsive force gain coefficient; The coordinates of the obstacle's location in the northeast coordinate system are: Indicates the distance between the robot and the obstacle. This represents the sensor's maximum detection range. The distance between the robot and the navigation target point; It is an adjustable positive coefficient. This was introduced to prevent the robot from being unable to stop due to repulsive force when it reaches the vicinity of the target point if the obstacle is too close to it, thus solving the problem of the target being unreachable.

[0054] Correspondingly, the repulsive force is the negative gradient of the repulsive potential field, expressed as follows:

[0055]

[0056]

[0057] In the formula, The direction is from the obstacle towards the robot; The direction is determined by the robot pointing towards the target point for navigation.

[0058] Based on the gravitational and force fields defined above, the resultant force field of the robot's motion space can be obtained. The magnitude of the resultant force field acting on the robot is the sum of the repulsive and gravitational fields it experiences, and its resultant force field is:

[0059]

[0060] Therefore, the net force acting on the robot is shown in Figure 1, and its value is:

[0061]

[0062] Under the combined force, the underwater robot The velocity and heading components for avoiding obstacles are calculated as follows:

[0063]

[0064]

[0065] in, It is a control cycle. yes exist direction and The directional component.

[0066] 3) Affine transformation generation of the navigator's nominal configuration

[0067] When a formation changes in a narrow channel or under mission constraints, the affine formation control method has a natural advantage because it does not require adjusting the formation parameters of the followers. The core of this method lies in characterizing the geometric properties and topological relationships of the formation through a specific stress matrix. Within this framework, once the initial nominal configuration is determined, its corresponding stress matrix becomes the foundation of the entire formation control system and remains unchanged during any affine transformation. This means that followers do not need to recalculate control law parameters due to translation, rotation, scaling, or shearing of the formation. One challenge is ensuring that the real-time configuration formed by the navigator is a complete affine transformation of the initial nominal configuration. To address this, this section proposes a hierarchical navigator structure design: the system includes one first-order navigator and multiple second-order navigators. The navigator uses an artificial potential field method to track a preset reference track in real time; the second-order navigators are responsible for tracking the desired position of the second-order navigator, which is calculated in real time by the first-order navigator through an affine transformation matrix.

[0068] In a two-dimensional plane, an affine transformation can be expressed by a rotation matrix. Scaling matrix Shearing matrix and translation vector Common representation. Let the position of the first-order navigator be defined. The heading angle is Second-order navigator in the initial nominal configuration Offset in the first-order navigator vehicle coordinate system The following is given , , Specific mathematical representation:

[0069]

[0070]

[0071] in, , The initial nominal configurations are in the first-order navigator vehicle coordinate system. direction and The time-varying scaling factor of the direction.

[0072]

[0073] in, , The initial nominal configurations are in the first-order navigator vehicle coordinate system. direction and Directional shearing factor.

[0074] Affine matrix Translation vector .but At that moment, the second-order navigator Location Its velocity and angular velocity are determined by time. The answer is obtained by differentiation.

[0075] By pre-setting the scaling factor , With shear factor , The system can dynamically generate second-order navigator motion states that meet mission requirements, thereby ensuring that the real-time formation of the navigator group strictly maintains the affine transformation of the initial nominal configuration. Under this mechanism, when the formation deforms due to environmental or mission requirements, the followers do not need to update their control parameters; they only need to achieve formation tracking based on a fixed stress matrix and local relative information, thus ensuring the stability and smoothness of the entire formation system during configuration transformation.

[0076] 4) Follower Affine Formation Control

[0077] In multi-underwater robot swarms, unidirectional topology is generally abstracted as a directed graph, and bidirectional topology as an undirected graph. Considering... There is space Several underwater robots, among which , Assume that there are One leader, there A follower can be represented by a diagram. To represent the communication topology between the underwater robots, where Represents a set of nodes. Represents the set of edges. Edges Indicates the first The robot can receive from the first Information about the robot. (Number) The set of neighbor nodes of an underwater robot can be represented as The first The location of the underwater robot is marked as follows The basic configuration of the entire underwater robot formation is then: .

[0078] Define formation as If for each edge in the formation Assign a scalar weight In an undirected graph If any node in the formation The stresses of all its adjacent edges satisfy the equation If this condition is met, the entire formation is said to be in a state of stress equilibrium.

[0079]

[0080] Stress can be physically compared to the interaction forces between robots: if Represents underwater robots With underwater robots There is attraction between them; conversely, This indicates an underwater robot. With underwater robots There is a repulsive force between them. For any node The impact it receives from all its neighboring nodes The resultant force can be expressed as .

[0081] The follower uses the constraints of the stress matrix for formation control. The core feature of affine formation is that once the initial nominal configuration and its stress matrix are determined, the follower's control parameters do not need to be readjusted during any translation, rotation, scaling, and shearing transformations. Therefore, during configuration transformations, the follower only needs to use the position information of neighboring nodes to maintain tracking of the target affine configuration.

[0082] For the The affine formation control law for a group of followers is expressed as:

[0083]

[0084] For the desired speed, The desired angular velocity; , , , The calculation method is as follows:

[0085]

[0086]

[0087]

[0088] in, For robots With robots The stress scalar between.

[0089] With this control variable, the follower can automatically track the affine configuration generated in real time by the navigator while maintaining local relative constraints.

[0090] 5) Collision risk assessment and mode switching

[0091] During formation sailing, define AUV With AUV The distance between them AUV The distance between it and the nearest obstacle within its detection range This is the safety threshold for collisions. When... At that time, i.e., AUV The distance between the AUV and other AUVs and the distance between the AUV and obstacles are both greater than its safety threshold. When there is no risk of collision, affine formation control laws are used to maintain formation. And when... At the same time, the artificial potential field control law and the affine formation control law are simultaneously activated and fused using the null space projection method. The mode switching during the heading process is shown in Figure 2.

[0092] 6) Joint control law based on null space projection

[0093] As shown in Figure 3. This involves projecting the control velocity into the null space of the high-priority obstacle avoidance task, resulting in a formation adjustment velocity component that does not affect obstacle avoidance performance at all. It is the velocity vector for controlling the obstacle avoidance range of the artificial potential field. It is a joint control velocity vector. When facing complex obstacle environments, the affine formation takes the affine formation control task and the artificial potential field collision avoidance control as two basic sub-tasks. This represents the desired velocity and angular velocity, i.e., the affine formation control velocity vector. The system first determines this based on... and Collision risk is assessed by determining whether to prioritize collision avoidance control laws based on the distances between AUVs and between AUVs and obstacles. In this scenario, the collision avoidance task using the artificial potential field is prioritized due to its impact on AUV navigation safety, while the affine formation control task is prioritized. Based on the core concept of null-space projection, the low-priority affine formation control vector is projected onto the null space of the high-priority artificial potential field collision avoidance vector. This ensures that the AUV prioritizes obstacle avoidance maneuvers when encountering obstacles, while maintaining formation as much as possible while ensuring safety.

[0094] The joint control law is given below. The mathematical derivation.

[0095] To address the singularity problem in control, the distance directly in front of the AUV is... Select a virtual point Its coordinates :

[0096]

[0097] right By taking the derivative, we obtain the virtual point. Velocity equation:

[0098]

[0099] Among them, matrix Let be the kinematic transformation matrix, and its determinant is... is a full-rank invertible matrix.

[0100] Define the task function for high-priority obstacle avoidance tasks. Distance between the virtual point and the obstacle:

[0101]

[0102] right Differentiate:

[0103]

[0104] in, It is a unit vector, with its direction being the direction of the line connecting the obstacle to the virtual point. The kinematics of the virtual point... Substitute into the above formula:

[0105]

[0106] in, It is the Jacobian matrix for high-priority obstacle avoidance tasks, representing the expected distance-to-distance rate. .

[0107] Define task variables for low-priority affine formation tasks. For virtual point coordinates:

[0108]

[0109] right Differentiate:

[0110]

[0111] in, For a low-priority affine formation task, the Jacobian matrix represents the expected rate of position change. .

[0112] Calculated according to the standard formula of zero-space behavior :

[0113]

[0114] in, and It is the pseudo-inverse of two Jacobian matrices. It is the null projection matrix. Based on the relevant properties of the matrix pseudoinverse, it can be calculated. and :

[0115]

[0116]

[0117]

[0118] Joint control law The calculation method is as follows:

[0119]

[0120] The above derivation is based on the control law of affine formations. and the control law of artificial potential field All are represented in Cartesian coordinates. direction and The derivation is based on the desired velocity in the direction. The output of the control law for affine formations. This represents the desired velocity and angular velocity, therefore it needs to be... Towards The conversion formula is shown below:

[0121]

[0122] The final control output is:

[0123]

[0124] 7) Control output and underlying execution

[0125] When no collision risk is detected, the system directly converts the affine formation control input into the desired velocity and angular velocity commands for the AUVs; when a collision risk is detected, the system outputs the joint control input ξ after null-space projection fusion. Subsequently, the underlying controller determines the desired velocity... and angular velocity By combining the thruster model, servo model, or PID speed closed-loop controller, thruster thrust and rudder angle control commands are generated.

[0126] This hierarchical control structure decouples the upper-level formation-obstacle avoidance decision-making from the lower-level execution control: the upper level is responsible for safety and formation target coordination, while the lower level is responsible for tracking by the execution mechanism, thereby improving the portability of the engineering implementation.

Claims

1. A method for controlling multiple underwater robots that integrates artificial potential fields and affine formations, characterized in that, Includes the following steps: An artificial potential field environment model is established to generate an obstacle avoidance control law consisting of target attraction and obstacle repulsion, so as to guide each underwater robot away from the obstacle and approach the target. Using the affine transformation matrix, the desired position of the second-order navigator is generated based on the real-time attitude and time-varying affine parameters of the first-order navigator, and each follower generates an affine formation control law for tracking the nominal configuration based on a fixed stress matrix. Determine whether there are obstacles or collision risks between robots in each AUV; If there is no risk of collision, the affine formation control law is directly executed to maintain the formation configuration; If there is a risk of collision, the obstacle avoidance control law is fused as a high-priority task and the affine formation control law is fused as a low-priority task through the null space projection method. The low-priority control law is projected onto the null space of the high-priority control law to generate a joint control law, so that each AUV can maintain or restore the preset formation configuration as much as possible while prioritizing obstacle avoidance safety. The desired velocity and angular velocity output from the joint control law are converted into thrust and rudder angle commands for the AUV thrusters, enabling coordinated maneuver control of multiple underwater robots in complex obstacle environments.

2. The method according to claim 1, characterized in that, The construction of the artificial potential field environment model includes: applying an attractive potential field to the AUV from the navigation target point and applying a repulsive potential field to the AUV from the obstacle; wherein, the magnitude of the attractive potential field is negatively correlated with the distance from the AUV to the target point, the magnitude of the repulsive potential field is negatively correlated with the distance from the AUV to the obstacle, and the repulsive potential field is zero when the obstacle is outside the detection range of the AUV sensor.

3. The method according to claim 1, characterized in that, The generation of the obstacle avoidance control law includes: calculating the resultant force on the AUV based on the positions of the target point and the obstacle, which is the vector sum of the target's gravitational force and the obstacle's repulsive force; and calculating the AUV's velocity component and heading component based on the resultant force and the control period, as the output of the obstacle avoidance control law.

4. The method according to claim 1, characterized in that, The affine transformation matrix includes a rotation matrix, a scaling matrix, a shearing matrix, and a translation vector; wherein, the first-order navigator tracks a preset reference track using an artificial potential field method, while the second-order navigator tracks the desired position calculated in real time by the first-order navigator using the affine transformation matrix.

5. The method according to claim 1, characterized in that, The generation of the affine formation control law includes: abstracting the formation into an undirected graph and assigning stress weights to each edge; if the resultant stress of any node on its adjacent edges is zero, the formation is said to be in stress equilibrium; each follower calculates control quantities based on its stress weights and position information with its neighboring nodes to track the affine configuration generated by the leader.

6. The method according to claim 1, characterized in that, The collision risk assessment is based on a preset safety threshold; when the distance between an AUV and other AUVs or the distance between an AUV and the nearest obstacle is less than the safety threshold, a collision risk is determined to exist.

7. The method according to claim 1, characterized in that, The null space projection method includes: defining the high-priority obstacle avoidance task function as the distance between the virtual point and the obstacle, and the low-priority affine formation task variable as the coordinates of the virtual point; calculating the Jacobian matrix and its pseudo-inverse matrix of the two tasks respectively; and projecting the control vector of the low-priority task onto the null space of the high-priority task vector according to the null space behavior formula to obtain the joint control law.

8. The method according to claim 1, characterized in that, The calculation of the joint control law also includes: to avoid singularity problems, selecting a virtual point directly in front of the AUV as a control reference; converting the velocity of the virtual point into the desired velocity and angular velocity of the AUV through a kinematic transformation matrix; and using the converted desired velocity and angular velocity as the final output of the joint control law.

9. The method according to claim 1, characterized in that, When no collision risk is detected, the desired velocity and angular velocity output by the affine formation control law are directly converted into thruster thrust and rudder angle commands; when a collision risk is detected, the desired velocity and angular velocity output by the joint control law are converted into thruster thrust and rudder angle commands; and the underlying control commands are generated through the thruster model, servo model, or PID speed closed-loop controller.

10. The method according to claim 1, characterized in that, The cooperative maneuver control method supports continuous deformation of formation configuration, such as translation, rotation, scaling, and shearing, and is suitable for cooperative maneuver of multiple AUVs in narrow waterways, areas with dense obstacles, and dynamic mission scenarios.

Images