Unmanned ship formation control method

Abstract

The invention relates to an unmanned ship formation control method. The method comprises the following steps: step 1, planning a plurality of formation patterns on a sea chart; step 2, connecting nodes with the same node serial number in each formation into a route according to a sequence during formation planning; 3, calculating the navigation distance of the navigation node; 4, calculating the navigation time of the navigation node; step 5, calculating the sailing distance of the following node; 6, calculating the expected navigational speed of the following node; 7, the pilot boat starts to track the pilot route; 8, calculating the real-time distance between the pilot boat and the next waypoint; step 9, calculating the real-time navigation time of the pilot boat; step 10, calculating the real-time distance between the following boat and the next waypoint; and step 11, completing unmanned ship formation control. According to the method, the dangerous conditions of large-angle steering, track redundancy, situation chaos and the like of the unmanned ship formation are avoided.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned boat cluster control technology, and particularly relates to a method for controlling the formation of unmanned boats. Background Art

[0002] At present, the existing methods for controlling the formation of unmanned boats are: the leader-follower method, the virtual leader method, and the behavior-based control method.

[0003] However, the existing methods for controlling the formation of unmanned boats have the following defects and deficiencies:

[0004] First, most methods only consider homogeneous types of unmanned boat clusters and do not take into account heterogeneous types of unmanned boat clusters. In real engineering applications, there are significant performance differences between individual unmanned boats, such as different speed ranges, different turning radii, and different acceleration and deceleration responses.

[0005] The present invention considers the actual engineering application scenario and proposes a method for controlling the formation of unmanned boats to overcome the differences between heterogeneous unmanned boats. Summary of the Invention

[0006] The purpose of the present invention is to overcome the deficiencies of the prior art and propose a formation control method that can be applied to unmanned boats with different speed ranges, has a simple logic, and can realize the transformation of formation shapes.

[0007] The present invention solves its practical problems by adopting the following technical solutions:

[0008] A method for controlling the formation of unmanned boats, comprising the following steps:

[0009] Step 1, plan a plurality of formation shapes on a nautical chart;

[0010] Step 2, connect the nodes with the same node numbers in each formation shape into a route in the order of priority when planning the formation shape, and each node with the same number is a waypoint;

[0011] Step 3, calculate the navigation distance of the leading node according to the current waypoint and the next waypoint of the leading route;

[0012] Step 4, calculate the navigation time of the leading node according to the expected speed of the leading node and the navigation distance of the leading node obtained in Step 3;

[0013] Step 5, calculate the navigation distance of the following node according to the current waypoint and the next waypoint of the following route;

[0014] Step 6: Calculate the expected speed of the following node based on the navigation distance of the following node obtained in Step 5 and the navigation time of the leading node obtained in Step 4. If the expected speed of the following node is not within the speed range of the following node, it is determined that the transformation of the current formation to the next formation is infeasible, and it is necessary to change the shape of the current formation or the speed of the leading node, and repeat the above steps.

[0015] Step 7: After determining that the formation transformation plan is feasible through the above steps, the leading boat starts to track the leading route.

[0016] Step 8: Calculate the real-time distance between the leading boat and the next waypoint based on the real-time position coordinates of the leading boat and the coordinates of the next waypoint of the leading node.

[0017] Step 9: Calculate the real-time navigation time of the leading boat based on the real-time distance and real-time speed of the leading boat.

[0018] Step 10: Calculate the real-time distance between the following boat and the next waypoint based on the real-time position coordinates of the following boat and the coordinates of the next waypoint of the following node.

[0019] Step 11: Calculate the real-time expected speed of the following boat based on the real-time distance of the following boat and the real-time navigation time of the leading boat. The following boat adjusts its speed through the speed control method. When the speed of the following boat is the same as that of the leading boat, the unmanned boat formation control is completed.

[0020] Moreover, the information of each formation in Step 1 includes: the node number of the leading node, the node number of the following node, the longitude and latitude of each node, and the expected speed of the leading node.

[0021] Moreover, the specific steps of Step 6 include:

[0022] 6.1 Divide the navigation distance of the following node by the navigation time of the leading node to obtain the expected speed of the following node, where the unit of navigation distance is nautical miles, the unit of navigation time is hours, and the unit of expected speed is knots.

[0023] 6.2 If the expected speed of the following node is greater than the maximum speed of the following boat or the expected speed of the following node is less than the minimum speed of the following boat, it is determined that the transformation of the current formation to the next formation is infeasible. Repeat Steps 1 to 6.

[0024] 6.3 If the expected speed of the following node is less than the maximum speed of the following boat and greater than the minimum speed of the following boat, it is determined that the transformation of the current formation to the next formation is feasible, and save the next formation.

[0025] Moreover, the specific steps of Step 11 include:

[0026] 11.1 Divide the remaining distance of the following boat by the remaining navigation time of the leading boat to obtain the expected speed of the following boat; where the unit of the remaining distance is nautical miles, the unit of the remaining navigation time is hours, and the unit of the expected speed is knots;

[0027] 11.2 When the expected speed of the following boat is more than 2 knots greater than the actual speed of the following boat, the expected speed of the following boat is the actual speed of the following boat + 1 knot; when the expected speed of the following boat is less than 3 knots less than the actual speed of the following boat, the expected speed of the following boat is the actual speed of the following boat - 2 knots.

[0028] 11.3 When the expected speed of the following boat is greater than the maximum speed of the following boat, the following boat sails at the maximum speed; when the expected speed of the following boat is less than the minimum speed of the following boat, the following boat sails at the minimum speed.

[0029] Advantages and beneficial effects of the present invention:

[0030] 1. The present invention proposes an unmanned boat formation control method. By separating the speed control module and the heading control module, it solves the problem of the coupling of heading control and speed control during the navigation of the unmanned boat formation, and avoids dangerous situations such as large-angle turning, trajectory redundancy, and situation chaos of the unmanned boat formation.

[0031] 2. The present invention designs the formation shape with the speed range and formation position of each following boat as constraints.

[0032] 3. The present invention considers that the acceleration and deceleration response times of each unmanned boat are different, and in order to avoid the high-load operation of the unmanned boat engine, it limits the magnitude of the change in the expected speed of each unmanned boat. Specific implementation manner

[0033] The following further details the embodiments of the present invention:

[0034] An unmanned boat formation control method includes the following steps:

[0035] Step 1. Plan multiple formation shapes on the nautical chart;

[0036] The information of each formation shape in the above step 1 includes: the node serial number of the leading node, the node serial number of the following nodes, the longitude and latitude of each node, and the expected speed of the leading node.

[0037] Step 2. Connect the nodes with the same node serial number in each formation shape into a route in the order of priority during the formation shape planning. Each node with the same serial number is a waypoint;

[0038] Step 3. Calculate the navigation distance of the leading node according to the current waypoint and the next waypoint of the leading route;

[0039] Step 4: Calculate the navigation time of the leading node based on the expected speed of the leading node and the navigation distance of the leading node obtained in Step 3.

[0040] Step 5: Calculate the navigation distance to the following node based on the current waypoint and the next waypoint of the following route.

[0041] Step 6: Calculate the expected speed of the following node based on the navigation distance of the following node obtained in Step 5 and the navigation time of the leading node obtained in Step 4. If the expected speed of the following node is not within the speed range of the following node, it is determined that the transformation of the current formation to the next formation is not feasible, and it is necessary to change the shape of the current formation or change the speed of the leading node, and repeat the above steps.

[0042] The specific steps of Step 6 include:

[0043] 6.1 Divide the navigation distance of the following node by the navigation time of the leading node to obtain the expected speed of the following node, where the unit of navigation distance is nautical miles, the unit of navigation time is hours, and the unit of expected speed is knots.

[0044] 6.2 If the expected speed of the following node is greater than the maximum speed of the following boat or the expected speed of the following node is less than the minimum speed of the following boat, it is determined that the transformation of the current formation to the next formation is not feasible. Repeat Steps 1 to 6.

[0045] 6.3 If the expected speed of the following node is less than the maximum speed of the following boat and greater than the minimum speed of the following boat, it is determined that the transformation of the current formation to the next formation is feasible, and save the next formation.

[0046] Step 7: After determining that the formation transformation plan is feasible through the above steps, the leading boat starts to track the leading route.

[0047] Step 8: Calculate the real-time distance between the leading boat and the next waypoint based on the real-time position coordinates of the leading boat and the coordinates of the next waypoint of the leading node.

[0048] Step 9: Calculate the real-time navigation time of the leading boat based on the real-time distance and the real-time speed of the leading boat.

[0049] Step 10: Calculate the real-time distance between the following boat and the next waypoint based on the real-time position coordinates of the following boat and the coordinates of the next waypoint of the following node.

[0050] Step 11: Calculate the real-time expected speed of the following boat based on the real-time distance of the following boat and the real-time navigation time of the leading boat. The following boat adjusts its speed through a speed control method. When the speed of the following boat is the same as the speed of the leading boat, the unmanned boat formation control is completed.

[0051] The specific steps of step 11 include:

[0052] 11.1 Divide the remaining distance of the following boat by the remaining navigation time of the leading boat to obtain the expected speed of the following boat; where the unit of the remaining distance is nautical miles, the unit of the remaining navigation time is hours, and the unit of the expected speed is knots.

[0053] 11.2 When the expected speed of the following boat is more than 2 knots greater than the actual speed of the following boat, the expected speed of the following boat is the actual speed of the following boat + 1 knot; when the expected speed of the following boat is less than 3 knots less than the actual speed of the following boat, the expected speed of the following boat is the actual speed of the following boat - 2 knots.

