CN112987737B - Bi-RRT unmanned ship multi-ship navigation method and equipment considering corner constraint - Google Patents

Bi-RRT unmanned ship multi-ship navigation method and equipment considering corner constraint Download PDF

Info

Publication number
CN112987737B
CN112987737B CN202110215822.1A CN202110215822A CN112987737B CN 112987737 B CN112987737 B CN 112987737B CN 202110215822 A CN202110215822 A CN 202110215822A CN 112987737 B CN112987737 B CN 112987737B
Authority
CN
China
Prior art keywords
unmanned
node
unmanned ship
ship
rrt
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.)
Active
Application number
CN202110215822.1A
Other languages
Chinese (zh)
Other versions
CN112987737A (en
Inventor
谢远龙
吴昊
王书亭
蒋立泉
孟杰
李鹏程
吴天豪
孙浩东
付清晨
严一鸣
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Huazhong University of Science and Technology
Original Assignee
Huazhong University of Science and Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Huazhong University of Science and Technology filed Critical Huazhong University of Science and Technology
Priority to CN202110215822.1A priority Critical patent/CN112987737B/en
Publication of CN112987737A publication Critical patent/CN112987737A/en
Application granted granted Critical
Publication of CN112987737B publication Critical patent/CN112987737B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

The invention belongs to the field of unmanned boats and discloses a Bi-RRT unmanned boat multi-boat navigation method and equipment considering corner constraint. According to the method, through global task allocation, each unmanned ship route connected with nodes is considered based on the allocated task planning, collision judgment and coordinated collision avoidance are carried out on the route nodes, whether the unmanned ships reach corresponding target points or not is judged through node release, and finally, all the unmanned ships reach the target points. In the whole process, the large scene, multiple tasks and flexibility of the unmanned ship are considered, so that the efficiency of the route planning is ensured, in the Bi-RRT route planning algorithm considering corner constraint, the corner constraint of the tail ends of the expansion nodes and the connecting nodes is considered to adapt to the motion constraint of the unmanned ship, and the tree is expanded more greedy and clear by heuristic expansion, and the efficiency is higher.

Description

Bi-RRT unmanned ship multi-ship navigation method and equipment considering corner constraint
Technical Field
The invention belongs to the field of unmanned boats, and particularly relates to a Bi-RRT unmanned boat multi-boat navigation method and equipment considering corner constraint.
Background
Unmanned ship navigation is a key technology for safe and autonomous navigation, and multi-ship navigation planning is a key for ensuring navigation efficiency, and has important significance for task coordination and efficient operation. The multi-boat navigation comprises task allocation, global route planning and route coordination strategies, for the multi-boat, on the basis of establishing the global route, the passing priority of the coordinated naval vessels needs to be considered, and a corresponding collision avoidance strategy is adopted, so that the course and the speed can be timely and effectively adjusted. On the premise of ensuring the target task, the navigation is as fast and safe as possible.
The fast-expansion Random tree (RRT) algorithm is based on a sampling motion planning idea, does not need to establish a space information model, has the advantages of complete state space probability, high searching speed and the like, is more applied and mature in the fields of unmanned aerial vehicles, autonomous mobile robots and the like, but is less researched in the field of unmanned boats. The connection type bidirectional rapid expansion random tree (Bi-RRT) has the characteristics of high generation speed and strong adaptability. The unmanned ship has high navigational speed and strong real-time performance, and the Bi-RRT algorithm has outstanding advantages and can adapt to the scene requirements. However, the Bi-RRT algorithm has two main problems in multi-boat navigation planning:
(1) the Bi-RRT algorithm has strong randomness and low convergence speed in global planning, and the path of a connecting point is tortuous;
(2) the Bi-RRT can avoid the collision of obstacles in the global map, but the node collision of multiple boats is difficult to solve.
Therefore, it is necessary to provide a new Bi-RRT global path planning method to solve the above problems.
Disclosure of Invention
Based on the defects or the improvement requirements of the prior art, the invention provides a Bi-RRT unmanned ship multi-ship navigation method and equipment considering corner constraint, and multi-task navigation of the unmanned ships is realized by combining a coordination collision avoidance strategy based on priority through multi-task global planning and single-route corner constraint.
In order to achieve the above object, according to an aspect of the present invention, there is provided a Bi-RRT unmanned surface vehicle multi-vessel navigation method considering corner constraints, comprising the steps of:
step 1: generating a multi-target-point task scheme of the unmanned ship according to an actual working environment and a principle that the direct distance from the unmanned ship to a target point total path is shortest, and distributing each target-point task scheme to the corresponding unmanned ship according to a preset task release sequence;
step 2: according to an electronic chart of an actual working environment and allocation tasks received by each unmanned ship, global planning is firstly carried out on each unmanned ship, specifically, according to a target point task scheme allocated in the step 1, Bi-RRT considering corner constraint is adopted to carry out route planning on a single unmanned ship, and random tree expansion and unmanned ship corner constraint at the tail end of a connecting point are considered; then each planned feasible route is sent to the corresponding unmanned ship in real time, and the route state space is stored;
and step 3: in the navigation process, adjusting a multi-boat task planning sequence according to target information and position information of the current environment of the unmanned boat, judging whether an airway conflict exists in the existing airway state space, and if the airway conflict exists, sequentially passing through a conflict area according to a set priority passing sequence so as to coordinate and avoid the airway;
and 4, step 4: judging whether the unmanned ship reaches a target point, if so, releasing all nodes of the corresponding route of the unmanned ship, and otherwise, continuing the step 3;
and 5: and (4) judging whether the airway state space is empty, namely whether all unmanned boats reach the task target point, if so, ending the task, otherwise, continuing to return to the step 3.
Further, in step 1, after analyzing the surrounding environment, according to the predetermined target, based on the selectable actions and the provided resource restrictions, comprehensively making an action sequence for realizing the multi-target task; when tasks are distributed, the multi-target-point tasks are distributed to the unmanned boats one by one, and a path scheme for the unmanned boats to reach corresponding target points is pre-planned; and then calculating the total driving distance of all unmanned boats in each path scheme and the total quantity of the crossed nodes of each path, and selecting a scheme with the minimum cost weighting as a global navigation task scheme.
Further, the objective function formula of the cost weighting is as follows:
Figure BDA0002953057310000031
in the formula:
f, task allocation cost function;
n is the number of tasks, namely the number of unmanned boats;
μ1-a distance coefficient;
μ2-the number of intersections coefficient;
Lengthk-distance of the kth path;
Nodek-number of crossing nodes of the kth path.
Further, in step 2, considering the random tree expansion and the unmanned ship corner constraint at the end of the connection point, the method specifically includes:
under a fixed coordinate system, a relation model between a path node and the step length deltas and the path curvature angle theta is as follows:
sx＝x+Δs cos(θ)
sy＝y+Δs sin(θ)
wherein, (x, y) is the last node,(s)x,sy) The expanded nodes are obtained; for a single random tree, there is a limit to the angle between two adjacent nodes, and the relationship to curvature and step size is expressed as a global path angle constraint:
Figure BDA0002953057310000032
Δθ≤ρmaxΔs＝θthreshold
where ρ is the real-time curvature of the node on the path, ρmaxFor the maximum curvature of the path, Δ s is the arc length (step size) between adjacent nodes, θthresholdIn order to obtain the allowed maximum rotation angle, delta theta is the steering angle of the expansion node;
for bi-directional connected RRT planning, Δ θ is vectorially expressed as:
Figure BDA0002953057310000041
Figure BDA0002953057310000042
Δθ1i＜θthreshold,Δθ2j＜θthreshold
Figure BDA0002953057310000043
representing a first random tree T1The last step of (2) to extend the step size vector,
Figure BDA0002953057310000044
represents T1New node xinewI and j are node numbers;
Figure BDA0002953057310000045
representing a second random tree T2The last step of (2) to extend the step size vector,
Figure BDA0002953057310000046
represents T2Go new node xi'newExpanding node vectors; delta theta1i、Δθ2jAre respectively xinew、ξ'newCorresponding extended nodal steering angle, θthresholdIs the largest corner of the unmanned boat.
Further, judging the distance between the expansion nodes of the two random trees in each step of expansion
Figure BDA0002953057310000047
When in use
Figure BDA0002953057310000048
T is indicated when less than a threshold value epsilon1And T2Meeting:
Figure BDA0002953057310000049
determining an extended node xi when encounteringnewAnd ξ'newWhether the connection line satisfies the rotationThe angle constraint, the steering angle γ of this line is expressed as:
Figure BDA00029530573100000410
if gamma is smaller than the maximum rotation angle theta of the unmanned boatthresholdIf the expansion nodes meet the corner constraint, otherwise, the nodes are considered to be invalid to meet, and T is exchanged1And T2And continuing the next step of search expansion until the connection meeting the corner constraint is found.
Further, in step 3, a priority-based route coordination collision avoidance strategy is adopted, including the establishment of priority, wherein:
when a plurality of unmanned boats simultaneously apply for the same cross node resource, calculating the priority of the task according to the current condition and the endurance state of each unmanned boat by weighting:
Figure BDA0002953057310000051
wherein A, B, C is a weighting coefficient, which is an empirical value; emergency represents the task urgency, Distance represents the remaining Distance to a target point, and crossNum represents the number of cross nodes in the remaining road section; energy represents the residual electric quantity of the unmanned ship and is used for measuring the endurance state of the unmanned ship;
the priority value is larger, the priority is higher, and the collision avoidance is performed by sequentially passing through the areas which are easy to collide according to the priority order.
Further, the step 3 adopts a priority-based route coordination collision avoidance strategy, and further comprises the formulation of the route coordination strategy, specifically comprising collision prediction and variable speed collision avoidance, wherein:
and (3) collision prediction:
recording the cross node area as Dis, triggering collision prediction when ships enter the area, namely when D (t) < Dis, sampling trend paths among unmanned ships at equal time intervals, and calculating the distance D (t) between each unmanned ship and the cross node at each moment t in real time:
Figure BDA0002953057310000052
the distances delta X (t) and delta Y (t) of the unmanned ship in two directions from the unmanned ship to the intersection node at the moment t are as follows:
Figure BDA0002953057310000053
Figure BDA0002953057310000054
(XT,YT) Is the coordinate of the cross node, (X, Y) is the current position of the unmanned ship, V is the current speed of the unmanned ship,
Figure BDA0002953057310000055
is the current heading angle; if a plurality of unmanned boats exist in the Dis area at the same time, the airway node does not meet the safety requirement and is a dangerous area, and the cross node is a collision node and is easy to collide;
speed change and collision avoidance:
let DminΔ d (t) is the difference between the distances of the two drones from the collision node, which is the minimum shift distance; the coordination collision avoidance process is implemented as follows:
(1) when a plurality of unmanned boats meet D at the same timeminIf < Δ D (t) and D (t) < Dis, triggering a speed change coordination strategy: in the area of the conflict node, according to the established priority, the priority value is high, the node resources are occupied preferentially, the unmanned ship passing through the node is accelerated, and the rest unmanned ships are decelerated until the unmanned ship of the previous priority drives away from the Dis area and then starts to pass through in an accelerated manner;
(2) when D (t) is less than or equal to DminOnly one unmanned ship in the area can pass through the unmanned ship at the same time, and the rest unmanned ships are temporarily stopped for waiting; and when the unmanned ship drives away from the area, releasing the node resources, and sequentially passing through the rest unmanned ships according to the sequence of the priority queue.
To achieve the above object, according to another aspect of the present invention, there is provided a computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements the Bi-RRT unmanned multi-boat navigation method considering turning angle constraints as described in any one of the preceding.
To achieve the above object, according to another aspect of the present invention, there is provided a Bi-RRT unmanned multi-boat navigation device considering turning angle constraints, comprising the computer-readable storage medium as described above and a processor for calling and processing a computer program stored in the computer-readable storage medium.
In general, compared with the prior art, the above technical solution contemplated by the present invention can obtain the following beneficial effects:
1. aiming at the multi-task target navigation planning of multiple unmanned boats, the large scene, multiple tasks and flexibility of the unmanned boats are considered firstly, global task allocation is carried out based on the principle that the direct distance of a target point total path is shortest and a preset task release sequence, and the global route planning condition of each unmanned boat is obtained preliminarily; on the basis, a Bi-RRT global path planning method considering corner constraint is provided, the method considers the corner constraint of the unmanned ship, solves the problem of strong randomness of single unmanned ship path optimization, enables the air route to be smooth, and avoids overlarge zigzag angle of a connecting node, thereby obtaining a global path state space; meanwhile, on the basis of the existing global path state space, a priority-based route coordination collision avoidance strategy is provided, and coordination collision avoidance is sequentially realized at the parts which are easy to conflict according to a priority passing sequence, so that the rapid planning and coordination collision avoidance of unmanned ship multi-boat navigation are completed.
2. The environment and resource conditions are fully considered in the task planning process, the global navigation task allocation optimization is ensured, the route planning efficiency is improved, and the flexibility is high.
3. The expansion of the Bi-RRT random tree of the unmanned ship and the corner constraint at the tail end of the connecting point are considered globally, the planned path is ensured to be a feasible route which accords with the motion limitation of the unmanned ship, the arc length limitation is introduced into the corner constraint simultaneously to carry out global path angle constraint, the algorithm randomness can be further limited, the search efficiency is improved, the expansion of the random tree is greedy and clear, and the heuristic property is achieved.
4. Because the connection mode in the Bi-RRT algorithm has weaker constraint on the connection points, the distance between new nodes of two trees is judged in each expansion step to optimize the connection mode, and the self-adaptive adjustment is carried out on the expansion sequence of the double trees by judging the satisfying condition of the node connection line on the rotation angle constraint, so that the updating condition of the newly expanded nodes is constrained, the expansion progress of the double trees is kept consistent as much as possible, the convergence is accelerated, and meanwhile, the path planning result is more reasonable.
5. The priority of the unmanned ship passing through the conflict area is set by comprehensively considering the task emergency degree, the task remaining time (measured by distance and cross nodes) and the cruising state of the unmanned ship, and different task scenes can be adapted by adjusting the weighting coefficient, so that the priority is more reasonable to make, and the task is ensured to be successfully completed.
6. The collision prediction is triggered only when the unmanned ship enters the cross node area, so that the energy of a processor can be saved, and the occurrence conditions of dangerous areas and collisions can be predicted in advance through the position, the speed and the orientation of the unmanned ship, so that the coordinated collision avoidance can be favorably carried out in advance. And a variable speed collision avoidance strategy is adopted according to the set priority, so that a plurality of unmanned boats with collision can rapidly and smoothly pass through the dangerous area, the task execution efficiency is improved, and the influence on the total task progress is reduced as much as possible.
7. The invention is also suitable for uniformly carrying out global path planning and scheduling on all unmanned boats in the central controller of the upper computer, so as to be convenient for multi-boat coordination collision avoidance and ensure the high efficiency of multi-boat navigation.
Drawings
FIG. 1 is a flow chart of a Bi-RRT multi-unmanned-vessel navigation method considering corner constraints.
FIG. 2 is a flow chart of Bi-RRT global path planning considering corner constraints.
FIG. 3 is a diagram of new node rotation angle constraints for a random spanning tree.
FIG. 4 is a schematic view of end connection point rotation angle constraints.
FIG. 5 is a coordinated route collision avoidance strategy based on variable speed adjustment.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention. In addition, the technical features involved in the respective embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.
The Bi-RRT unmanned ship multi-ship navigation method considering the corner constraint is mainly conceived in the preferred embodiment of the invention, aiming at the establishment of the global air route of each unmanned ship, the Bi-RRT global path planning method considering the corner is proposed based on an angle constraint model, the method considers the corner constraint of the unmanned ship, solves the problem of strong randomness, enables the air route to be smooth, and avoids the overlarge bending angle of a connecting node. Meanwhile, on the basis of the existing global path state space, a priority-based route coordination collision avoidance strategy is provided, and local reactive navigation is carried out through coordination of an upper computer central controller.
Firstly, taking the Bi-RRT of the corner constraint into consideration, carrying out global route planning on each unmanned ship and generating a feasible route of the unmanned ship with the corner constraint. And then the feasible routes are sent to the corresponding unmanned ships in real time according to the task allocation scheme, whether conflicts exist in the existing route state space is judged in real time in the motion process, whether a collision avoidance strategy needs to be triggered is determined, and all nodes of the route are released until the unmanned ships reach a target point. And releasing all the route nodes until all the unmanned ships reach the target point, and terminating the whole planning process. In the whole process, the flexibility of large scenes and motions of an unmanned ship environment is considered, the planning efficiency is required, two rapid expansion random trees are grown simultaneously from an initial state point and a target state point by the bidirectional Bi-RRT to search a state space in a Bi-RRT route planning algorithm considering corner constraint, and the tree expansion is greedy and clear due to the heuristic expansion, and the efficiency is higher. Meanwhile, the maximum angle limit of the tail ends of the expansion nodes and the connecting nodes is considered to meet the motion constraint of the unmanned ship. And after each route is generated, each route is issued to the corresponding unmanned ship in real time, the upper computer central controller monitors the state of each unmanned ship in real time, performs collision detection and prediction, and adopts a corresponding route coordination strategy.
The following detailed description of the principles of the invention is provided in connection with the accompanying drawings.
The invention discloses a Bi-RRT unmanned ship multi-ship navigation method considering corner constraint, which is shown in a flow chart of an attached figure 1.
Step 1: the unmanned ship multi-target-point task planning method is characterized in that efficient sailing of the unmanned ship is taken as a target, actual environment is combined, cost weighting of the shortest straight path sum and the minimum cross node is considered, a multi-target-point task scheme of each unmanned ship is generated, global routes of all the unmanned ships are preliminarily planned according to a task release sequence (preferably, task emergency degree), namely, the target points are correspondingly distributed to the unmanned ships one by one, and the starting sequence of the unmanned ships is set.
And 2, step: the unmanned vehicle is first globally planned according to the electronic chart and the received assignment tasks. And according to the planning scheme, the Bi-RRT of the corner constraint is considered to carry out route planning on each unmanned ship, and random tree expansion and unmanned ship corner constraint at the tail end of the connecting point are considered. And then the feasible airway is sent to the corresponding unmanned boat in real time, and the airway state space is stored.
And step 3: in the process of navigation, the task planning module can adjust a multi-boat task planning sequence according to the current environment target information and the position information, judge whether conflicts exist in the existing route state space, and trigger a priority-based route coordination collision avoidance strategy if the conflicts exist.
And 4, step 4: and (4) judging whether the unmanned ship reaches a target point, if so, releasing all nodes of the route, and otherwise, continuing the step (3).
And 5: and (4) judging whether the airway state space is empty, namely whether all unmanned boats reach the task target point, if so, ending, and if not, continuing to return to the step 3.
Further, in step 1, during global task planning, according to the principle that the linear distance to the total path of the target point is shortest, the generation of the multi-target tasks of each unmanned ship specifically includes:
the task planning is to analyze the surrounding environment, reason the selectable actions and the provided resource limits according to the preset target, and comprehensively work out the action sequence for realizing the multi-target task. When tasks are distributed, the system distributes all multi-target-point tasks to each unmanned ship, and a corresponding path scheme is planned. And calculating the sum of the straight-line driving distances of all unmanned boats in each scheme and the sum of the number of crossed nodes of each straight-line path, and selecting the scheme with the minimum cost weighting. The target function formula is as follows:
Figure BDA0002953057310000101
in the formula:
f, task allocation cost function;
n is the number of tasks or the number of unmanned boats;
μ1，μ2-distance coefficient, number of intersections coefficient;
Lengthi-distance of the ith linear path;
Nodei-number of crossing nodes of ith path.
After the task allocation scheme is determined, global route planning is sequentially performed according to the release sequence of the tasks, for example, the starting sequence of each unmanned ship is set.
Further, in step 2, considering the random tree expansion and the unmanned ship corner constraint at the end of the connection point, the method specifically includes:
fig. 2 is a Bi-RRT global path planning flow chart considering corner constraints, and fig. 3 is a schematic diagram of a new node corner constraint of a random expansion tree. And (3) judging whether the node expansion of the two random trees needs to be subjected to corner constraint, judging whether the end of the meeting connection point is subjected to angle judgment, and if the end of the meeting connection point does not meet the corner constraint, discarding the currently expanded node and carrying out next search.
Under a fixed coordinate system, a relation model between a path node and the step length deltas and the path curvature angle theta is as follows:
sx＝x+Δs cos(θ)
sy＝y+Δs sin(θ)
wherein, (x, y) is the last node,(s)x,sy) The expanded nodes are obtained; in order to limit the randomness of the algorithm and thus improve the search efficiency, for a single random tree, the included angle between two adjacent nodes is limited, and the relation with the curvature and the step length is expressed as a global path angle constraint:
Figure BDA0002953057310000111
Δθ≤ρmaxΔs＝θthreshold
where ρ is the real-time curvature of the node on the path, ρmaxFor the maximum curvature of the path, Δ s is the arc length (step size) between adjacent nodes, θthresholdIn order to obtain the maximum allowable rotation angle, delta theta is the steering angle of the expansion node;
for bi-directional connected RRT planning, Δ θ is expressed as a vector:
Figure BDA0002953057310000112
Figure BDA0002953057310000113
Figure BDA0002953057310000114
representing a first random tree T1The last step of (2) is to extend the step size vector,
Figure BDA0002953057310000115
represents T1Expanding the node vector;
Figure BDA0002953057310000116
representing a second random tree T2The last step of (2) to extend the step size vector,
Figure BDA0002953057310000117
represents T2And expanding the node vector. Delta theta1i,Δθ2jTo extend the node steering angle, thetathresholdIs the maximum corner of the unmanned boat and should meet the following conditions:
Δθ1i＜θthreshold,Δθ2j＜θthreshold
the connection mode in the Bi-RRT algorithm has weak constraint on connection points, so the connection mode needs to be optimized, and the judgment of the distance between new nodes of two trees is added in each step of expansion
Figure BDA0002953057310000118
When the temperature is higher than the set temperature
Figure BDA0002953057310000119
Less than the threshold epsilon indicates T1And T2Meeting:
Figure BDA00029530573100001110
determining the node xi when meetingnew,ξ'newWhether the connecting line is constrained by a turning angle or not, and the steering angle gamma of the tail end of the connecting node can be expressed as:
Figure BDA00029530573100001111
FIG. 4 is a schematic diagram illustrating the corner constraint of the end connection point, where the corner constraint of the connection point is: when the steering angle gamma at the tail end of the connecting node is smaller than the maximum rotation angle theta of the unmanned shipthresholdIf the expansion nodes meet the corner constraint, otherwise, the nodes are considered to be encountered ineffectively, the expansion sequence of the random search tree is exchanged to continue the next search expansion until the expansion nodes meet the corner constraintAnd (4) connecting the bundles. In the figure, γ will be greater than θthresholdWhen the encounter is invalid, the next search is performed with T2Begin to expand, set xijExpanding a new node xi 'for a node closest to the random point'new2And judging the steering angle of the tail end of the node, meeting the motion constraint requirement at the moment, terminating the expansion and recording the path node.
Further, in step 3, the priority-based route coordination collision avoidance strategy specifically includes:
in order to ensure the coordination of the system, the sensor information of each unmanned ship is transmitted to the central controller of the upper computer, and the central controller uniformly plans each unmanned ship according to all grasped information. The route coordination collision avoidance strategy based on the priority is divided into the establishment of priority and the route collision avoidance coordination strategy.
Further, the setting of the priority specifically includes:
although the global route is already planned preferentially according to the Emergency degree of the task in step 1, in the course of navigation, if a plurality of unmanned boats appear in a certain area of the intersection node, that is, a plurality of unmanned boats simultaneously apply for the same intersection node resource, according to the current operating condition of each unmanned boat, such as the Emergency degree of the task, the required execution time (the remaining Distance to the target point, the number of intersection nodes in the remaining road segment CrossNum), and the cruising state of the unmanned boat, such as the remaining electric quantity Energy of the unmanned boat, the task priority at the intersection node may need to be planned locally again. And the priority of the task at the cross node is calculated according to the weight:
Figure BDA0002953057310000121
here, A, B, C are weighting coefficients, and are empirical values. The local task priority can be changed by adjusting A, B, C the value to better suit the task requirements according to different work scenarios and task requirements.
Further, the coordination strategy for route collision avoidance specifically includes:
the node collision problem is solved by collision prediction and speed change collision avoidance. And the cross node area is Dis, collision prediction is triggered when ships enter the area, namely D (t) < Dis, the trend paths among the ships are sampled at equal time intervals, and the distance D (t) between each ship and the cross node at each moment is calculated in real time. As shown in FIG. 5, boats O and T will be steered towards a uniform node (X)T,YT) Distance delta X between two directions at which boat O arrives at intersection node at time t0(t)，ΔY0(t) are respectively:
Figure BDA0002953057310000131
Figure BDA0002953057310000132
Figure BDA0002953057310000133
distance delta X between two directions of ship T reaching cross node at time T1(t)，ΔY1(t) are respectively:
Figure BDA0002953057310000134
Figure BDA0002953057310000135
Figure BDA0002953057310000136
wherein, the two boats correspond to each other, (X)T,YT) Is the coordinate of the cross node, (X)0,Y0),(X1,Y1) Is the current position of the unmanned ship, V0，V1The current speed of the unmanned ship is the current speed,
Figure BDA0002953057310000137
as the current orientation angle, D0(t),D1(t) is the distance from the intersection node at each time instant. If a plurality of unmanned boats exist in the Dis area at the same moment, the route node does not meet the safety requirement and is a dangerous area, and the cross node is a conflict node.
Let DminΔ D (t) ═ D as the minimum shift distance0(t)-D1(t) | is the difference in distance from the conflicting node. When a plurality of unmanned boats meet D at the same timeminIf D (t) is less than D (t), triggering a speed change coordination strategy, preferentially occupying node resources in the area of the conflict node according to the established priority and the priority weight, accelerating to pass through the node, decelerating the rest unmanned boats until the previous boat is driven away from the Dis area, namely the corresponding D (t) is more than Dis; when D (t) is less than or equal to DminIn time, only one unmanned ship in the area can be allowed to pass through at the same time, and the rest unmanned ships are temporarily stopped and wait. And when the unmanned ship drives away from the area, releasing the node resources, and sequentially passing through the rest unmanned ships according to the sequence of the priority queue.
It will be understood by those skilled in the art that the foregoing is only a preferred embodiment of the present invention, and is not intended to limit the invention, and that any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the scope of the present invention.

