CN112987737A - 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
CN112987737A
CN112987737A CN202110215822.1A CN202110215822A CN112987737A CN 112987737 A CN112987737 A CN 112987737A CN 202110215822 A CN202110215822 A CN 202110215822A CN 112987737 A CN112987737 A CN 112987737A
Authority
CN
China
Prior art keywords
unmanned
node
unmanned ship
rrt
ship
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.)
Granted
Application number
CN202110215822.1A
Other languages
Chinese (zh)
Other versions
CN112987737B (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, routes of all unmanned boats connected by nodes are considered based on allocated task planning, collision judgment and coordination collision avoidance are carried out on the routes, whether the unmanned boats reach corresponding target points or not is judged through node release, and finally, all the unmanned boats 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.
To achieve the above object, according to one aspect of the present invention, there is provided a Bi-RRT unmanned multi-boat navigation method considering corner constraint, 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 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 action and the provided resource limitation, comprehensively making an action sequence for realizing the multi-target task; 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.
Further, the cost weighted objective function formula 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) Is composed ofExpanding the nodes; 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 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 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 or not the link satisfies the turning angle constraint, the steering angle γ of the link 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 orientationAn 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 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 point, the invention adds judgment on the distance between the new nodes of the two trees in each step of expansion to optimize the connection mode, and adaptively adjusts the expansion sequence of the double trees by judging the satisfying condition of the node connection line on the corner constraint, thereby constraining the updating condition of the newly expanded nodes, keeping the expansion progress of the double trees consistent as much as possible, accelerating convergence and simultaneously making the path planning result 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 corner 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 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.
Step 2: 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 the random tree expansion and the unmanned ship corner constraint at the tail end of the connecting point are considered. And then the feasible route is sent to the corresponding unmanned boat in real time, and the route 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 (3) judging whether the unmanned ship reaches a target point, releasing all nodes of the route if the unmanned ship reaches the target point, 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, ρmaxIs 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 BDA0002953057310000112
Figure BDA0002953057310000113
Figure BDA0002953057310000114
representing a first random tree T1The last step of (2) 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 ship and should satisfy 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 in use
Figure BDA0002953057310000119
Less than the threshold epsilon indicates T1And T2Meeting:
Figure BDA00029530573100001110
determine node xi when meetingnew,ξ'newWhether the connecting line is constrained by a rotation 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 shipthresholdAnd if the expansion nodes meet the corner constraint, otherwise, the nodes are considered to be encountered ineffectively, and the expansion sequence of the search random tree is exchanged to continue the next search expansion until the connection meeting the corner constraint is found. In the figure, γ will be greater than θthresholdWhen the encounter is invalid, the next search is performed with T2Begin to expand, set xijExpanding new node xi 'for the 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 formulation of priority and the route coordination strategy for collision avoidance.
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 task priority 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 route collision avoidance coordination strategy specifically includes:
the method comprises collision prediction and speed change collision avoidance for solving the node collision problem. 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 DeltaX between two directions of ship O reaching cross 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 the 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 (9)

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 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 multi-boat navigation method considering corner constraints as claimed in claim 1, wherein in step 1, after analyzing the surrounding environment, an action sequence for realizing multi-objective tasks is comprehensively formulated based on selectable actions and provided resource limitations according to a preset target; 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 FDA0002953057300000021
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 FDA0002953057300000022
Δθ≤ρ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 FDA0002953057300000031
Figure FDA0002953057300000032
Δθ1i＜θthreshold,Δθ2j＜θthreshold
Figure FDA0002953057300000033
representing a first random tree T1The last step of (2) to extend the step size vector,
Figure FDA0002953057300000034
represents T1New node xinewI and j are node numbers;
Figure FDA0002953057300000035
representing a second random tree T2The last step of (2) to extend the step size vector,
Figure FDA0002953057300000036
represents T2New node xin'ewExpanding node vectors; delta theta1i、Δθ2jAre respectively xinew、ξn'ewCorresponding extended nodal steering angle, θthresholdIs the largest corner of the unmanned boat.
5. The Bi-RRT unmanned multi-boat navigation method considering corner constraint as claimed in claim 4, wherein the distance between the extended nodes of two random trees is judged in each step of extension
Figure FDA0002953057300000037
When in use
Figure FDA0002953057300000038
T is indicated when less than a threshold value epsilon1And T2Meeting:
Figure FDA0002953057300000039
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 FDA00029530573000000310
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 FDA0002953057300000041
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. The Bi-RRT unmanned multi-boat navigation method considering corner constraint as claimed in claim 1 or 6, wherein step 3 adopts a priority-based route coordination collision avoidance strategy, and further comprises the formulation of a route collision avoidance 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 FDA0002953057300000042
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 FDA0002953057300000043
Figure FDA0002953057300000044
(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 FDA0002953057300000045
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 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.
8. 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 7.
9. A Bi-RRT unmanned multi-boat navigation device taking into account corner constraints, comprising the computer-readable storage medium of claim 8 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 true CN112987737A (en) 2021-06-18
CN112987737B 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)

Cited By (6)

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

Citations (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
CN110196598A (en) * 2019-06-25 2019-09-03 北京航天控制仪器研究所 A kind of dynamic collision prevention method of unmanned boat
CN110262473A (en) * 2019-04-29 2019-09-20 上海交通大学 A kind of unmanned boat automatic Collision Avoidance method based on improvement Bi-RRT algorithm
CN110687917A (en) * 2019-09-06 2020-01-14 哈尔滨工程大学 Multi-unmanned-boat distributed formation reconfiguration formation control method
CN111678523A (en) * 2020-06-30 2020-09-18 中南大学 Rapid BI _ RRT obstacle avoidance trajectory planning method based on STAR algorithm optimization
CN111913481A (en) * 2020-06-30 2020-11-10 北京航天控制仪器研究所 Multi-unmanned-boat collaborative formation method with variable pilot mode
CN111928853A (en) * 2020-07-30 2020-11-13 西南电子技术研究所（中国电子科技集团公司第十研究所） Method for rapidly planning air-based platform route under complex environment
CN112327914A (en) * 2020-10-30 2021-02-05 广东工业大学 Indoor collaborative path planning method and system for high-rise fire-fighting multi-unmanned aerial vehicle
CN112327838A (en) * 2020-10-29 2021-02-05 哈尔滨工程大学 Multi-unmanned surface vessel multi-task allocation method based on improved self-mapping algorithm
CN112327850A (en) * 2020-11-06 2021-02-05 大连海事大学 Unmanned surface vehicle path planning method

Patent Citations (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
CN110262473A (en) * 2019-04-29 2019-09-20 上海交通大学 A kind of unmanned boat automatic Collision Avoidance method based on improvement Bi-RRT algorithm
CN110196598A (en) * 2019-06-25 2019-09-03 北京航天控制仪器研究所 A kind of dynamic collision prevention method of unmanned boat
CN110687917A (en) * 2019-09-06 2020-01-14 哈尔滨工程大学 Multi-unmanned-boat distributed formation reconfiguration formation control method
CN111678523A (en) * 2020-06-30 2020-09-18 中南大学 Rapid BI _ RRT obstacle avoidance trajectory planning method based on STAR algorithm optimization
CN111913481A (en) * 2020-06-30 2020-11-10 北京航天控制仪器研究所 Multi-unmanned-boat collaborative formation method with variable pilot mode
CN111928853A (en) * 2020-07-30 2020-11-13 西南电子技术研究所（中国电子科技集团公司第十研究所） Method for rapidly planning air-based platform route under complex environment
CN112327838A (en) * 2020-10-29 2021-02-05 哈尔滨工程大学 Multi-unmanned surface vessel multi-task allocation method based on improved self-mapping algorithm
CN112327914A (en) * 2020-10-30 2021-02-05 广东工业大学 Indoor collaborative path planning method and system for high-rise fire-fighting multi-unmanned aerial vehicle
CN112327850A (en) * 2020-11-06 2021-02-05 大连海事大学 Unmanned surface vehicle path planning method

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
向金林等: "基于改进双向RRT的无人艇局部路径规划算法研究", 《中国造船》 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113389186A (en) * 2021-07-07 2021-09-14 安徽工程大学 Multi-unmanned-boat-cooperated sea surface oil stain cleaning method and system
CN113467476A (en) * 2021-08-02 2021-10-01 福州大学 Non-collision detection rapid stochastic tree global path planning method considering corner constraint
CN114241349A (en) * 2021-11-04 2022-03-25 中国船舶工业系统工程研究院 Multi-unmanned-boat collaborative identification method and device
CN114526750A (en) * 2022-01-10 2022-05-24 深圳市灵星雨科技开发有限公司 Path optimization 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
CN118362130A (en) * 2024-06-18 2024-07-19 上海海事大学 Ship path planning method based on improved rapid search random tree
CN118362130B (en) * 2024-06-18 2024-09-10 上海海事大学 Ship path planning method based on improved rapid search random tree

Also Published As

Publication number Publication date
CN112987737B (en) 2022-06-17

Similar Documents

Publication Publication Date Title
CN112987737B (en) Bi-RRT unmanned ship multi-ship navigation method and equipment considering corner constraint
CN110956853B (en) Multi-ship collision prediction method, system and storage medium
CN107168335A (en) A kind of unmanned surface vehicle path trace method of guidance for considering mixing multiple target avoidance
Shi et al. Study on intelligent collision avoidance and recovery path planning system for the waterjet-propelled unmanned surface vehicle
CN110687917A (en) Multi-unmanned-boat distributed formation reconfiguration formation control method
CN109613921A (en) Based on the unmanned ship local paths planning method for fast moving glowworm swarm algorithm
CN114879666A (en) RRT algorithm-based water surface unmanned ship path planning method and device
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
Ø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
CN115774397A (en) Optimal formation anti-collision control algorithm of multi-Agents system based on collision risk
CN112214037B (en) Unmanned aerial vehicle remote sensing networking flight path planning method based on field station
Fu et al. Method for collision avoidance by USV based on improved genetic algorithm
Nichting et al. Case study on gap selection for automated vehicles based on deep q-learning
Jiang et al. A Docking Control Technology Using Artificial Potential Field and NMPC for Pontoon Bridge Blocks
Wang et al. Research on Local Path Planning for Autonomous Collision Avoidance of USV
Cheng Formation control for unmanned surface vehicles based on minimum snap trajectory generation
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
CN114518760B (en) Unmanned vehicle navigation method and system server based on risk zone management
CN112987776A (en) Single-machine fault return path planning method and system, unmanned aerial vehicle and storage medium

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