[0054] 11.3 When the expected speed of the following boat is greater than the maximum speed of the following boat, the following boat sails at the maximum speed; when the expected speed of the following boat is less than the minimum speed of the following boat, the following boat sails at the minimum speed.

[0055] The following further illustrates the present invention through specific calculation examples:

[0056] Step 1: Plan multiple formation shapes on the electronic nautical chart. Specifically, each formation shape includes multiple nodes, and the information contained in each node is: node type (divided into nodes of the leading boat and nodes of the following boat), node serial number (from 0 to n, where n is the number of unmanned boats), the longitude and latitude of the node. If it is a node of the leading boat, the expected speed is also required. The expected speed of the leading boat is the average of the maximum speed and the minimum speed of the formation. The maximum speed of the formation is the minimum value of the maximum speeds of all following boats, and the minimum speed of the formation is the maximum value of the minimum speeds of all following boats.

[0057] Step 2: Connect the nodes with the same node serial number in each formation shape in the order of precedence during the formation shape planning to form a route, and each node is a waypoint.

[0058] Specifically, the nodes in the first formation shape are the current waypoints of each route, and the serial number of the waypoint is 0; the nodes in the next formation shape are the next waypoints of each route, and the serial number of the waypoint is 2; the nodes in the mth formation shape are the mth waypoints of each route; finally, n routes are formed, and each route has m waypoints. Among them, the route composed entirely of leading nodes is the route of the leading boat, and the route composed entirely of following nodes is the route of the following boat.

[0059] Step 3: Calculate the navigation distance of the leading node according to the ith waypoint and the (i + 1)th waypoint of the route of the leading boat. Specifically, calculate the Euclidean distance between the longitude and latitude coordinates of the ith waypoint and the (i + 1)th waypoint.

[0060] Step 4: Calculate the navigation time of the leading node based on the desired speed of the leading node and the navigation distance of the leading node. Specifically, the navigation time is the navigation distance divided by the desired speed, where the unit of the navigation distance is nautical miles and the unit of the desired speed is knots. The unit of the navigation time is hours.

[0061] Step 5: Calculate the navigation distance of the following node based on the current waypoint and the next waypoint of the following route. Specifically, calculate the Euclidean distance between the longitude and latitude coordinates of the i-th waypoint and the (i + 1)-th waypoint.

[0062] Step 6: Calculate the desired speed of the following node based on the navigation distance of the following node and the navigation time of the leading node. If the desired speed of the following node is not within the speed range of the following boat, it is determined that the plan to change the current formation to the next formation is not feasible, and it is necessary to change the shape of the next formation or change the speed of the leading node, and repeat the above steps.

[0063] The specific steps of Step 6 include:

[0064] 6.1 Divide the navigation distance of the following node by the navigation time of the leading node to obtain the desired speed of the following node, where the unit of the navigation distance is nautical miles, the unit of the navigation time is hours, and the unit of the desired speed is knots.

[0065] 6.2 If the desired speed of the following node is greater than the maximum speed of the following boat or the desired speed of the following node is less than the minimum speed of the following boat, it is determined that the plan to change the current formation to the next formation is not feasible. Repeat Steps 1 to 6.

[0066] 6.3 If the desired speed of the following node is less than the maximum speed of the following boat and greater than the minimum speed of the following boat, it is determined that the plan to change the current formation to the next formation is feasible, and save the next formation.

[0067] Step 7: After determining that the plan for formation transformation is feasible through the above steps, the leading boat starts to track the leading route. Specifically, issue a command for the leading boat to track the leading route through the remote control software, and the leading boat sails according to the leading route.

[0068] Step 8: Calculate the remaining distance of the leading boat from the next waypoint based on the real-time position coordinates of the leading boat and the coordinates of the next waypoint of the leading node. Specifically, calculate the Euclidean distance between the current position of the leading boat and the next waypoint.

[0069] Step 9: Calculate the remaining navigation time of the pilot boat based on the remaining distance and real-time speed of the pilot boat. Specifically, divide the remaining distance by the real-time speed to obtain the remaining navigation time, where the unit of the remaining distance is nautical miles, the unit of the real-time speed is knots, and the unit of the remaining time is hours.

[0070] Step 10: Calculate the remaining distance of the following boat from the next waypoint based on the real-time position coordinates of the following boat and the coordinates of the next waypoint of the following node. Specifically, calculate the Euclidean distance between the current position of the following boat and the next waypoint, which are the longitude and latitude coordinates of the two points.

[0071] Step 11: Calculate the real-time desired speed of the following boat based on the remaining distance of the following boat and the real-time navigation time of the pilot boat.

[0072] The specific steps of Step 11 include:

[0073] 11.1 Divide the remaining distance of the following boat by the remaining navigation time of the pilot boat to obtain the desired speed of the following boat. The unit of the remaining distance is nautical miles, the unit of the remaining navigation time is hours, and the unit of the desired speed is knots.

[0074] 11.2 When the desired speed of the following boat is more than 2 knots greater than the actual speed of the following boat, the desired speed of the following boat is the actual speed of the following boat + 1 knot; when the desired speed of the following boat is less than 3 knots less than the actual speed of the following boat, the desired speed of the following boat is the actual speed of the following boat - 2 knots.

[0075] 11.3 When the desired speed of the following boat is greater than the maximum speed of the following boat, the following boat sails at the maximum speed; when the desired speed of the following boat is less than the minimum speed of the following boat, the following boat sails at the minimum speed.

[0076] It should be emphasized that the embodiments described in the present invention are illustrative rather than restrictive. Therefore, the present invention includes but is not limited to the embodiments described in the specific implementation manners. Any other implementation manners obtained by those skilled in the art according to the technical solutions of the present invention also fall within the scope of protection of the present invention.

Claims

1. A control method for unmanned boat formation, characterized in that: It includes the following steps: Step 1: Plan multiple formation shapes on the nautical chart; Step 2: Connect the nodes with the same node numbers in each formation shape into a route in the order of priority when planning the formation shape. Each node with the same number is a waypoint; Step 3: Calculate the navigation distance of the leading node based on the current waypoint and the next waypoint of the leading route; Step 4: Calculate the navigation time of the leading node based on the desired speed of the leading node and the navigation distance of the leading node obtained in Step 3; Step 5: Calculate the navigation distance to the following node based on the current waypoint and the next waypoint of the following route; Step 6: Calculate the desired speed of the following node based on the navigation distance of the following node obtained in Step 5 and the navigation time of the leading node obtained in Step 4; if the desired speed of the following node is not within the speed range of the following boat, it is judged that the transformation of the current formation shape to the next formation shape is infeasible, and it is necessary to change the shape of the current formation shape or change the speed of the leading node, and repeat the above steps; Step 7: After judging that the plan for changing the formation shape is feasible through the above steps, the leading boat starts to follow the leading route; Step 8: Calculate the real-time distance between the leading boat and the next waypoint based on the real-time position coordinates of the leading boat and the coordinates of the next waypoint of the leading node; Step 9: Calculate the real-time navigation time of the leading boat based on the real-time distance and the real-time speed of the leading boat; Step 10: Calculate the real-time distance between the following boat and the next waypoint based on the real-time position coordinates of the following boat and the coordinates of the next waypoint of the following node; Step 11: Calculate the real-time desired speed of the following boat based on the real-time distance of the following boat and the real-time navigation time of the leading boat. The following boat adjusts its speed through a speed control method. When the speed of the following boat is the same as that of the leading boat, the unmanned boat formation control is completed.

2. The unmanned boat formation control method according to claim 1, characterized in that: The information of each formation shape in Step 1 includes: the node number of the leading node, the node number of the following node, the latitude and longitude of each node, and the desired speed of the leading node.

3. A method for controlling an unmanned boat formation according to claim 1, characterized in that: The specific steps of Step 6 include: 6.1 Divide the navigation distance of the following node by the navigation time of the leading node to obtain the desired speed of the following node, where the unit of navigation distance is nautical miles, the unit of navigation time is hours, and the unit of desired speed is knots; 6.2 If the desired speed of the following node is greater than the maximum speed of the following boat, or the desired speed of the following node is less than the minimum speed of the following boat, it is judged that the transformation of the current formation shape to the next formation shape is infeasible; repeat Step 1 to Step 6; 6.3 If the desired speed of the following node is less than the maximum speed of the following boat and greater than the minimum speed of the following boat, it is judged that the transformation of the current formation shape to the next formation shape is feasible, and save the next formation shape.

4. A method for controlling an unmanned boat formation according to claim 1, characterized in that: The specific steps of Step 11 include: 11.1 Divide the remaining distance of the following boat by the remaining navigation time of the leading boat to obtain the desired speed of the following boat; where the unit of the remaining distance is nautical miles, the unit of the remaining navigation time is hours, and the unit of the desired speed is knots; 11.2 When the expected speed of the following boat is more than 2 knots greater than the actual speed of the following boat, the expected speed of the following boat is the actual speed of the following boat + 1 knot; when the expected speed of the following boat is less than the actual speed of the following boat by 3 knots, the expected speed of the following boat is the actual speed of the following boat - 2 knots; 11.3 When the expected speed of the following boat is greater than the maximum speed of the following boat, the following boat sails at the maximum speed; when the expected speed of the following boat is less than the minimum speed of the following boat, the following boat sails at the minimum speed.