Ship tracking and navigation planning method for water sports targets

Abstract

The application discloses a ship tracking navigation planning method for a water sports target, comprising the following steps: step 1, obtaining water surface and underwater target information around a controlled ship and tracking task information; step 2, calculating a tracking situation according to the water surface and underwater target information and the tracking task information; step 3, calculating an approach point of a guided unmanned ship navigation to an expected tracking point or a target point for keeping tracking according to the tracking situation; and step 4, tracking navigation control of the ship based on the approach point of the expected tracking point or the target point for keeping tracking. The ship tracking navigation planning method of the application can obtain water surface target information in real time, dynamically adjust a navigation path, and optimize selection of an approach point and a target point, so as to reduce navigation time and energy consumption from any relative position of a tracked target to a desired tracking position.

Description

Technical Field

[0001] This invention belongs to the field of shipbuilding and marine engineering technology, and discloses a method for ship tracking and navigation planning of moving targets on water. Background Technology

[0002] In the field of naval architecture and ocean engineering, when autonomous vessels track and respond to attacks by non-cooperative targets in designated sea areas, or when unmanned tugboats follow and escort large vessels, or when autonomous vessel convoys are sailing in formation, a target tracking navigation planning method suitable for vessels is needed. This method enables the controlled autonomous vessel to enter a specific tracking position for surface targets and maintain stable and continuous tracking. The design of a target tracking navigation planning method for vessels needs to consider factors such as the actual vessel's maneuverability parameters, hull size, and speed limits.

[0003] Existing tracking and planning algorithms for robots and drones can achieve stable tracking control of the controlled object to the target. However, on the one hand, the controlled object of these two algorithms is not an underactuated system, and the maneuverability of ships places higher demands on tracking navigation planning and collision avoidance of water targets during the tracking process; on the other hand, the navigation environment of these two algorithms does not interfere as strongly with the controlled object as the marine environment, which makes navigation control of autonomous ships and information perception of the tracking target more difficult. Summary of the Invention

[0004] This invention discloses a navigation planning method for tracking moving targets on water, which includes the following steps:

[0005] Step 1: Obtain information on surface and underwater targets around the controlled vessel and tracking mission information;

[0006] Step 2: Calculate the tracking situation based on surface and underwater target information and tracking mission information;

[0007] Step 3: Calculate the approach point or target point to guide the unmanned vessel to the expected tracking point or maintain tracking based on the current situation.

[0008] Step 4: Track and control the ship's navigation based on the approach point coordinates of the expected tracking point or the target point coordinates to maintain tracking.

[0009] Furthermore, in step 1, the tracking task information includes the specified target identification batch number, tracking distance, tracking relative azimuth angle, and tracking duration;

[0010] The target identification batch number is the basis for the target tracking algorithm to identify and extract the target information from the fused situational information.

[0011] Tracking distance is the expected distance that the controlled vessel maintains with the tracked target during the tracking process;

[0012] The relative azimuth angle is the relative angle between the line connecting the position of the tracked target to the desired tracking point of the controlled vessel during the tracking process and the heading of the tracked target, with a range of (-180°, +180°).

[0013] The tracking duration controls the overall tracking task duration. When the actual tracking execution time exceeds the given tracking duration, the tracking task is considered to have ended and tracking stops.

[0014] Furthermore, step 2 also includes the following steps:

[0015] Step 21: Locate the tracked target in the target situation information according to the target identification batch number, and read the target's latitude, longitude, speed, and heading;

[0016] Based on the tracking distance and relative azimuth angle in the tracking task information, the expected tracking point of the target tracking task at the current moment is calculated;

[0017] Plan a route from the current position of the controlled vessel to its tracking point, so that the controlled vessel approaches and remains near the desired tracking point, and consider it to be being tracked by the target.

[0018] When time changes and the target situation information is updated, the latitude, longitude, speed and heading information of the tracked target are reread, the desired tracking point is recalculated, and the control variables are updated.

[0019] Step 22: The approach tracking method based on tracking situation identification guides the controlled vessel to approach the desired tracking point along a path that will not collide with the tracked target;

[0020] Step 23: Calculate the tracking situation for two scenarios: the target point is located near the rear of the target ship, and the target point is located on either side of the target ship.

[0021] Furthermore, in step 22, the target tracking algorithm first reads the position and heading of the tracked target and the position of the controlled vessel, and calculates the relative position (x, y) of the controlled vessel relative to the tracked target. rel ,y rel The expected relative position (X) of the tracking point with respect to the tracked target. e ,y e ), and the relative position (x) of the controlled vessel with respect to the desired tracking point. g ,y g ),in:

[0022]

[0023] θrel This indicates the current relative position of the monitored vessel to the tracked target, r e θ represents the tracking distance. e This indicates tracking relative azimuth angle.

[0024] Furthermore, in step 23, when θ e When ∈[160°,180°]∪(-180°,-160°], the target point is considered to be located directly behind the target ship, and the tracking situation is divided as follows:

[0025] Tracking Situation #0: When The controlled vessel is in the normal tracking phase, and continuous tracking navigation is achieved by controlling its speed;

[0026] Tracking situation #1: When |y rel |≤k1·(L t +L j And X rel >0, the monitored vessel is at risk of collision on the future course of the tracked target;

[0027] Tracking the situation #2: When |y rel |>k1·(L t +L j And X rel >0, the controlled vessel is in front of the tracked target, and if the controlled vessel is stationary, there is no risk of collision for the tracked target;

[0028] Tracking situation #3: When ((θ) rel ∈[160°,180°]∪(-180°,-160°] or |y rel |≤k1·(L t +L j And x g <0, the controlled vessel is located within 40° directly behind the tracked target, and the controlled vessel is located behind the desired tracking point in the target's body coordinate system;

[0029] Tracking situation #4: When ((θ) rel ∈[160°,180°]∪(-180°,-160°] or |y rel |≤k1·(L t +L j And x g >0, the controlled vessel is located within 40° directly behind the tracked target, and the controlled vessel is ahead of the expected tracking point;

[0030] Tracking Situation #5: Other situations, the controlled vessel is located to the side and rear of the tracked target;

[0031] Among them, L tL represents the scale of the tracked target. j Let r be the length of the controlled vessel, k1 be the collision distance coefficient, and r be the length of the vessel being controlled. t The allowable radius for tracking error.

