A method for autonomous switching control of static paths in ship navigation

Abstract

This invention discloses an autonomous switching control method for ship navigation dynamic and static paths, comprising the following steps: calculating the coordinates of the intersection point between the LOS circle and the straight line containing the current path segment based on a variable radius LOS circle formed by the line-of-sight (LOS) navigation algorithm, and determining the LOS point coordinates from the two intersection point coordinates; changing the path segment switching strategy according to different uncertainties and different scenarios, and calculating the new bow-oriented tracking point; optimizing the path segment switching strategy, considering scenarios where there is a risk of obstacle collision on the current path segment. Based on this invention, ships can follow the mission route well in various situations, and quickly return to the preset path after avoiding obstacles and ensuring safety during the following process.

Description

Technical Field

[0001] This invention belongs to the field of ship navigation control technology, specifically relating to a method for autonomous switching control of ship navigation dynamic and static paths. Background Technology

[0002] As ship intelligence matures, autonomous navigation has become a hot topic. Research and application of unmanned surface vessel (USV) technology can effectively reduce labor costs, minimize human error, and improve operational efficiency and safety. With the diversification of maritime missions, new demands are being placed on autonomous navigation technology, and path following, a key technology for achieving autonomous navigation, is facing increasing challenges. When ships are used for maritime search and rescue or military patrols, they typically need to perform search and detection tasks, requiring them to cruise along predetermined trajectories. When used for target tracking, ships need to quickly complete the planned ideal trajectory to reach the tracking location. Therefore, accurate path following can improve navigation efficiency and mission completion rates.

[0003] In path following, ships often switch between dynamic and static paths to meet mission and safety requirements, but the process and timing of these switches are usually difficult to grasp. Summary of the Invention

[0004] The purpose of this invention is to provide a method for autonomous switching control of ship navigation dynamic and static paths, ensuring that the ship meets both mission and safety requirements during path following in real-world scenarios.

[0005] To solve the above technical problems, the technical solution of the present invention is: a method for autonomous switching control of ship navigation dynamic and static paths, comprising the following steps:

[0006] S1. Based on the line-of-sight (LOS) navigation algorithm, the coordinates of the intersection point between the LOS circle and the straight line of the current path segment are calculated using the variable radius LOS circle. The coordinates of the LOS point are then determined from the coordinates of the two intersection points.

[0007] S2. Based on different uncertainties and different scenarios, change to the corresponding path segment switching strategy and calculate the new ship heading tracking point;

[0008] S3. Optimize the path segment switching strategy, taking into account scenarios where there is a risk of obstacle collision on the current path segment.

[0009] S1 specifically refers to:

[0010] Let the coordinates of the starting point of the current path segment be P. n (x n y n The endpoint coordinates are P. n+1 (x n+1y n+1 The ship's coordinates are O. b (x b y b The distance e between the ship and the current path segment is represented as:

[0011]

[0012] in, This represents the slope of the path segment.

[0013] To ensure that the LOS circle intersects with the line containing the current path segment, the radius of the LOS circle is defined as follows:

[0014] R los =e+aL (2)

[0015] Where L is the ship's length, and a is a parameter related to the ship's speed. The value of a affects the path following effect and is selected through testing.

[0016] The intersection point of the LOS circle and the straight line containing the current path segment is obtained by solving equation (3):

[0017]

[0018] Solving for the two intersection points (x) yields the solution. L1 y L1 ), (x L2 y L2 In the LOS navigation algorithm, any intersection point is selected as the LOS point, and its coordinates are represented as follows:

[0019]

[0020] Where, x LOS y LOS , where x and y are the x and y coordinates of the LOS point, respectively.

[0021] The coordinates of the LOS point are obtained by combining equations (1)-(4).

[0022] The range of values ​​for a is [1, 4].

[0023] S2 specifically refers to:

[0024] (1) When the LOS point is on the reverse extension of the path segment, in order to quickly enter the preset path, select the starting point P of the path segment. n As the new heading tracking point at the current moment, that is:

[0025]

[0026] Where, x LOS ′、y LOS′ are the x and y coordinates of the new bow-to-tracking point, respectively.

[0027] (2) When the ship's LOS point is on the forward extension of a path segment, consider whether to switch to the next path segment. The switching strategy is determined based on mission requirements. When the ship is performing a detection-type mission and following a path, the ship should traverse all path segments on the preset path as much as possible. Therefore, no path segment switching is performed, and the starting point Pk of the path segment is taken as the ship's bow-oriented tracking point at the current moment, i.e.:

[0028]

[0029] Among them, T A T is a boolean variable used to determine the type of mission. When a ship performs a reconnaissance mission... A =0, T when the ship is performing target tracking tasks A =1;

[0030] (3) When a ship performs a target point tracking task and follows a path, the ship should reach the end of the path as quickly as possible. At this time, it should switch to the next path segment to follow the path. Then, based on the start and end positions of the next path segment and the ship's current position, the ship's heading tracking point should be recalculated and the path segment switching should be determined. At the same time, when switching the path segment, it is also necessary to consider whether the current path segment is the last path segment of the preset path. If so, the path segment switching should not be performed, and the end of the current path segment should be used as the new heading tracking point.

[0031] S3 specifically refers to:

[0032] When obstacles exist on the path segment, navigational safety hazards may arise. In this case, autonomous collision avoidance is required. In order for the ship to continue following the original path after avoiding the obstacle, the target point will be set on the global path of the mission. After obtaining the local path, the ship will stop following the current global path segment and start following the newly planned local path. After completing obstacle avoidance within the target point's acceptance circle, it will return to the global path to follow the path. The current global path segment followed after returning is the path segment where the target point is located.

[0033] To ensure the ship can better follow its original path after returning to its original position, the target point needs to be as close as possible to the ship's position before collision avoidance began, while remaining outside the collision risk zone. The target point selection method follows the Line of Sight (LOS) navigation algorithm. When there is a collision risk, a circle with radius R is drawn around the ship. C Using the target circle as the target circle and the global path segment followed by the ship before collision avoidance begins as the global path segment selected for the current target point, and referring to the calculation principle of the ship's heading tracking point in S2, the target circle is used to replace the LOS circle to calculate the target point P. t (x t yt To ensure safe navigation, the radius of the target circle is related to the ship's speed. Furthermore, to ensure it remains on the global path, its radius is not smaller than the LOS circle. The radius of the target circle is expressed as:

[0034] R C =R los +bv (7)

[0035] Where b is a parameter related to ship maneuverability; the better the ship maneuverability, the smaller the value of b.

[0036] The following judgment is made: Determine if the target point is within the obstacle risk field. If so, move the target point forward a distance cL along the path segment direction until it moves out of the obstacle risk field. Simultaneously, consider the scenario where the target point moves outside the current global path segment. In this case, switch the currently selected global path segment to the next global path segment, and use the starting point of the switched global path segment as the new target point. Repeat this judgment and movement process until the target point moves out of the obstacle risk field.

[0037] P t (1) = P t (x t y t (8)

[0038]

[0039] if r(P t (m))≤co P t ′(x t ′,y t ′)=P t (m) (10)

[0040] Where c is the proportional parameter of the moving distance and is selected through testing, P t (m) represents the target point P. t (x t y t The new target point obtained after moving m-1 times, x t (m), y t (m) represent P t The x and y coordinates of (m), P t ′(x t ′,y t ′) represents the target point selected at the current moment, r(·) is the function for calculating the risk value of the target point, and co is the risk threshold.

[0041] In S3, it is also necessary to consider the possibility that the target point moves to the extension line of the current global path segment. In this case, the global path segment selected by the current target point is switched to the next global path segment. Then, the starting point of the switched global path segment is used as the new target point. The judgment and movement process is repeated until it moves out of the obstacle risk field.

[0042] When selecting a target point, the path segment switching strategy always applies to the preset global path segment, and the target point is always only on the global path; while when tracking in LOS, the path segment switching strategy when selecting the ship's heading tracking point applies to the current path, that is, the local path or the global path. When avoiding a collision locally, the ship's heading tracking point is on the local path, and when following the global path, the ship's heading tracking point is on the global path.

[0043] A computer device is also provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, performs the steps of the method as described in any of the preceding claims.

[0044] A computer-readable storage medium is also provided, on which a computer program is stored, characterized in that, when the computer program is executed by a processor, it implements the steps of the method as described in any of the preceding claims.

[0045] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0046] Based on this invention, ships can follow mission routes well in various situations, and after avoiding obstacles and ensuring safety during the following process, quickly return to the preset path. Attached Figure Description

[0047] Figure 1 This is a flowchart of ship path following considering dynamic and static path switching in an embodiment of the present invention;

[0048] Figure 2 This is a schematic diagram of the LOS navigation principle in an embodiment of the present invention;

[0049] Figure 3 This is a schematic diagram of the LOS point on the reverse extension line of the path segment in an embodiment of the present invention;

[0050] Figure 4 This is a schematic diagram of the LOS point being on the positive extension line of the path segment in an embodiment of the present invention;

[0051] Figure 5 This is a flowchart of the bow-tracking calculation and path segment switching in an embodiment of the present invention;

[0052] Figure 6 This is a flowchart of the target point calculation and global path segment switching strategy in an embodiment of the present invention;

[0053] Figure 7 This is a schematic diagram of a path following scenario in an embodiment of the present invention when the LOS point is on the reverse extension line of the global path segment;

[0054] Figure 8 This is a schematic diagram of a path following scenario in an embodiment of the present invention when the LOS point is on the positive extension line of the global path segment;

[0055] Figure 9 This is a schematic diagram of a path following scenario in an embodiment of the present invention when there is an obstacle threat on the simulated path segment. Detailed Implementation

[0056] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0057] The technical solution adopted by this invention to solve the above-mentioned technical problems is as follows: a ship geometry tracking method considering the switching between dynamic and static paths of ships under multiple conditions, comprising the following steps:

[0058] S1. Further, the LOS point is calculated based on the LOS circle with a variable radius. Then, the distance between the ship and the straight line containing the current path segment, as well as the coordinates of the intersection point of the ship's LOS circle and the straight line, are calculated according to geometric principles. Let the coordinates of the starting point of the current path segment be P. n (x n y n The endpoint coordinates are P. n+1 (x n+1 y n+1 The ship's coordinates are O. b (x b y b ),like Figure 2 As shown. The distance between the ship and the current path segment can be represented by equation (1).

[0059]

[0060] in, This represents the slope of the path segment.

[0061] To ensure that the LOS circle intersects with the line containing the current path segment, the radius of the LOS circle is defined as follows:

[0062] R los =e+aL (2)

[0063] Where L is the ship's length, and a is a parameter related to the ship's speed. The value of a can affect the path following effect and can be selected through testing. Usually, a value in the range [1, 4] can be selected.

[0064] The intersection point of the LOS circle and the straight line containing the current path segment can be obtained by solving equation (3).

[0065]

[0066] Solving this problem will yield two intersection points (x) L１ y L1 ), (x L2 y L2 In the LOS navigation algorithm, one of the intersection points is selected as the LOS point, and its coordinates can be represented as:

[0067]

[0068] Where, x LOS y LOS , where x and y are the x and y coordinates of the LOS point, respectively.

[0069] The coordinates of the LOS point can be obtained by combining equations (1)-(4).

[0070] S2. Furthermore, consider the path segment switching strategy under scenarios where significant lateral deviations are caused by uncertainties. When a vessel is performing path following, path segment switching typically only occurs after the vessel has entered the acceptance circle of the current path segment's endpoint, such as... Figure 1 As shown, where R a The radius of the receiving circle. However, when a vessel experiences significant lateral deviation due to system malfunctions or environmental interference, or follows a curved path with a large curvature, it may find itself outside the receiving circle and without intersection with the current following path segment. In this case, the intersection will occur on the forward or reverse extension of the current path segment, such as... Figure 3 , 4 As shown, using the traditional path segment switching strategy and using the LOS point as the heading tracking point may cause the ship to have a large path following error.

[0071] New methods for calculating the heading tracking point and path segment switching strategies have been added to suit different scenarios. When the LOS point is on the reverse extension of a path segment, the starting point P of the path segment is selected to quickly enter the preset path. 竹 As the new heading tracking point at the current moment, that is:

[0072]

[0073] Where, x LOS ′、y LOS ′ are the x and y coordinates of the new bow-to-tracking point, respectively.

[0074] When the ship's LOS point is on the forward extension of a path segment, it is considered whether to switch to the next path segment. The switching strategy is determined based on mission requirements. When the ship is performing a reconnaissance mission and following a path, the ship should traverse all path segments on the preset path as much as possible. Therefore, no path segment switching is performed, and the starting point Pk of the path segment is taken as the ship's bow-head tracking point at the current moment.

[0075]

[0076] Among them, T A T is a boolean variable used to determine the type of mission. When a ship performs a reconnaissance mission... A =0, T when the ship is performing target tracking tasks A =1.

[0077] When a vessel performs a target point tracking task and follows a path, it should reach the path endpoint as quickly as possible. At this point, it should switch to the next path segment for path following. Then, based on the start and end points of the next path segment and the vessel's current position, the bow-oriented tracking point calculation and path segment switching decision are recalculated. Simultaneously, during path segment switching, it's necessary to consider whether the current path segment is the last segment of the preset path. If so, the path segment switching is not performed, and the endpoint of the current path segment is used as the new bow-oriented tracking point. The bow-oriented tracking point calculation and path segment switching process is as follows: Figure 5 As shown.

[0078] S3. Further, consider the path switching strategy when there is a risk of collision with an obstacle on the current path segment. When there is an obstacle on the path segment, there is a navigational safety hazard. In this case, autonomous collision avoidance is required. The autonomous collision avoidance algorithm usually plans a local path based on the current ship position and the target point. In order for the ship to continue the original path following task after avoiding the obstacle, the target point will be set on the task's global path. After obtaining the local path, the ship will stop following the current global path segment and start following the newly planned local path. After completing obstacle avoidance within the target point's acceptance circle, it will return to the global path for path following. The current global path segment followed after returning is the path segment where the target point is located. The selection of the target point determines the timing of the switch between local path following and global path following, as well as the selection of the global path segment after the switch.

[0079] To ensure the vessel can effectively follow its original path after returning to its starting position, the target point needs to be as close as possible to the vessel's position before the collision avoidance began, while remaining outside the collision risk zone. The target point selection method borrows from the LOS (Lane of Order) circle approach; when there is a collision risk, a circle with radius R is drawn around the vessel. CThe target circle (hereinafter referred to as the target circle) is selected using the global path segment followed by the ship before it begins collision avoidance as the current target point selection global path segment. Referring to the calculation principle of the ship's heading tracking point in S2, the target circle is used to replace the LOS circle to calculate the target point P. t (x t y t To ensure the safety of ship navigation, the radius of the target circle is related to the speed of the ship. At the same time, to ensure that its radius is not less than the LOS circle on the global path, the radius of the target circle can be represented by equation (7).

[0080] R C =R los +bv (7)

[0081] Where 'b' is a parameter related to ship maneuverability. A smaller value for 'b' indicates better ship maneuverability, while a larger value indicates poorer ship maneuverability. The target point calculated based on this may be located within the obstacle risk field, which could prevent the collision avoidance algorithm from planning a suitable collision avoidance path. Therefore, further processing of the target point is required. The algorithm determines whether the target point is within the obstacle risk field. If it is, the target point is moved forward a distance 'cL' along the path segment direction until it moves out of the obstacle risk field. Simultaneously, it needs to be considered that if the target point moves outside the current global path segment, the selected global path segment is switched to the next global path segment. The starting point of the switched global path segment is then used as the new moving target point. This process of determination and movement continues until the target point moves out of the obstacle risk field.

[0082] P t (1) = P t (x t y t (8)

[0083]

[0084] if r(P t (m))≤co P t ′(x t ′,y t ′)=P t (m) (10)

[0085] Where c is the proportional parameter of the movement distance, which can be selected through testing, and P t (m) represents the target point P. t (x t y t The new target point obtained after moving m-1 times, x t (m), y t (m) represent P t The x and y coordinates of (m), Pt ′(x t ′,y t ′) represents the target point selected at the current moment, r(·) is the function for calculating the risk value of the target point, and co is the risk threshold.

[0086] In addition, it is necessary to consider the possibility that the target point may move to the extension line of the current global path segment. In this case, the global path segment selected by the current target point is switched to the next global path segment, and the starting point of the switched global path segment is used as the new target point. Then, the above judgment and movement process continues until it moves out of the obstacle risk field. The target point calculation process is as follows: Figure 6 As shown.

[0087] It's important to note that the path segment switching strategy during target point selection always applies to the preset global path segment, meaning the target point is always on the global path. However, during LOS tracking, the path segment switching strategy for selecting the ship's heading tracking point applies to the current path, which could be a local or global path. In local collision avoidance, the heading tracking point will be on the local path; in global following, the heading tracking point will be on the global path. The ship path following process considering dynamic and static path switching is as follows: Figure 1 As shown.

[0088] Simulations using the above method can yield the effectiveness of ship path following control under various scenarios, such as... Figures 7 to 9 As shown in the figure, the thick straight lines represent the preset global path, the thick curves represent the local path, the white polygons represent obstacles, the black dotted lines represent the actual navigation trajectory of the ship, and the concentric circles and black polygons represent simulated ships. Figure 7 This simulation demonstrates a path following scenario when the LOS point lies on the reverse extension of the global path segment. Figure 8 This simulation addresses a path following scenario when the LOS point lies on the positive extension of the global path segment. Figure 9 This simulates a path-following scenario where obstacles threaten the path segment. Based on this method, ships can follow the mission route well in various situations, avoid obstacles during the following process, ensure safety, and quickly return to the preset path.

[0089] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for autonomous switching control of ship navigation dynamic and static paths, characterized in that, Includes the following steps: S1. Based on the line-of-sight (LOS) navigation algorithm, the coordinates of the intersection point between the LOS circle and the straight line of the current path segment are calculated using the variable radius LOS circle. The coordinates of the LOS point are then determined from the coordinates of the two intersection points. S2. Based on different uncertainties and different scenarios, change to the corresponding path segment switching strategy and calculate the new ship heading tracking point; S3. Optimize the path segment switching strategy, taking into account scenarios where there is a risk of obstacle collision on the current path segment; S2 specifically refers to: (1) When the LOS point is on the reverse extension of the path segment, in order to quickly enter the preset path, the starting point of the path segment is selected. As the new heading tracking point at the current moment, that is: in, , Here are the x and y coordinates of the LOS point, respectively. , These are the horizontal and vertical coordinates of the new bow-to-tracking point; (2) When the ship's LOS point is on the forward extension of the path segment, consider whether to switch to the next path segment. The switching strategy is formulated according to the mission requirements. When the ship is performing a detection mission and following the path, the ship should try to complete all the path segments on the preset path. Therefore, no path segment switching is performed. The starting point Pk of the path segment is taken as the ship's bow tracking point at the current moment, that is: in, This is a Boolean variable used to determine the type of mission when a ship is performing a survey mission. When ships perform target tracking tasks ; (3) When a ship performs a target point tracking task and follows a path, the ship should reach the end of the path as quickly as possible. At this time, it should switch to the next path segment to follow the path. Then, based on the start and end positions of the next path segment and the ship's current position, the ship's heading tracking point should be recalculated and the path segment switching should be determined. At the same time, when switching the path segment, it is also necessary to consider whether the current path segment is the last path segment of the preset path. If so, the path segment switching should not be performed, and the end of the current path segment should be used as the new heading tracking point.

2. The method for autonomous switching control of ship navigation dynamic and static paths according to claim 1, characterized in that, S1 specifically refers to: Let the coordinates of the starting point of the current path segment be... The endpoint coordinates are The ship's coordinates are Distance between the ship and the current path segment Represented as: in, , where is the slope of the path segment; To ensure that the LOS circle intersects with the line containing the current path segment, the radius of the LOS circle is defined as follows: in, For the ship's captain, For parameters related to ship speed, The size of the value affects the path following effect and should be selected through testing; The intersection point of the LOS circle and the straight line containing the current path segment is obtained by solving equation (3): The solution yields two intersection points ( , (), , In the LOS navigation algorithm, any intersection point is selected as the LOS point, and its coordinates are represented as follows: The coordinates of the LOS point are obtained by combining equations (1)-(4).

3. The method for autonomous switching control of ship navigation dynamic and static paths according to claim 2, characterized in that, The value range is [1, 4].

4. The method for autonomous switching control of ship navigation dynamic and static paths according to claim 1, characterized in that, S3 specifically refers to: When obstacles exist on the path segment, navigational safety hazards may arise. In this case, autonomous collision avoidance is required. In order for the ship to continue following the original path after avoiding the obstacle, the target point will be set on the global path of the mission. After obtaining the local path, the ship will stop following the current global path segment and start following the newly planned local path. After completing obstacle avoidance within the target point's acceptance circle, it will return to the global path to follow the path. The current global path segment followed after returning is the path segment where the target point is located. To ensure the ship can better follow its original path after returning to its original position, the target point needs to be as close as possible to the ship's position before collision avoidance began, while remaining outside the collision risk zone. The target point selection method follows the Line of Sight (LOS) navigation algorithm. When there is a collision risk, a circle with a radius of [missing information] is drawn with the ship as the center. Using the target circle as the objective circle and the global path segment followed by the ship before collision avoidance begins as the global path segment selected for the current target point, and referring to the calculation principle of the ship's heading tracking point in S2, the target circle is used to replace the LOS circle to calculate the target point. To ensure safe navigation, the radius of the target circle is related to the ship's speed. Furthermore, to ensure it remains on the global path, its radius is not smaller than the LOS circle. The radius of the target circle is expressed as: in, These are parameters related to ship maneuverability; the better the ship's maneuverability, the better. The smaller the value; Perform the following judgment: Determine whether the obtained target point is within the obstacle risk field. If it is, move the target point forward along the path segment direction. The distance is measured until the target point moves out of the obstacle risk field. Simultaneously, if the target point moves outside the current global path segment, the selected global path segment is switched to the next global path segment, and the starting point of the switched global path segment is used as the new target point. This judgment and movement process is repeated until the target point moves out of the obstacle risk field. in, This is a proportional parameter for the distance traveled and was selected through testing. ( ) as the target point move The new target point obtained after this , They are respectively ( The x and y coordinates of ) The final target point selected at the current moment, A function for calculating the risk value of the target point. This is the risk threshold.

5. The method for autonomous switching control of ship navigation dynamic and static paths according to claim 4, characterized in that, In S3, it is also necessary to consider the possibility that the target point moves to the extension line of the current global path segment. In this case, the global path segment selected by the current target point is switched to the next global path segment. Then, the starting point of the switched global path segment is used as the new target point. The judgment and movement process is repeated until it moves out of the obstacle risk field.

6. The method for autonomous switching control of ship navigation dynamic and static paths according to claim 5, characterized in that, When selecting a target point, the path segment switching strategy always applies to the preset global path segment, and the target point is always only on the global path; while when tracking in LOS, the path segment switching strategy when selecting the ship's heading tracking point applies to the current path, that is, the local path or the global path. When avoiding a collision locally, the ship's heading tracking point is on the local path, and when following the global path, the ship's heading tracking point is on the global path.

7. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the method as described in any one of claims 1-6.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method as described in any one of claims 1-6.

Images