A method for controlling unmanned boat cooperative formation

Abstract

The present invention provides a method for controlling an unmanned boat cooperative formation, which acquires static and dynamic environmental data of the ocean in real time through multiple sensors deployed, performs time synchronization, coordinate conversion, data denoising and data fusion on the acquired data, obtains the total data set after fusion, constructs a three-dimensional grid map, and divides the environment into fixed grid units; generates a real-time optimal path for the unmanned boat formation through a motion planning module, performs obstacle avoidance based on an obstacle avoidance mechanism module and ensures that the unmanned boat can gradually return to the predetermined path after obstacle avoidance; and enables the unmanned boat to stably follow the adjusted path based on a path tracking and control module. The present invention realizes comprehensive environmental perception, and the motion planning module design can adapt to complex environments, the obstacle avoidance mechanism module design improves the flexibility of obstacle avoidance, and the path tracking and control module design ensures that the unmanned boat can stably follow the path without excessive heading deviation due to changes in the external environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned boat intelligent control, and in particular relates to a method for controlling an unmanned boat cooperative formation. Background Art

[0002] In recent years, with the rapid development of unmanned technology and automated control, unmanned surface vehicles (USVs) have gradually become an important research object in the marine field. These autonomous surface vessels are widely used in environmental monitoring, marine exploration and other fields. The autonomous operation capability of unmanned boats not only greatly reduces the danger of marine operations, but also significantly improves operating efficiency and reduces costs. Therefore, research on the performance of unmanned boats in complex marine environments has attracted more and more attention. Early research on unmanned boat technology focused on the autonomous navigation and control system of a single boat. However, single boat operation has obvious limitations in terms of mission complexity and coverage. A single unmanned boat is difficult to handle large-scale, multi-task complex marine operations, such as wide-area marine monitoring, rescue and search, and other tasks. Therefore, with the increase in demand, the coordinated formation control technology of multiple unmanned boats has gradually emerged. Through multi-boat collaboration, the operating range of unmanned boats can be effectively expanded, the robustness and fault tolerance of the system can be enhanced, and the efficiency of task execution can be improved.

[0003] When multiple unmanned boats work together, ensuring that they maintain formation and avoid collision in a complex marine environment is a key challenge. Multi-boat formation control requires solving multiple technical problems, including path planning, collision avoidance decision-making, communication cooperation, and dynamic environment adaptation. Especially in the marine environment, the challenges faced by unmanned boats are not limited to fixed obstacles in the ocean, but also include other dynamic factors such as moving ships, currents, wind and waves. In terms of path planning, how to generate safe and efficient routes for unmanned boats in an uncertain marine environment is a core issue. Although traditional path planning methods perform well in static environments, they often perform poorly when faced with dynamically changing marine environments. On the other hand, the collision avoidance problem is also a core challenge in unmanned boat formations. In an open marine environment, especially in areas where ships frequently move at sea, how to avoid collisions with other ships or obstacles is the key to ensuring the safe operation of unmanned boat formations. In multi-boat formation control, the effectiveness of collaborative operations depends on real-time communication and information sharing between each unmanned boat. Unmanned boats must exchange key data such as position information, speed, and heading in real time through wireless networks to ensure that the formation can move synchronously and quickly adjust the formation shape in the event of an emergency. In addition, the leader-follower formation structure is a common unmanned boat formation control mode. The leader boat is responsible for planning the global path, while the follower boat adjusts its position according to the motion state of the leader boat to maintain a relatively stable formation. However, the existing technology has the following defects:

[0004] 1. Path planning is not accurate enough. The existing path planning algorithm cannot quickly respond to complex changes in a dynamic ocean environment, which affects the mission execution of the unmanned boat and may cause it to deviate from the ideal route.

[0005] 2. Limitations of collision avoidance algorithms. Collision avoidance algorithms based on fuzzy logic react slowly to emergencies and have poor adaptability, and are unable to make timely collision avoidance decisions in high-speed dynamic environments.

[0006] 3. The adjustment of formation shape is not flexible. Most existing formation controls adopt fixed shapes, which are difficult to adapt to changes in complex or narrow waters and lack dynamic adjustment capabilities, resulting in difficulties in formation passage.

[0007] 4. Limited intelligence and adaptability. Although some systems have introduced intelligent algorithms, the intelligence level of existing technologies is limited, and the unmanned boats have insufficient adaptive and autonomous learning capabilities, making it difficult to cope with changes in complex tasks. Summary of the invention

[0008] In view of the above problems, the present invention provides a method for controlling an unmanned boat cooperative formation, comprising the following steps:

[0009] S1, through the deployment of multiple sensors, real-time acquisition of static and dynamic environmental data of the ocean, including obstacles and visual environment changes;

[0010] S2, the data acquired by each sensor are time synchronized, coordinate transformed, denoised and fused to obtain the total data set P after fusion, which contains the complete environmental data information of the sea surface and underwater;

[0011] S3, using the multi-sensor fusion data P, constructs a three-dimensional grid map M, divides the environment into fixed grid cells, and each grid cell is marked by occupied state, idle state or unknown state;

[0012] S4, based on the motion planning module, generates a real-time optimal path for the unmanned boat formation; the motion planning module is based on the fast marching method FMM and introduces the heading angle and the safety boundary potential map to obtain the angle transformation fast marching method ACFSM; the heading angle constrains the path change range to ensure that the movement of the unmanned boat meets its steering capability, and the safety boundary potential map increases the path's avoidance of obstacles to keep the path away from potential danger areas;

[0013] S5, based on the obstacle avoidance mechanism module, the collision detection unit predicts the potential collision risk by calculating the distance to the nearest approach point DCPA and the time to the nearest approach point TCPA, and the fuzzy logic algorithm unit adjusts the heading and speed of the unmanned boat in real time according to the collision risk to ensure that the unmanned boat can gradually return to the predetermined path after avoiding obstacles;

[0014] S6, based on the path tracking and control module, enables the unmanned boat to stably follow the adjusted path;

[0015] Among them, the pilot boat generates a global path and avoids obstacles based on S1 to S6, and the follower boat dynamically adjusts its position through the formation rules with the pilot boat and the obstacle avoidance mechanism module in S5.

[0016] Preferably, the specific process of constructing the three-dimensional grid map M in S3 is:

[0017] Determine the grid parameters: set the grid size to 0.5m × 0.5m × 0.5m, and the map range to 100m × 100m × 100m;

[0018] Initialize the grid map: Create a three-dimensional array representing the grid map of the entire environment. Each grid cell is initialized to an "unknown" state with a value of -1, indicating information that has not yet been detected. The array dimensions are derived based on the map range and the grid size.

[0019] Marking grid cells: First, traverse the fused data P, analyze each piece of data in the fused data P, and identify the location of the object, the three-dimensional coordinates of the obstacle and other information; secondly, update the grid status; in the occupancy detection, if an object is detected at a grid position in the data set, the grid cell is marked as "occupied" and the value is set to 1; in the idle state, if no object is detected in a grid cell, the grid is marked as "idle" and the value is set to 0; in the undetected area, the grid cells not covered by the data set remain in the "unknown" state, and the grid marking formula is as follows:

[0020]

[0021] in, Indicates the total data set Unit data;

[0022] Generate smoother results by calculating the average or weight of the state of a grid and its neighboring grids;

[0023] Finally, all updated grid cell states are integrated to generate the final 3D grid map M.

[0024] Preferably, the motion planning module in S4 introduces the heading angle as follows:

[0025] Assume the initial heading angle of the unmanned boat is , a heading range is defined for path planning. The shape of the heading range is fan-shaped, and its angle is determined by the turning radius and maximum yaw angle of the unmanned boat. Assuming that the turning radius of the unmanned boat is R and the angle of the heading range is α, the calculation formula is:

[0026]

[0027] Where W is the width of the unmanned boat, and the generation of path points is limited to the steering capability of the unmanned boat, that is, when calculating new path points, the direction of the path cannot deviate from the current heading angle of the unmanned boat. The range exceeds α / 2;

[0028] During the path generation process, the grid point time update is limited as follows:

[0029]

[0030] in, is the arrival time, Based on the heading angle The adjustment amount, when the direction of the new point deviates from the current heading angle When larger, will increase, making the priority of this waypoint lower, thus avoiding sharp turns.

[0031] Preferably, the motion planning module in S4 introduces the safety boundary potential map specifically as follows:

[0032] Each grid point is assigned a value to indicate the safety of the point. The value ranges from 0 to 1. This value indicates the shortest distance to the obstacle and is regarded as an indicator of the safety of the local point. The farther away from the obstacle, the higher the value. When the value is 1, it indicates a completely safe area, that is, far away from the obstacle; when the value is 0, it indicates the area where the obstacle is located, that is, the danger zone;

[0033] The generation of the potential map depends on the distance between the unmanned boat and the obstacle, and the safety value of each grid point Calculated by the following formula:

[0034]