Claims (8)

1. A Bi-RRT unmanned ship multi-ship navigation method considering corner constraint is characterized by comprising the following steps:
step 1: generating a multi-target-point task scheme of the unmanned ship according to an actual working environment and a principle that the direct distance from the unmanned ship to a target point total path is shortest, and distributing each target-point task scheme to the corresponding unmanned ship according to a preset task release sequence;
step 2: according to an electronic chart of an actual working environment and allocation tasks received by each unmanned ship, global planning is firstly carried out on each unmanned ship, specifically, according to a target point task scheme allocated in the step 1, Bi-RRT considering corner constraint is adopted to carry out route planning on a single unmanned ship, and random tree expansion and unmanned ship corner constraint at the tail end of a connecting point are considered; then each planned feasible route is sent to the corresponding unmanned ship in real time, and the route state space is stored;
and step 3: in the navigation process, adjusting a multi-boat task planning sequence according to target information and position information of the current environment of the unmanned boat, judging whether an airway conflict exists in the existing airway state space, and if the airway conflict exists, sequentially passing through a conflict area according to a set priority passing sequence to coordinate and avoid the airway;
and 3, adopting a priority-based route coordination collision avoidance strategy, and making the route coordination collision avoidance strategy, specifically comprising collision prediction and variable speed collision avoidance, wherein:
and (3) collision prediction:
recording the cross node area as Dis, triggering collision prediction when the ship enters the area, namely when D (t) is less than the radius of Dis, sampling trend paths among unmanned ships at equal time intervals, and calculating the distance D (t) between each unmanned ship and the cross node at each time t in real time:
Figure FDA0003637063650000011
the distances delta X (t) and delta Y (t) of the unmanned ship in two directions from the unmanned ship to the intersection node at the moment t are as follows:
Figure FDA0003637063650000012
Figure FDA0003637063650000021
(XT,YT) Is the coordinate of the cross node, (X, Y) is the current position of the unmanned ship, V is the current speed of the unmanned ship,
Figure FDA0003637063650000022
is the current heading angle; if a plurality of unmanned boats exist in the Dis area at the same moment, the route node does not meet the safety requirement and is a dangerous area, and the cross node is a collision node and is easy to collide;
speed change and collision avoidance:
let DminIs the minimum shift distance, Δ d (t) is the difference between the distances of the two drones from the collision node; the coordination collision avoidance process is implemented as follows:
(1) when a plurality of unmanned boats meet D at the same momentminIf the radius is less than Delta D (t) and D (t) is less than Dis, the speed change coordination strategy is triggered: in the area of the conflict node, according to the established priority, the priority value is high, the node resources are occupied preferentially, the unmanned ship passing through the node is accelerated, and the rest unmanned ships are decelerated until the unmanned ship of the previous priority drives away from the Dis area and then starts to pass through in an accelerated manner;
(2) when D (t) is less than or equal to DminOnly one unmanned boat in the area can pass through the unmanned boat at the same time, and the rest unmanned boats are temporarily stopped and wait; when the unmanned ship drives away from the area, the node resources are released, and the nodes sequentially pass through the remaining unmanned ships according to the sequence of the priority queue;
and 4, step 4: judging whether the unmanned ship reaches a target point, if so, releasing all nodes of the corresponding route of the unmanned ship, and otherwise, continuing the step 3;
and 5: and (4) judging whether the airway state space is empty, namely whether all unmanned boats reach the task target point, if so, ending the task, otherwise, continuing to return to the step 3.
2. The Bi-RRT unmanned ship multi-boat navigation method considering corner constraints as claimed in claim 1, characterized in that in step 1, after analyzing the surrounding environment, according to the predetermined target, based on the selectable actions and the provided resource restrictions, the action sequence for realizing the multi-target task is comprehensively formulated; when tasks are distributed, the tasks of multiple target points are distributed to the unmanned boats in a one-to-one correspondence mode, and a path scheme for the unmanned boats to reach the corresponding target points is pre-planned; and then calculating the total driving distance of all unmanned boats in each path scheme and the total quantity of the crossed nodes of each path, and selecting a scheme with the minimum cost weighting as a global navigation task scheme.
3. The Bi-RRT unmanned multi-boat navigation method considering corner constraints as claimed in claim 2, wherein the cost weighted objective function formula is as follows:
Figure FDA0003637063650000031
in the formula:
f, task allocation cost function;
n is the number of tasks, namely the number of unmanned boats;
μ1-a distance coefficient;
μ2-the number of intersections coefficient;
Lengthk-distance of the kth path;
Nodek-number of crossing nodes of the kth path.
4. The Bi-RRT unmanned ship multi-boat navigation method considering corner constraint as claimed in any one of claims 1-3, wherein in the step 2, considering random tree expansion and unmanned ship corner constraint at the end of a connection point specifically comprises:
under a fixed coordinate system, a relation model between a path node and the step length deltas and the path curvature angle theta is as follows:
sx＝x+Δscos(θ)
sy＝y+Δssin(θ)
wherein, (x, y) is the last node,(s)x,sy) The expanded nodes are obtained; for a single random tree, there is a limit to the angle between two adjacent nodes, and the relationship to curvature and step size is expressed as a global path angle constraint:
Figure FDA0003637063650000032
Δθ≤ρmaxΔs＝θthreshold
where ρ is the real-time curvature of the node on the path, ρmaxFor the maximum curvature of the path, Δ s is the arc length (step size) between adjacent nodes, θthresholdIn order to obtain the maximum allowable rotation angle, delta theta is the steering angle of the expansion node;
for bi-directional connected RRT planning, Δ θ is vectorially expressed as:
Figure FDA0003637063650000041
Figure FDA0003637063650000042
Δθ1i＜θthreshold,Δθ2j＜θthreshold
Figure FDA0003637063650000043
representing a first random tree T1The last step of (2) to extend the step size vector,
Figure FDA0003637063650000044
represents T1New node xinewThe expansion node vector of (1), i and j are node numbers;
Figure FDA0003637063650000045
representing a second random tree T2The last step of (2) to extend the step size vector,
Figure FDA0003637063650000046
represents T2New node xin'ewExpanding node vectors;Δθ1i、Δθ2jare respectively xinew、ξn'ewCorresponding extended nodal steering angle, θthresholdIs the largest corner of the unmanned boat.
5. The Bi-RRT unmanned ship multi-boat navigation method considering corner constraint as claimed in claim 4, characterized in that the distance between the expansion nodes of two random trees is judged in each expansion step
Figure FDA0003637063650000047
When in use
Figure FDA0003637063650000048
T is indicated when less than a threshold value epsilon1And T2Meeting:
Figure FDA0003637063650000049
determining an extended node xi when encounteringnewAnd ξ'newWhether or not the link satisfies the turning angle constraint, the steering angle γ of the link is expressed as:
Figure FDA00036370636500000410
if gamma is smaller than the maximum rotation angle theta of the unmanned ship, the expansion nodes meet rotation angle constraint, otherwise, the nodes are considered to be invalid to meet, and T is exchanged1And T2And continuing the next step of search expansion until the connection meeting the corner constraint is found.
6. The Bi-RRT unmanned ship multi-boat navigation method considering corner constraint as claimed in claim 1, wherein a priority-based route coordination collision avoidance strategy is adopted in step 3, and the priority is formulated, wherein:
when a plurality of unmanned boats simultaneously apply for the same cross node resource, calculating the priority of the task according to the current condition and the endurance state of each unmanned boat by weighting:
Figure FDA0003637063650000051
wherein A, B, C is a weighting coefficient, which is an empirical value; emergency represents the task urgency, Distance represents the remaining Distance to a target point, and crossNum represents the number of cross nodes in the remaining road section; energy represents the residual electric quantity of the unmanned ship and is used for measuring the endurance state of the unmanned ship;
the priority value is larger, the priority is higher, and the collision avoidance is performed by sequentially passing through the areas which are easy to collide according to the priority order.
7. A computer-readable storage medium, wherein a computer program is stored on the computer-readable storage medium, and when executed by a processor, the computer program implements the Bi-RRT unmanned multi-boat navigation method considering corner constraints according to any one of claims 1 to 6.
8. A Bi-RRT unmanned-craft multi-craft navigation device taking into account corner constraints, comprising the computer-readable storage medium of claim 7 and a processor for invoking and processing a computer program stored in the computer-readable storage medium.
CN202110215822.1A 2021-02-26 2021-02-26 Bi-RRT unmanned ship multi-ship navigation method and equipment considering corner constraint Active CN112987737B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110215822.1A CN112987737B (en) 2021-02-26 2021-02-26 Bi-RRT unmanned ship multi-ship navigation method and equipment considering corner constraint

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110215822.1A CN112987737B (en) 2021-02-26 2021-02-26 Bi-RRT unmanned ship multi-ship navigation method and equipment considering corner constraint