[0032] Furthermore, in step 23, when θ e ∈(-160°,+160°), assuming the target point is located on either side of the tracked target, the tracking situation is divided as follows:

[0033] Tracking Situation #0: When The controlled vessel is located near the desired tracking point, and it is considered to be in the normal tracking phase. The speed is controlled to achieve continuous tracking navigation.

[0034] Tracking the situation #6: When |y rel |≤k1·(L t +L j And X rel >0, the monitored vessel is at risk of collision on the future course of the tracked target;

[0035] Tracking the situation #7: When b cs =1 and x rel ≥0, the controlled vessel is located ahead of the tracking target and needs to switch sides;

[0036] Tracking the situation #8: When b cs =1 and x rel <0, the controlled vessel is located behind the tracking target and needs to switch sides;

[0037] Tracking the situation #9: When b cs =0 and x g <0, the controlled vessel is located behind the desired tracking point and does not need to change sides;

[0038] Tracking the situation #10: When b cs =0 and (0 <x g ≤x set And |y g |≤C·(r t The controlled vessel was slightly ahead of the expected tracking point and did not need to change sides.

[0039] Tracking the situation #11: When b cs In other cases where the value is 0, the controlled vessel is ahead of the desired tracking point and does not need to change sides.

[0040] Among them, L t L represents the scale of the tracked target. j Let r be the length of the controlled vessel, k1 be the collision distance coefficient, and r be the length of the vessel being controlled. t To track the allowable radius of error, x setLet C be the offset distance coefficient, and b be the amount that slightly leads the expected tracking point. cs The variable used to determine the side switching.

[0041] Furthermore, in step 3, the approach point calculation strategies and methods for each tracking situation are as follows:

[0042] Tracking situation #0, guiding the controlled vessel to maintain tracking, with the target point being the desired tracking point:

[0043] (x a ,y a )=(x e ,y e )

[0044] Tracking Situation #1: Guide the controlled vessel to quickly leave the tracked target's forward path area and enter Tracking Situation #2. The set approach point is the side forward of the tracked target, and the side of the approach point is selected as the side of the controlled vessel's current bow heading.

[0045] ψ rel =ψ j -ψ t ,ψ rel ∈(-180°,+180°]

[0046] (x a ,y a )=(x rel ,sign(ψ rel )·[k1·(L t +L j )+50])

[0047] Tracking Situation #2: Guide the controlled vessel to navigate from the side-front of the tracked target to the side-rear to enter Tracking Situation #5. The set approach point is located to the side-rear of the tracked target, and a certain safe distance is maintained between the two targets.

[0048] (x a ,y a )=(-50,sign(y rel )·[k2·(L t +L j )+50])

[0049] Tracking Situation #3: Guide the controlled vessel to pursue the desired tracking point from a considerable distance directly behind the tracked target, entering Tracking Situation #0. The set approach point is located at the desired tracking point (x). e ,y e Let be the center of the circle, and let r be the radius of the tracking error tolerance. t Inside the circle:

[0050] (xa ,y a )=(x e -0.5·r t ,y e )

[0051] Tracking situation #4: Guide the controlled vessel to stop or reduce propulsion, maintain the bow heading consistent with the tracked target's course, wait for the desired tracking point to catch up with the controlled vessel, then enter tracking situation #0. The set approach point is located at the desired tracking point (x e ,y e Let be the center of the circle, and let r be the radius of the tracking error tolerance. t Inside the circle:

[0052] (x a ,y a )=(x rel -5,y e )

[0053] Tracking Situation #5: Guide the controlled vessel to navigate from the side and rear of the tracked target, directly behind the desired tracking point, and enter Tracking Situation #3. The set approach point is located at the origin (0,0) of the body coordinate system, with a radius of 1.2 times the tracking distance r. e On the circle:

[0054] (x a ,y a )=(1.2r e ·sinθ e 1.2r e ·cosθ e )

[0055] Where, x a This represents the x-coordinate of the approach point in the body coordinate system of the tracked target at this moment, y a L represents the ordinate of the approach point in the body coordinate system of the tracked target, sign represents the sign function, and L represents the sign function. t L represents the scale of the tracked target. j For the captain of the controlled vessel, ψ t For the heading / following of the tracked target, ψ j K1 represents the heading of the controlled vessel, k2 represents the collision distance coefficient, and k2 represents the safety distance coefficient.

[0056] Furthermore, in step 3, the approach point calculation strategies and methods for each tracking situation are as follows:

[0057] Tracking situation #0, guiding the controlled vessel to maintain tracking, with the target point being the desired tracking point:

[0058] (x a ,y a )=(xe ,y e )

[0059] Tracking situation #6, guide the controlled vessel to leave the tracked target's forward path area as soon as possible, and enter tracking situation #11, with the approach point set to the side forward of the tracked target's desired tracking point:

[0060] (x a ,y a )=(x rel ,sign(y e )·[k1·(L t +L j )+50])

[0061] Tracking Situation #7: Guide the controlled vessel from the side-forward side relative to the target and the expected tracking point to the side-rear side, entering Tracking Situation #8. The set approach point is located at the side-rear side of the target, maintaining a certain safe distance from the target.

[0062] (x a ,y a )=(-50,sign(y rel )·[k2·(L t +L j )+50])

[0063] Tracking Situation #8: Guide the controlled vessel to navigate from the side and aft of the target relative to the expected tracking point to directly aft of the target, entering Tracking Situation #9. The set approach point is directly aft of the target, maintaining a safe distance from the target.

[0064] (x a ,y a )=(min{-[k2·(L t +L j )+50],x e -20},0)

[0065] Tracking situation #9: Guide the controlled vessel to navigate from the side and rear of the target vessel relative to the same side as the expected tracking point to the expected tracking point, then enter tracking situation #0. The set approach point is located at the desired tracking point (x e ,y e Let be the center of the circle, and let r be the radius of the tracking error tolerance. t Inside the circle:

[0066] (x a ,y a )=(x e -0.5·r t ,y e )

[0067] Tracking situation #10: Guide the controlled vessel to stop or reduce propulsion, maintain the bow heading consistent with the tracked target's course, and wait for the desired tracking point to catch up with the controlled vessel, then enter tracking situation #0:

[0068] (x a ,y a )=(x rel -5,y e )

[0069] Tracking situation #11: Guide the controlled vessel from the side forward relative to the same side as the expected tracking point to the side aft, entering tracking situation #9. The set approach point is located at the origin (0,0) of the body coordinate system with a radius of 1.2 times the tracking distance r. e On the circle:

[0070] (x a ,y a )=(1.2r e ·sinθ e 1.2r e ·cosθ e )

[0071] Where, x a This represents the x-coordinate of the approach point in the body coordinate system of the tracked target at this moment, y a The approach point represents the ordinate in the body coordinate system of the tracked target, sign represents the sign function, and ψ t For the heading / following of the tracked target, ψ j K1 represents the heading of the controlled vessel, k2 represents the collision distance coefficient, and k2 represents the safety distance coefficient.

[0072] Furthermore, in step 4, the state variables of the controlled vessel are defined:

[0073] η=[xyψ] T ,

[0074] v represents the velocity of the ship in its three-degree-of-freedom motion state space. This represents the velocity along the horizontal axis in the geodetic coordinate system. Let denot r represent the ship's velocity in the vertical direction of the geodetic coordinate system, r represent the ship's bow angular velocity, x represent the ship's horizontal coordinate in the geodetic coordinate system, y represent the ship's vertical coordinate in the geodetic coordinate system, ψ represent the ship's heading angle in the geodetic coordinate system, and η represent the ship's three-degree-of-freedom motion state space. Indicates the speed within the space of the ship's motion state;

[0075] The acceleration is:

[0076]

[0077] Let represent the acceleration in the space of the ship's motion state, M represent the mass matrix of the ship, C(v)v is the Coriolis force term, D(v)v is the damping force term, g(η) is the restoring force term, and τ is the generalized control force.

[0078] The propeller model is as follows:

[0079]

[0080] V ai =(1-w)V i

[0081] Among them, T i Q is the propeller thrust. i ρ is the propeller torque, ρ is the fluid density, D is the propeller diameter, α1 and α2 are the propeller thrust characteristic parameters, β1 and β2 are the propeller torque characteristic parameters, and V is the propeller torque. ai V represents the propeller advance speed. i The value represents the ship's speed, w is the wake coefficient, and i is the propeller number.

[0082] Considering the motor model again, the control equation of the motor is:

[0083]

[0084] Among them, J z R is the rotor moment of inertia, L is the armature inductance, and R is the rotor moment of inertia. a B is the armature internal resistance, and k is the damping coefficient. e k is the electromagnetic induction coefficient. t The electromagnetic torque coefficient is given by the propeller torque Q. i It can be obtained from the propeller model, ω i =2πn i , This represents the rate of change of angular acceleration of thruster i. ω represents the angular acceleration of thruster i. i The angular velocity of propeller i is given; the thrust T of each propeller is obtained. i With input voltage V si In terms of the relationship, neglecting the influence of the torque generated by the propeller rotation on ordinary surface vessels, the generalized control force on the horizontal plane generated by the propeller is:

[0085]

[0086] R i α represents the horizontal distance between the propeller and the ship's center of gravity. iβ represents the angle between the propeller and the x-axis of the body coordinate system. i The angle between the reverse direction of the propeller thrust and the horizontal line connecting the propeller and the center of gravity is represented by X, which is the sway force, Y, and N, which is the yaw torque.

[0087] The relationship between thruster thrust and generalized force is as follows:

[0088] τ=ΠT

[0089] Π is the generalized force matrix, and T represents the propeller thrust vector;

[0090] Generalized control:

[0091]

[0092] τ c For generalized control force, M is the mass matrix, J is the coordinate transformation matrix between geodetic coordinates and body coordinates, C is the offset distance coefficient, and η is the displacement coefficient. d For the desired ship tracking point, i.e., the desired tracking point / approach point at the current moment, e = η d -η is the error term. Let e ​​be the first derivative with respect to time. Let be the second derivative of the desired ship tracking motion navigation point with respect to time, and k be the control parameter of the sliding mode method.

[0093] Based on the ship's state variables, propeller model, motor control equations, propeller advance speed, the relationship between propeller thrust and generalized force, and generalized control force, a control system is obtained that uses propeller motor voltage as input to control the ship's tracking navigation.

[0094] The beneficial effects achieved by this invention are:

[0095] This patent's ship tracking navigation planning method acquires real-time information on surface targets, dynamically adjusts the navigation path, and optimizes the selection of approach and target points, thereby reducing the navigation time and energy consumption from any relative position of the tracked target to the desired tracking position; at the same time, it considers collision avoidance with the tracked target during the planning process, thus achieving safe tracking of moving targets on the surface.

[0096] Compared to existing manned target tracking navigation, this patented method realizes unmanned autonomous planning for tracking navigation of moving targets on the water surface, enhancing the ship's autonomous mission execution capability and possessing good adaptability and flexibility. At the same time, compared to traditional visual tracking methods, the planning method of this patent can calculate and correct the tracking distance and azimuth error in real time, thus improving the tracking accuracy compared to manned visual tracking navigation. Attached Figure Description

[0097] Figure 1A flowchart illustrating a navigation planning method for tracking moving targets on water provided by the present invention;

[0098] Figure 2 This is a schematic diagram of tracking task information in a navigation planning method for tracking moving targets on water provided by the present invention.

[0099] Figure 3 A schematic diagram of the Kalman filter process for phase noise removal in a resonant device phase noise suppression method based on the Kalman filter principle provided by this invention;

[0100] Figure 4 A comparison diagram of the output signal after Kalman filtering and the original signal in a phase noise suppression method for resonant devices based on the Kalman filtering principle provided by this invention;

[0101] Figure 5 A schematic diagram of the equivalent kinematic model of the resonator in a resonant device phase noise suppression method based on the Kalman filter principle provided by the present invention;

[0102] Figure 6 This is a schematic diagram of the sinusoidal waveform after being disturbed by phase noise in a resonant device phase noise suppression method based on the Kalman filtering principle provided by the present invention. Detailed Implementation

[0103] The present invention will be further described below with reference to specific embodiments, and the advantages and features of the present invention will become clearer as a result. However, these embodiments are merely exemplary and do not constitute any limitation on the scope of the present invention. Those skilled in the art should understand that modifications or substitutions can be made to the details and form of the technical solutions of the present invention without departing from the spirit and scope of the present invention, but all such modifications and substitutions fall within the protection scope of the present invention.

[0104] As attached Figure 1 As shown, the present invention provides a navigation planning method for tracking moving targets on water, comprising the following steps:

[0105] Step 1: Read the surface and underwater target information and tracking mission information around the controlled vessel;

[0106] The targets considered include nearby ships, buoys, submersibles, reefs, etc. The data source is the fused situational information obtained by processing the outputs of the Automatic Identification System (AIS), radar, camera, sonar and other equipment in the ship's sensor system through target fusion software. The data of interest include the target's attributes, position, speed and size.

[0107] like Figure 2As shown, based on the target situation information, tracking task information is read. Tracking task information includes the specified target identification batch number, tracking distance, tracking relative azimuth angle, and tracking duration. The target identification batch number is the credential used by the target tracking algorithm to identify and extract target information from the fused situation information. When the target tracking algorithm reads the identification batch number, it iterates through the target batch numbers in the fused situation information. If an input identification batch number is detected, the target information is extracted as input for the tracking algorithm's operation; the tracking distance r... e It is the expected distance maintained between the controlled vessel and the tracked target during the tracking process; the tracking relative azimuth angle θ e It is the relative angle between the line connecting the location of the tracked target to the expected tracking point of the controlled vessel during the tracking process and the heading of the tracked target, with a range of (-180°, +180°); the tracking duration controls the total duration of the tracking task. When the actual tracking execution time exceeds the tracking duration given by the task, the tracking task is considered to have ended and tracking is stopped.

[0108] Step 2: Calculate the tracking situation based on surface and underwater target information and tracking mission information;

[0109] Step 21: Locate the target in the target situation information based on the target identification batch number, and read the target's latitude, longitude, speed, and heading. Based on the target information, calculate the expected tracking point of the target tracking task at the current time according to the tracking distance and relative tracking azimuth in the tracking task information. Plan a route from the current position of the controlled vessel to the tracking point, so that the controlled vessel approaches and stays near the expected tracking point, which is considered to be target tracking. When time changes, the target situation information is updated, and the latitude, longitude, speed, and heading information of the tracked target are read again, the expected tracking point is recalculated, and the control variables are updated. This process is repeated until the tracking execution time reaches the time requirement in the task information or a tracking termination command is received.

[0110] like Figure 3 As shown, based on the above technical solution, in some cases, the path from the current position of the controlled vessel to the desired tracking point may collide with the tracked target.

[0111] Step 22: The approach tracking method based on tracking situation identification guides the controlled vessel to approach the desired tracking point along a path that will not collide with the tracked target.

[0112] During the situational awareness process, the target tracking algorithm first reads the position and heading of the tracked target and the position of the controlled vessel, and then calculates the relative position (x, y) of the controlled vessel relative to the tracked target. rel ,y relThe expected relative position (x) of the tracking point with respect to the tracked target. e ,y e ), and the relative position (x) of the controlled vessel with respect to the desired tracking point. g ,y g ),in:

[0113]

[0114] θ rel This indicates the current bearing (0° bow) of the monitored vessel relative to the tracked target.

[0115] like Figure 4 As shown, the current tracking situation is then determined based on the relative position. The coordinate system for calculating the relative position is chosen to be the body coordinate system of the tracked target, with the bow direction as the positive x-axis and the starboard direction as the positive y-axis.

[0116] Step 23: This patent classifies and discusses the tracking target point being located near the rear of the target ship and the tracking target point being located on both sides of the target ship as two different cases.

[0117] Step 231, when θ e When the value is ∈[160°,180°]∪(-180°,-160°], the target point is considered to be directly behind the target ship. The tracking situation is then defined as follows:

[0118] 1. When At this time, the controlled vessel is near the desired tracking point. It is considered that the controlled vessel is in the normal tracking phase and can achieve continuous tracking navigation by controlling its speed. This is recorded as tracking situation #0.

[0119] 2. When |y rel |≤k1·(L t +L j And x rel >0 indicates that the controlled vessel is located on the future course of the tracked target and there is a risk of collision, which is recorded as tracking status #1;

[0120] 3. When |y rel |>k1·(L t +L j And x rel >0 indicates that the controlled vessel is located in front of the tracked target but not on its future course. If the controlled vessel is stationary at this time, it does not pose a collision risk to the tracked target. This is recorded as tracking situation #2.

[0121] 4. When ((θ) rek ∈[160°,180°]∪(-180°,-160°] or |y rel |≤k1·(L t+L j And x g <0 indicates that the controlled vessel is located within 40° directly behind the tracked target, and the controlled vessel is located behind the desired tracking point in the above target body coordinate system, which is recorded as tracking situation #3;

[0122] 5. When ((θ) rel ∈[160°,180°]∪(-180°,-160°] or |y rel |≤k1·(L t +L j And x g >0 indicates that the controlled vessel is located within 40° directly behind the tracked target and is ahead of the expected tracking point, which is recorded as tracking situation #4;

[0123] 6. Other situations, such as Figure 5 As shown, the controlled vessel is considered to be located to the side and rear of the tracked target at this time, which is recorded as tracking situation #5;

[0124] Step 232, when θ e ∈(-160°,+160°), assuming the tracking target point is located on either side of the tracked target. First, determine the relative position (x, y) of the controlled vessel. rel ,y rel ) and expected tracking point (x) e ,y e ) Whether it is on the same side of the tracked target. If θ e ·θ rel >0, or (θ) rel If ∈[160°,180°]∪(-180°,-160°], then the two points are considered to be on the same side, and there is no need to guide the controlled vessel around the stern of the tracked target. Let b be the variable for switching sides. cs If the value is 0, then the controlled vessel is considered to be on a different side of the target than the desired tracking point. The controlled vessel needs to be guided around the stern of the target to the other side. Let the variable b be the side-changing judgment variable. cs The value is 1. The current tracking situation is divided as follows:

[0125] 1. When At this point, the controlled vessel is near the desired tracking point and is considered to be in the normal tracking phase. Continuous tracking can be achieved by controlling its speed, and this is recorded as tracking situation #0.

[0126] 2. When |y rel |≤k1·(L t +L j And x rel >0 indicates that the controlled vessel is currently on the tracked target's future route and there is a risk of collision; this is recorded as tracking status #6.

[0127] 3. When b cs =1 and x rel ≥0 indicates that the controlled vessel is located ahead of the tracking target and needs to change sides, which is recorded as tracking situation #7.

[0128] 4. When b cs =1 and x rel If the value is less than 0, it is considered that the controlled vessel is located behind the target and needs to change sides, which is recorded as tracking situation #8.

[0129] 5. When b cs =0 and x g If the value is less than 0, it is assumed that there is no need to change the hull side at this time, and the controlled vessel is located behind the desired tracking point, which is recorded as tracking situation #9.

[0130] 6. When b cs =0 and (0 <x g ≤x set And |y g |≤C·(r t It is determined that there is no need to change sides at this time, and the controlled vessel is slightly ahead of the expected tracking point, which is recorded as tracking situation #10.

[0131] 7. When b cs Other cases where =0, such as Figure 6 As shown, it is assumed that there is no need to change sides at this time and the controlled vessel is in front of the desired tracking point, which is recorded as tracking situation #11.

[0132] Among them, L t L represents the scale of the tracked target. j Let r be the length of the controlled vessel, k1 be the collision distance coefficient, and r be the length of the vessel being controlled. t To track the allowable radius of error, x set C is the offset distance coefficient, which is considered to be a preset amount that slightly leads the expected tracking point.

[0133] Step 3: Based on the assessment of the current tracking situation, calculate the approach point or target point for maintaining tracking to guide the unmanned vessel to the expected tracking point under the current situation (only for tracking situation #0), denoted as (x a ,y a The approach point calculation strategies and formulas for various tracking situations are as follows:

[0134] Tracking Situation #0. Guide the controlled vessel to maintain tracking, with the target point being the desired tracking point.

[0135] (x a ,y a )=(x e ,y e (3)

[0136] Tracking Situation #1. Guide the controlled vessel to leave the tracked target's forward path area as soon as possible and enter Tracking Situation #2. At this time, the approach point is set to the side forward of the tracked target, and the side of this approach point is selected as the side of the controlled vessel's current heading.

[0137] ψ rel =ψ j -ψ t ,ψ rel ∈(-180°,+180°). (4)

[0138] (x a ,y a )=(x rel ,sign(ψ rel )·[k1·(L t +L j (5)

[0139] Tracking Situation #2. Guide the controlled vessel to sail from the side-front of the tracked target to the side-rear to enter Tracking Situation #5. At this time, the set approach point is located to the side-rear of the tracked target, and a certain safe distance is maintained between the two targets.

[0140] (x a ,y a )=(-50,sign(y rel )·[k2·(L t +L j (6)

[0141] Tracking Situation #3. Guide the controlled vessel to approach the desired tracking point from a considerable distance directly behind the tracked target, entering Tracking Situation #0. At this time, the set approach point is located at the desired tracking point (x). e ,y e Let be the center of the circle, and let r be the radius of the tracking error tolerance. t Inside the circle.

[0142] (x a ,y a )=(x e -0.5·r t ,y e (7)

[0143] Tracking Situation #4. Guide the controlled vessel to stop or reduce propulsion, maintain its bow aligned with the tracked target's course, and wait for the desired tracking point to catch up with the controlled vessel, entering Tracking Situation #0. At this point, the set approach point is located at the desired tracking point (x e ,y e Let be the center of the circle, and let r be the radius of the tracking error tolerance.t Inside the circle.

[0144] (x a ,y a )=(x rel -5,y e (8)

[0145] Tracking Situation #5. Guide the controlled vessel to sail from the side and rear of the tracked target, directly behind the desired tracking point, and enter Tracking Situation #3. At this time, the set approach point is located at the origin (0,0) of the body coordinate system with a radius of 1.2 times the tracking distance r. e On the circle.

[0146] (x a ,y a )=(1.2r e ·sinθ e 1.2r e ·cosθ e (9)

[0147] Tracking Situation #6. Guide the controlled vessel to leave the forward path area of ​​the tracked target as soon as possible and enter Tracking Situation #11. At this time, the approach point is set to the side forward of the side of the tracked target's expected tracking point.

[0148] (x a ,y e )=)x rel ,sign(y e )·[k1·(L t +L j (10)

[0149] Tracking Situation #7. Guide the controlled vessel to sail from the side forward relative to the target and the expected tracking point to the side aft, and enter Tracking Situation #8. At this time, the set approach point is located to the side aft of the target and maintains a certain safe distance from the target.

[0150] (x a ,y a )=(-50,sign(y rel )·[k2·(L t +L j (11)

[0151] Tracking Situation #8. Guide the controlled vessel to sail from the side and rear of the target relative to the target and the expected tracking point to the direct rear of the target, and enter Tracking Situation #9. At this time, the set approach point is located directly rear of the target and maintains a certain safe distance from the target.

[0152] (xa ,y a )=(min{-[k2·(L t +L j )+50],x e -20},0). (12)

[0153] Tracking Situation #9. Guide the controlled vessel to navigate from the side and aft of the target relative to the expected tracking point to the expected tracking point, entering Tracking Situation #0. At this time, the set approach point is located at the desired tracking point (x e ,y e Let be the center of the circle, and let r be the radius of the tracking error tolerance. t Inside the circle.

[0154] (x a ,y a )=(x e -0.5·r t ,y e (13)

[0155] Tracking Situation #10. Guide the controlled vessel to stop or reduce propulsion, maintain the bow heading consistent with the course of the tracked target, wait for the desired tracking point to catch up with the controlled vessel, and enter Tracking Situation #0.

[0156] (x a ,y a )=(x rel -5,y e (14)

[0157] Tracking Situation #11. Guide the controlled vessel from the side-forward side relative to the same side as the expected tracking point to the side-rear side, entering Tracking Situation #9. At this time, the set approach point is located at the origin (0,0) of the body coordinate system with a radius of 1.2 times the tracking distance r. e On the circle.

[0158] (x a ,y a )=(1.2r e ·sinθ e 1.2r e ·cosθ e (15)

[0159] Where, x a This represents the x-coordinate of the approach point in the body coordinate system of the tracked target at this moment, y a The approach point represents the ordinate in the body coordinate system of the tracked target, sign represents the sign function, and ψ t For the heading / following of the tracked target, ψ jK1 represents the heading of the controlled vessel, k2 represents the collision distance coefficient, and k2 represents the safety distance coefficient.

[0160] Step 4: Determine the approach point of the target tracking point or maintain the relative coordinates (x, y) of the target tracking point. a ,y a This can be converted to absolute latitude and longitude coordinates in the geodetic coordinate system (London). a ,Lat a By using this method, the absolute position of the current approach point can be obtained, which can be used for tracking and navigation control of the controlled vessel.

[0161] During the tracking and navigation control phase, this patent first establishes a three-degree-of-freedom dynamic model of the controlled vessel. This paper defines the symbols and meanings of each variable in the six-degree-of-freedom motion model of the controlled vessel, and obtains the symbols and meanings of each variable in the defined six-degree-of-freedom motion coordinate system, as shown in Table 1 below:

[0162] Table 1. Coordinate Variable Table

[0163]

[0164] The control section of this patent assumes constrained heave, roll, and pitch motions, and therefore defines the state variables of the controlled vessel as follows:

[0165]

[0166] v represents the velocity of the ship in its three-degree-of-freedom motion state space. This represents the velocity along the horizontal axis in the geodetic coordinate system. Let denot r represent the ship's velocity in the vertical direction of the geodetic coordinate system, r represent the ship's bow angular velocity, x represent the ship's horizontal coordinate in the geodetic coordinate system, y represent the ship's vertical coordinate in the geodetic coordinate system, ψ represent the ship's heading angle in the geodetic coordinate system, and η represent the ship's three-degree-of-freedom motion state space. Indicates the speed within the space of the ship's motion state;

[0167] The motion control equations of the controlled vessel are:

[0168]

[0169] Where M represents the ship's mass matrix, C(v)v is the Coriolis force term, D(v)v is the damping force term, g(η) is the restoring force term, and τ is the generalized control force, τ=[XYN] T .

[0170] The mass matrix M of the controlled vessel during its motion includes the mass matrix M of the controlled vessel itself. RB Additional mass matrix M with the surrounding fluid A .

[0171]

[0172] M(v)=M RB (v)+M A (v). (19)

[0173] m represents the ship's mass, y G This indicates the y-coordinate of the ship's center of gravity in the body coordinate system, and the x-coordinate of the position. G This indicates the x-coordinate of the ship's center of gravity in the body coordinate system. zz This represents the moment of inertia of the ship about the z-axis. This represents the additional mass coefficient that affects the longitudinal acceleration due to the additional sway force. This represents the mass coefficient that adds to the lateral acceleration due to the sway force. The mass coefficient representing the additional mass of the turning torque on the bow angular acceleration;

[0174] The Coriolis matrix C of the controlled vessel during its motion includes the Coriolis matrix C of the controlled vessel itself. RB Additional Coriolis array C with surrounding fluid A .

[0175]

[0176] C(v)=C RB (v)+C A (v). (21)

[0177] For a typical controlled ship, the degree of motion coupling between the degrees of freedom is relatively low, and it can be approximated by a diagonal matrix containing only first- and second-order damping coefficients.

[0178]

[0179] This represents the second-order damping parameter in the x-direction. N represents the second-order damping parameter in the y-direction. r|r| This represents the second-order damping parameter in the z-direction;

[0180] For a symmetrical controlled vessel, its generalized restoring force is:

[0181]

[0182] For this paper, since the pitch and roll degrees of freedom are constrained, θ≡0, φ≡0, we have:

[0183] g(η) = [0 0 0] T . (twenty four)

[0184] According to the ship dynamics equation (17), we can obtain:

[0185]

[0186] Taking an electrically propelled controlled ship as an example, considering the motor and propeller model, a system is established based on the control voltage V. s The input is the controlled ship dynamics model. The propeller model can be simplified to generate thrust and torque simultaneously during rotation, with magnitudes as follows:

[0187]

[0188] V ai =(1-w)V i (27)

[0189] Among them, T i Q represents propeller thrust. i n represents the propeller torque. i V represents the propeller speed. ai V represents the propeller advance speed. i Let ρ represent ship speed, ρ be fluid density, D be propeller diameter, α1 and α2 be propeller thrust characteristic parameters, β1 and β2 be propeller torque characteristic parameters, and w be the wake coefficient.

[0190] Considering the motor model again, the control equations of the motor are:

[0191]

[0192] Among them, J z R is the rotor moment of inertia, L is the armature inductance, and R is the rotor moment of inertia. a B is the armature internal resistance, and k is the damping coefficient. e k is the electromagnetic induction coefficient. t This is the electromagnetic torque coefficient. Torque Q i This can be obtained from a propeller model. Because ω i =2πn i The thrust T of each propeller can then be obtained. i With input voltage V si Ignoring the influence of the torque generated by the propeller rotation on typical surface vessels, the generalized control force on the horizontal plane generated by the propeller is:

[0193]

[0194] R i α represents the horizontal distance between the propeller and the ship's center of gravity. i β represents the angle between the propeller and the x-axis of the body coordinate system. i This represents the angle between the reverse direction of the propeller thrust and the horizontal line connecting the propeller and the center of gravity.

[0195] This yields the generalized force matrix Π, and the relationship between the thruster thrust and the generalized force is as follows:

[0196] τ=ΠT. (30)

[0197] T represents the propeller thrust vector;

[0198] To maintain the effectiveness and stability of the controlled vessel's navigation control and prevent control chattering, this patent employs a sliding mode method to establish a ship dynamics controller under certain tracking situations. This controller is used at the approach point where the expected tracking point is obtained, or at the target point where tracking is maintained (x... a ,y a Based on this, we first establish the current state variable η0 = [X] of the controlled vessel. rel Y rel ψ abs ] T Go to η a =[X a Y a ψ a ] T A feasible Durbins curve is obtained, requiring that the minimum radius of curvature of the curve not be less than the turning radius of the controlled vessel. After discretizing the curve according to time steps, the desired trajectory of the controlled vessel running along this curve is obtained as η. d (t), error e=η d -η.

[0199] Let the sliding mode control variable Where C > 0.

[0200] Then there is

[0201]

[0202] Let the sliding mode approach law

[0203] And because

[0204]

[0205] Generalized control can be obtained:

[0206]

[0207] By combining (25)(26)(28)(29)(31)(35), a control system that uses the propeller motor voltage as input to control the ship's tracking navigation can be obtained.

[0208] The above are merely specific steps of the present invention and do not constitute any limitation on the scope of protection of the present invention; all technical solutions formed by equivalent transformation or equivalent substitution fall within the scope of protection of the present invention; the parts of the present invention not described in detail are common knowledge to those skilled in the art.

Claims

1. A method for ship-to-ship navigation planning to track moving targets on water, characterized in that, The method for tracking and planning navigation targets on water includes the following steps: Step 1: Obtain information on surface and underwater targets around the controlled vessel and tracking mission information; Step 2: Calculate the tracking situation based on surface and underwater target information and tracking mission information; Step 3: Calculate the approach point or target point to guide the unmanned vessel to the expected tracking point or maintain tracking based on the current situation. Step 4: Tracking navigation control of the vessel based on the approach point coordinates of the expected tracking point or the target point coordinates to maintain tracking; Step 2 also includes the following steps: Step 21: Locate the tracked target in the target situation information according to the target identification batch number, and read the target's latitude, longitude, speed, and heading; Based on the tracking distance and relative azimuth angle in the tracking task information, the expected tracking point of the target tracking task at the current moment is calculated; Plan a route from the current position of the controlled vessel to its tracking point, so that the controlled vessel approaches and remains near the desired tracking point, and consider it to be being tracked by the target. When time changes and the target situation information is updated, the latitude, longitude, speed and heading information of the tracked target are reread, the desired tracking point is recalculated, and the control variables are updated. Step 22: The approach tracking method based on tracking situation identification guides the controlled vessel to approach the desired tracking point along a path that will not collide with the tracked target; Step 23: Calculate the tracking situation for two scenarios: the target point is located near the rear of the target ship, and the target point is located on either side of the target ship. In step 23, when At that time, the target point is considered to be located directly behind the target ship, and the tracking situation is divided as follows: Tracking situation #0: When The controlled vessel is currently in the normal tracking phase, and continuous tracking navigation is achieved by controlling its speed; Tracking the situation #1: When and The monitored vessel is located on the future course of the tracked target and there is a risk of collision. Tracking the situation #2: When and If the controlled vessel is located in front of the tracked target, and the controlled vessel is stationary, it does not pose a collision risk to the tracked target. Tracking the situation #3: When ,and The controlled vessel is located within 40° directly behind the tracked target, and the controlled vessel is located behind the desired tracking point in the target's body coordinate system; Tracking the situation #4: When ,and The controlled vessel is located within 40° directly behind the tracked target, and the controlled vessel is ahead of the expected tracking point; Tracking Situation #5: Other situations, the controlled vessel is located to the side and rear of the tracked target; in, The scale of the tracked target. The captain of the vessel under investigation. This is the collision distance coefficient. The allowable radius for tracking error; This indicates the current relative position of the monitored vessel to the tracked target. Indicates the tracking distance. Indicates tracking relative azimuth angle, The relative position of the controlled vessel with respect to the tracked target. The relative position of the desired tracking point with respect to the tracked target. The relative position of the controlled vessel with respect to the desired tracking point.

2. The method for tracking and navigation planning of moving targets on water as described in claim 1, characterized in that, In step 1, the tracking task information includes the specified target identification batch number, tracking distance, tracking relative azimuth angle, and tracking duration; The target identification batch number is the basis for the target tracking algorithm to identify and extract the target information from the fused situational information. Tracking distance is the expected distance that the controlled vessel maintains with the tracked target during the tracking process; The relative azimuth angle is the relative angle between the line connecting the position of the tracked target to the desired tracking point of the controlled vessel during the tracking process and the heading of the tracked target. Its range is... ; The tracking duration controls the overall tracking task duration. When the actual tracking execution time exceeds the given tracking duration, the tracking task is considered to have ended and tracking stops.

3. The method for tracking and navigation planning of moving targets on water as described in claim 1, characterized in that, In step 22, the target tracking algorithm first reads the position and heading of the tracked target and the position of the controlled vessel, and calculates the relative position of the controlled vessel with respect to the tracked target. The relative position of the expected tracking point with respect to the tracked target. The relative position of the controlled vessel with respect to the desired tracking point ,in: ； ； This indicates the current relative position of the monitored vessel to the tracked target. Indicates the tracking distance. This indicates tracking relative azimuth angle.

4. The method for tracking and navigation planning of moving targets on water as described in claim 1, characterized in that, In step 23, when Assuming the target point is located on either side of the tracked target, the tracking situation is divided as follows: Tracking situation #0: When When the controlled vessel is near the desired tracking point, it is considered to be in the normal tracking phase, and the speed is controlled to achieve continuous tracking navigation; Tracking the situation #6: When and The monitored vessel is located on the future course of the tracked target and there is a risk of collision. Tracking the situation #7: When and The controlled vessel is located ahead of the target and needs to switch sides; Tracking the situation #8: When and The controlled vessel was located behind the tracking target and needed to change sides. Tracking the situation #9: When and The controlled vessel is located behind the expected tracking point and does not need to change sides; Tracking the situation #10: When and The controlled vessel was slightly ahead of the expected tracking point and did not need to change sides. Tracking the situation #11: When In other cases, the controlled vessel is ahead of the desired tracking point and does not need to change sides; in, The scale of the tracked target. The captain of the vessel under investigation. This is the collision distance coefficient. To track the allowable radius of error, This is considered to be a slightly ahead-of-the-preset amount of the expected tracking point. This is the offset distance coefficient. The variable used to determine the side switching.

5. The method for tracking and navigation planning of moving targets on water as described in claim 1, characterized in that, In step 3, the approach point calculation strategies and methods for each tracking situation are as follows: Tracking situation #0, guiding the controlled vessel to maintain tracking, with the target point being the desired tracking point: ； Tracking Situation #1: Guide the controlled vessel to quickly leave the tracked target's forward path area and enter Tracking Situation #2. The set approach point is the side forward of the tracked target, and the side of the approach point is selected as the side of the controlled vessel's current bow heading. ； ； Tracking Situation #2: Guide the controlled vessel to navigate from the side-front of the tracked target to the side-rear to enter Tracking Situation #5. The set approach point is located to the side-rear of the tracked target, and a certain safe distance is maintained between the two targets. ； Tracking Situation #3: Guide the controlled vessel to pursue the desired tracking point from a considerable distance directly behind the tracked target, then enter Tracking Situation #0, with the set approach point located at the desired tracking point. The center is a circle, and the radius is the allowable radius of the tracking error. Inside the circle: ； Tracking situation #4: Guide the controlled vessel to stop or reduce propulsion, maintain the bow heading consistent with the tracked target's course, wait for the desired tracking point to catch up with the controlled vessel, then enter tracking situation #0, with the set approach point located at the desired tracking point. The center is a circle, and the radius is the allowable radius of the tracking error. Inside the circle: ； Tracking situation #5: Guide the controlled vessel to navigate from the side and rear of the tracked target, directly behind the desired tracking point, and enter tracking situation #3. The set approach point is located at the origin of the body coordinate system. Centered on a circle with a radius of 1.2 times the tracking distance. On the circle: ； in, This indicates the x-coordinate of the approach point in the body coordinate system of the tracked target at this moment. This represents the ordinate of the approach point in the body coordinate system of the tracked target. Represents a symbolic function. The scale of the tracked target. The captain of the vessel under investigation. For the heading / following of the tracked target, The bow direction of the controlled vessel. This is the collision distance coefficient. For safety distance coefficient, This indicates the relative heading angle of the controlled vessel relative to the tracked target.

6. The method for tracking and navigation planning of moving targets on water as described in claim 4, characterized in that, In step 3, the approach point calculation strategies and methods for each tracking situation are as follows: Tracking situation #0, guiding the controlled vessel to maintain tracking, with the target point being the desired tracking point: ； Tracking situation #6, guide the controlled vessel to leave the tracked target's forward path area as soon as possible, and enter tracking situation #11, with the approach point set to the side forward of the tracked target's desired tracking point: ； Tracking Situation #7: Guide the controlled vessel from the side-forward side relative to the target and the expected tracking point to the side-rear side, entering Tracking Situation #8. The set approach point is located at the side-rear side of the target, maintaining a certain safe distance from the target. ； Tracking Situation #8: Guide the controlled vessel to navigate from the side and aft of the target relative to the expected tracking point to directly aft of the target, entering Tracking Situation #9. The set approach point is directly aft of the target, maintaining a safe distance from the target. ； Tracking situation #9: Guide the controlled vessel to navigate from the side and rear of the target vessel relative to the expected tracking point to the expected tracking point, then enter tracking situation #0, with the set approach point located at the desired tracking point. The center is a circle, and the radius is the allowable radius of the tracking error. Inside the circle: ； Tracking situation #10: Guide the controlled vessel to stop or reduce propulsion, maintain the bow heading consistent with the tracked target's course, and wait for the desired tracking point to catch up with the controlled vessel, then enter tracking situation #0: ； Tracking situation #11 guides the controlled vessel from the side forward relative to the same side as the expected tracking point to the side aft, entering tracking situation #9, with the set approach point located at the origin of the body coordinate system. Centered on a circle with a radius of 1.2 times the tracking distance. On the circle: ； in, This indicates the x-coordinate of the approach point in the body coordinate system of the tracked target at this moment. This represents the ordinate of the approach point in the body coordinate system of the tracked target. Represents a symbolic function. For the heading / following of the tracked target, The bow direction of the controlled vessel. This is the collision distance coefficient. This is the safety distance coefficient.

7. The method for tracking and navigation planning of moving targets on water as described in claim 1, characterized in that, In step 4, the state variables of the controlled vessel are defined: ； Represents the velocity in the three-degree-of-freedom motion state space of the ship. This represents the velocity along the horizontal axis in the geodetic coordinate system. This represents the velocity along the vertical direction in the geodetic coordinate system. Indicates the angular velocity of the ship's bow turn. This represents the x-coordinate of the ship in the geodetic coordinate system. This represents the vertical coordinate of the ship in the geodetic coordinate system. This indicates the ship's heading angle in the geodetic coordinate system. This represents the space of a ship's three degrees of freedom of motion. Indicates the speed within the space of the ship's motion state; The acceleration is: ； Represents the acceleration of a ship in space during its motion. The mass matrix of the ship. For Coriolis force, For the damping force term, For restoring force, For the purpose of broad control; The propeller model is as follows: ； ； in, Indicates the propeller speed. For propeller thrust, For propeller torque, For fluid density, The diameter of the propeller. Propeller thrust characteristic parameters These are the propeller torque characteristic parameters. Indicates the propeller advance speed. Indicates ship speed. For the wake coefficient, Number the propellers; Considering the motor model again, the control equation of the motor is: ； in, The moment of inertia of the rotor. For armature inductance, For armature internal resistance, The damping coefficient is... The electromagnetic induction coefficient, Electromagnetic torque coefficient, propeller torque It can be obtained from a propeller model. , Indicates thruster The rate of change of angular acceleration, Indicates thruster angular acceleration, Indicates thruster Angular velocity; Calculate the thrust of each propeller. With input voltage In terms of the relationship, neglecting the influence of the torque generated by the propeller rotation on the surface vessel, the generalized control force on the horizontal plane generated by the propeller is: ； This indicates the horizontal distance between the propeller and the ship's center of gravity. This represents the angle between the propeller and the x-axis of the body coordinate system. This represents the angle between the reverse direction of the propeller thrust and the horizontal line connecting the propeller and the center of gravity. For oscillation force, For swaying force, For the turning torque; The relationship between thruster thrust and generalized force is as follows: ； For generalized force matrix, This represents the propeller thrust vector; Generalized control: ； For the purpose of broad control, For the quality matrix, This is the coordinate transformation matrix between geodetic coordinates and body coordinates. This is the offset distance coefficient. The desired ship tracking point, i.e., the desired tracking point / approach point at the current moment. For error terms, for The first derivative with respect to time, Let be the second derivative of the desired ship tracking motion navigation point with respect to time. These are the control parameters for the sliding mode method. ; Based on the ship's state variables, propeller model, motor control equations, propeller advance speed, the relationship between propeller thrust and generalized force, and generalized control force, a control system is obtained that uses propeller motor voltage as input to control the ship's tracking navigation.

Images