[0035] in, For point The distance to the nearest obstacle, is a constant term used to control the decay rate of the potential map. The closer the distance, the safer the value. The lower it is, the less safe the point is;

[0036] A potential term is added to the time update formula so that the path avoids areas with low potential values ​​as much as possible; the time update formula is:

[0037]

[0038] in The state of the grid map is due to the low potential area Smaller, will become larger, thus increasing the time at that point and making the path more inclined to move away from obstacles.

[0039] Preferably, the collision detection unit is specifically:

[0040] DCPA is used to represent the closest distance between two ships on their respective navigation paths, that is, the closest contact position between the ships. If the DCPA value is large, the ships have enough space to avoid collision during the journey; if the DCPA value is close to 0, it means that the ships may collide. The calculation formula is:

[0041]

[0042] in, are the current positions of unmanned boat 1 and unmanned boat 2 respectively; are the velocity vectors of unmanned boat 1 and unmanned boat 2 respectively;

[0043] TCPA represents the time it takes for the unmanned boat to reach the closest point under the current heading and speed, that is, the time it takes for the two boats to be closest to each other. If the TCPA value is negative, it means that the unmanned boat has passed the closest point; if the TCPA value is small, it means that the two boats are about to approach and may collide. The calculation formula is:

[0044]

[0045] in, Represents the relative position unit vector of the unmanned boat 2;

[0046] When the DCPA value is close to 0 or the TCPA value is close to 0, it indicates that the unmanned boat may collide, and the system initiates obstacle avoidance operation.

[0047] Preferably, the fuzzy logic algorithm unit processing process specifically includes:

[0048] Input fuzzification: Convert continuous input variable data into fuzzy sets; input variables include collision area, relative collision angle and safety potential map value ;

[0049] The collision area is defined as the relative position area between the unmanned boat and the obstacle, which is divided into eight areas: due north, northeast, due east, southeast, due south, southwest, due west, and northwest. to divide;

[0050] The calculation of the relative collision angle Indicates the relative motion direction between the unmanned boat and obstacles or other ships, which is used to determine the obstacle avoidance strategy. The calculation formula is as follows:

[0051]

[0052] in, is the dot product of the velocity vector and the relative position vector; is the modulus of the velocity vector of the unmanned boat; is the modulus of the relative position vector, i.e., the distance between the unmanned boat and the obstacle;

[0053] The safety potential map value Based on the generated potential map, determine the safety level of each grid point, from 0 to 1, the larger the value, the safer;

[0054] Each input variable is fuzzified by the fuzzy membership function, and the relative collision angle is It is divided into three fuzzy areas: small: 0~60 degrees, indicating that the collision angle is small, the obstacle is in front of the unmanned boat, and the collision risk is high; medium: 30~150 degrees, indicating that the collision angle is medium, the obstacle is in front of the unmanned boat, and the collision risk is medium; large: 120~180 degrees, indicating that the collision angle is large, the obstacle is behind or far away from the unmanned boat, the collision risk is low, and the safety potential map is The membership function is divided into three sub-areas: dangerous, medium and safe; through the fuzzy membership function, the values ​​of the relative collision angle and the potential map are converted into fuzzy values ​​to generate a fuzzy set;

[0055] Design fuzzy rule base: Based on the input fuzzy set, a set of fuzzy rules is used to determine the adjustment direction and speed of the unmanned boat;

[0056] Defuzzification: Use the centroid method to convert the weighted average of the fuzzy membership into specific heading and speed adjustments. The formula of the centroid method is:

[0057]

[0058]

[0059] in, is the output heading adjustment of each fuzzy rule, is the corresponding fuzzy membership; through defuzzification, a clear heading adjustment value is obtained and speed adjustment value ;

[0060] According to the environmental information, the heading adjustment value and speed adjustment value are determined, and the path Adpath is adjusted in real time based on the original determined Path:

[0061]

[0062]

[0063] According to the above formula, the system ensures a smooth offset based on the original path Path. After avoiding obstacles, the system will try to redirect the unmanned boat back to the Path path to reduce path deviation. The adjusted fusion path is:

[0064]

[0065] in, It is expressed as a dynamic fusion coefficient, which gradually decreases from 1 to 0 according to the distance after obstacle avoidance is completed.

[0066] Preferably, the path tracking and control module includes a global path guidance algorithm unit and a heading controller;

[0067] The global path guidance algorithm unit is used to provide a global path to ensure that the unmanned boat can gradually return to the correct trajectory through continuous heading adjustment when tracking the predetermined path; the heading controller is used to control the unmanned boat to stably follow the adjusted path; wherein, the heading controller adopts the feedback control deep deterministic optimization network DDON to optimize the proportional integral differential parameters of the controller to ensure the response accuracy and stability of the controller in complex environments.

[0068] Preferably, the global path guidance algorithm unit is specifically:

[0069] A continuous vector field is generated around the predetermined path. The vector at each point represents the desired heading direction of the point. When the unmanned boat deviates from the path, the vector field will generate a suitable heading adjustment direction according to the deviation of the position of the unmanned boat, and gradually guide the unmanned boat back to the correct track.

[0070] Specifically, a transition area is divided on both sides of the path, located on both sides of the center line of the path, and the width of the transition area is , when the deviation distance d of the unmanned boat is greater than When the unmanned boat is outside the transition area, it needs to sail in the direction of the path at a fixed angle. When the unmanned boat enters the transition area, it needs to gradually adjust its course according to the guidance rules;

[0071] The control rules for global path guidance are set as follows:

[0072]

[0073] in, is the desired heading angle, is the path angle, represents the inlet angle, is the lateral deviation, and k are control parameters.

[0074] Preferably, the heading controller is specifically:

[0075] According to the model describing the heading dynamic characteristics of the ship, the Nomoto model is applied to the heading controller based on the feedback control DDON network. Its first-order transfer function is as follows:

[0076]

[0077] in, represents the heading angular velocity of the unmanned boat, represents the heading adjustment rudder angle, K represents the gain coefficient, T is the time constant, and , then the above formula is expressed as:

[0078]

[0079] In the time domain, the governing equations are expressed as:

[0080]

[0081] in, is the heading angle of the unmanned boat;

[0082] The output control equation of the proportional-integral-derivative controller is:

[0083]

[0084] in, , , are proportional, integral and differential gain coefficients respectively, s represents the state of the system in the frequency domain, represents the desired heading angle, is the current heading angle, represents the rate of change of heading angle, Indicates that the proportional integral differential controller adjusts the width of the transition area; dynamically adjusts the rudder angle according to the deviation of the unmanned boat from the track , so that it returns to the desired course;

[0085] For speed control, a speed controller based on feedback linearization is used; first, the speed error of the unmanned boat is calculated:

[0086]

[0087]

[0088] in, is the expected speed, is the actual speed of the current unmanned boat;

[0089] Secondly, according to the speed error, the proportional-integral-derivative controller generates the control signal S of the thruster, and adjusts the thrust of the thruster through proportional, integral and differential:

[0090]

[0091] in, , , are proportional, integral and differential gain coefficients respectively, and the control signal S is used to adjust the speed of the unmanned boat;

[0092] Feedback linearization is used to handle nonlinear terms in propulsion dynamics. Assume that the propulsion model of the unmanned boat contains nonlinear terms. and the linear term , Indicates the surge speed, Represents the swing speed, and through feedback linearization, a new control signal is defined :

[0093]

[0094] After feedback linearization, the control signal S of the proportional-integral-differential controller is converted into the actual propulsion control signal of the unmanned boat through the feedback linearization model. The final thrust of the propeller is combined with the output of the proportional-integral-differential controller and the control term after feedback linearization, and the formula is:

[0095]

[0096] in, is the thrust of the propeller, is the nonlinear interference term, represents the effective mass of the unmanned boat, Indicates the actual surge velocity and swing velocity values.

[0097] Preferably, the specific process of using feedback control deep deterministic optimization network DDON to optimize the proportional integral differential parameters of the controller is:

[0098] First, the deep neural network DDON predicts the initial proportional integral differential parameters. The DDON network predicts the initial parameters by inputting the environmental feature vector X. , , :

[0099] , ,

[0100] in, represents the neural network model, which performs nonlinear mapping through multiple hidden layers and activation functions. The number of hidden layers is 4, and the number of neurons in each layer decreases layer by layer. B represents the weight and bias parameters of the neural network, which are obtained through offline training. The model predicts the initial controller parameters through feature input. , , ;

[0101] Secondly, local optimization adjustment calculation, DDON uses the gradient descent algorithm to adjust the parameters to minimize the control error L; L is the real-time control deviation of the system, that is, the sum of the squares of the path following error:

[0102]

[0103] in, is the target output, is the actual output of the unmanned boat, T is the time step;

[0104] According to the control error L, adjust by gradient descent , , ;

[0105] , ,

[0106] in, is the learning rate, which controls the update step size; assuming that e(t) is the system error, specifically the path deviation and speed deviation, the gradient of L is expressed as follows:

[0107]

[0108] in, It is found through the governing equations of the system state;

[0109] Finally, the heading angle error and speed error are calculated according to the formula as the real-time error signal of the DDON prediction model; the PID parameters predicted by DDON are used to calculate the controller output S, which is used to adjust the thrust of the propeller and the heading of the unmanned boat.

[0110] Compared with the prior art, the present invention has the following beneficial effects:

[0111] 1. Comprehensive environmental perception: By integrating radar, sonar, and camera multi-sensor data, the system can build a multi-dimensional environmental model and capture various environmental information. This fusion enables the system to provide more comprehensive and accurate perception data in real time in complex marine environments, covering information on obstacles on the surface and underwater. Various sensor data complement each other, enhancing the reliability of the system. Even if the data quality of a sensor deteriorates, other sensors can still provide support to ensure that the system operates stably under harsh conditions. By uniformly processing different data formats, the problem of difficulty in fusing heterogeneous data is solved, ensuring the coherence and consistency of data processing. Spatiotemporal synchronization and calibration technology ensures that each sensor accurately perceives the same spatial area at the same time, improving the accuracy of the perception results.

[0112] 2. Adapt to complex environments: By introducing heading angles and safety boundary potential maps, the path generated by the system is not only the shortest path, but also takes into account the dynamic characteristics of the unmanned boat, ensuring a smoother path and avoiding the problem that the path generated by the traditional fast marching method (FMM) is unnatural or difficult to execute. The angle-changing fast marching method combines the movement capabilities of the unmanned boat with the complexity of the environment, especially in complex marine environments, so that the unmanned boat can travel smoothly along the optimized path. It avoids unrealistic movement trajectories in areas with dense obstacles.

[0113] 3. Improve the flexibility of obstacle avoidance: Fuzzy logic does not require precise mathematical modeling and can generate flexible obstacle avoidance strategies based on dynamic environmental factors. Especially in the face of highly uncertain marine environments, fuzzy logic systems can better handle various complex scenarios, such as dynamic obstacles or randomly moving ships.

[0114] 4. Improved path following accuracy: The global path guidance algorithm generates a vector field around the path. Even if the unmanned boat deviates from the predetermined trajectory, it can quickly return to the correct path through guidance, ensuring the accuracy of path following and adapting to the complex marine environment. The nonlinear dynamics of the unmanned boat are handled by feedback linearization technology. The improved controller can stably cope with disturbances inside and outside the system, ensuring the robustness and response speed of the control system. Deep deterministic network optimization is used to dynamically adjust the parameters of the proportional integral differential controller to ensure that the unmanned boat can find the best parameter combination according to different mission requirements, thereby optimizing the overall control performance and improving the system responsiveness and tracking accuracy. The feedback control DDON network heading controller of global path guidance can adjust the thrust and heading of the propeller in real time, ensuring that the unmanned boat can stably follow the path without excessive heading deviation due to changes in the external environment, thereby improving the reliability of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0115] Figure 1 It is an overall logic block diagram of the unmanned boat cooperative formation control method of the present invention.

[0116] Figure 2 It is a schematic diagram of the data collection and data processing process of the present invention.

[0117] Figure 3 Schematic diagram of the grid map construction process of the present invention.

[0118] Figure 4 The present invention is a flow chart for optimizing the parameters of the proportional-integral-derivative controller for the deep deterministic network.

[0119] Figure 5 This is a parameter comparison chart of the DDON network and particle swarm optimization proportional integral derivative controller. DETAILED DESCRIPTION

[0120] The invention will be further described below in conjunction with specific embodiments.

[0121] The present invention aims to solve many existing technical problems such as environmental perception, path planning, collision avoidance decision-making and real-time control through innovative technical solutions, and provide a new solution for the efficient coordinated operation of multiple unmanned boats in complex marine environments. Figure 1 As shown, and includes the following processes:

[0122] S1, through the deployment of multiple sensors, real-time acquisition of static and dynamic environmental data of the ocean, including obstacles and visual environment changes;

[0123] S2, the data acquired by each sensor are time synchronized, coordinate transformed, denoised and fused to obtain the total data set P after fusion, which contains the complete environmental data information of the sea surface and underwater;

[0124] S3, using the multi-sensor fusion data P, constructs a three-dimensional grid map M, divides the environment into fixed grid cells, and each grid cell is marked by occupied state, idle state or unknown state;

[0125] S4, based on the motion planning module, generates a real-time optimal path for the unmanned boat formation; the motion planning module is based on the fast marching method FMM and introduces the heading angle and the safety boundary potential map to obtain the angle transformation fast marching method ACFSM; the heading angle constrains the path change range to ensure that the movement of the unmanned boat meets its steering capability, and the safety boundary potential map increases the path's avoidance of obstacles to keep the path away from potential danger areas;

[0126] S5, based on the obstacle avoidance mechanism module, the collision detection unit predicts the potential collision risk by calculating the distance to the nearest approach point DCPA and the time to the nearest approach point TCPA, and the fuzzy logic algorithm unit adjusts the heading and speed of the unmanned boat in real time according to the collision risk to ensure that the unmanned boat can gradually return to the predetermined path after avoiding obstacles;

[0127] S6, based on the path tracking and control module, enables the unmanned boat to stably follow the adjusted path;

[0128] Among them, the pilot boat generates a global path and avoids obstacles based on S1 to S6, and the follower boat dynamically adjusts its position through the formation rules with the pilot boat and the obstacle avoidance mechanism module in S5.

[0129] 1. Data Collection and Environmental Perception

[0130] This phase aims to collect environmental data, including obstacles, other ships and environmental conditions, through sensors, and build a grid map of the environment as input for path planning and obstacle avoidance. This process is crucial to the performance of the model.

[0131] The unmanned boat system needs to obtain the surrounding environmental data in real time to ensure the coordination and effective path planning between multiple boats. In order to obtain the real unmanned boat cooperative formation control environment data, the present invention uses the unmanned boat to collect the data information of the ocean environment through the sensors carried by the unmanned boat to form multi-sensor fusion. The unmanned boat perceives the environment through a variety of sensor devices, thereby obtaining real-time environmental information and dynamic data inside and outside the formation. The multi-sensors include: radar (working bandwidth 24 GHz, detection distance 1km, scanning frequency 10Hz, resolution 20cm, data is saved in two-dimensional grid polar coordinate format), sonar (data sampling rate 5Hz, detection distance 70 meters, resolution 15cm, data is saved in three-dimensional grid polar coordinate format) and camera (resolution 1920*1080 pixels, frame rate 80fps, focal length 3.6mm, image format PNG format). The data of these sensors includes not only static and dynamic obstacles on the water surface (such as seabed terrain, other ships), but also real-time changes in the environment (such as waves, wind speed). Among them, radar is mainly used to detect obstacles on the sea surface, including other ships and floating objects; sonar is mainly used for underwater environment detection, including underwater terrain and obstacles, such as reefs and submarines; cameras are mainly used to obtain image information of the surface environment, including obstacles above the sea surface and changes in the visual environment. In order to ensure the accuracy and consistency of the data, the unmanned boat needs to stabilize the sensor during the collection process to avoid external interference.

[0132] 2. Data Preprocessing

[0133] It is understandable that in the multi-sensor environmental perception of unmanned boats, the radar, sonar and camera each collect different types of data.

[0134] Specifically, radar usually collects spectrum data. Radar emits electromagnetic waves, detects the frequency and intensity of echo signals, and calculates the distance, speed and direction of the target object through spectrum analysis. This spectrum data can be converted into two-dimensional polar coordinate point cloud data to represent the target position and its relative motion characteristics. Sonar collects sound wave echo data, which is represented in the form of three-dimensional point cloud. Sonar emits sound waves and measures the depth and distance of underwater objects by receiving sound wave reflection signals. Through the relationship between time and the intensity of the reflected signal, a grid or point cloud data in three-dimensional space is generated to reflect the position and shape of the underwater terrain and objects. The camera collects two-dimensional image data. Image data provides visual information and can provide visual features for object recognition and target detection.

[0135] In order to integrate different data types collected by multi-source sensors, data preprocessing is required. The data processing flow of this part is as follows: Figure 2 shown.

[0136] 1. Time synchronization:

[0137] Since the data collection frequencies of radar, sonar and camera are different, it is necessary to synchronize the data of these sensors first to ensure that the data at the same time can correspond to the same environmental state. The data collection of multiple sensors in the present invention is in seconds (t) to ensure that the system has sufficient response speed. After the data is collected, the timestamp mechanism is used in the present invention to align the collected data.

[0138] Since the sampling frequencies of the sensors are different, the data of each sensor needs to be time-aligned. The sampling times of radar, sonar and camera are set to , . Introducing a global time base , align the time:

[0139]

[0140] in, Indicates the actual sampling time of each sensor. Through the timestamp alignment method, the data of each sensor is adjusted to a unified global time point .

[0141] 2. Coordinate transformation:

[0142] Radar, sonar, and camera usually use different coordinate systems and data formats, so before fusion, the data of all sensors should be converted to the same reference coordinate system. Setting: The polar coordinate representation of radar data is ( ), the polar coordinates of the sonar data are expressed as ( ), the camera data is the two-dimensional image coordinates (u, v). The three sensor data are converted into polar coordinates respectively.

[0143] (1) Radar polar coordinate conversion: The radar provides long-range detection information in two-dimensional polar coordinates. After conversion to three-dimensional rectangular coordinates, the data obtained is the data on the sea surface. The data set is represented as .in is the target distance, is the azimuth. Convert it into a three-dimensional rectangular coordinate system, the formula is as follows:

[0144]

[0145]

[0146]

[0147] (2) Sonar polar coordinate conversion: Sonar provides underwater depth information. After coordinate conversion, we get information describing the underwater environment and obstacle locations. The data set is represented as .in is the target distance, is the horizontal angle, is the vertical angle. Convert it into a three-dimensional rectangular coordinate system, the formula is as follows:

[0148]

[0149]

[0150]

[0151] (3) Polar coordinate conversion of the camera. Where u and v are the pixel coordinates of the image. The dataset is Represented. Convert it to a three-dimensional rectangular coordinate system, knowing the depth value of the pixel in the image Known. The formula is as follows:

[0152]

[0153]

[0154]

[0155] in, is the focal length of the camera in the horizontal and vertical directions, It is the coordinate of the optical center of the camera image plane, that is, the coordinate of the center point of the image.

[0156] 3. Data denoising:

[0157] In the multi-sensor fusion system of unmanned boats, the key step of data preprocessing is denoising. Its purpose is to reduce the noise caused by sensor hardware, external interference, etc., so as to improve the accuracy of sensor data. This is crucial for subsequent operations such as path planning, obstacle avoidance, and environmental perception.

[0158] (1) Radar data denoising: Radar detects targets by emitting and receiving electromagnetic waves, and the data collected contains noise points (wrong distance, azimuth, etc.). The present invention uses a random sampling consistency algorithm to perform data denoising.

[0159] The implementation steps of this part are: first, randomly select a sub-data set from the radar data and fit it into a straight line model; second, calculate the distance between other points and the model to determine whether they are internal points (conform to the model); finally, repeat the steps until the best model is found. The model fitting formula is as follows:

[0160]

[0161] in, is the i-th point of the radar data, is a point on the model; Is the distance threshold. When the distance between a point and the model is less than this value, it is an inlier. The random sampling consistency algorithm can effectively remove erroneous data information caused by reflection.

[0162] (2) Sonar data denoising: Sonar uses sound waves to detect underwater environments and is usually affected by noise such as underwater reflections, multipath interference, and echoes. The present invention uses median filtering to remove speckle noise. Median filtering is a common nonlinear filter that can effectively remove speckle noise. The principle is to sort all pixel values ​​within a certain window and select the median as the new value of the current pixel. Since speckle noise usually causes sudden changes in pixel values, median filtering can smooth these sudden changes and ultimately effectively remove speckle noise.

[0163] (3) Camera data denoising: The image data of the camera is usually affected by light changes, sensor thermal noise, environmental noise, etc., resulting in Gaussian noise, speckle noise and other problems in the image. The present invention uses Gaussian filtering to remove high-frequency noise in the image. Gaussian filtering can effectively smooth the image and remove noise by convolving with a Gaussian kernel. Specifically, the calculation formula of the Gaussian filter is:

[0164]

[0165] Where σ is the standard deviation of the Gaussian kernel, u and v represent the pixel positions in the image, and are used to represent the offset of each pixel to the center of the filter. This offset is used to calculate the value of the Gaussian distribution function to determine the weight of the pixel. By performing a convolution operation with the image, Gaussian filtering can effectively reduce the Gaussian white noise in the image.

[0166] Through the processing of random sampling consistency algorithm, median filtering and Gaussian filtering, the quality of data collected by unmanned boats underwater can be significantly improved and noise interference can be removed.

[0167] 4. Data Fusion:

[0168] The purpose of data fusion is to combine the environmental information from different sensors to provide a high-precision environmental model for subsequent path planning, obstacle avoidance, and target recognition and tracking tasks.

[0169] By combining radar data, sonar data, and camera data, we can get a complete picture of the surface and underwater environment. The total data set after fusion is: The dataset contains complete obstacle information on the sea surface and underwater.

[0170] Through data fusion, a 3D environment model is constructed, which includes all the perception information from radar, sonar and camera. This kind of data set fusion is suitable for global environment perception. For convenience, the total data set is Denoted as P, it is the preprocessed dataset of cooperative formation control of unmanned boats.

[0171] 3. Grid Map Construction

[0172] Grid map construction is the process of dividing the environment into fixed-size grid cells in order to effectively represent and process environmental information. In this process, the environmental data P collected by multi-sensor fusion is fused and converted into a three-dimensional grid structure. Each grid cell corresponds to a specific area and is marked as "occupied", "free" or "unknown". This method enables the unmanned boat to update its environmental model in real time, realize autonomous navigation, path planning and dynamic obstacle avoidance, and improve its safety and efficiency in complex environments. The grid map construction process is as follows: Figure 3 shown.

[0173] Determine the grid parameters: Set the grid size to 0.5m x 0.5m x 0.5m. Set the map range to 100m x 100m x 100m.

[0174] Initialize the grid map: Create a 3D array representing a grid map of the entire environment. Each grid cell is initialized to an "unknown" state (value set to -1), indicating that it has not yet been detected. The array dimensions are derived from the map extent and the grid size.

[0175] Mark grid cells: First, traverse the data set P, analyze each data in the total data set P, and identify the location of objects, the three-dimensional coordinates of obstacles and other information; secondly, update the grid status. Occupancy detection: If an object is detected at a grid position in the data set, mark the grid cell as "occupied" (set the value to 1). Idle state: If no object is detected in a grid cell, mark the grid as "idle" (set the value to 0). Undetected area: Grid cells not covered by the data set remain in the "unknown" state. The grid marking formula is as follows:

[0176]

[0177] in, Indicates the total data set Unit data.

[0178] Map smoothing and optimization: Ensure that grid maps updated at different time points do not conflict. Check the consistency of the states of adjacent grids to improve the reliability of the overall map. Generate smoother results by calculating the average or weight of the state of a grid and its neighboring grids. If a grid is mostly "idle" around it, but the grid is displayed as "occupied", it may be caused by noise. At this time, the grid state can be adjusted to "idle" through a smoothing algorithm.

[0179] Finally generate the grid map: According to the above, integrate all the updated grid unit states to generate the final three-dimensional grid map M.

[0180] Real-time update: During the actual operation of the unmanned boat, new data is continuously collected and the 3D grid map is updated in real time to adapt to the dynamic environment. This ensures the safe navigation and efficient execution of the unmanned boat in complex environments.

[0181] 4. Motion Planning Module Design

[0182] In the coordinated formation control of unmanned boats, motion planning is the core part to ensure that multiple unmanned boats can operate efficiently and safely in complex marine environments. The main task of motion planning is to generate the optimal path from the starting point to the target point of the unmanned boat, avoid collisions, determine the initial navigation route for the pilot boat and the follower boat, and adjust the course in time when encountering dynamic obstacles. To solve this problem, the present invention proposes an angle change fast-marching square method (ACFSM) for the motion characteristics of unmanned boats, which can effectively solve the problems of dynamic obstacle avoidance and heading control, so that the unmanned boat can complete the navigation task smoothly and safely.

[0183] A fast-marching method using angle guidance is constructed for global path planning. This method is based on the classic fast-marching method (FMM) by solving the Eikonal equation. The ACFSM method considers the heading angle of the unmanned boat and the potential map of the safety boundary to improve the planning effect.

[0184] According to the total data set P obtained through data fusion, the environment modeling M is obtained through the grid map construction process as an application scenario of the unmanned boat collaborative formation control motion planning, so that the unmanned boat can realize the motion planning of collaborative formation control in this scenario.

[0185] In the motion planning of cooperative formation control of unmanned boats, it is of great significance to use the fast marching method as the basic algorithm. The path generated by FMM can provide the optimal path for each unmanned boat in the formation without destroying the formation structure. By using the same grid in the environment, all unmanned boats use the heading angle and safety boundary potential map to ensure that the ships in the formation can act in unison.

[0186] The core of FMM is to simulate the process of propagation from the starting point outward by iteratively solving the Eikonal equation. It can efficiently calculate the arrival time of each grid point and generate a globally optimal path. This method is suitable for irregular ocean environments, can handle a variety of terrain and obstacle distributions, and adapt to dynamically changing environments. The form of the Eikonal equation is as follows:

[0187]

[0188] in, It is the border arrival point time, is the propagation speed. Assume is the point on the grid that needs to be calculated .and The adjacent points are a set of four grid points, namely: , , , . The calculation can be obtained according to the following formula:

[0189]

[0190]

[0191]

[0192]

[0193] 1. Introduction of heading angle

[0194] It is understandable that in the classic FMM, the path generation does not take into account the actual motion constraints of the unmanned boat. The purpose of introducing the heading angle is to limit the range of path changes so that it conforms to the motion constraints of the boat and avoids actions that cannot be performed in practice, such as sharp turns.

[0195] Assume the initial heading angle of the unmanned boat is , the movement of the unmanned boat is limited by the turning radius. In order to match the planned path with the heading angle, a heading range is defined for path planning in the ACFSM algorithm. The shape of the heading range can be regarded as a fan, and its angle is determined by the turning radius and maximum yaw angle of the unmanned boat. Assuming that the turning radius of the unmanned boat is R and the angle of the heading range is α, the calculation formula is:

[0196]

[0197] Where W is the width of the unmanned boat. In order to ensure the smoothness and executability of the path, the design limits the generation of path points to the steering capability of the unmanned boat. That is, when calculating new path points, the direction of the path cannot deviate from the current heading angle of the unmanned boat. Exceeding the range of α / 2.

[0198] Furthermore, during the path generation process, the FMM algorithm updates the time of each grid point and calculates its arrival time. In order to introduce the heading angle into this process, the present invention limits the time update of the new grid point as follows:

[0199]

[0200] in, Based on the heading angle When the direction of the new point deviates from the current heading angle When larger, will increase, making the priority of this waypoint lower, thus avoiding sharp turns.

[0201] 2. Introduction of Safety Boundary Potential Map

[0202] In addition to the constraint of the heading angle, the ACFSM method designed in the present invention further optimizes the path by introducing a safety boundary potential map to keep it away from obstacles and other potential risks.

[0203] Specifically, the potential map assigns a value to each grid point to indicate the safety of the point. The value ranges from 0 to 1. This value represents the shortest distance to the obstacle and is regarded as an indicator of the safety of the local point. The farther away from the obstacle, the higher the value. When the value is 1, it indicates a completely safe area, that is, far away from the obstacle; when the value is 0, it indicates the area where the obstacle is located, that is, the danger zone, and therefore unsafe. Therefore, the unmanned boat should sail in the high-value area.

[0204] The generation of the potential map depends on the distance between the unmanned boat and the obstacle, and the safety value of each grid point Calculated by the following formula:

[0205]

[0206] in, For point The distance to the nearest obstacle, is a constant term used to control the decay rate of the potential map. Therefore, the closer the distance, the safer the value. The lower it is, the less safe the point is.

[0207] In the process of path generation, ACFSM not only takes into account the arrival time , and also combined with the safety value of the potential map That is, a potential term is added to the time update formula of FMM so that the path avoids areas with low potential values ​​(i.e., close to obstacles) as much as possible. When updating the path, not only the arrival time is considered, but also the state of the grid map is considered. According to the network marking formula and the time update limit formula, the time update formula is:

[0208]

[0209] Due to the low potential area Smaller, will become larger, thus increasing the time at that point and making the path more inclined to move away from obstacles.

[0210] By adding the heading angle and the safety boundary potential map, the heading angle constrains the path change range to ensure that the movement of the unmanned boat is consistent with its steering capability and avoids the generation of an unfeasible sharp turn path. The safety boundary potential map increases the path's avoidance of obstacles, making the path away from potential danger areas and increasing the safety of navigation.

[0211] Finally, ACFSM combines the heading angle and the safety boundary potential map, and can generate a safe path that is more in line with the actual motion constraints of the unmanned boat under the environment perception grid map M while ensuring the smoothness of the path. The improved method adopted in the present invention makes the ACFSM method suitable for complex marine environments, especially in the motion planning of unmanned boat formations, which can effectively deal with dynamic obstacles and path adjustments and determine the predetermined initial navigation route Path.

[0212] 5. Obstacle avoidance module design