Publications (2)

Publication Number Publication Date
CN112987737A CN112987737A (en) 2021-06-18
CN112987737B true CN112987737B (en) 2022-06-17

Family

ID=76351020

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110215822.1A Active CN112987737B (en) 2021-02-26 2021-02-26 Bi-RRT unmanned ship multi-ship navigation method and equipment considering corner constraint

Country Status (1)

Country Link
CN (1) CN112987737B (en)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113389186B (en) * 2021-07-07 2022-05-17 安徽工程大学 Multi-unmanned-boat-cooperated sea surface oil stain cleaning method and system
CN113467476B (en) * 2021-08-02 2023-04-28 福州大学 Collision-free detection rapid random tree global path planning method considering corner constraint
CN114241349A (en) * 2021-11-04 2022-03-25 中国船舶工业系统工程研究院 Multi-unmanned-boat collaborative identification method and device
CN114526750B (en) * 2022-01-10 2024-02-27 深圳市灵星雨科技开发有限公司 Path optimization method and device
CN114593743A (en) * 2022-03-02 2022-06-07 杭州华鲲云起信息技术有限公司 Path planning method and device based on improved bidirectional RRT algorithm
CN118362130B (en) * 2024-06-18 2024-09-10 上海海事大学 Ship path planning method based on improved rapid search random tree

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109540136A (en) * 2018-10-25 2019-03-29 广东华中科技大学工业技术研究院 A kind of more unmanned boat collaboration paths planning methods
CN110262473B (en) * 2019-04-29 2021-09-14 上海交通大学 Unmanned ship automatic collision avoidance method based on improved Bi-RRT algorithm
CN110196598B (en) * 2019-06-25 2022-03-04 北京航天控制仪器研究所 Dynamic collision avoidance method for unmanned ship
CN110687917A (en) * 2019-09-06 2020-01-14 哈尔滨工程大学 Multi-unmanned-boat distributed formation reconfiguration formation control method
CN111678523B (en) * 2020-06-30 2022-05-17 中南大学 Rapid BI _ RRT obstacle avoidance trajectory planning method based on STAR algorithm optimization
CN111913481B (en) * 2020-06-30 2023-06-06 航天时代(青岛)海洋装备科技发展有限公司 Multi-unmanned-ship cooperative formation method with variable pilot modes
CN111928853B (en) * 2020-07-30 2023-06-02 西南电子技术研究所（中国电子科技集团公司第十研究所） Rapid route planning method for space-based platform in complex environment
CN112327838B (en) * 2020-10-29 2023-03-31 哈尔滨工程大学 Multi-unmanned surface vessel multi-task allocation method based on improved self-mapping algorithm
CN112327914B (en) * 2020-10-30 2023-08-22 广东工业大学 Indoor collaborative path planning method and system for high-rise fire-fighting multiple unmanned aerial vehicles
CN112327850B (en) * 2020-11-06 2023-10-13 大连海事大学 Path planning method for unmanned surface vehicle

Also Published As

Publication number Publication date
CN112987737A (en) 2021-06-18

Similar Documents

Publication Publication Date Title
CN112987737B (en) Bi-RRT unmanned ship multi-ship navigation method and equipment considering corner constraint
CN108594853B (en) Unmanned aerial vehicle formation control method
CN110956853B (en) Multi-ship collision prediction method, system and storage medium
CN111045453A (en) Cooperative control system and method based on unmanned ship and multi-underwater robot
CN110687917A (en) Multi-unmanned-boat distributed formation reconfiguration formation control method
CN112230657B (en) Intelligent vehicle-oriented regional collaborative driving intention scheduling method, system and medium
CN113359737A (en) Ship formation self-adaptive event trigger control method considering formation expansion
CN114019984B (en) Unmanned ship and unmanned ship formation online track planning method and system
CN114088094A (en) Intelligent route planning method and system for unmanned ship
Ødven et al. Static and dynamic multi-obstacle avoidance and docking of ASVs using computational geometry and numerical optimal control
Ma et al. Overtaking Path Planning for CAV based on Improved Artificial Potential Field
Zhai et al. Path planning algorithms for USVs via deep reinforcement learning
CN112650276B (en) Integral return flight path planning method, planning system, storage medium and unmanned aerial vehicle
CN114613140B (en) Intelligent scheduling method and device for cross area of unmanned vehicle
CN116203990A (en) Unmanned plane path planning method and system based on gradient descent method
CN115774397A (en) Optimal formation anti-collision control algorithm of multi-Agents system based on collision risk
Zhuang et al. Multiple Moving Obstacles Avoidance for USV using Velocity Obstacle Method
Nichting et al. Case study on gap selection for automated vehicles based on deep q-learning
Fu et al. Method for collision avoidance by USV based on improved genetic algorithm
Jiang et al. A Docking Control Technology Using Artificial Potential Field and NMPC for Pontoon Bridge Blocks
Kim et al. Frenet Frame Based Local Motion Planning in Racing Environment
Wu et al. DRL-based path planning and obstacle avoidance of autonomous underwater vehicle
Li et al. Path Planning of Unmanned Ships Using Modified APF Algorithm
Ning et al. Multi-unmanned Surface Vehicles Formation Based on DMPC and Improved APF Method
CN114518760B (en) Unmanned vehicle navigation method and system server based on risk zone management

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
GR01 Patent grant
GR01 Patent grant