CN116257067A - Collaborative formation multi-level planning control method for unmanned ship cluster - Google Patents
Collaborative formation multi-level planning control method for unmanned ship cluster Download PDFInfo
- Publication number
- CN116257067A CN116257067A CN202310399624.4A CN202310399624A CN116257067A CN 116257067 A CN116257067 A CN 116257067A CN 202310399624 A CN202310399624 A CN 202310399624A CN 116257067 A CN116257067 A CN 116257067A
- Authority
- CN
- China
- Prior art keywords
- unmanned
- unmanned ship
- planning
- track
- coordinate system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 42
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 26
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 27
- 238000011156 evaluation Methods 0.000 claims abstract description 7
- 238000005070 sampling Methods 0.000 claims description 18
- 230000006870 function Effects 0.000 claims description 17
- 230000001133 acceleration Effects 0.000 claims description 14
- 230000009471 action Effects 0.000 claims description 9
- 230000007613 environmental effect Effects 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 8
- 238000004088 simulation Methods 0.000 claims description 8
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 4
- 238000004138 cluster model Methods 0.000 claims description 3
- 238000013461 design Methods 0.000 claims description 3
- 239000002245 particle Substances 0.000 claims description 3
- 239000013598 vector Substances 0.000 claims description 3
- 238000013016 damping Methods 0.000 claims description 2
- 238000001514 detection method Methods 0.000 claims description 2
- 239000012530 fluid Substances 0.000 claims description 2
- 230000008447 perception Effects 0.000 claims description 2
- 230000002194 synthesizing effect Effects 0.000 claims description 2
- 238000005755 formation reaction Methods 0.000 claims 7
- 229920000535 Tan II Polymers 0.000 claims 1
- 238000002121 ultrasonic speckle velocimetry Methods 0.000 description 8
- 230000036461 convulsion Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 3
- 229910003460 diamond Inorganic materials 0.000 description 2
- 239000010432 diamond Substances 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000012888 cubic function Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 230000002195 synergetic effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
According to the motion characteristics of a single unmanned ship in each degree of freedom, a single-ship kinematic model is established, dynamic re-planning of a single-ship path is carried out through an A-algorithm based on obstacle avoidance requirements, and a path generated by global path planning is obtained; establishing an unmanned ship cluster formation method, taking a path generated by global path planning as a reference line, and generating planned path points of all unmanned ships in the cluster by improving a Lattice planning algorithm; the method comprises the steps of constructing a single boat obstacle avoidance model among clusters, and realizing that each unmanned boat in the clusters can keep a safe distance through the obstacle avoidance model under the condition that the distance between the boats or between the boats and the obstacle is too close. The invention plans and controls the movement of unmanned boat group from three layers of global path planning, local cluster control and single boat obstacle avoidance algorithm, and adds compensation measures in track evaluation; the unmanned ship cluster obstacle avoidance algorithm based on the improved artificial potential field method is used for solving the obstacle avoidance requirement of unmanned ships among clusters.
Description
Technical Field
The invention relates to a technology in the unmanned ship track planning field, in particular to a collaborative formation multi-level planning control method for unmanned ship clusters.
Background
Offshore autonomous operation based on Unmanned Ships (USV) has been widely focused on worldwide countries in recent years, and USV has the advantages of small volume, high speed, flexible arrangement and the like, and the safety of offshore operation is greatly improved because direct control of people is not needed, so that the unmanned ship has excellent application potential in the scenes of offshore rescue, marine investigation and the like. Compared with a single USV, the unmanned ship cluster formed by a plurality of USVs can make up for the disadvantage of insufficient reliability and limited operation capacity of the single boat, and has better task capacity. The main problem brought by the clusters is that the interaction between the USVs is complex, the complexity is mainly expressed in two aspects, firstly, unmanned boats among the clusters are required to meet the collision avoidance requirement, but the USVs among the clusters are required to keep relatively stable relative distances, so that the USV formation technology has a key effect on improving the overall efficiency of the USV clusters.
Disclosure of Invention
Aiming at the defects that the prior art cannot process the real-time update of the upper path planning and does not provide a solution for the collision avoidance requirement among boat clusters, the invention provides a collaborative formation multi-level planning control method of an unmanned boat cluster, which is used for planning and controlling the movement of the unmanned boat cluster from three layers of global path planning, local cluster control and a single boat obstacle avoidance algorithm, solving the problem of large linear interpolation calculation amount for the Lattice planning through drop planning, and adding compensation measures in track evaluation; the unmanned ship cluster obstacle avoidance algorithm based on the improved artificial potential field method is used for solving the obstacle avoidance requirement of unmanned ships among clusters.
The invention is realized by the following technical scheme:
the invention relates to a collaborative formation multi-level planning control method of unmanned ship clusters, which comprises the steps of establishing a single-ship kinematic model according to the motion characteristics of a single unmanned ship in each degree of freedom, and carrying out dynamic re-planning of a single-ship path through an A-type algorithm based on obstacle avoidance requirements to obtain a path generated by global path planning; establishing an unmanned ship cluster model, taking a path generated by global path planning as a reference line, and generating planning path points of each unmanned ship in the cluster by improving a Lattice planning algorithm; the method comprises the steps of constructing a single boat obstacle avoidance model among clusters, and realizing that each unmanned boat in the clusters can keep a safe distance through the obstacle avoidance model under the condition that the distance between the boats or between the boats and the obstacle is too close.
The invention relates to a multi-level planning control system for realizing the method, which comprises the following steps: unmanned ship list ship simulation module, cluster planning algorithm module and system configuration module, wherein: the unmanned ship system configuration module sets environmental information and outputs the environmental information to the cluster planning algorithm module; the cluster planning algorithm module calculates a global optimal path and a local track smooth path according to the environmental information to obtain a global track reference path and a local track reference path; the single-boat simulation module calculates and obtains real-time paths of all unmanned boats in the unmanned boat cluster according to the dynamic constraint and collision avoidance constraint of the unmanned boats based on the track reference path.
Technical effects
The invention considers global optimum and local constraint at the same time, takes global path planning as a reference line of local path planning, and can reflect the real track characteristics of unmanned ship clusters. The real-time performance of unmanned ship cluster track planning can be improved based on the improved local track planning of Lattice, and the track re-planning time is not more than 0.5s. Meanwhile, the maneuver performance and energy consumption constraint of the unmanned ship are considered, and the generated reference track avoids overlarge variation of the motion output amplitude of the unmanned ship. The single-boat obstacle avoidance considers the cooperative relationship of unmanned boats among clusters, and the method ensures that control signals output to a power system are smoother, so that the real-time obstacle avoidance requirements among unmanned boat clusters can be better met.
Drawings
FIG. 1 is a flow chart of the present invention;
FIG. 2 is a schematic diagram of an unmanned single boat dynamics model in an embodiment;
fig. 3 is a schematic diagram of a dynamic path re-planning of an algorithm in an embodiment;
FIG. 4 is a schematic diagram of an unmanned boat cluster pilot-follower formation mode in an embodiment;
FIG. 5 is a schematic diagram of input and output of a Lattice planner in an embodiment;
FIG. 6 is a schematic diagram showing the interrelationship of the Lattice plan under different coordinate systems according to the embodiment;
FIG. 7 is a schematic diagram of unmanned boat cluster improved artificial potential field method obstacle avoidance in an embodiment;
fig. 8-10 are schematic views of the effects of the embodiments.
Detailed Description
As shown in fig. 1, this embodiment relates to a hierarchical planning control method for unmanned ship cluster collaborative formation, which includes:
step 1) establishing an unmanned single boat model as shown in fig. 2: for unmanned vessels, movements in 3 degrees of freedom of heave, roll and pitch are typically ignored and are typically described in two coordinate systems: selecting the earth coordinate system X E -Y E As a reference coordinate system, a coordinate system X fixedly connected to the unmanned ship B -Y B As an unmanned ship coordinate system, in the figure: psi phi type i For the bow rocking angle v i ＝[u i ，v i ，r i ]For the linear velocity and angular velocity vector of the ith unmanned boat, u i And v i Respectively X B -Y B Heave and roll speeds in a coordinate system, r i For yaw rate, beta i Is the sideslip angle.
Considering a cluster system formed by N unmanned vessels, the kinematics and dynamics equation of the ith unmanned vessel can be described by a three-degree-of-freedom nonlinear model as follows: η (eta) i ＝R(ψ i )v i ，
η i ＝[x i ，y i ，ψ i ]For the position and the bow angle of the ith unmanned ship under the reference coordinate system, tau i For controlling input τ dis G is input by disturbance caused by stormy waves and marine environment i (v i ) Including unmodeled fluid dynamics and modeling errors, rotation matrix of unmanned aerial vehicle coordinate system relative to reference coordinate system +.>
Inertial mass matrix->
Centripetal force and coriolis force matrix>
Damping matrix->
m ij ,c ij ,d ij Are all constant coefficients, and the sideslip angle beta generated by the transverse drift speed of the unmanned ship during navigation i ＝atan2(υ i ,u i )。
The control input satisfies the constraint condition: - τ ic ≤τ i ≤τ ic Wherein: τ ic Is tau i Upper and lower limit values of (2).
Step 2) carrying out dynamic re-planning of the single boat path through an A-algorithm to obtain a path generated by global path planning: as shown in fig. 3, according to the coordinates of the starting point, the coordinates of the target point, the coordinates of the obstacle and the size of the occupied grid, selecting the subsequent point by minimizing the sum of the actual cost and the heuristic cost according to an a-algorithm and obtaining the collision-free path point from the starting point to the target point; and when the unmanned ship detects the surrounding obstacle, adding the position of the obstacle and the size of the occupied map grid into the environmental information according to the perception and identification result, and at the moment, re-calling the algorithm A to calculate a new collision-free planning path until no new obstacle is detected.
Step 3) as shown in fig. 4, establishing an unmanned ship cluster header-Follower model: taking two unmanned boats as an example,
the position and heading of the pilot boat and the following boat respectively, l is the relative distance between the pilot boat and the following boat, l is x And l y For the transversal-longitudinal distance between two boats, < >>
And->
The course of the pilot boat and the following boat respectively. In actual sailing, when givenPiloting the position of the boats and determining the relative distance and angle between the two boats, then l x And l y The value of (2) is a fixed value, i.e. the position of the following boat is unique. So can be achieved by adjusting the formation pitch (l x ,l y ) Is used for realizing the formation control of a plurality of unmanned boats, and specifically comprises the following steps: />
Deriving the relative motion equation of the navigator and the follower>
Thereby obtaining the kinematic object->
Dynamic target->
Wherein: u (u) l ，v l For the speed of the pilot boat in the heave and heave directions, u f ，v f To follow the speed of the boat in the heave and heave directions, r l And r f Angular velocities of pilot and follower vessels, respectively,/->
The x-axis and y-axis components, e, respectively, of the distance desired to be maintained between the two boats x And e y The distance tracking error in the x and y directions is the distance tracking error, so that the relative position between the boats in the formation is kept unchanged as much as possible during the travelling process, and the motion error with the boat of the convoy is as small as possible.
The unmanned ship cluster header-Follower model further comprises a safe distance constraint, and specifically comprises the following steps: i v -l obs ||≥d safe Wherein: d, d safe Is a predetermined safe distance l v Is the x, y coordinates, l of the unmanned ship v obs The x and y coordinates of the boundary of the obstacle are used for realizing the relative coincidence of the motion paths of all the boats in the longitudinal formation process, and meanwhile, a certain safety distance needs to be kept between the boats and surrounding obstacles.
Step 4) as shown in fig. 5, taking the path generated by the global path planning in step 2 as a Reference line, completing the local track planning of the unmanned ship by adopting an improved Lattice planner, namely, after converting the unmanned ship coordinate system into a Frenet coordinate system, planning d-axis and s-axis of Frenet respectively under the Frenet coordinate system to form a planned track under the Frenet coordinate system, and then synthesizing the track under the Frenet coordinate system into a track under the world coordinate system to restore the track under the world coordinate system, wherein the method specifically comprises the following steps:
4.1 As shown in fig. 6, converting the current pose information of the unmanned ship to the Frenet coordinate system to obtain the initial state of the unmanned ship in the Frenet coordinate system; calculating a look-ahead distance according to the current speed to obtain a look-ahead point and a target state of the unmanned ship under a look-ahead point position Frenet coordinate system; and projecting the coordinate point P of the unmanned ship onto a reference line to obtain a projection point R on the reference line, wherein the path length from the starting point of the reference line to the projection point is the longitudinal offset s of the unmanned ship under the Frenet coordinate system, and the distance l(s) from the projection point R to the unmanned ship position is the transverse offset of the unmanned ship under the Frenet coordinate system.
In this embodiment, the transverse offset l(s) is designed as a function of the longitudinal offset s, and the purpose of converting the coordinate points into the Frenet coordinate system is to facilitate the generation of the planning curve and the track sampling in the transverse and longitudinal directions of the track line, so as to obtain a smooth sampling track covering the whole track.
4.2 The method comprises the steps of) obtaining a sampling state by taking track running time t, target speed v and transverse displacement d to a reference line as planning parameters, and performing transverse sampling, longitudinal sampling and track time period sampling for forming different transverse offsets by a Lattice planner, wherein the track running time period sampling comprises the following steps: generating a transverse offset l(s) and a longitudinal offset s (t) which are generated by fitting and solving a polynomial and a planning function according to the sampling state, wherein the polynomial function of 5 times of longitudinal fitting is as follows:
constraint function:
wherein: s (t), v(t) and a (t) are respectively the longitudinal offset, the speed and the acceleration of the unmanned ship at the moment t, c 1 ～c 6 Coefficients of linear interpolation, respectively; s (t) 0 )，v(t 0 )，a(t 0 ) Respectively planning the position, the speed and the acceleration of the unmanned ship track at the initial moment; s (t) 1 )，v(t 1 )，a(t 1 ) Longitudinal offset, speed and acceleration at the end of unmanned ship track planning are respectively given.
In the embodiment, a downsampling method is adopted, namely, a cubic function is adopted to fit the track, only the speed and the track are fitted, and smoothness of the generated track is guaranteed. And for acceleration, constraint is carried out by adding a punishment item in an objective function of track evaluation, and the acceleration in the action process of the unmanned ship is constrained by the punishment item, so that the change of the acceleration is prevented from being too fast as much as possible. The reduced fitting curve expression is: s (t) =c 1 t 3 +c 2 t 2 +c 3 t+c 4 ，v(t)＝3c 1 t 2 +2c 2 t 1 +c 3 The method comprises the steps of carrying out a first treatment on the surface of the Constraint conditions:
wherein: a, a max The maximum acceleration of the unmanned ship is related to the performance of the unmanned ship.
4.3 A polynomial programming function l(s), s (t) of the transverse offset and the longitudinal offset is constructed, after the programming function of the transverse displacement and the longitudinal displacement is obtained, time interpolation is carried out to obtain track points under a reference line Frenet coordinate system, and finally the track points are converted from the Frenet coordinate system to a cart coordinate system to obtain a physical world sampling track.
Since both the transverse and longitudinal directions are obtained by higher order polynomial interpolation, the trajectories in the cart coordinate system are also smooth.
4.4 The collision detection, curvature constraint and optimal track scoring of the sampling track are carried out through a track evaluation function, so that an optimal collision-free smooth track meeting constraint conditions is obtained, namely, the track is output to a controller for the unmanned ship to follow.
The track evaluation function is as follows:
wherein: s (t), a (t), j (t) are the position, acceleration and jerk of the unmanned ship at the moment t, s opt ，a 0 ，j 0 The reference position, the acceleration and the jerk of the unmanned ship are obtained through linear interpolation. />
Is a constant coefficient. k (k) lat Cost lat Is the sum of the deviations of the actual speed/acceleration/jerk and the ideal speed/acceleration/jerk of the unmanned ship. R is the threat radius of the obstacle, x (t), y (t) is the x, y coordinates of the unmanned ship at the moment t, and x obs ，y obs For obstacle coordinates, k obs Cost obs Punishment for the distance between the unmanned boat and the obstacle is positive when the unmanned boat enters the obstacle threat radius R. After the expected output track is generated, the unmanned ship bottom layer controller calls PID control, and finally control signals of the unmanned ship are generated, wherein the control signals comprise jerk (propeller output power) and angular speed (rudder angle).
Step 5) adopting an improved artificial potential field method to design an obstacle avoidance algorithm of the unmanned ship to establish a single-ship obstacle avoidance model so as to avoid the problem that the unmanned ship possibly deviates from a reference path to cause too close distance with other ships in the actual simulation operation process, specifically comprising the following steps: simulating a starting point, an ending point, an obstacle and a robot into an artificial potential field, abstracting the motion of the robot into moving particles in an electronic potential field with different symbols or two magnetic poles in a magnetic field, and simultaneously considering the cooperative obstacle avoidance actions of a plurality of unmanned boats to finish the obstacle avoidance actions at the minimum cost.
For example, both the unmanned boat and the obstacle particles are positively charged, the obstacle generating a repulsive potential field to the unmanned boat; the target generates a gravitational potential field to the unmanned ship, so that the unmanned ship moves towards the target and spontaneously avoids the obstacle due to a resultant potential field generated by the obstacle and the target.
As shown in FIG. 7, the repulsive potential field is
Wherein: lambda (lambda) 1 > 0 is a constant, d (X ij )＝||X i -X tar The I is the unmanned ship i and the target point X tar Is a distance of (3). />
Is the influence distance of the ith obstacle, d ij (X ij ) Is the perpendicular distance from the unmanned ship to the obstacle. Wherein the potential field force is proportional to the potential field function negative gradient, and the potential field attraction force and the potential field repulsion force are respectively: />
Wherein: and delta d is to set a certain operation dead zone by considering the linearity of the power output of the unmanned ship as much as possible in the practical engineering application process, namely, the power output and rudder angle of the unmanned ship are not adjusted in the dead zone. Considering the driving force of the unmanned ship in the forward direction
Resultant force of unmanned ship->
For a formation cluster formed by more than 2 unmanned boats, the resultant force expression is the same, namely, superposition of potential field force vectors among a plurality of unmanned boats is considered: />
Through specific practical experiments, the method is subjected to simulation experiments on a simulation platform, 5 unmanned boats are arranged to form unmanned boat formation, and the initial position is positioned in a circle with 150 meters radius by taking a Cartesian coordinate system (200 ) as a circle center. The target point for the unmanned ship to travel is 3200, 5000, and the cruising speed of the unmanned ship cluster is set to 6 knots. And arranging unmanned ships in the open water area in a diamond formation mode. In terms of environmental parameters, a scene that the boat group passes through the narrow water channel is simulated, the threat radius of the obstacle is set to be 100 meters, and 5 unmanned boats need to pass through the narrow water channel and then go to a target point. The simulation program was run and the test results obtained are shown in fig. 8-10. As can be seen from fig. 8, the unmanned boat cluster can keep the formation spacing substantially unchanged in the open water, and after entering the narrow water channel, the unmanned boat cluster cannot pass in parallel due to the requirement of meeting collision prevention constraint between the obstacle and the boat, so that the unmanned boat cluster passes through the narrow water channel in a straight line. As can be seen from fig. 9, the spacing between the clusters is maintained within a certain range by improving the artificial potential field method, and the speed of the clusters is maintained substantially uniform. As can be seen from fig. 10, in the open water area, the unmanned ship group returns to the diamond formation again, the reference track is given according to the proposed track planning method, and the unmanned ship performs smooth formation switching according to the reference track.
Compared with the prior art, the method adopts the idea of hierarchical planning control, combines upper path planning, middle layer cluster planning, track planning and obstacle avoidance control of the bottom single boat, and plans the local path by taking the global path as a reference on the premise of optimizing the global path, so that the generated local path is more in accordance with practical significance. The method simplifies and improves the Lattice algorithm, improves the speed of local path planning, considers the problem of insufficient jerk due to linear interpolation drop, designs corresponding punishment items in the evaluation function, and ensures that the control action of the unmanned ship is not changed drastically. The invention provides a method for calculating potential field force of unmanned ships among clusters based on improvement of a traditional artificial potential field method by considering synergistic effect of the unmanned ships among the clusters. The algorithm can reflect the potential field force relation of the unmanned ships and guide the obstacle avoidance action of each unmanned ship. The method provided distributes obstacle avoidance actions to all related unmanned ships in the cluster, optimizes the action amplitude of a single unmanned ship, and enables the output of the unmanned ship to be smoother.
The foregoing embodiments may be partially modified in numerous ways by those skilled in the art without departing from the principles and spirit of the invention, the scope of which is defined in the claims and not by the foregoing embodiments, and all such implementations are within the scope of the invention.
Claims (4)
1. A collaborative formation multi-level planning control method of unmanned ship clusters is characterized in that a single-ship kinematic model is established according to the motion characteristics of a single unmanned ship in each degree of freedom, dynamic re-planning of a single-ship path is carried out through an A-algorithm based on obstacle avoidance requirements, and a path generated by global path planning is obtained; establishing an unmanned ship cluster model, taking a path generated by global path planning as a reference line, and generating planning path points of each unmanned ship in the cluster by improving a Lattice planning algorithm; the method comprises the steps of constructing a single boat obstacle avoidance model among clusters, and realizing that each unmanned boat in the clusters can keep a safe distance through the obstacle avoidance model under the condition that the distance between the boats or between the boats and the obstacle is too close.
2. The collaborative formation multi-level planning control method of the unmanned ship cluster according to claim 1, which is characterized by comprising the following steps:
step 1) establishing an unmanned ship single-ship model: for unmanned vessels, movements in 3 degrees of freedom of heave, roll and pitch are typically ignored and described in two coordinate systems: selecting the earth coordinate system X E -Y E As a reference coordinate system, a coordinate system X fixedly connected to the unmanned ship B -Y B As an unmanned ship coordinate system, consider a cluster system composed of N unmanned ships, where the kinematic and kinetic equations of the ith unmanned ship can be described by a three-degree-of-freedom nonlinear model as: η (eta) i ＝R(ψ i )v i ，
Wherein: psi phi type i For the bow rocking angle v i ＝[u i ，v i ，r i ]For the linear velocity and angular velocity vector of the ith unmanned boat, u i And v i Respectively X B -Y B Heave and roll speeds in a coordinate system, r i For yaw rate, beta i Angle of sideslip, eta i ＝[x i ，y i ，ψ i ]For the position and the bow angle of the ith unmanned ship under the reference coordinate system, tau i For controlling input τ dis G is input by disturbance caused by stormy waves and marine environment i (v i ) Including unmodeled fluid dynamics and modeling errors, rotation matrix of unmanned aerial vehicle coordinate system relative to reference coordinate system +.>
Inertial mass matrix->
Centripetal force and coriolis force matrix>
Damping matrix->
m ij ，c ij ，d ij Are all constant coefficients, and the sideslip angle beta generated by the transverse drift speed of the unmanned ship during navigation i ＝a tan2(υ i ，u i )；
Step 2) carrying out dynamic re-planning of the single boat path through an A-algorithm to obtain a path generated by global path planning: selecting a subsequent point by an A-algorithm through minimizing the sum of actual cost and heuristic cost according to the initial point coordinate, the target point coordinate, the obstacle coordinate and the occupied grid size, and obtaining a collision-free path point from the initial point to the target point; when the unmanned ship detects surrounding obstacles, the positions of the obstacles and the size of the occupied map grid are added into environmental information according to the perception and identification result, and at the moment, a new collision-free planning path is calculated by re-calling the algorithm A until no new obstacle is detected;
step 3) establishing an unmanned ship cluster model: taking two unmanned boats as an example,
the position and heading of the pilot boat and the following boat respectively, l is the relative distance between the pilot boat and the following boat, l is x And l y For the transversal-longitudinal distance between two boats, < >>
And->
Heading of pilot boat and following boat respectively, in actual sailing, when the position of pilot boat is given and the relative distance and angle between two boats are determined, then x And l y Is a fixed value, i.e. the following boat is uniquely located, so that the distance between the formations (i x ，l y ) Is used for realizing the formation control of a plurality of unmanned boats, and specifically comprises the following steps: />
Deriving the relative motion equation of the navigator and the follower>
Thereby obtaining the kinematic object->
Dynamic target->
Wherein: u (u) l ，v i For the speed of the pilot boat in the heave and heave directions, u f ，u f To follow the speed of the boat in the heave and heave directions, r l And r f Respectively pilot boat andfollowing the angular velocity of the boat>
The x-axis and y-axis components, e, respectively, of the distance desired to be maintained between the two boats x And e y The distance tracking error in the x and y directions is the distance tracking error, so that the relative position between the boats in the formation is kept unchanged as much as possible in the travelling process, and the motion error between the boats and the collarband is as small as possible;
step 4) taking the path generated by the global path planning in the step 2 as a reference line, adopting an improved Lattice planner to complete the local track planning of the unmanned ship, namely after the unmanned ship coordinate system is converted into a Frenet coordinate system, respectively planning d-axis and s-axis of Frenet under the Frenet coordinate system to form a planned track under the Frenet coordinate system, and then synthesizing the track under the Frenet coordinate system into the track under the world coordinate system to be restored to the track under the world coordinate system;
step 5) adopting an improved artificial potential field method to design an obstacle avoidance algorithm of the unmanned ship to establish a single-ship obstacle avoidance model so as to avoid the problem that the unmanned ship possibly deviates from a reference path to cause too close distance with other ships in the actual simulation operation process, specifically comprising the following steps: simulating a starting point, an ending point, an obstacle and a robot into an artificial potential field, abstracting the motion of the robot into moving particles in an electronic potential field with different symbols or two magnetic poles in a magnetic field, and simultaneously considering the cooperative obstacle avoidance actions of a plurality of unmanned boats to finish the obstacle avoidance actions at the minimum cost.
3. The method for collaborative formation multi-level planning control of unmanned ship clusters according to claim 2, wherein the step 4 specifically comprises:
4.1 Converting the current pose information of the unmanned ship to a Frenet coordinate system to obtain an initial state of the unmanned ship in the Frenet coordinate system; calculating a look-ahead distance according to the current speed to obtain a look-ahead point and a target state of the unmanned ship under a look-ahead point position Frenet coordinate system; projecting a coordinate point P of the unmanned ship onto a reference line to obtain a projection point R on the reference line, wherein the path length from the starting point of the reference line to the projection point is the longitudinal offset s of the unmanned ship under the Frenet coordinate system, and the distance l(s) from the projection point R to the position of the unmanned ship is the transverse offset of the unmanned ship under the Frenet coordinate system;
4.2 The method comprises the steps of) obtaining a sampling state by taking track running time t, target speed v and transverse displacement d to a reference line as planning parameters, and performing transverse sampling, longitudinal sampling and track time period sampling for forming different transverse offsets by a Lattice planner, wherein the track running time period sampling comprises the following steps: generating a transverse offset l(s) and a longitudinal offset s (t) which are generated by fitting and solving a polynomial and a planning function according to the sampling state, wherein the polynomial function of 5 times of longitudinal fitting is as follows:
constraint function:
wherein: s (t), v (t) and a (t) are respectively the longitudinal offset, the speed and the acceleration of the unmanned ship at the moment t, and c 1-c 6 Coefficients of linear interpolation, respectively; s (t) 0 )，v(t 0 )，a(t 0 ) Respectively planning the position, the speed and the acceleration of the unmanned ship track at the initial moment; s (t) 1 )，v(t 1 )，a(t 1 ) Longitudinal offset, speed and acceleration at the end of unmanned ship track planning are respectively given;
4.3 Constructing polynomial programming functions s (t), d(s) of transverse offset and longitudinal offset, obtaining programming functions of transverse displacement and longitudinal displacement, performing time interpolation to obtain track points under a reference line Frenet coordinate system, and finally converting the track points from the Frenet coordinate system to a carrier coordinate system to obtain a physical world sampling track;
4.4 The collision detection, curvature constraint and optimal track scoring of the sampling track are carried out through a track evaluation function, so that an optimal collision-free smooth track meeting constraint conditions is obtained, namely, the track is output to a controller for the unmanned ship to follow.
4. A multi-level planning control system for implementing the collaborative formation multi-level planning control method of the unmanned aerial vehicle cluster according to any one of claims 1 to 3, comprising: unmanned ship list ship simulation module, cluster planning algorithm module and system configuration module, wherein: the unmanned ship system configuration module sets environmental information and outputs the environmental information to the cluster planning algorithm module; the cluster planning algorithm module calculates a global optimal path and a local track smooth path according to the environmental information to obtain a global track reference path and a local track reference path; the single-boat simulation module calculates and obtains real-time paths of all unmanned boats in the unmanned boat cluster according to the dynamic constraint and collision avoidance constraint of the unmanned boats based on the track reference path.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310399624.4A CN116257067A (en) | 2023-04-14 | 2023-04-14 | Collaborative formation multi-level planning control method for unmanned ship cluster |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310399624.4A CN116257067A (en) | 2023-04-14 | 2023-04-14 | Collaborative formation multi-level planning control method for unmanned ship cluster |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116257067A true CN116257067A (en) | 2023-06-13 |
Family
ID=86688151
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310399624.4A Pending CN116257067A (en) | 2023-04-14 | 2023-04-14 | Collaborative formation multi-level planning control method for unmanned ship cluster |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116257067A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117742323A (en) * | 2023-12-06 | 2024-03-22 | 江苏大学 | Target distribution and route planning method for multi-agent unmanned ship |
-
2023
- 2023-04-14 CN CN202310399624.4A patent/CN116257067A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117742323A (en) * | 2023-12-06 | 2024-03-22 | 江苏大学 | Target distribution and route planning method for multi-agent unmanned ship |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106444806B (en) | The drive lacking AUV three-dimensional track tracking and controlling method adjusted based on biological speed | |
| CN102768539B (en) | AUV (autonomous underwater vehicle) three-dimension curve path tracking control method based on iteration | |
| CN108445892A (en) | A kind of drive lacking unmanned boat formation control device structure and design method | |
| Cao et al. | Toward optimal rendezvous of multiple underwater gliders: 3D path planning with combined sawtooth and spiral motion | |
| CN111930141A (en) | Three-dimensional path visual tracking method for underwater robot | |
| CN113534668B (en) | Maximum entropy based AUV (autonomous Underwater vehicle) motion planning method for actor-critic framework | |
| CN107239075A (en) | The intelligent S faces Guidance and control method of Autonomous Underwater Vehicle plane and straight line course line tracking | |
| CN109933074B (en) | Design method of multi-unmanned ship cluster motion controller structure with leader | |
| Wang et al. | A simultaneous planning and control method integrating APF and MPC to solve autonomous navigation for USVs in unknown environments | |
| CN104950882A (en) | Global consistent asymptotic type path tracking guiding control method | |
| CN109917795A (en) | A kind of drive lacking unmanned boat cluster cooperative guidance structure and design method | |
| Zhao et al. | Broken lines path following algorithm for a water-jet propulsion USV with disturbance uncertainties | |
| CN116257067A (en) | Collaborative formation multi-level planning control method for unmanned ship cluster | |
| Zhuang et al. | Motion control and collision avoidance algorithms for unmanned surface vehicle swarm in practical maritime environment | |
| Liu et al. | Model predictive controller design with disturbance observer for path following of unmanned surface vessel | |
| CN114721409B (en) | Underwater vehicle docking control method based on reinforcement learning | |
| CN115903800A (en) | Strict safety control method for multi-unmanned ship formation based on leader cooperation | |
| CN113741433B (en) | Distributed formation method of unmanned ship on water surface | |
| CN113126492B (en) | Automatic path tracking method for dynamic positioning ship | |
| CN113093804B (en) | Unmanned ship formation control method and control system based on inversion sliding mode control | |
| CN113296505B (en) | Unmanned ship multi-mode path tracking control method based on speed change LOS | |
| CN113359737A (en) | Ship formation self-adaptive event trigger control method considering formation expansion | |
| CN117389312A (en) | Model-based three-dimensional tracking control method for counter roll of underwater vehicle | |
| CN115258073B (en) | Ship dragging system track tracking method under environment interference | |
| CN114564015B (en) | Distributed formation control method for under-actuated unmanned ship under refusing environment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |