Disclosure of Invention
Aiming at the problems, the invention provides a control method for cooperative formation of unmanned ships, which comprises the following steps:
S1, acquiring static and dynamic environmental data of the ocean, including obstacle and visual environmental change, in real time through a plurality of distributed sensors;
s2, respectively carrying out time synchronization, coordinate conversion, data denoising and data fusion on the data acquired by each sensor to obtain a total data set P obtained after fusion, wherein the total data set P contains complete environmental data information of sea surfaces and underwater;
s3, constructing a three-dimensional grid map M by using data P fused by multiple sensors, dividing the environment into fixed grid units, and marking each grid unit by an occupied state, an idle state or an unknown state;
s4, generating a real-time optimal Path for unmanned ship formation based on a motion planning module, wherein the motion planning module is based on a fast travel method FMM and introduces a course angle and a safety boundary potential map to obtain an angle conversion fast travel method ACFSM;
s5, based on an obstacle avoidance mechanism module, calculating and predicting potential collision risks by a collision detection unit through a DCPA (dynamic pressure gradient algorithm) from a nearest approaching point and a TCPA (time TCPA) from the nearest approaching point, and adjusting the heading and the speed of the unmanned ship in real time according to the collision risks by a fuzzy logic algorithm unit to ensure that the unmanned ship can gradually return to a preset path after obstacle avoidance;
S6, enabling the unmanned ship to stably follow the adjusted path based on the path tracking and control module;
the navigation boat generates a global path and avoids barriers based on the steps S1 to S6, and the following boat dynamically adjusts the position through a formation rule with the navigation boat and the obstacle avoidance mechanism module in the step S5.
Preferably, the specific process of constructing the three-dimensional grid map M in S3 is as follows:
determining grid parameters, namely setting the size of the grid to be 0.5 meter multiplied by 0.5 meter, and setting the map range to be 100 meter multiplied by 100 meter;
initializing a grid map, namely creating a three-dimensional array to represent the grid map of the whole environment, initializing each grid unit to be in an unknown state, setting the value to be-1 to represent the information which is not detected yet, and obtaining array dimensions according to the map range and the grid size;
Marking grid cells, namely traversing the fusion data P, analyzing each piece of data in the fusion data P, identifying information such as the position of an object, the three-dimensional coordinates of an obstacle and the like, updating a grid state, wherein occupancy detection is carried out, marking a grid cell as occupied if the object is detected at a certain grid position in the data set, setting a value as 1, marking the grid as idle if the object is not detected in a certain grid cell, setting a value as 0, wherein the grid cell which is not covered by the data set is in an unknown state in an undetected area, and a grid marking formula is as follows:
Wherein,Representing the first of the total data setsIndividual cell data;
Generating smoother results by calculating the average value or weight of the state of a certain grid and the neighbor grids thereof;
And finally, integrating all updated grid cell states to generate a final three-dimensional grid map M.
Preferably, the motion planning module in S4 introduces a heading angle specifically as follows:
let the initial heading angle of unmanned ship beDefining a course range for path planning, wherein the course range is fan-shaped, the angle of the course range is determined by the steering radius and the maximum yaw angle of the unmanned ship, the steering radius of the unmanned ship is R, the angle of the course range is alpha, and the calculation formula is as follows:
wherein W is the width of the unmanned ship, the generation of the path point is limited within the steering capability range of the unmanned ship, namely, the direction of the path cannot deviate from the current course angle of the unmanned ship when calculating a new path pointExceeding the alpha/2 range;
In the path generation process, the grid point time update is limited as follows:
Wherein,In order for the time of arrival to be sufficient,Is based on course angleWhen the direction of the new point deviates from the current course angleWhen the number of the holes is large,Will increase such that the priority of the waypoint decreases, thereby avoiding sharp turns.
Preferably, the motion planning module in S4 introduces a safety boundary potential map specifically as follows:
Giving each grid point a value to represent the safety of the point, the value ranging from 0 to 1, the value representing the shortest distance to the obstacle, being regarded as an index indicating the safety of the local point, the farther from the obstacle, the higher the value, when the value is 1, the region of complete safety, i.e. away from the obstacle;
the generation of the potential map depends on the distance between the unmanned boat and the obstacle, the safety value of each grid pointCalculated by the following formula:
Wherein,Is taken as a pointThe distance to the nearest obstacle is set,As a constant term, for controlling the decay rate of the latent map, the closer the distance is, the safety valueThe lower the point is, the less secure the point is;
adding a potential term into a time updating formula so that the path avoids the area with a low potential value as much as possible, wherein the time updating formula is as follows:
wherein the method comprises the steps ofIn the state of a grid map, due to low potential areasThe size of the particles is smaller and the particles,Will become larger, increasing the time at that point, making the path more prone to areas away from the obstacle.
Preferably, the collision detecting unit specifically includes:
The DCPA is used for representing the closest distance between two ships on the navigation paths, namely the closest connection position between the ships, if the DCPA value is large, the ship has enough space to avoid collision during the running process, if the DCPA value is close to 0, the ship can collide, and the calculation formula is as follows:
Wherein,The current positions of the unmanned boats 1 and 2 are respectively; speed vectors of the unmanned ship 1 and the unmanned ship 2 respectively;
The TCPA is used for representing the time of the unmanned ship reaching the nearest joint under the current heading and speed, namely the time required by the two ships when the two ships are closest to each other, if the TCPA value is negative, the unmanned ship passes the nearest joint, and if the TCPA value is smaller, the unmanned ship is about to approach and possibly collide, and the calculation formula is as follows:
Wherein,A relative position unit vector representing the unmanned boat 2;
when the value of DCPA is close to 0 or the value of TCPA is close to 0, the unmanned ship is possibly collided, and the system starts obstacle avoidance operation.
Preferably, the processing procedure of the fuzzy logic algorithm unit specifically includes:
Input blurring, namely converting continuous input variable data into fuzzy sets, wherein the input variables comprise collision areas and relative collision anglesSafety potential map value;
The collision area is defined as the relative position area of the unmanned ship and the obstacle, and is divided into 8 areas of north, northeast, east, south, west and north, and the relative collision angle is calculatedDividing;
said calculating the relative collision angleRepresenting the relative direction of motion between the unmanned boat and an obstacle or other vessel, for determining an obstacle avoidance strategy, the calculation formula of which is as follows:
Wherein,Is the dot product of the velocity vector and the relative position vector; is the model of the unmanned ship velocity vector; Is a model of the relative position vector, i.e. the distance between the unmanned boat and the obstacle;
Safety potential map valueAccording to the generated potential diagram, determining the security level of each grid point, wherein the security level is from 0 to 1, and the larger the value is, the safer the value is;
Each input variable is fuzzified by a fuzzy membership function, corresponding to the collision angleThe three fuzzy areas are divided into three fuzzy areas, wherein the three fuzzy areas are small, 0-60 degrees are used for indicating that the collision angle is small, the obstacle is in front of the unmanned ship, the collision risk is high, the middle fuzzy areas are 30-150 degrees are used for indicating that the collision angle is medium, the obstacle is in front of the unmanned ship side, the collision risk is medium, the fuzzy areas are large, the number of the fuzzy areas is 120-180 degrees, the collision angle is large, the obstacle is in rear of or far from the unmanned ship side, the collision risk is low, and the safety potential diagram is lowThe membership functions of the map are divided into three sub-areas, namely danger, medium and safety, and the values of the relative collision angle and the potential map are converted into fuzzy values through fuzzy membership functions to generate a fuzzy set;
a fuzzy rule base is designed, wherein the adjusting direction and speed of the unmanned ship are determined by using a group of fuzzy rules according to the input fuzzy set;
deblurring, namely converting a weighted average value of fuzzy membership into specific heading and speed adjustment quantity by using a centroid method, wherein the centroid method has the following formula:
Wherein,Is the output heading adjustment amount for each fuzzy rule,Is a corresponding fuzzy membership degree, and a clear course adjustment value is obtained through defuzzificationAnd a speed adjustment value;
According to the environment information, a course adjustment value and a speed adjustment value are determined, and a Path Adpath is adjusted in real time on the basis of the originally determined Path:
According to the formula, smooth deviation is ensured on the basis of the original Path, after obstacle avoidance, the system can try to redirect the unmanned ship back to the Path so as to reduce Path deviation, and the adjusted fusion Path is as follows:
Wherein,And the dynamic fusion coefficient is expressed, and the dynamic fusion coefficient gradually decreases from 1 to 0 according to the distance after obstacle avoidance is completed.
Preferably, the path tracking and controlling module comprises a global path guiding algorithm unit and a course controller;
The global path guiding algorithm unit is used for providing a global path, ensuring that the unmanned aerial vehicle can gradually return to a correct track through continuous course adjustment when tracking a preset path, and the course controller is used for controlling the unmanned aerial vehicle to stably follow the adjusted path, wherein the course controller adopts a feedback regulation depth deterministic optimization network DDON to optimize proportional integral derivative parameters of the controller, and ensuring response precision and stability of the controller in a complex environment.
Preferably, the global path guidance algorithm unit specifically includes:
Generating a continuous vector field around the preset path, wherein the vector at each point represents the expected heading direction of the point, and when the unmanned aerial vehicle deviates from the path, the vector field generates a proper heading adjustment direction according to the deviation of the position of the unmanned aerial vehicle, so as to gradually guide the unmanned aerial vehicle to return to a correct track;
Specifically, two sides of the path are divided into a transition area which is positioned at two sides of the central line of the path, and the width of the transition area is thatWhen the deviation distance d of the unmanned ship is greater thanWhen the unmanned ship is positioned outside the transition area, the unmanned ship needs to navigate towards the path direction according to a fixed angle, and when the distance is smaller thanWhen the unmanned ship enters the transition area, the heading is required to be adjusted step by step according to the guiding rule;
The control rule for global path guidance is set as follows:
Wherein,Is the desired heading angle and,In order to be the path angle,The angle of entry is indicated as being indicative of the angle of entry,In the event of a lateral deviation,And k is a control parameter.
Preferably, the heading controller specifically includes:
According to a course dynamics characteristic model for describing a ship, a Nomoto model is adopted to be applied to a course controller based on a feedback regulation DDON network, and a first-order transfer function is as follows:
Wherein,Represents the heading angular rate of the unmanned boat,Represents the course adjustment rudder angle, K represents the gain coefficient, T is the time constant, letThe above formula is expressed as:
in the time domain, the control equation is expressed as:
Wherein,Is the course angle of the unmanned ship;
The output control equation of the proportional-integral-derivative controller is:
Wherein,,,Proportional, integral and differential gain coefficients, respectively, s represents the state of the system in the frequency domain,Indicating the desired heading angle of the vehicle,For the current heading angle,Indicating the rate of change of the heading angle,The width of the transition area is regulated by a proportional-integral-derivative controller, and the rudder angle is dynamically regulated according to the deviation track condition of the unmanned shipReturning it to the desired heading;
in speed control, a speed controller based on feedback linearization is used, and first, the speed error of the unmanned ship is calculated:
Wherein,Is the desired speed at which the vehicle is traveling,Is the actual speed of the current unmanned boat;
secondly, according to the speed error, the proportional integral derivative controller generates a control signal S of the propeller, and the propeller thrust is adjusted through proportion, integral and derivative:
Wherein,,,The control signal S is used for adjusting the speed of the unmanned ship;
linearizing nonlinear terms in propeller dynamics using feedback, assuming that the propulsive force model of the unmanned ship contains nonlinear termsAnd a linear term,Indicating the surge speed of the surge,Representing the wobble speed, defining a new control signal by feedback linearization:
After feedback linearization, the control signal S of the proportional-integral-derivative controller is converted into an actual propulsion control signal of the unmanned ship through a feedback linearization model, and the final propulsion is combined with the output of the proportional-integral-derivative controller and the control term after feedback linearization, and the formula is as follows:
Wherein,Representing the thrust of the propeller(s),As a non-linear interference term,Indicating the effective mass of the unmanned boat,Representing the actual surge speed and sway speed values.
Preferably, the specific process of optimizing the proportional integral derivative parameter of the controller by adopting the feedback control depth deterministic optimization network DDON is as follows:
first, the deep neural network DDON predicts the initial pid parameters, DDON predicts the initial parameters from the input environmental feature vector X,,:
,,
Wherein,Representing a neural network model, carrying out nonlinear mapping through a plurality of hidden layers and an activation function, wherein the hidden layer number is 4, and the neuron number of each layer is gradually decreased layer by layer,,;
Secondly, local optimization adjustment calculation is carried out, DDON uses a gradient descent algorithm to adjust parameters so as to minimize a control error L, wherein L is the real-time control deviation of the system, namely the square sum of path following errors:
Wherein,For the output of the object to be achieved,The actual output of the unmanned ship is that T is the number of time steps;
According to the control error L, by gradient descent adjustment,,;
,,
Wherein,For learning rate, controlling update step length, assuming e (t) is systematic error, specifically path deviation and speed deviation, the gradient of L is expressed as follows:
Wherein,Solving by a control equation of the system state;
finally, the heading angle error and the speed error are calculated according to a formula and used as real-time error signals of DDON prediction models, and the PID parameters predicted by DDON are used for calculating controller output S, wherein the output signals are used for adjusting the thrust of the propeller and the heading of the unmanned ship.
Compared with the prior art, the invention has the following beneficial effects:
1. And the comprehensive environmental perception is that by fusing radar, sonar and camera multi-sensor data, the system can construct a multi-dimensional environmental model to capture various environmental information. This fusion enables the system to provide more comprehensive and accurate sensory data in real time in complex marine environments, covering both surface and underwater obstacle information. The data of various sensors are mutually complemented, so that the reliability of the system is enhanced. Even if the data quality of one sensor is reduced, other sensors can still provide support, so that the system can stably operate under severe conditions. By uniformly processing different data formats, the problem that heterogeneous data are difficult to fuse is solved, and the consistency and consistency of data processing are ensured. The space-time synchronization and calibration technology ensures that each sensor accurately perceives the same space area at the same time, and improves the accuracy of the perceiving result.
2. The method is suitable for complex environments, and by introducing course angles and safety boundary potential diagrams, the path generated by the system is not only the shortest path, but also the dynamic characteristics of the unmanned ship are considered, so that the path is ensured to be smoother, and the problem that the path generated by the traditional fast travelling method (FMM) is unnatural or difficult to execute is avoided. The angle conversion rapid travelling method combines the movement capability and the environmental complexity of the unmanned ship, and particularly under a complex marine environment, the unmanned ship can smoothly travel along an optimized path. The path is prevented from generating an unrealistic motion trail in the obstacle-dense area.
3. The flexibility of obstacle avoidance is improved, namely the fuzzy logic does not need accurate mathematical modeling, and a flexible obstacle avoidance strategy can be generated according to dynamic environment factors. In particular, the fuzzy logic system is able to better handle various complex scenarios, such as dynamic obstacles or randomly moving vessels, in the face of a highly uncertain marine environment.
4. The accuracy of path following is improved, namely the global path guiding algorithm can quickly return to the correct path through guiding by generating a vector field around the path even if the unmanned ship deviates from a preset track, so that the accuracy of path following is ensured, and the method is suitable for a complex marine environment. The nonlinear dynamics problem of the unmanned ship is processed through a feedback linearization technology, and the improved controller can stably cope with disturbance inside and outside a system, so that the robustness and the response speed of the control system are ensured. The depth deterministic network optimization is used for dynamically adjusting parameters of the proportional-integral-derivative controller, so that an unmanned ship can find an optimal parameter combination according to different task requirements, and therefore overall control performance is optimized, and system response capability and tracking precision are improved. The feedback regulation DDON network course controller guided by the global path can regulate the thrust and the course of the propeller in real time, ensure that the unmanned ship can stably follow the path, avoid overlarge course deviation caused by external environment change and improve the reliability of the system.
Detailed Description
The invention will be further described with reference to specific examples.
The invention aims to solve the technical problems of environmental perception, path planning, collision avoidance decision, real-time control and the like in the prior art by adopting an innovative technical scheme, provides a new solution for realizing efficient collaborative operation of a plurality of unmanned boats in a complex marine environment, has the overall process shown in figure 1, and comprises the following steps:
S1, acquiring static and dynamic environmental data of the ocean, including obstacle and visual environmental change, in real time through a plurality of distributed sensors;
s2, respectively carrying out time synchronization, coordinate conversion, data denoising and data fusion on the data acquired by each sensor to obtain a total data set P obtained after fusion, wherein the total data set P contains complete environmental data information of sea surfaces and underwater;
s3, constructing a three-dimensional grid map M by using data P fused by multiple sensors, dividing the environment into fixed grid units, and marking each grid unit by an occupied state, an idle state or an unknown state;
s4, generating a real-time optimal Path for unmanned ship formation based on a motion planning module, wherein the motion planning module is based on a fast travel method FMM and introduces a course angle and a safety boundary potential map to obtain an angle conversion fast travel method ACFSM;
s5, based on an obstacle avoidance mechanism module, calculating and predicting potential collision risks by a collision detection unit through a DCPA (dynamic pressure gradient algorithm) from a nearest approaching point and a TCPA (time TCPA) from the nearest approaching point, and adjusting the heading and the speed of the unmanned ship in real time according to the collision risks by a fuzzy logic algorithm unit to ensure that the unmanned ship can gradually return to a preset path after obstacle avoidance;
S6, enabling the unmanned ship to stably follow the adjusted path based on the path tracking and control module;
the navigation boat generates a global path and avoids barriers based on the steps S1 to S6, and the following boat dynamically adjusts the position through a formation rule with the navigation boat and the obstacle avoidance mechanism module in the step S5.
1. Data acquisition and environmental awareness
The unmanned ship is used for collecting environmental data including information of obstacles, other ships and environmental conditions through the sensor, and establishing a grid map of the environment as input of path planning and obstacle avoidance. This process is critical to the performance of the model.
The unmanned ship system needs to acquire surrounding environment data in real time, so that cooperation and effective path planning among multiple ships are ensured. In order to obtain real unmanned ship cooperative formation control environment data, the unmanned ship is adopted to acquire data information of the marine environment through the carried sensors, so that multi-sensor fusion is formed. The unmanned ship senses the environment through various sensor devices, so that environment information and dynamic data inside and outside the formation are acquired in real time. The multi-sensor comprises a radar (24 GHz mm of working bandwidth, 1km of detection distance, 10Hz of scanning frequency and 20 cm of resolution, data are stored in a two-dimensional grid polar coordinate format), a sonar (5 Hz of data sampling rate, 70 m of detection distance and 15 cm of resolution, data are stored in a three-dimensional grid polar coordinate format), and a camera (1920 x 1080 pixels of resolution, 80fps of frame rate, 3.6 mm of focal length and PNG format). The data from these sensors include not only static and dynamic obstructions to the water surface (e.g., seafloor terrain, other vessels), but also real-time changes in the environment (e.g., sea waves, wind speed). The radar is mainly used for detecting obstacles on the sea surface, including other ships and floaters, the sonar is mainly used for underwater environment detection and comprises underwater topography and obstacles, such as submerged reefs and submarines, and the camera is mainly used for acquiring image information of the water surface environment, including the obstacles above the sea surface and visual environment changes. In order to ensure the accuracy and consistency of data, the unmanned ship needs to stabilize the sensor in the acquisition process, so that external interference is avoided.
2. Data preprocessing
It will be appreciated that in the multi-sensor environmental awareness of unmanned boats, the types of data acquired by the radar, sonar, and camera are different.
Specifically, the radar collects typically spectral data. The radar emits electromagnetic waves, detects the frequency and the intensity of echo signals, and calculates the distance, the speed and the azimuth of a target object through spectrum analysis. The spectral data can be converted into two-dimensional polar coordinate point cloud data for representing the target position and the relative motion characteristics thereof. The sonar acquires acoustic echo data, which is expressed in the form of a three-dimensional point cloud. The sonar emits sound waves, and the depth and the distance of the underwater object are measured by receiving sound wave reflection signals. And generating grid or point cloud data in a three-dimensional space through the relation between time and reflected signal intensity, and reflecting the positions and shapes of underwater terrains and objects. The camera collects two-dimensional image data. The image data provides visual information that can provide visual features for object recognition and target detection.
In order to integrate the different data types acquired by the multisource sensors, data preprocessing is required. The data processing flow of this part is shown in fig. 2.
1. Time synchronization:
because the data acquisition frequencies of the radar, the sonar and the camera are different, the data of the sensors are needed to be time-synchronized at first, so that the data at the same moment can correspond to the same environment state, and the data acquisition of the multiple sensors in the invention takes seconds (t) as a unit, so that the system has enough response speed. After the data is acquired, a time stamp mechanism is adopted to align the acquired data.
Because of the different sampling frequencies of the sensors, time alignment of the data of each sensor is required. Setting the sampling moments of the radar, the sonar and the camera as respectively,. Introducing global time referenceTime is subjected to the following steps:
Wherein,Representing the actual sampling time of each sensor. Adjusting the data of each sensor to a uniform global time point by a time stamp alignment method。
2. Coordinate conversion:
Radar, sonar, and cameras are typically represented using different coordinate systems and data formats, so the data from all sensors is converted to the same reference coordinate system prior to fusion. Let the polar coordinates of the radar data be expressed as @) The polar coordinates of sonar data are expressed as%) The camera data are two-dimensional image coordinates (u, v). And respectively performing polar coordinate conversion on the three sensor data.
(1) The polar coordinate conversion of the radar, namely the radar provides remote detection information of two-dimensional polar coordinates, the remote detection information is converted into three-dimensional rectangular coordinates, the data on the sea surface are obtained, and a data set is expressed as. Wherein the method comprises the steps ofFor the distance of the object to be a target,Is azimuth. Converts the three-dimensional rectangular coordinate system representation into a three-dimensional rectangular coordinate system representation, the formula is as follows:
(2) Polar coordinate conversion of sonar, which provides depth information under water. After coordinate transformation, information describing the underwater environment and the position of the obstacle is obtained, and the data set is expressed as a data set expressed as. Wherein the method comprises the steps ofFor the distance of the object to be a target,Is a horizontal angle, and is provided with a plurality of grooves,Is a vertical angle. Converts the three-dimensional rectangular coordinate system representation into a three-dimensional rectangular coordinate system representation, the formula is as follows:
(3) And (5) converting polar coordinates of the camera. Where u, v are the pixel coordinates of the image. Data setAnd (3) representing. Converting it into three-dimensional rectangular coordinate system to represent, knowing the depth value of the pixel of the imageIs known. The formula is as follows:
Wherein,Is the focal length of the camera in the horizontal and vertical directions,Is the optical center coordinates of the camera image plane, i.e. the coordinates of the center point of the image.
3. Denoising data:
In an unmanned boat multi-sensor fusion system, the key step of data preprocessing is denoising. The method aims to reduce noise caused by sensor hardware, external interference and the like, so that the accuracy of sensor data is improved. This is critical to subsequent path planning, obstacle avoidance, environmental awareness, etc.
(1) Radar data denoising-radar detects a target by transmitting and receiving electromagnetic waves, and the data collected by the radar contains noise points (incorrect distance, azimuth angle, etc.). The invention adopts a random sampling consistency algorithm to carry out data denoising processing.
The implementation steps of the method comprise the steps of firstly, randomly selecting a sub-data set from radar data and fitting the sub-data set into a linear model, secondly, calculating the distance between other points and the model, judging whether the other points are inner points (accord with the model), and finally, repeating the steps until the optimal model is found. The model fitting formula is as follows:
Wherein,Is the i-th point of the radar data,Is a point on the model; is a distance threshold, and when the distance between the point and the model is smaller than the distance, the point is an inner point. The error data information caused by reflection can be effectively removed according to the random sampling consistency algorithm.
(2) Sonar data denoising, in which sonar uses sound waves to detect underwater environments, is generally affected by noise such as underwater reflection, multipath interference, echo, and the like. The invention adopts median filtering to remove speckle noise. Median filtering is a common nonlinear filtering that can effectively remove speckle noise. The principle is that all pixel values are ordered within a certain window, and the median value is selected as the new value of the current pixel. Since speckle noise typically results in abrupt changes in pixel values, median filtering can smooth these abrupt values, eventually effectively removing the speckle noise.
(3) Denoising camera data, namely, the image data of the camera is generally influenced by light change, sensor thermal noise, environmental noise and the like, so that the problems of Gaussian noise, speckle noise and the like of the image are caused. The invention uses gaussian filtering to remove high frequency noise from the image. Gaussian filtering by convolution with the gaussian kernel can effectively smooth the image and remove noise. Specifically, the calculation formula of the gaussian filter is:
Where σ is the standard deviation of the gaussian kernel and u and v represent the pixel positions in the image, respectively, to represent the offset of each pixel point 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. Gaussian filtering can effectively reduce gaussian white noise in an image by convolving the image.
The quality of data collected by the unmanned ship under water can be obviously improved and noise interference is removed through the processing of a random sampling consistency algorithm, median filtering and Gaussian filtering.
4. Data fusion:
the data fusion aims to combine the environmental information of different sensors together and provide a high-precision environmental model for subsequent path planning, obstacle avoidance and target recognition and tracking tasks.
And combining the data of the radar, the data of the sonar and the data of the camera to obtain complete water and underwater environment perception diagrams. The total data set obtained after fusion is: . The data set contains complete obstacle information for the sea surface and the water.
A three-dimensional environment model is constructed through data fusion, and all perception information from radar, sonar and cameras is contained. Such data set fusion is suitable for global context awareness, for convenience, the total data setDenoted as P, as a data set of the unmanned aerial vehicle cooperative formation control after the pretreatment.
3. Grid map construction
Grid mapping is a process of dividing an environment into grid cells of a fixed size in order to efficiently represent and process environment information. In this process, the environmental data P collected by the multi-sensor fusion is fused and converted into a three-dimensional grid structure, and each grid cell corresponds to a specific area and is marked as an "occupied", "idle" or "unknown" state. The method enables the unmanned ship to update the environment model in real time, realizes autonomous navigation, path planning and dynamic obstacle avoidance, improves the safety and efficiency of the unmanned ship in a complex environment, and the grid map construction process is shown in fig. 3.
Determining grid parameters, namely setting the size of the grid to be 0.5 m multiplied by 0.5 m. The map range is set at 100m×100 m.
Initializing a grid map, namely creating a three-dimensional array, and representing the grid map of the whole environment. Each grid cell is initialized to an "unknown" state (value set to-1), indicating information that has not been detected. And obtaining array dimensions according to the map range and the grid size.
Marking the grid unit, namely traversing the data set P, analyzing each piece of data in the data set P, identifying the position of an object, the three-dimensional coordinates of an obstacle and the like, and updating the grid state. Occupancy detection-if an object is detected at a grid location in the dataset, the grid cell is marked as "occupied" (value set to 1). Idle state-if no object is detected within a certain grid cell, the grid is marked as "idle" (value set to 0). Undetected area-grid cells not covered by the dataset remain in an "unknown" state. The grid marking formula is as follows:
Wherein,Representing the first of the total data setsAnd unit data.
Map smoothing and optimization, namely ensuring that grid maps updated at different time points do not collide. And carrying out consistency check on the states of the adjacent grids so as to improve the reliability of the whole map. By calculating the average value or weight of the state of a certain grid and its neighbor grids, smoother results are generated. If a grid is mostly "idle" around and the grid is shown as "occupied", possibly noise-induced, the grid state may be adjusted to "idle" by a smoothing algorithm.
And finally generating a grid map, namely integrating all updated grid cell states according to the above, and generating a final three-dimensional grid map M.
And updating in real time, namely continuously collecting new data and updating the three-dimensional grid map in real time in the actual operation process of the unmanned ship so as to adapt to a dynamic environment. The safe navigation and efficient execution of the unmanned ship in a complex environment are ensured.
4. Motion planning module design
In unmanned ship cooperative formation control, motion planning is a core part for ensuring that a plurality of unmanned ships can efficiently and safely operate in a complex marine environment. The main task of motion planning is to generate an optimal path from a starting point to a target point of an unmanned ship, avoid collision, determine an initial navigation route for a piloting ship and a following ship, and adjust the heading in time when encountering a dynamic obstacle. In order to solve the problem, the invention provides an angle transformation rapid advancing method (ANGLE CHANGE FAST-marching square method, ACFSM) for the movement characteristics of the unmanned ship, which can effectively solve the problems of dynamic obstacle avoidance and course control and enable the unmanned ship to stably and safely complete navigation tasks.
And constructing a fast traveling method using angle guidance, and performing global path planning. The Method is based on classical Fast-marching Method (FMM) by solving Eikonal equations. The ACFSM method considers the course angle and the safety boundary potential map of the unmanned ship to improve the planning effect.
And according to the total data set P obtained through data fusion, obtaining an environment modeling M through a grid map construction process, and using the environment modeling M as an application scene of unmanned aerial vehicle cooperative formation control motion planning, so that unmanned aerial vehicle can realize the cooperative formation control motion planning in the scene.
In the motion planning of unmanned ship cooperative formation control, a fast traveling method is used as a basic algorithm to have important significance barrier. The paths generated by the FMM can be optimal paths provided for each unmanned boat in the formation without damaging the formation structure. By using the same grid in the environment, all unmanned boats employ heading angles and safety boundary potential maps, ensuring that the vessels in the fleet can act in concert.
The core of the FMM is to simulate the process of propagation from the starting point outwards by iteratively solving the Eikonal equation. It is possible to efficiently calculate the arrival time of each grid point and generate a globally optimal path. The method is suitable for irregular ocean environments, can process various terrains and obstacle distribution, and is suitable for dynamically changing environments. The Eikonal equation is of the form:
Wherein,Is the boundary arrival pointIs used for the time period of (a),Is the propagation velocity. Assume thatIs a point on the grid that requires computation. And (3) withAdjacent points are sets of four grid points, respectively:,,,。 the calculation of (2) can be calculated according to the following formula:
1. Introduction of heading angle
It will be appreciated that in a classical FMM, the generation of the path does not take into account the actual motion constraints of the unmanned boat. The aim of introducing the course angle is to limit the variation range of the path, so that the course angle accords with the motion limit of the ship, and actions which cannot be practically executed such as sharp turns are avoided.
Let the initial heading angle of unmanned ship beThe movement of the unmanned boat is limited by the turning radius. In order to match the planned path to the heading angle, a heading range is defined for the path plan in ACFSM algorithm. The shape of the heading range can be seen as a sector whose angle is determined by the steering radius and the maximum yaw angle of the unmanned boat. Assuming that the steering radius of the unmanned ship is R, the angle of the heading range is alpha, and the calculation formula is as follows:
Wherein W is the width of the unmanned boat. In order to ensure smoothness and performability of the path, the design limits the generation of the path points to the steering capability range of the unmanned boat. I.e. the direction of the path cannot deviate from the current heading angle of the unmanned ship when calculating the new path pointExceeding the alpha/2 range.
Further, in the path generation process, since the FMM algorithm performs time update on each grid point, the arrival time is calculated. In order to introduce heading angle into this process, the new grid point time update is limited in the present invention as follows:
Wherein,Is based on course angleIs used for adjusting the adjustment amount of the (a). When the direction of the new point deviates from the current course angleWhen the number of the holes is large,Will increase such that the priority of the waypoint decreases, thereby avoiding sharp turns.
2. Introduction of security boundary potential maps
In addition to the heading angle constraints, the ACFSM method of the present design further optimizes the path away from obstacles and other potential risks by introducing a safety boundary potential map.
Specifically, the potential map gives each grid point a value representing the safety of the point, the range of values being 0 to 1, this value representing the shortest distance to the obstacle, as an index indicating the safety of the local point. The farther from the obstacle, the higher the value. When the value is 1, it indicates a completely safe area, i.e. far from the obstacle, and when the value is 0, it indicates an area where the obstacle is located, i.e. a dangerous area, and is therefore unsafe. Therefore, the unmanned boat should navigate in a high value area.
The generation of the potential map depends on the distance between the unmanned boat and the obstacle, the safety value of each grid pointCalculated by the following formula:
Wherein,Is taken as a pointThe distance to the nearest obstacle is set,Is a constant term used to control the decay rate of the latent map. Thus, the closer the distance, the security valueThe lower the point is, the less secure the point is.
ACFSM takes into account not only the arrival time in the path generation processWill also incorporate the security value of the latent map. That is, a potential term is added to the time update formula of the FMM so that the path avoids the area with low potential value (i.e., close to the obstacle) as much as possible. The path updating not only considers the arrival time, but also combines the state of the grid mapAnd (5) performing calculation. According to the network marking formula and the time updating limit formula, the time updating formula is as follows:
Due to low potential areasThe size of the particles is smaller and the particles,Will become larger, increasing the time at that point, making the path more prone to areas away from the obstacle.
By adding the course angle and the safety boundary potential diagram, the course angle passes through the constraint path change range, so that the movement of the unmanned ship accords with the steering capability of the unmanned ship, and the generation of a hard turning path which cannot be executed is avoided. The safety boundary potential map increases the safety of sailing by increasing the avoidance of the path to the obstacle so that the path is far away from the potential dangerous area.
Finally, ACFSM is combined with the course angle and the safety boundary potential map, so that the safety path which is more in line with the actual motion constraint of the unmanned ship can be generated under the environment-aware grid map M while the smoothness of the path is ensured. The improvement mode adopted in the invention enables the ACFSM method to be suitable for complex ocean environments, particularly in the movement planning of unmanned ship formation, and can effectively cope with dynamic obstacle and Path adjustment and determine the preset initial sailing route Path.
5. Obstacle avoidance mechanism module design
The design of the obstacle avoidance mechanism module mainly comprises a collision detection unit and a fuzzy logic algorithm unit. Real-time monitoring Path along preset sailing routeObstacles or other vessels that may be encountered during travel, the path is adjusted as needed to avoid collisions. When potential collision is detected, the collision prevention mechanism can adjust heading according to the relative position and speed of the shipSum speed of. The specific process comprises the following steps:
collision detection unit the collision detection unit is responsible for determining if there is a potential collision in the course if two vessels have a potential collision at some point of the desired trajectory. To successfully eliminate the collision situation, the distance to the nearest point of approach (DCPA) and the time to the nearest point of approach (TCPA) must be calculated.
In particular, DCPA represents the closest distance between two vessels on the respective sailing paths, i.e. the closest splice location between the vessels. If the DCPA value is large, the ship has enough space to avoid collision during running, and if the DCPA value is close to 0, the ship can collide. The calculation formula is as follows:
Wherein,The current positions of the unmanned boats 1 and 2 are respectively; The velocity vectors of unmanned boat 1 and unmanned boat 2, respectively.
Further, TCPA represents the time for an unmanned boat to reach the nearest junction, i.e., the time required for two boats to be closest to each other, at the current heading and speed. If the TCPA value is negative, it indicates that the unmanned boat has passed the nearest joint, and if the TCPA value is small, it indicates that the two boats are approaching, and a collision may occur. The calculation formula is as follows:
Wherein,A relative position unit vector representing the unmanned boat 2.
When the value of DCPA approaches 0 or the value of TCPA approaches 0, indicating that an unmanned boat may collide, the system may initiate obstacle avoidance operations.
The invention designs a fuzzy logic based obstacle avoidance algorithm, and adjusts the course and speed of the unmanned ship based on a collision detection Result through a fuzzy rule so as to avoid collision. The fuzzy logic algorithm module comprises the following steps:
Input blurring, namely in an obstacle avoidance system of an unmanned ship, the blurring is to convert continuous input data into a fuzzy set. In the fuzzy logic unit of the present invention, the input variables mainly include:
The collision area is defined as the relative position area of the unmanned ship and the obstacle (divided into 8 areas, ① - ⑧: north, northeast, east, south, west, north and west), and the relative collision angle is calculatedTo divide.
Relative angle of impactRepresenting the direction of relative movement between the unmanned boat and the obstacle (or other vessel) for determining the obstacle avoidance strategy. The calculation formula is as follows:
Wherein,Is the dot product of the velocity vector and the relative position vector; is the model of the unmanned ship velocity vector; Is a model of the relative position vector, i.e. the distance between the unmanned boat and the obstacle.
Safety potential map valueThe security level (from 0 to 1) of each grid point is determined according to the generated potential diagram, and the larger the value is, the safer the value is.
Each input variable is blurred by a fuzzy membership function. Will be relative to the angle of impactThe unmanned ship can be divided into three fuzzy areas, wherein small (v 1: 0-60 degrees) means that the collision angle is small, the obstacle is in front of the unmanned ship, and the collision risk is high. The middle (v 2: 30-150 degrees) represents a moderate collision angle, the obstacle is in front of the unmanned ship side, and the collision risk is moderate. A large (v3:120-180 degrees) means that the collision angle is large, the obstacle is behind or far from the unmanned ship side, and the collision risk is low. Safety potential diagramThe membership functions of (a) are divided into three sub-areas, dangerous, medium and safe. The corresponding membership degrees are respectively as follows:
values of the relative collision angle and the potential map are converted into fuzzy values by the fuzzy membership functions.
The fuzzy values represent membership bases of the input variables in the fuzzy regions. The relative collision angle can belong to two areas of medium and small according to the value, and the membership degree of the relative collision angle in each area is calculated according to the membership function. For the security potential values, depending on the potential value size, both "medium" and "security" regions may be attributed, and the membership function will also calculate the membership degree for each region.
And designing a fuzzy rule base, namely determining the adjustment direction and speed of the unmanned ship by using a group of fuzzy rules according to the input fuzzy set. The rule base is designed according to COLREGs, 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, and if the obstacle is on the rear, no adjustment is needed. The general form of setting the fuzzy rule is as follows:
IF IS small ANDIS high THEN heading adjustmentBig size
Defuzzification, defuzzification is the process of converting a fuzzy output into an actual control signal. In the design of the invention, the defuzzification uses a centroid method to convert the weighted average of fuzzy membership into specific heading and speed adjustment. The centroid method has the formula:
Wherein,Is the output heading adjustment amount for each fuzzy rule,Is the corresponding fuzzy membership. By defuzzification, the system can obtain a clear heading adjustment valueAnd a speed adjustment valueThereby performing an obstacle avoidance action.
According to the environment information, a heading adjustment value and a speed adjustment value are determined, and the Path Adpath is adjusted in real time on the basis of the originally determined Path.
The smooth offset on the basis of the original Path is ensured according to the above formula. After obstacle avoidance, the system may attempt to redirect the unmanned boat back to the Path to reduce Path deviation. The adjusted fusion path is as follows:
Wherein,And the dynamic fusion coefficient is expressed, and the dynamic fusion coefficient gradually decreases from 1 to 0 according to the distance after obstacle avoidance is completed.
6. Path tracking and control module design
When the unmanned ship completes obstacle avoidance decision, a guiding module is required to ensure that the formation keeps a preset track and formation. According to the invention, the course and the speed of the unmanned ship are adjusted through the feedback regulation DDON network for designing the global path guidance, and the propeller and the rudder angle are adjusted to ensure that each ship accurately executes the planned path. The unmanned ship is ensured to advance according to the planned path through feedback control. After obstacle avoidance or path adjustment, the controller quickly adjusts the new target heading and speed. The unmanned ship is guaranteed to continue to run according to the adjusted path, and the preset formation shape is maintained.
1. Global path guidance algorithm unit:
The main purpose of the global path guidance algorithm is to provide a global path that ensures that the unmanned craft can gradually return to the correct trajectory through continuous heading adjustments while tracking the intended path.
Specifically, the global path guidance algorithm generates a continuous vector field around the predetermined path, with the vector at each point representing the desired heading direction for that point. When the unmanned ship deviates from the path, the vector field can generate a proper course adjustment direction according to the deviation of the position of the unmanned ship, and the unmanned ship is guided to return to the correct track step by step. Second, unmanned boats have complex navigation environments, and dynamic obstructions or other uncertainty factors may be present in the path. The vector field guidance can enable the unmanned aerial vehicle to gradually return to the track through local course adjustment without interfering with overall path planning.
Specifically, two sides of the path are divided into a transition area which is positioned at two sides of the central line of the path and has the width of. When the deviation distance d of the unmanned ship is greater thanWhen the unmanned ship is positioned outside the transition area, the unmanned ship needs to navigate towards the path direction according to a fixed angle. When the distance is smaller thanWhen the unmanned ship enters the transition area, the heading is required to be adjusted step by step according to the guiding rule.
The control rule for global path guidance is set as follows:
Wherein,Is the desired heading angle and,In order to be the path angle,The angle of entry is indicated as being indicative of the angle of entry,In the event of a lateral deviation,And k is a control parameter.
Continuous course adjustment information can be provided for the unmanned ship by designing a global path guiding algorithm, and smoothness of path tracking is guaranteed.
2. And (3) designing a course controller of a feedback regulation DDON network:
In the path tracking control of the unmanned ship, a proportional-integral-derivative controller is mainly used for accurately adjusting the heading and speed of the unmanned ship, so that the unmanned ship can effectively follow the path indication provided by the vector field. A pid controller is essentially a linear controller that controls the system by simple proportional, integral and differential adjustments. However, unmanned vessels' movements are nonlinear, especially in marine environments, where external disturbances can cause complex dynamics. The proportional-integral-derivative controller has difficulty in accurately handling these complex dynamics in the face of nonlinearities, which can lead to undesirable control effects. Therefore, the invention provides a course controller of a global path-guided feedback regulation DDON network, which is used for unmanned aerial vehicle cooperative formation control.
The speed controller is an important component to ensure that the unmanned craft maintains a desired speed and maintains a predetermined formation while the unmanned craft performs collaborative work. In complex marine environments, the speed controller can dynamically adjust the speed of the vessel when various static and dynamic obstacles are encountered, to cope with different sailing situations, such as obstacle avoidance and path tracking.
Specifically, according to a course dynamics characteristic model for describing a ship, a Nomoto model is adopted to be applied to a course controller based on a feedback regulation DDON network, and a first-order transfer function is as follows:
Wherein,Represents the heading angular rate of the unmanned boat,Represents the course adjustment rudder angle, K represents the gain coefficient, T is the time constant, letThe above formula can be expressed as:
in the time domain, the control equation can be expressed as:
Wherein,Is the course angle of the unmanned ship.
Further, the output control equation of the pid controller is:
Wherein,,,Proportional, integral and differential gain coefficients, respectively, s represents the state of the system in the frequency domain,Indicating the desired heading angle of the vehicle,For the current heading angle,Indicating the rate of change of the heading angle,Indicating that the pid controller adjusts the width of the transition region. Through the controller, the proportional-integral-derivative system can dynamically adjust the rudder angle according to the deviation track condition of the unmanned shipReturning it to the desired heading.
Further, in the speed control, a speed controller based on feedback linearization is used. The forward speed of the unmanned ship is controlled by adjusting the power of the propeller in real time, so that the unmanned ship can keep a preset speed, and meanwhile, the speed can be adjusted as required to avoid obstacles or other dynamic behaviors.
First, calculating the speed error of the unmanned ship:
Wherein,Is the desired speed at which the vehicle is traveling,Is the actual speed of the current unmanned boat.
Secondly, according to the speed error, the proportional integral derivative controller generates a control signal S of the propeller, and the propeller thrust is adjusted through proportion, integral and derivative:
Wherein,,,The control signal S is used to adjust the speed of the unmanned aerial vehicle, respectively proportional, integral and differential gain coefficients.
To deal with nonlinear terms in propeller dynamics, feedback linearization is used. Assuming that the propulsive force model of the unmanned ship contains nonlinear termsAnd a linear term,Indicating the surge speed of the surge,Representing the wobble speed, defining a new control signal by feedback linearization。
In this way, the non-linear term is eliminated, the system becomes a linear control problem,Is a control signal in a linear system. After feedback linearization, the control signal S of the proportional-integral-derivative controller is converted into an actual propulsion control signal of the unmanned aerial vehicle through a feedback linearization model. The final propeller thrust combines the output of the proportional integral derivative controller and the control term after feedback linearization, and the formula is:
Wherein,Representing the thrust of the propeller(s),As a non-linear interference term,Indicating the effective mass of the unmanned boat.Representing the actual surge speed and sway speed values.
3. And (3) parameter design of a depth deterministic network optimization proportional-integral-derivative controller:
The proportional-integral-derivative controller is used for controlling the heading and speed of the unmanned ship, and the stable navigation of the unmanned ship on a preset path is kept by adjusting the thrust and rudder angle of the propeller. However, parameters of the conventional pid controller are generally determined empirically, and in a complex dynamic marine environment, the static and dynamic obstacles change at a high speed, and the pid controller responds slowly when coping with the changes, which easily causes tracking errors or cannot avoid the obstacles in time. Meanwhile, the motion dynamics of unmanned ships are nonlinear, and the unmanned ships contain the influence of external disturbance such as stormy waves, so that the proportional-integral-derivative controllers with fixed parameters are difficult to provide accurate control.
In unmanned ship formation control and path tracking, parameters of a controller,,Directly affects the speed, heading and track tracking accuracy of the unmanned boat. The invention provides a depth deterministic optimization network, which can automatically find the optimal parameter combination in a complex environment and ensure the optimal performance of the system. The deep deterministic optimization network (DEEP DETERMINISTIC Optimization Network, DDON) is a novel control strategy combining deep learning and optimization algorithm, and dynamically adjusts control parameters in a real-time control system. DDON utilizes the strong prediction capability of the deep neural network, learns the nonlinear relation between the environmental characteristics and the optimal control parameters in the control system, outputs proper parameter combinations in real time, and further carries out fine tuning through a local optimization algorithm to ensure that the optimal control effect is achieved in complex and dynamic environments.
Generating a grid map and a total data set P according to the obtained result, and transversely shifting the unmanned ship to the target pathDesired speedAnd the current speedHeading angle of targetAnd the current heading angleInput feature vector x= [ as current environment,];
First, the deep neural network DDON predicts the initial pid parameters. DDON network predicts preliminary parameters by input environmental feature vector X,,。
,,
Wherein,The method is characterized in that the hidden layer number is4, the number of neurons in each layer is gradually decreased layer by layer, and the weight and bias parameters of the neural network are represented by B, and the neural network is obtained through offline training. Model prediction of initial controller parameters by feature input,,。
And secondly, locally optimizing and adjusting calculation. DDON adjust the parameters using a gradient descent algorithm to minimize the control error L. L is the real-time control deviation of the system, i.e. the sum of squares of the path following errors:
Wherein,For the output of the object to be achieved,The actual output of the unmanned ship is that T is the number of time steps.
Further, according to the control error L, the gradient descent is adjusted,,。
,,
Wherein,For learning rate, the update step is controlled. Assuming that e (t) is the systematic error, path deviation and speed deviation in the present invention, the gradient of L can be expressed as follows:
Wherein,Is calculated by a control equation of the system state.
And finally, calculating feedback errors. And calculating the course angle error and the speed error according to a control signal S calculation formula. As a real-time error signal for DDON prediction models. And calculating the output of the proportional-integral-derivative controller. The controller output S is calculated using DDON predicted PID parameters. The output signal is used to adjust the propeller thrust and unmanned ship heading.
DDON the overall optimization process is as follows:
1) Offline training, namely modeling a neural network through a large amount of simulation dataTraining, and learning a mapping relation between environmental characteristics and optimal PID parameters;
2) In real control, DDON neural network model predicts initial according to input environmental characteristic X,,;
3) Local optimization fine tuning, namely performing error-driven fine tuning on proportional-integral-derivative parameters output by a neural network by using a gradient descent algorithm, so as to ensure that control errors are minimized;
4) And (3) feedback updating, namely feeding back control errors and new environment characteristics in each period, and repeatedly carrying out real-time prediction and optimization to ensure the optimal performance of the controller in a dynamic environment.
And adjusting the heading and speed of the unmanned ship according to the feedback regulation DDON network guided by the global path, and adjusting the propeller and rudder angle to ensure that each ship accurately executes the planned path. The unmanned ship is ensured to advance according to the planned path through feedback control. After obstacle avoidance or path adjustment, the controller quickly adjusts the new target heading and speed. The unmanned ship is guaranteed to continue to run according to the adjusted path, and the preset formation shape is maintained. The overall optimization process is shown in fig. 4.
The invention relates to a method for verifying the validity of a heading controller of a proposed global path-guided feedback regulation DDON network on parameter regulation. Comparing the method with the traditional parameter-adjusting optimization algorithm-particle swarm optimization algorithm to control the effect of the system output tracking target value, and evaluating the difference in the control system tracking precision and response speed. The control output signal is used to adjust the behavior of the system to gradually approach the target value even if the system is maintained in a desired operating state. The comparison result is shown in FIG. 5.
In fig. 5, the target value curve, which represents the desired output of the system in an ideal situation, i.e. the target value that the system should reach and stabilize at, is shown by the black dashed line. The blue curve represented by DDON network method represents the PID control output based on the depth deterministic optimization network optimization. DDON optimized PID parameters cause the control output to gradually approach and stabilize at the target value, which can converge quickly to the target value with less fluctuation, indicating DDON performs better in this control system. In contrast, the PID control output of the particle swarm algorithm represented by the red curve shows larger fluctuation in control output compared with the DDON network when the time step is 28, which indicates that the output signal is unstable when the particle swarm algorithm is adopted to adjust. The comparison result shows that the algorithm of the invention has better performance.
7. Unmanned ship formation control
In the invention, unmanned ship formation adopts a pilot-follower control structure. The pilot boat is responsible for global path planning, and a safe and feasible path is generated through ACFSM path planning algorithm, and meanwhile obstacles are avoided. The following boat adjusts its own position depending on the state of the pilot boat, maintaining a predetermined formation shape.
And maintaining the formation, namely dynamically adjusting the position of the following boat according to the state of the piloting boat and the preset formation shape. The following boat not only follows the pilot boat, but also needs to keep relative positions with other following boats in the formation.
Setting the position of pilot boat) The formation shape is triangular. Following the relative position of the boat 1) The method can be calculated by the following formula:
is the preset distance between the pilot boat and the following boat, i.e. the fixed distance defined by the requirement of maintaining the formation.Is the angle of formation, i.e. the angle between the following boat and the pilot boat.
And dynamically adjusting, namely when encountering an obstacle or changing the environment, the pilot boat can re-plan a path, and automatically adjusting the formation position according to the new state of the pilot boat along with the pilot boat. The formation can deform according to actual conditions, so that safe running is ensured. When the formation encounters an obstacle, the obstacle avoidance mechanism module is used for avoiding the obstacle, and the position is dynamically adjusted based on a set formation rule.
And designing a pilot-follower control structure, and generating a path and guiding formation by the pilot boat through a path planning, obstacle avoidance, track tracking and control module. The follower boat dynamically adjusts its relative position through the pilot boat's position information and is maintained in the formation using the control module. And the unmanned ships are cooperatively formed to control.
The above description is only of the preferred embodiments of the present application and is not intended to limit the present application, but various modifications and variations can be made to the present application by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the protection scope of the present application.
While the foregoing describes the embodiments of the present invention, it should be understood that the present invention is not limited to the embodiments, and that various modifications and changes can be made by those skilled in the art without any inventive effort.