US12172739B2ActiveUtilityA1
Path planning system and method for sea-aerial cooperative underwater target tracking
Est. expiryMay 21, 2041(~14.9 yrs left)· nominal 20-yr term from priority
G05D 1/689G05D 1/49B63G 2008/008G05D 1/0094G05D 1/0206B63G 2008/002B63G 8/001
45
PatentIndex Score
0
Cited by
10
References
7
Claims
Abstract
Disclosed is path planning system and method for sea-aerial cooperative underwater target tracking, the method comprises: obtaining the position information of a detection target, carrying out a first path planning along a channel of sea surface monitoring device according to the position information of the detection target; carrying out a second path planning along the channel of sea surface monitoring device according to the water surface navigation map and its own position information, constructing an underwater obstacle map; performing a third path planning according to the underwater obstacle map, and tracking to the position of the detection target to complete the tracking task. This disclosure adopts the collaborative optimization of several clusters to reduce the number of iterations and improve the optimization efficiency, making the path planning reasonable, as a result, the target position can be quickly tracked, and the autonomous collaborative tracking capability is improved.
Claims
exact text as granted — not AI-modifiedWhat is claimed is:
1. A path planning system for sea-aerial cooperative underwater target tracking, comprising a cluster of aerial monitoring devices, a cluster of sea surface monitoring devices, and a cluster of underwater monitoring devices, wherein:
the cluster of aerial monitoring devices is used to obtain position information of a detection target, carry out a first path planning along a channel of the cluster of sea surface monitoring devices according to the position information of the detection target, construct a water surface navigation map including the position information of the detection target and information of all obstacles in the channel of the cluster of sea surface monitoring devices, and transmit the water surface navigation map to the cluster of sea surface monitoring devices, wherein the cluster of aerial monitoring devices is a cluster of unmanned aerial vehicles;
the cluster of sea surface monitoring devices is used to carry out a second path planning along the channel of the cluster of sea surface monitoring devices based on a cooperative particle swarm algorithm, according to the water surface navigation map and position information of the cluster of sea surface monitoring devices, reaching an adjacent area of the detection target, and to detect underwater environment of the adjacent area, construct an underwater obstacle map, and transmit the underwater obstacle map to the cluster of underwater monitoring devices, wherein the cluster of sea surface monitoring devices is a cluster of unmanned surface vehicles;
the cluster of underwater monitoring devices is used to perform a third path planning based on the cooperative particle swarm algorithm, according to the underwater obstacle map, and track to the detection target to complete a tracking task, wherein the cluster of underwater monitoring devices is a cluster of underwater vehicles;
wherein a first number of cooperative particle swarms of the cooperative particle swarm algorithm corresponding to the second path planning is equal to a number of the unmanned surface vehicles;
a second number of cooperative particle swarms of the cooperative particle swarm algorithm corresponding to the third path planning is equal to a number of the underwater vehicles;
wherein the path planning steps of the first path planning, the second path planning and the third path planning include:
obtaining initialization parameters, initializing a path point particle swarm, wherein the path point particle swarm includes the cluster of aerial monitoring devices corresponding to the first path planning, the cluster of sea surface monitoring devices corresponding to the second path planning, and the cluster of underwater monitoring devices corresponding to the third path planning;
obtaining set multiple iteration parameters and iteration times;
adding starting position coordinates and ending position coordinates to the path point particle swarm to determine an initial route coordinate matrix;
calculating an independent fitness and an overall fitness corresponding to the current position of each particle, and iterating and updating coordinate data of the path point particle according to the multiple iterative parameters, wherein the overall fitness is determined according to the independent fitness in the path point particle swarm; and
ending the iteration, and outputting optimal path point coordinates when the number of iterations is reached to the set iteration times;
wherein the calculation process of the independent fitness includes:
determining corresponding collision optimization function value, turning optimization function value and time optimization function for a single path point particle; and
performing a weighted sum of the collision optimization function value, the turning optimization function value, and the time optimization function to determine the independent fitness;
wherein the calculation process of the turning optimization function value includes:
for a single path point particle, calculating path angle, first distance, and limit turning radius according to every three adjacent path points, which are respectively expressed by the following formulas:
α
=
arccos
l
1
+
l
2
-
l
3
2
l
1
·
l
2
l
=
min
(
l
1
,
l
2
)
ρ
=
l
·
tan
α
2
where α represents the path angle determined by every three adjacent path points, l represents the first distance, l 1 represents distance between the first two path points, l 2 represents distance between the last two path points, l 3 represents distance between the first path point and the third path point, p represents the limit turning radius, which is maximum inscribed circle radius of the first distance l;
according to the limit turning radius and minimum turning radius of the sea surface monitoring device, determining the turning optimization function value, which is expressed by the following formula:
tu
(
p
k
-
1
,
p
k
,
p
k
+
1
)
=
{
1
ρ
≥
R
0
ρ
<
R
where tu(p k−1 ,p k ,p k+1 ) represents the turning optimization function value, p k−1 , p k , and p k+1 represent three adjacent path points respectively, ρ the limit turning radius,
R represents the minimum turning radius, for a single path point particle, if at least one of the turning optimization function values takes a value of 0, the turning optimization function value corresponding to the single path point particle takes 0;
for the single path point particle, the corresponding time optimization function is expressed by the following formula:
l
ij
=
∑
k
=
1
dim
+
1
l
ijk
;
l
i
=
min
(
l
ij
)
,
j
=
1
,
…
,
P
t
i
=
l
i
v
i
;
tim
=
∑
i
=
1
nn
t
i
nn
;
where l ijk represents distance between the i-th particle swarm, the j-th particle, and the k-th group of adjacent path points; l jj represents total path length of the i-th particle swarm and the j-th particle; l i represents minimum sailing distance of the i-th particle swarm; t i represents minimum sailing time of the i-th particle swarm; tim represents average minimum sailing time of the i cooperative swarm; v i represents navigation speed of the i-th particle swarm; and nn represents a number of cooperative swarms of the particle swarm.
2. A path planning method for sea-aerial cooperative underwater target tracking, based on the path planning system for sea-aerial cooperative underwater target tracking, comprising a cluster of aerial monitoring devices, a cluster of sea surface monitoring devices, and a cluster of underwater monitoring devices, the path planning method comprising:
obtaining position information of a detection target, carrying out a first path planning along a channel of the cluster of sea surface monitoring devices according to the position information of the detection target, and constructing a water surface navigation map including the position information of the detection target and information of all obstacles in the channel of the cluster of sea surface monitoring devices, and transmitting the water surface navigation map to the cluster of sea surface monitoring devices, wherein the cluster of aerial monitoring devices is a cluster of unmanned aerial vehicles;
carrying out a second path planning along the channel of sea surface monitoring device based on a cooperative particle swarm algorithm, according to the water surface navigation map and its own position information of the cluster of sea surface monitoring devices, reaching an adjacent area of the detection target, detecting underwater environment of the adjacent area, constructing an underwater obstacle map, and transmitting the underwater obstacle map to the cluster of underwater monitoring devices, wherein the cluster of sea surface monitoring devices is a cluster of unmanned surface vehicles;
performing a third path planning based on the cooperative particle swarm algorithm, according to the underwater obstacle map, and tracking to the detection target to complete a tracking task, wherein the cluster of underwater monitoring devices is a cluster of underwater vehicles;
wherein a first number of cooperative particle swarms of the cooperative particle swarm algorithm corresponding to the second path planning is equal to a number of the unmanned surface vehicles;
a second number of cooperative particle swarms of the cooperative particle swarm algorithm corresponding to the third path planning is equal to a number of the underwater vehicles;
wherein the path planning steps of the first path planning, the second path planning and the third path planning include:
obtaining initialization parameters, initializing a path point particle swarm, wherein the path point particle swarm includes the cluster of aerial monitoring devices corresponding to the first path planning, the cluster of sea surface monitoring devices corresponding to the second path planning, and the cluster of underwater monitoring devices corresponding to the third path planning;
obtaining set multiple iteration parameters and iteration times;
adding starting position coordinates and ending position coordinates to the path point particle swarm to determine an initial route coordinate matrix;
calculating an independent fitness and an overall fitness corresponding to the current position of each particle, and iterating and updating coordinate data of the path point particle according to the multiple iterative parameters, wherein the overall fitness is determined according to the independent fitness in the path point particle swarm;
ending the iteration, and outputting optimal path point coordinates when the number of iterations is reached to the set iteration times;
wherein the calculation process of the independent fitness includes:
determining corresponding collision optimization function value, turning optimization function value and time optimization function for a single path point particle; and
performing a weighted sum of the collision optimization function value, the turning optimization function value, and the time optimization function to determine the independent fitness;
wherein the calculation process of the turning optimization function value includes:
for a single path point particle, calculating path angle, first distance, and limit turning radius according to every three adjacent path points, which are respectively expressed by the following formulas:
α
=
arc
cos
l
1
+
l
2
-
l
3
2
l
1
·
l
2
l
=
min
(
l
1
,
l
2
)
ρ
=
l
·
tan
α
2
where α represents the path angle determined by every three adjacent path points, l represents the first distance, l 1 represents distance between the first two path points, l 2 represents distance between the last two path points, l 3 represents distance between the first path point and the third path point, p represents the limit turning radius, which is maximum inscribed circle radius of the first distance l;
according to the limit turning radius and minimum turning radius of the sea surface monitoring device, determining the turning optimization function value, which is expressed by the following formula:
tu
(
p
k
-
1
,
p
k
,
p
k
+
1
)
=
{
1
ρ
≥
R
0
ρ
<
R
where tu tu(p k−1 ,p k ,p k+1 ) represents the turning optimization function value, p k−1 , p k , and p k+1 represent three adjacent path points respectively, ρ the limit turning radius,
R represents the minimum turning radius, for a single path point particle, if at least one of the turning optimization function values takes a value of 0, the turning optimization function value corresponding to the single path point particle takes 0;
for the single path point particle, the corresponding time optimization function is expressed by the following formula:
l
ij
=
∑
k
=
1
dim
+
1
l
ijk
;
l
i
=
min
(
l
ij
)
,
j
=
1
,
…
,
P
t
i
=
l
i
v
i
;
tim
=
∑
i
=
1
nn
t
i
nn
;
where l ijk represents distance between the i-th particle swarm, the j-th particle, and the k-th group of adjacent path points; l ij represents total path length of the i-th particle swarm and the j-th particle; l j represents minimum sailing distance of the i-th particle swarm; t i represents minimum sailing time of the i-th particle swarm; tim represents average minimum sailing time of the i cooperative swarm; v i represents navigation speed of the i-th particle swarm; and nn represents a number of cooperative swarms of the particle swarm.
3. The path planning method for sea-aerial cooperative underwater target tracking according to claim 2 , wherein the step that obtaining position information of a detection target, carrying out a first path planning along a channel of the cluster of sea surface monitoring devices according to the position information of the detection target comprising the following steps:
obtaining starting position of the cluster of aerial monitoring devices, channel information of the channel of the cluster of sea surface monitoring devices, and the tai=get position information of the detection target;
determining a channel of the cluster of aerial monitoring devices according to the starting position, the channel information of the cluster of sea surface monitoring devices, and the target position information of the detection target, wherein the channel of the cluster of aerial monitoring devices is the navigable airspace corresponding to the channel of the cluster of sea surface monitoring devices; and
carrying out the first path planning in the channel of the cluster of aerial monitoring devices, planning a navigation detection path of the cluster of aerial monitoring devices, and navigating.
4. The path planning method for sea-aerial cooperative underwater target tracking according to claim 3 , wherein the step that constructing a water surface navigation map including the position information of the target position and information of all obstacles in the channel of the cluster of sea surface monitoring devices, and transmitting the water surface navigation map to the cluster of sea surface monitoring devices comprising the following steps:
during navigation of the cluster of aerial monitoring devices, detecting water surface by a detection device in the channel of the cluster of aerial monitoring devices, collecting the information of all obstacles that is stored as coordinate data information along the channel of the cluster of sea surface monitoring devices; and
constructing the water surface navigation map according to the coordinate data information and the position of the detection target, and transmitting the water surface navigation map to the cluster of sea surface monitoring devices.
5. The path planning method for sea-aerial cooperative underwater target tracking according to claim 2 , wherein the initialization parameters include number of cooperative particle swarms, particle swarm's size, particle dimension and initial velocity; the parameters include channel space range, inertia factor, individual learning factor and group learning factor;
the step iterating and updating coordinate data of the path point particle according to the multiple iterative parameters includes the following steps:
updating velocity of the path point particle according to the channel space range, the inertia factor, the individual learning factor, and the group learning factor; and
updating coordinates of the path point particle according to the updated velocity and current coordinate position of the particle.
6. The path planning method for sea-aerial cooperative underwater target tracking according to claim 5 , wherein the inertia factor decreases uniformly with the increase of the number of iterations, which is expressed by the following formula:
ω
t
=
ω
t
-
1
-
ω
max
-
ω
min
g
where t represents current iteration number, ω t represents inertia factor corresponding to the current iteration number, ω t-1 represents inertia factor corresponding to the previous iteration number, ω max represents maximum value corresponding to the preset inertia factor, ω min represents minimum value corresponding to the preset inertia factor, g represents preset constant.
7. The path planning method for sea-aerial cooperative underwater target tracking according to claim 2 , wherein the calculation process of the overall fitness includes:
determining the independent fitness of each particle for the cluster of aerial monitoring devices, the cluster of sea surface monitoring devices, and the cluster of underwater monitoring devices;
determining an optimal fitness corresponding to different clusters according to the independent fitness; and
summing the optimal fitness to determine the overall fitness.Cited by (0)
No later patents cite this yet.
References (0)
No backward citations on record.