[0213] The obstacle avoidance mechanism module of the present invention is mainly composed of two parts: a collision detection unit and a fuzzy logic algorithm unit. Real-time monitoring of the navigation process along the predetermined navigation route Path ( The collision avoidance mechanism adjusts the course of the vessel based on the relative position and speed of the vessel when a potential collision is detected. and speed The specific process includes:

[0214] Collision Detection Unit: The Collision Detection Unit is responsible for determining if there is a potential conflict in the route, if two ships are at a point in the desired trajectory where a potential collision would occur. To successfully eliminate a collision situation, the distance to the closest point of approach (DCPA) and the time to the closest point of approach (TCPA) must be calculated.

[0215] Specifically, DCPA represents the closest distance between two ships on their respective navigation paths, that is, the closest contact position between the ships. If the DCPA value is large, the ships have enough space to avoid collision during navigation; if the DCPA value is close to 0, it means that the ships may collide. The calculation formula is:

[0216]

[0217] in, are the current positions of unmanned boat 1 and unmanned boat 2 respectively; are the velocity vectors of unmanned boat 1 and unmanned boat 2 respectively.

[0218] Furthermore, TCPA indicates the time it takes for the unmanned boat to reach the closest point of contact under the current heading and speed, that is, the time it takes for the two boats to be closest to each other. If the TCPA value is negative, it means that the unmanned boat has passed the closest point of contact; if the TCPA value is small, it means that the two boats are about to approach and may collide. The calculation formula is:

[0219]

[0220] in, Represents the relative position unit vector of unmanned boat 2.

[0221] When the DCPA value is close to 0 or the TCPA value is close to 0, it indicates that the unmanned boat may collide, and the system will initiate obstacle avoidance operations.

[0222] Fuzzy logic algorithm unit: The present invention adopts an obstacle avoidance algorithm based on fuzzy logic. Through fuzzy rules, the heading and speed of the unmanned boat are adjusted based on the collision detection result to avoid collision. The fuzzy logic algorithm module includes the following steps:

[0223] Input fuzzification: In the obstacle avoidance system of the unmanned boat, fuzzification is to convert continuous input data into fuzzy sets. In the fuzzy logic unit of the present invention, the input variables mainly include:

[0224] Collision area: defined as the relative position area between the unmanned boat and the obstacle (divided into 8 areas, ① to ⑧: due north, northeast, due east, southeast, due south, southwest, due west, northwest), by calculating the relative collision angle To divide.

[0225] Relative collision angle : Indicates the relative motion direction between the unmanned boat and the obstacle (or other ship), which is used to determine the obstacle avoidance strategy. The calculation formula is as follows:

[0226]

[0227] in, is the dot product of the velocity vector and the relative position vector; is the modulus of the velocity vector of the unmanned boat; It is the modulus of the relative position vector, that is, the distance between the unmanned boat and the obstacle.

[0228] Safety Potential Graph Value : Based on the generated potential map, determine the safety level of each grid point (from 0 to 1), the larger the value, the safer it is.

[0229] Each input variable will be fuzzified by the fuzzy membership function. It can be divided into three fuzzy areas: small (v1: 0~60 degrees) means the collision angle is small, the obstacle is in front of the unmanned boat, and the collision risk is high. Medium (v2: 30~150 degrees) means the collision angle is medium, the obstacle is in front of the unmanned boat, and the collision risk is medium. Large (v3: 120~180 degrees) means the collision angle is large, the obstacle is behind or far away from the unmanned boat, and the collision risk is low. Safety potential map The membership function is divided into three sub-areas: dangerous, medium and safe. The corresponding membership degrees are as follows:

[0230]

[0231]

[0232]

[0233]

[0234]

[0235]

[0236] The values ​​of the relative collision angle and the potential map are converted into fuzzy values ​​through the above fuzzy membership function.

[0237] The fuzzy value represents the membership basis of the input variable in each fuzzy region. For the relative collision angle, according to its value, it can belong to both the "medium" and "small" regions, and its membership in each region is calculated according to the membership function. For the safety potential value, according to the size of the potential value, it can belong to both the "medium" and "safe" regions, and the membership function will also calculate the membership for each region.

[0238] Design fuzzy rule base: Based on the input fuzzy set, a set of fuzzy rules is used to determine the adjustment direction and speed of the unmanned boat. The rule base is designed according to the COLREGs regulations, for example: if the obstacle is in front, the heading needs to be changed quickly; if the obstacle is on the side, the adjustment angle is small; if the obstacle is in the back, no adjustment is required. The general form of setting fuzzy rules is as follows:

[0239] IF IS SMALL AND IS High THEN Heading Adjustment big

[0240] Defuzzification: Defuzzification is the process of converting fuzzy output into actual control signals. In the design of the present invention, defuzzification uses the centroid method to convert the weighted average of the fuzzy membership into specific heading and speed adjustments. The formula of the centroid method is:

[0241]

[0242]

[0243] in, is the output heading adjustment of each fuzzy rule, is the corresponding fuzzy membership. Through defuzzification, the system can obtain a clear heading adjustment value and speed adjustment value This performs obstacle avoidance actions.

[0244] According to the environmental information, the heading adjustment value and the speed adjustment value are determined, and the path Adpath is adjusted in real time based on the originally determined Path path.

[0245]

[0246]

[0247] According to the above formula, it is ensured that the original path Path is smoothly offset. After avoiding obstacles, the system will try to redirect the unmanned boat back to the Path path to reduce path deviation. The adjusted fusion path is:

[0248]

[0249] in, It is expressed as a dynamic fusion coefficient, which gradually decreases from 1 to 0 according to the distance after obstacle avoidance is completed.

[0250] 6. Path tracking and control module design

[0251] After the unmanned boat has completed the obstacle avoidance decision, a guidance module is required to ensure that the formation maintains the predetermined trajectory and formation. The present invention adjusts the heading and speed of the unmanned boat by designing a feedback control DDON network guided by a global path, and adjusts the thrusters and rudder angles to ensure that each ship accurately executes the planned path. Feedback control ensures that the unmanned boat moves along the planned path. After obstacle avoidance or path adjustment, the controller quickly adjusts the new target heading and speed. Ensure that the unmanned boat continues to travel according to the adjusted path and maintains the predetermined formation shape.

[0252] 1. Global path guidance algorithm unit:

[0253] The main purpose of the global path guidance algorithm is to provide a global path to ensure that the unmanned boat can gradually return to the correct trajectory through continuous heading adjustments while tracking the predetermined path.

[0254] Specifically, the global path guidance algorithm generates a continuous vector field around the predetermined path, and the vector at each point represents the desired heading direction of the point. When the unmanned boat deviates from the path, the vector field will generate a suitable heading adjustment direction based on the deviation of the unmanned boat's position, and gradually guide the unmanned boat back to the correct track. Secondly, the navigation environment of the unmanned boat is complex, and there may be dynamic obstacles or other uncertain factors on the path. Vector field guidance can gradually return the unmanned boat to the track through local heading adjustments without interfering with the overall path planning.

[0255] Specifically, a transition area is divided on both sides of the path, located on both sides of the center line of the path, and the width of the transition area is When the deviation distance d of the unmanned boat is greater than When the distance is less than When the unmanned boat enters the transition area, it needs to gradually adjust its course according to the guidance rules.

[0256] The control rules for global path guidance are set as follows:

[0257]

[0258] in, is the desired heading angle, is the path angle, represents the inlet angle, is the lateral deviation, and k are control parameters.

[0259] By designing a global path guidance algorithm, continuous heading adjustment information can be provided to the unmanned boat to ensure the smoothness of path tracking.

[0260] 2. Design of the heading controller for feedback control DDON network:

[0261] In the path tracking control of the unmanned boat, the proportional-integral-differential controller is mainly used to accurately adjust the heading and speed of the unmanned boat so that it can effectively follow the path indication provided by the vector field. The proportional-integral-differential controller is essentially a linear controller that controls the system through simple proportional, integral and differential adjustments. However, the movement of the unmanned boat is nonlinear, especially in the marine environment, where external interference can cause complex dynamic changes. When faced with nonlinearity, the proportional-integral-differential controller has difficulty in accurately processing these complex dynamic characteristics, which may result in unsatisfactory control effects. Therefore, the present invention proposes a global path-guided feedback-controlled DDON network heading controller for use in the collaborative formation control of unmanned boats.

[0262] When unmanned boats perform collaborative operations, the speed controller is an important component to ensure that the unmanned boats maintain the desired speed and maintain the predetermined formation. In complex marine environments, when encountering various static and dynamic obstacles, the speed controller can dynamically adjust the ship's speed to cope with different navigation situations, such as obstacle avoidance and path tracking.

[0263] Specifically, according to the model describing the heading dynamic characteristics of the ship, the Nomoto model is applied to the heading controller based on the feedback control DDON network, and its first-order transfer function is as follows:

[0264]

[0265] in, represents the heading angular velocity of the unmanned boat, represents the heading adjustment rudder angle, K represents the gain coefficient, T is the time constant, and , then the above formula can be expressed as:

[0266]

[0267] In the time domain, the governing equation can be expressed as:

[0268]

[0269] in, is the heading angle of the unmanned boat.

[0270] Furthermore, the output control equation of the proportional-integral-derivative controller is:

[0271]

[0272] in, , , are proportional, integral and differential gain coefficients respectively, s represents the state of the system in the frequency domain, represents the desired heading angle, is the current heading angle, represents the rate of change of heading angle, The proportional-integral-differential controller adjusts the width of the transition zone. Through the above controller, the proportional-integral-differential system can dynamically adjust the rudder angle according to the deviation of the unmanned boat from the track. , returning it to the desired heading.

[0273] Furthermore, in terms of speed control, a speed controller based on feedback linearization is used. It controls the forward speed of the unmanned boat by adjusting the power of the propeller in real time, so that the unmanned boat can maintain a predetermined speed, and can also adjust the speed as needed to avoid obstacles or perform other dynamic behaviors.

[0274] First, calculate the speed error of the unmanned boat:

[0275]

[0276]

[0277] in, is the expected speed, is the actual speed of the current unmanned boat.

[0278] Secondly, according to the speed error, the proportional-integral-derivative controller generates the control signal S of the thruster, and adjusts the thrust of the thruster through proportional, integral and differential:

[0279]

[0280] in, , , They are proportional, integral and differential gain coefficients respectively. The control signal S is used to adjust the speed of the unmanned boat.

[0281] In order to deal with the nonlinear terms in the propulsion dynamics, feedback linearization is used. Assume that the propulsion model of the unmanned vehicle contains nonlinear terms and the linear term , Indicates the surge speed, Represents the swing speed, and through feedback linearization, a new control signal is defined .

[0282]

[0283] In this way, the nonlinear term is eliminated and the system becomes a linear control problem. is the control signal in the linear system. After feedback linearization, the control signal S of the proportional-integral-differential controller is converted into the actual propulsion control signal of the unmanned boat through the feedback linearization model. The final thrust of the propeller combines the output of the proportional-integral-differential controller and the control term after feedback linearization, and the formula is:

[0284]

[0285] in, is the thrust of the propeller, is the nonlinear interference term, Represents the effective mass of the unmanned boat. Indicates the actual surge velocity and swing velocity values.

[0286] 3. Parameter design of the deep deterministic network optimized proportional integral derivative controller:

[0287] The proportional-integral-differential controller is used to control the heading and speed of the unmanned boat, and to keep the unmanned boat sailing stably on the predetermined path by adjusting the thrust and rudder angle of the propeller. However, the parameters of the traditional proportional-integral-differential controller usually need to be determined empirically. In a complex and dynamic marine environment, static and dynamic obstacles change rapidly, and the proportional-integral-differential controller responds slowly to these changes, which can easily lead to tracking errors or failure to avoid obstacles in time. At the same time, the motion dynamics of unmanned boats are often nonlinear and include the influence of external disturbances such as wind and waves, making it difficult for the proportional-integral-differential controller with fixed parameters to provide precise control.

[0288] In the unmanned boat formation control and path tracking, the controller parameters , , It directly affects the speed, heading and trajectory tracking accuracy of the unmanned boat. The present invention proposes the use of a deep deterministic optimization network, which can automatically find the best parameter combination in a complex environment to ensure the optimal system performance. The Deep Deterministic Optimization Network (DDON) is a new control strategy that combines deep learning and optimization algorithms to dynamically adjust control parameters in real-time control systems. DDON uses the powerful predictive ability of deep neural networks to learn the nonlinear relationship between environmental characteristics and optimal control parameters in the control system, outputs the appropriate parameter combination in real time, and further fine-tunes it through local optimization algorithms to ensure the best control effect in complex and dynamic environments.

[0289] Based on the generated grid map and the total data set P; based on the lateral offset of the unmanned boat from the target path , expected speed and current speed , target heading angle and the current heading angle As the input feature vector of the current environment X=[ , ];

[0290] First, the deep neural network DDON predicts the initial proportional integral differential parameters. The DDON network predicts the initial parameters by inputting the environmental feature vector X , , .

[0291] , ,

[0292] in, represents the neural network model, which performs nonlinear mapping through multiple hidden layers and activation functions. The number of hidden layers in the present invention is 4, and the number of neurons in each layer decreases layer by layer; B represents the weight and bias parameters of the neural network, which are obtained through offline training. The model predicts the initial controller parameters through feature input , , .

[0293] Secondly, local optimization is performed to adjust the calculation. DDON uses the gradient descent algorithm to adjust the parameters to minimize the control error L. L is the real-time control deviation of the system, that is, the sum of the squares of the path following error:

[0294]

[0295] in, is the target output, is the actual output of the unmanned boat, and T is the number of time steps.

[0296] Furthermore, according to the control error L, the gradient descent is used to adjust , , .

[0297] , ,

[0298] in, is the learning rate, controlling the update step size. According to the control signal S calculation formula, assuming that e(t) is the system error, which is the path deviation and speed deviation in the present invention, the gradient of L can be expressed as follows:

[0299]

[0300] in, is solved by the control equation of the system state.

[0301] Finally, the feedback error is calculated. The heading angle error and speed error are calculated according to the control signal S calculation formula. It is used as the real-time error signal of the DDON prediction model. The output of the proportional integral differential controller is calculated. The controller output S is calculated using the PID parameters predicted by DDON. This output signal is used to adjust the thrust of the propeller and the heading of the unmanned boat.

[0302] The overall optimization process of DDON is as follows:

[0303] 1) Offline training: The neural network model is trained using a large amount of simulation data. Conduct training to learn the mapping relationship between environmental characteristics and optimal PID parameters;

[0304] 2) Real-time prediction: In actual control, the DDON neural network model predicts the initial , , ;

[0305] 3) Local optimization fine-tuning: Use the gradient descent algorithm to perform error-driven fine-tuning on the proportional-integral-differential parameters of the neural network output to ensure that the control error is minimized;

[0306] 4) Feedback update: The control error and new environmental characteristics are fed back in each cycle, and real-time prediction and optimization are repeated to ensure the best performance of the controller in a dynamic environment.

[0307] The DDON network adjusts the heading and speed of the unmanned boat according to the feedback control guided by the global path, and adjusts the thrusters and rudder angles to ensure that each boat accurately executes the planned path. Feedback control ensures that the unmanned boat moves along the planned path. After obstacle avoidance or path adjustment, the controller quickly adjusts the new target heading and speed. Ensure that the unmanned boat continues to travel according to the adjusted path and maintains the predetermined formation shape. The overall optimization process is as follows Figure 4 shown.

[0308] The present invention verifies the effectiveness of the proposed global path guided feedback control DDON network heading controller in parameter control. This method is compared with the traditional parameter adjustment optimization algorithm - particle swarm algorithm to control the output tracking target value of the control system and evaluate the difference in control system tracking accuracy and response speed. The control output signal is used to adjust the behavior of the system so that it gradually approaches the target value, that is, the system remains in the desired operating state. The comparison results are shown in Figure 5 shown.

[0309] exist Figure 5 In the target value curve, the black dotted line represents the expected output of the system under ideal conditions, that is, the target value that the system should reach and stabilize at. The blue curve represented by the DDON network method represents the PID control output optimized based on the deep deterministic optimization network. The PID parameters optimized by DDON make the control output gradually approach and stabilize at the target value. It can converge to the target value quickly and has small fluctuations, indicating that DDON performs better in this control system. On the contrary, the PID control output of the particle swarm algorithm represented by the red curve, when the time step is 28, the control output fluctuates greatly, indicating that the output signal is unstable when the particle swarm algorithm is used to adjust the output. The comparison results show that the algorithm of the present invention has better performance.

[0310] 7. Unmanned Boat Formation Control

[0311] In the present invention, the unmanned boat formation adopts a "leader-follower" control structure. The pilot boat is responsible for global path planning, generating a safe and feasible path through the ACFSM path planning algorithm while avoiding obstacles. The follower boat adjusts its own position based on the state of the pilot boat to maintain the predetermined formation shape.

[0312] Formation maintenance: The following boat dynamically adjusts its position according to the status of the pilot boat and the preset formation shape. The following boat not only follows the pilot boat, but also needs to maintain a relative position with other following boats in the formation.

[0313] Set the pilot boat position ( ) and the formation shape is a triangle. The relative position of follower boat 1 ( ) can be calculated by the following formula:

[0314]

[0315]

[0316] It is the preset distance between the lead boat and the following boat, that is, the fixed distance defined by the need to maintain the formation. It is the formation angle, that is, the angle between the following boat and the leading boat.

[0317]

[0318] Dynamic adjustment: When encountering obstacles or environmental changes, the pilot boat will re-plan the path, and the follower boats will automatically adjust the formation position according to the new state of the pilot boat. The formation will deform according to the actual situation to ensure safe driving. When the formation encounters an obstacle, it will avoid the obstacle based on the obstacle avoidance mechanism module and dynamically adjust the position based on the set formation rules.

[0319] The leader-follower control structure is designed. The pilot boat generates a path and guides the formation through path planning, obstacle avoidance, trajectory tracking and control modules. The follower boat dynamically adjusts its relative position based on the position information of the pilot boat and uses the control module to stay in the formation. Together, the unmanned boats can realize the coordinated formation control.

[0320] The above description is only the preferred embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

[0321] Although the above describes the specific implementation methods of the present invention, it is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art on the basis of the technical solution of the present invention without creative work are still within the scope of protection of the present invention.

Claims

1. A method for controlling an unmanned boat cooperative formation, characterized in that: The following steps are involved: S1, through the deployment of multiple sensors, real-time acquisition of static and dynamic environmental data of the ocean, including obstacles and visual environment changes; S2, the data acquired by each sensor are time synchronized, coordinate transformed, denoised and fused to obtain the total data set P after fusion, which contains the complete environmental data information of the sea surface and underwater; S3, using the multi-sensor fusion data P, constructs a three-dimensional grid map M, divides the environment into fixed grid cells, and each grid cell is marked by occupied state, idle state or unknown state; S4, based on the motion planning module, generates a real-time optimal path for the unmanned boat formation; the motion planning module is based on the fast marching method FMM and introduces the heading angle and the safety boundary potential map to obtain the angle transformation fast marching method ACFSM; the heading angle constrains the path change range to ensure that the movement of the unmanned boat meets its steering capability, and the safety boundary potential map increases the path's avoidance of obstacles to keep the path away from potential danger areas; S5, based on the obstacle avoidance mechanism module, the collision detection unit predicts the potential collision risk by calculating the distance to the nearest approach point DCPA and the time to the nearest approach point TCPA, and the fuzzy logic algorithm unit adjusts the heading and speed of the unmanned boat in real time according to the collision risk to ensure that the unmanned boat can gradually return to the predetermined path after avoiding obstacles; S6, based on the path tracking and control module, enables the unmanned boat to stably follow the adjusted path; Among them, the pilot boat generates a global path and avoids obstacles based on S1 to S6, and the follower boat dynamically adjusts its position through the formation rules with the pilot boat and the obstacle avoidance mechanism module in S5.

2. The method for controlling an unmanned boat cooperative formation according to claim 1, characterized in that: The specific process of constructing the three-dimensional grid map M in S3 is as follows: Determine the grid parameters: set the grid size to 0.5m × 0.5m × 0.5m, and the map range to 100m × 100m × 100m; Initialize the grid map: Create a three-dimensional array representing the grid map of the entire environment. Each grid cell is initialized to the "unknown" state, with a value of -1, indicating information that has not yet been detected. The array dimensions are derived based on the map range and the grid size. Marking grid cells: First, traverse the fused data P, analyze each piece of data in the fused data P, and identify the location of the object and the three-dimensional coordinate information of the obstacle; second, update the grid status; in the occupancy detection, if an object is detected at a grid position in the data set, the grid cell is marked as "occupied" and the value is set to 1; in the idle state, if no object is detected in a grid cell, the grid is marked as "idle" and the value is set to 0; in the undetected area, the grid cells not covered by the data set remain in the "unknown" state, and the grid marking formula is as follows: in, Indicates the total data set Unit data; Generate smoother results by calculating the average or weight of the state of a grid and its neighboring grids; Finally, all updated grid cell states are integrated to generate the final 3D grid map M.

3. The method for controlling an unmanned boat cooperative formation as claimed in claim 1, characterized in that: The motion planning module in S4 introduces the heading angle as follows: Assume the initial heading angle of the unmanned boat is , a heading range is defined for path planning. The shape of the heading range is fan-shaped, and its angle is determined by the turning radius and maximum yaw angle of the unmanned boat. Assuming that the turning radius of the unmanned boat is R and the angle of the heading range is α, the calculation formula is: Where W is the width of the unmanned boat, and the generation of path points is limited to the steering capability of the unmanned boat. That is, when calculating new path points, the direction of the path cannot deviate from the current heading angle of the unmanned boat. The range exceeds α / 2; During the path generation process, the grid point time update is limited as follows: in, is the arrival time, Based on the heading angle The adjustment amount, when the direction of the new point deviates from the current heading angle When larger, will increase, making the priority of this waypoint lower, thus avoiding sharp turns.

4. The method for controlling the coordinated formation of unmanned boats according to claim 3, characterized in that: The motion planning module in S4 introduces the safety boundary potential map as follows: Each grid point is assigned a value to indicate the safety of the point. The value ranges from 0 to 1. This value represents the shortest distance to the obstacle and is regarded as an indicator of the safety of the local point. The farther away from the obstacle, the higher the value. When the value is 1, it indicates a completely safe area, that is, far away from the obstacle. When the value is 0, it indicates the area where the obstacle is located, that is, the danger zone; The generation of the potential map depends on the distance between the unmanned boat and the obstacle, and the safety value of each grid point Calculated by the following formula: in, For point The distance to the nearest obstacle, is a constant term used to control the decay rate of the potential map. The closer the distance, the safer the value. The lower it is, the less safe the point is; A potential term is added to the time update formula so that the path avoids areas with low potential values ​​as much as possible; the time update formula is: in The state of the grid map is due to the low potential area Smaller, will become larger, thus increasing the time at that point and making the path more inclined to move away from obstacles.

5. The method for controlling the coordinated formation of unmanned boats according to claim 1, characterized in that: The collision detection unit is specifically: DCPA is used to represent the closest distance between two ships on their respective navigation paths, that is, the closest contact position between the ships. If the DCPA value is large, the ships have enough space to avoid collision during the journey; if the DCPA value is close to 0, it means that the ships may collide. The calculation formula is: in, are the current positions of unmanned boat 1 and unmanned boat 2 respectively; are the velocity vectors of unmanned boat 1 and unmanned boat 2 respectively; TCPA represents the time it takes for the unmanned boat to reach the closest point under the current heading and speed, that is, the time it takes for the two boats to be closest to each other. If the TCPA value is negative, it means that the unmanned boat has passed the closest point; if the TCPA value is small, it means that the two boats are about to approach and may collide. The calculation formula is: in, Represents the relative position unit vector of the unmanned boat 2; When the DCPA value is close to 0 or the TCPA value is close to 0, it indicates that the unmanned boat may collide, and the system initiates obstacle avoidance operation.

6. The method for controlling the coordinated formation of unmanned boats according to claim 5, characterized in that: The fuzzy logic algorithm unit processing process specifically includes: Input fuzzification: Convert continuous input variable data into fuzzy sets; input variables include collision area, relative collision angle and safety potential map value ; The collision area is defined as the relative position area between the unmanned boat and the obstacle, which is divided into eight areas: due north, northeast, due east, southeast, due south, southwest, due west, and northwest. to divide; The calculation of the relative collision angle Indicates the relative motion direction between the unmanned boat and obstacles or other ships, which is used to determine the obstacle avoidance strategy. The calculation formula is as follows: in, is the dot product of the velocity vector and the relative position vector; is the modulus of the velocity vector of the unmanned boat; is the modulus of the relative position vector, i.e., the distance between the unmanned boat and the obstacle; The safety potential map value Based on the generated potential map, determine the safety level of each grid point, from 0 to 1, the larger the value, the safer; Each input variable is fuzzified by the fuzzy membership function, and the relative collision angle is It is divided into three fuzzy areas: small: 0~60 degrees, indicating that the collision angle is small, the obstacle is in front of the unmanned boat, and the collision risk is high; medium: 30~150 degrees, indicating that the collision angle is medium, the obstacle is in front of the unmanned boat, and the collision risk is medium; large: 120~180 degrees, indicating that the collision angle is large, the obstacle is behind or far away from the unmanned boat, the collision risk is low, and the safety potential map is The membership function is divided into three sub-areas: dangerous, medium and safe; through the fuzzy membership function, the values ​​of the relative collision angle and the potential map are converted into fuzzy values ​​to generate a fuzzy set; Design fuzzy rule base: Based on the input fuzzy set, a set of fuzzy rules is used to determine the adjustment direction and speed of the unmanned boat; Defuzzification: Use the centroid method to convert the weighted average of the fuzzy membership into specific heading and speed adjustments. The formula of the centroid method is: in, is the output heading adjustment of each fuzzy rule, is the corresponding fuzzy membership; through defuzzification, a clear heading adjustment value is obtained and speed adjustment value ; According to the environmental information, the heading adjustment value and speed adjustment value are determined, and the path Adpath is adjusted in real time based on the original Path: According to the above formula, the system ensures a smooth offset based on the original path Path. After avoiding obstacles, the system will try to redirect the unmanned boat back to the Path path to reduce path deviation. The adjusted fusion path is: in, It is expressed as a dynamic fusion coefficient, which gradually decreases from 1 to 0 according to the distance after obstacle avoidance is completed.

7. The unmanned boat cooperative formation control method according to claim 1, characterized in that: The path tracking and control module includes a global path guidance algorithm unit and a heading controller; The global path guidance algorithm unit is used to provide a global path to ensure that the unmanned boat can gradually return to the correct trajectory through continuous heading adjustment when tracking the predetermined path; the heading controller is used to control the unmanned boat to stably follow the adjusted path; wherein, the heading controller adopts the feedback control deep deterministic optimization network DDON to optimize the proportional integral differential parameters of the controller to ensure the response accuracy and stability of the controller in complex environments.

8. The method for controlling the coordinated formation of unmanned boats according to claim 7, characterized in that: The global path guidance algorithm unit is specifically: A continuous vector field is generated around the predetermined path. The vector at each point represents the desired heading direction of the point. When the UAV deviates from the path, the vector field will generate a suitable heading adjustment direction according to the deviation of the UAV's position, and gradually guide the UAV back to the correct track. Specifically, a transition area is divided on both sides of the path, located on both sides of the center line of the path, and the width of the transition area is , when the deviation distance d of the unmanned boat is greater than When the unmanned boat is outside the transition area, it needs to sail in the direction of the path at a fixed angle. When the unmanned boat enters the transition area, it needs to gradually adjust its course according to the guidance rules; The control rules for global path guidance are set as follows: in, is the desired heading angle, is the path angle, represents the entrance angle, is the lateral deviation, and k are control parameters.

9. The unmanned boat cooperative formation control method according to claim 7, characterized in that: The heading controller is specifically: According to the model describing the heading dynamic characteristics of the ship, the Nomoto model is applied to the heading controller based on the feedback control DDON network. Its first-order transfer function is as follows: in, represents the heading angular velocity of the unmanned boat, represents the heading adjustment rudder angle, K represents the gain coefficient, T is the time constant, and , then the above formula is expressed as: In the time domain, the governing equations are expressed as: in, is the heading angle of the unmanned boat; The output control equation of the proportional-integral-derivative controller is: in, , , are proportional, integral and differential gain coefficients respectively, s represents the state of the system in the frequency domain, represents the desired heading angle, is the current heading angle, represents the rate of change of heading angle, Indicates that the proportional integral differential controller adjusts the width of the transition area; dynamically adjusts the rudder angle according to the deviation of the unmanned boat from the track , so that it returns to the desired course; For speed control, a speed controller based on feedback linearization is used; first, the speed error of the unmanned boat is calculated: in, is the expected speed, is the actual speed of the current unmanned boat; Secondly, according to the speed error, the proportional-integral-derivative controller generates the control signal S of the thruster, and adjusts the thrust of the thruster through proportional, integral and differential: in, , , are proportional, integral and differential gain coefficients respectively, and the control signal S is used to adjust the speed of the unmanned boat; Feedback linearization is used to handle nonlinear terms in propulsion dynamics. Assume that the propulsion model of the unmanned boat contains nonlinear terms. and the linear term , Indicates the surge speed, Represents the swing speed, and through feedback linearization, a new control signal is defined : After feedback linearization, the control signal S of the proportional-integral-differential controller is converted into the actual propulsion control signal of the unmanned boat through the feedback linearization model. The final thrust of the propeller is combined with the output of the proportional-integral-differential controller and the control term after feedback linearization, and the formula is: in, is the thrust of the propeller, is the nonlinear interference term, represents the effective mass of the unmanned boat, Indicates the actual surge velocity and swing velocity values.

10. The unmanned boat cooperative formation control method according to claim 7, characterized in that: The specific process of using feedback control deep deterministic optimization network DDON to optimize the proportional integral differential parameters of the controller is: First, the deep neural network DDON predicts the initial proportional integral differential parameters. The DDON network predicts the initial parameters by inputting the environmental feature vector X. , , : ， ， in, represents the neural network model, which performs nonlinear mapping through multiple hidden layers and activation functions. The number of hidden layers is 4, and the number of neurons in each layer decreases layer by layer. B represents the weight and bias parameters of the neural network, which are obtained through offline training. The model predicts the initial controller parameters through feature input. , , ; Secondly, local optimization adjustment calculation, DDON uses the gradient descent algorithm to adjust the parameters to minimize the control error L; L is the real-time control deviation of the system, that is, the sum of the squares of the path following error: in, is the target output, is the actual output of the unmanned boat, T is the time step; According to the control error L, adjust by gradient descent , , ; ， ， in, is the learning rate, which controls the update step size; assuming that e(t) is the system error, specifically the path deviation and speed deviation, the gradient of L is expressed as follows: in, It is found through the governing equations of the system state; Finally, the heading angle error and speed error are calculated according to the formula as the real-time error signal of the DDON prediction model; the PID parameters predicted by DDON are used to calculate the controller output S, which is used to adjust the thrust of the propeller and the heading of the unmanned boat.

Images