Ship tracking navigation planning method for water moving target

Abstract

The invention discloses a ship tracking navigation planning method for a water moving target. The method comprises the following steps: step 1, acquiring water surface and underwater target information and tracking task information around a controlled ship; 2, calculating a tracking situation according to the water surface and underwater target information and the tracking task information; 3, according to the tracking situation, calculating an approach point for guiding the unmanned ship to navigate to an expected tracking point or a target point for keeping tracking under the current situation; and 4, controlling the tracking navigation of the ship based on the approaching point of the expected tracking point or the coordinates of the target point for keeping tracking. According to the ship tracking navigation planning method, the water surface target information is acquired in real time, the navigation path is dynamically adjusted, and selection of an approach point and a target point is optimized, so that the navigation time and energy consumption from any relative position of a tracked target to an expected tracking position are reduced.

Description

Technical Field

[0001] The invention belongs to the technical field of ships and ocean engineering, and discloses a method for tracking and navigating a ship to a water sports target. Background Art

[0002] In the field of shipbuilding and marine engineering, when autonomous ships track and respond to attacks by non-cooperative targets in designated sea areas, or when unmanned tugboats follow large ships, or when autonomous ship fleets sail in formation, a target tracking navigation planning method suitable for ships is needed, so that the controlled autonomous ship can enter a specific tracking position for the surface target and perform stable and continuous tracking. The design of the target tracking navigation planning method for ships needs to take into account factors such as the actual ship's maneuverability parameters, hull size, and ship speed limit.

[0003] The existing tracking and planning algorithms for robots and drones can achieve stable tracking and control of the controlled objects under the control of the algorithms. However, on the one hand, the controlled objects of these two algorithms are not underactuated systems, and the maneuverability of the ship puts higher requirements on the tracking and 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 with the controlled objects as strongly as the marine environment, and the marine environment makes it more difficult for autonomous ships to control their navigation and perceive information about the tracking targets. Summary of the invention

[0004] The present invention discloses a method for tracking and navigating a ship to a water sports target. The method comprises the following steps:

[0005] Step 1, obtaining surface and underwater target information and tracking mission information around the controlled ship;

[0006] Step 2, calculating the tracking situation based on the surface and underwater target information and the tracking task information;

[0007] Step 3, according to the tracking situation, calculate the approach point to the expected tracking point or the target point to be tracked for guiding the unmanned ship to navigate under the current situation;

[0008] Step 4, tracking navigation control of the ship based on the approach point of the expected tracking point or the coordinates of the target point to be tracked.

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

[0010] The identification batch number of the tracked target is the basis for the target tracking algorithm to distinguish and extract the tracked target information from the fused situation information;

[0011] Tracking distance is the expected distance between the controlled ship and the tracked target during the tracking process;

[0012] The tracking relative azimuth is the relative angle between the line connecting the position of the tracked target to the expected tracking point of the controlled ship during the tracking process and the heading of the tracked target, ranging from (-180°, +180°];

[0013] The tracking duration controls the duration of the entire tracking task. When the actual tracking execution time is longer than the given tracking duration of the task, the tracking task is considered to be completed and the tracking is stopped.

[0014] Furthermore, in step 2, the following steps are also included:

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

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

[0017] Planning a route from the current position of the controlled ship to its tracking point so that the controlled ship approaches the desired tracking point and remains near the tracking point, and is considered to be tracking the target;

[0018] When the time changes and the target situation information is updated, the latitude and longitude and speed and heading information of the tracked target are re-read, the expected tracking point is recalculated, and the control amount is updated;

[0019] Step 22, the approach tracking method based on tracking situation recognition guides the controlled ship to approach the desired tracking point in a path that will not collide with the tracked target;

[0020] Step 23, the tracking situation is calculated by taking 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 cases.

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

[0022]

[0023] θrel Indicates the relative position of the currently controlled ship to the tracked target, r e represents the tracking distance, θ e Represents the tracking relative azimuth.

[0024] Furthermore, in step 23, when θ e ∈[160°,180°]∪(-180°,-160°], the tracking 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 is achieving continuous tracking navigation by controlling the speed;

[0026] Tracking Situation #1: When |y rel |≤k1·(L t +L j ) and X rel >0, the controlled ship is on the future route of the tracked target and there is a risk of collision;

[0027] Tracking Situation #2: When |y rel |>k1·(L t +L j ) and X rel >0, the controlled ship is in front of the tracked target. If the controlled ship is stationary, there is no collision risk to 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 ship is located within 40° behind the tracked target, and the controlled ship is behind the expected tracking point in the target 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 ship is within 40° behind the tracked target and is ahead of the expected tracking point;

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

[0031] Among them, L tis the scale of the tracked target, L j is the length of the accused ship, k1 is the collision distance coefficient, r t is the tracking error tolerance radius.

[0032] Furthermore, in step 23, when θ e ∈(-160°,+160°), it is considered that the tracking target point is located on both sides of the tracked target, and the tracking situation is divided as follows:

[0033] Tracking Situation #0: When The controlled ship is located near the expected tracking point and is considered to be in the normal tracking stage. The controlled ship can achieve continuous tracking navigation by controlling the speed.

[0034] Tracking Situation #6: When |y rel |≤k1·(L t +L j ) and X rel >0, the controlled ship is on the future route of the tracked target and there is a risk of collision;

[0035] Tracking Situation #7: When b cs =1 and x rel ≥0, the controlled ship is in front of the tracking target and needs to change sides;

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

[0037] Tracking Situation #9: When b cs = 0 and x g <0, the controlled ship is behind the expected tracking point and does not need to change side;

[0038] Tracking Situation #10: When b cs =0 and (0 <x g ≤x set And|y g |≤C·(r t )), the controlled vessel is slightly ahead of the expected tracking point and does not need to change sides;

[0039] Tracking Situation #11: When b cs =0, the controlled ship is in front of the expected tracking point and does not need to change sides;

[0040] Among them, L t is the scale of the tracked target, L j is the length of the accused ship, k1 is the collision distance coefficient, r t is the tracking error tolerance radius, x setis the preset amount that is considered to be slightly ahead of the expected tracking point, C is the offset distance coefficient, and b cs is the variable for judging the side change.

[0041] Furthermore, in step 3, the approach point calculation strategy and method under each tracking situation are as follows:

[0042] Tracking situation #0, guide the controlled ship to keep tracking, the target point is the expected tracking point:

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

[0044] Tracking situation #1, guide the controlled ship to leave the forward route area of ​​the tracked target as soon as possible and enter tracking situation #2. The approach point is set to the side and front of the tracked target, and the side of the approach point is selected as the side to which the controlled ship's current heading is pointing:

[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 ship to sail from the side and front of the tracked target to the side and rear of the tracked target to enter tracking situation #5, the set approach point is located at the side and rear of the tracked target, and maintains a certain safe distance from the tracked target:

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

[0049] Tracking situation #3, guide the controlled ship to catch up with the expected tracking point from a long distance behind the tracked target, enter tracking situation #0, and set the approach point to be at the expected tracking point (x e ,y e ) is the center of the circle, and the radius is the tracking error allowable radius r t Inside the circle:

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

[0051] Tracking situation #4, guide the controlled ship to stop or reduce propulsion, keep the heading consistent with the tracked target, wait for the expected tracking point to catch up with the controlled ship, enter tracking situation #0, and set the approach point at the expected tracking point (x e ,y e ) is the center of the circle, and the radius is the tracking error allowable radius r t Inside the circle:

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

[0053] Tracking situation #5, guide the controlled ship to sail from the side and rear of the tracked target to the rear of the desired tracking point, enter tracking situation #3, and set the approach point to be located at the origin of the body coordinate system (0,0) as the center of the circle, 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] Among them, x a It indicates the horizontal coordinate of the approach point in the coordinate system of the tracked target, y a represents the ordinate of the approach point in the tracked target body coordinate system, sign represents the sign function, L t is the scale of the tracked target, L j is the captain of the accused vessel, ψ t is the heading / course of the tracked target, ψ j is the bow direction of the controlled ship, k1 is the collision distance coefficient, and k2 is the safety distance coefficient.

[0056] Furthermore, in step 3, the approach point calculation strategy and method under each tracking situation are as follows:

[0057] Tracking situation #0, guide the controlled ship to keep tracking, the target point is the expected tracking point:

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

[0059] Tracking situation #6, guide the controlled ship to leave the forward route area of ​​the tracked target as soon as possible, and enter tracking situation #11. The approach point is set to the side and front of the expected tracking point of the tracked target:

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

[0061] Tracking situation #7, guide the controlled ship to sail from the side in front of the tracked target on a different side from the expected tracking point to the side behind, and enter tracking situation #8, the set approach point is located behind the tracked target and maintains a certain safety distance from the tracked target:

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

[0063] Tracking situation #8, guide the controlled ship to sail from the side and rear of the tracked target on a different side from the expected tracking point to the rear of the tracked target, and enter tracking situation #9, the set approach point is located directly behind the tracked target and maintains a certain safety distance from the tracked target:

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

[0065] Tracking situation #9, guide the controlled ship to sail from the side and rear of the same side of the tracked target and the expected tracking point to the expected tracking point, enter tracking situation #0, and the set approach point is located at the expected tracking point (x e ,y e ) is the center of the circle, and the radius is the tracking error allowable radius r t Inside the circle:

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

[0067] Tracking situation #10, guide the controlled ship to stop or reduce propulsion, keep the heading consistent with the tracked target, wait for the expected tracking point to catch up with the controlled ship, and enter tracking situation #0:

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

[0069] Tracking situation #11, guide the controlled ship to sail from the side front to the side rear relative to the expected tracking point and enter tracking situation #9. The approach point is set at the origin of the body coordinate system (0,0) as the center of the circle, and the radius is 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] Among them, x a It indicates the horizontal coordinate of the approach point in the coordinate system of the tracked target, y a represents the ordinate of the approach point in the tracked target body coordinate system, sign represents the sign function, ψ t is the heading / course of the tracked target, ψ j is the bow direction of the controlled ship, k1 is the collision distance coefficient, and k2 is the safety distance coefficient.

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

[0073] η=[xyψ] T ,

[0074] v represents the velocity of the ship in the three-degree-of-freedom motion state space, It represents the speed of movement in the horizontal direction of the geodetic coordinate system. represents the velocity in the ordinate direction in the geodetic coordinate system, r represents the ship's bow angular velocity, x represents the abscissa of the ship in the geodetic coordinate system, y represents the ordinate of the ship in the geodetic coordinate system, ψ represents the ship's bow angle in the geodetic coordinate system, η represents the ship's three-degree-of-freedom motion state space, Represents the velocity in the ship's motion state space;

[0075] The acceleration is:

[0076]

[0077] represents the acceleration in the ship's motion state space, 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;

[0078] The propeller model is:

[0079]

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

[0081] Among them, T i is the propeller thrust, Q i is the propeller torque, ρ is the fluid density, D is the propeller diameter, α1, α2 are the propeller thrust characteristic parameters, β1, β2 are the propeller torque characteristic parameters, V ai Indicates propeller advance speed, V i represents the ship 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 is the rotor moment of inertia, L is the armature inductance, R a is the armature internal resistance, B is the damping coefficient, k e is the electromagnetic induction coefficient, k t is the electromagnetic torque coefficient, propeller torque Q i It can be obtained from the propeller model, ω i =2πn i , represents the rate of change of angular acceleration of thruster i, represents the angular acceleration of thruster i, ω i represents the angular velocity of propeller i; the thrust of each propeller T is obtained i With input voltage V si Ignoring the influence of the torque generated by the propeller rotation on general surface ships, the horizontal plane generalized control force generated by the propeller is:

[0085]

[0086] R i represents the horizontal distance between the propeller and the center of gravity of the ship, α irepresents the angle between the propeller and the x-axis of the body coordinate system, β i It represents the angle between the reverse direction of propeller thrust and the horizontal line connecting the propeller and the center of gravity. X is the longitudinal force, Y is the transverse force, and N is the bow moment.

[0087] The relationship between the propeller thrust and the generalized force is:

[0088] τ=ΠT

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

[0090] Generalized control:

[0091]

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

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

[0094] The beneficial effects achieved by the present invention are:

[0095] The ship tracking navigation planning method of this patent obtains surface target information in real time, dynamically adjusts the navigation path, and optimizes the selection of approach points 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, collision avoidance with the tracked target is taken into account during the planning process, thereby achieving safe tracking of moving surface targets.

[0096] Compared with the existing manned target tracking navigation, the method of this patent realizes unmanned autonomous planning of surface moving target tracking navigation, enhances the ship's autonomous task execution capability, and has good adaptability and flexibility; at the same time, compared with the traditional visual tracking method, the planning method of this patent can calculate the tracking distance and azimuth error in real time and correct it, which improves the tracking accuracy compared to manned visual tracking navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0097] Figure 1A schematic diagram of a flow chart of a method for tracking and navigating a water sports target provided by the present invention;

[0098] Figure 2 A schematic diagram of tracking task information in a method for tracking a moving target on water by a ship provided by the present invention;

[0099] Figure 3 A schematic diagram of a process of filtering out phase noise by Kalman filtering in a method for suppressing phase noise of a resonant device based on the Kalman filtering principle provided by the present invention;

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

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

[0102] Figure 6 A schematic diagram of a sinusoidal waveform interfered by phase noise in a resonant device phase noise suppression method based on the Kalman filtering principle provided by the present invention. DETAILED DESCRIPTION

[0103] The present invention will be further described below in conjunction with specific embodiments, and the advantages and features of the present invention will become clearer as the description proceeds. However, these embodiments are exemplary only and do not constitute any limitation to the scope of the present invention. It should be understood by those skilled in the art that the details and forms of the technical solution of the present invention may be modified or replaced without departing from the spirit and scope of the present invention, but these modifications and replacements all fall within the scope of protection of the present invention.

[0104] As attached Figure 1 As shown, the present invention provides a method for tracking and navigating a ship to a water moving target, comprising the following steps:

[0105] Step 1, reading the surface and underwater target information and tracking mission information around the controlled ship;

[0106] Targets considered include nearby ships, buoys, submersibles, reefs, etc. The data source is the output of the automatic identification system (AIS), radar, camera, sonar and other equipment in the ship sensor system, which is processed by the target fusion software to obtain the fusion situation information. The data of concern include the target's attributes, position, speed, size, etc.

[0107] like Figure 2As shown in the figure, on the basis of reading the target situation information, the tracking task information is read. The tracking task information includes the specified identification batch number of the tracked target, the tracking distance, the tracking relative azimuth, the tracking duration, etc. Among them, the identification batch number of the tracked target is the credential for the target tracking algorithm to distinguish and extract the information of the tracked target from the fused situation information. When the target tracking algorithm reads the identification batch number, it traverses the target batch numbers in the fused situation information. If the input identification batch number is detected, the information of the target is extracted as the input during the operation of the tracking algorithm; the tracking distance r e is the expected distance between the controlled ship and the tracked target during the tracking process; the tracking relative azimuth angle θ e It is the relative angle between the line from the position of the tracked target to the expected tracking point of the controlled ship during the tracking process and the heading of the tracked target, ranging from (-180°, +180°]; the tracking duration controls the task duration of the entire tracking task. When the actual tracking execution time is greater than the given tracking duration of the task, the tracking task is considered to be completed and the tracking is stopped.

[0108] Step 2, calculating the tracking situation based on the surface and underwater target information and the tracking task information;

[0109] Step 21, find the tracked target in the target situation information according to the identification batch number of the tracked target, and read the latitude and longitude, speed and heading of the target; based on the above target information, according to the tracking distance and tracking relative azimuth in the tracking task information, the expected tracking point of the target tracking task at the current moment can be calculated; plan a route from the current position of the controlled ship to its tracking point, so that the controlled ship approaches the expected tracking point and remains near the tracking point, and it is considered that the target is being tracked; when the time changes, the target situation information is updated, and the latitude and longitude and speed and heading information of the tracked target are re-read, the expected tracking point is recalculated, and the control amount is updated, and this cycle is repeated until the tracking execution time reaches the time requirement in the task information or the tracking termination instruction is received.

[0110] like Figure 3 As shown, based on the above technical solution, in some cases, the straight line guidance from the current position of the controlled ship to the expected tracking point may cause a collision with the tracked target.

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

[0112] In the process of tracking situation identification, the target tracking algorithm first reads the position, heading and position of the tracked target and the controlled ship, and calculates the relative position (x rel ,y rel), the relative position of the expected tracking point relative to the tracked target (x e ,y e ), and the relative position of the controlled ship relative to the desired tracking point (x g ,y g ),in:

[0113]

[0114] θ rel Indicates the relative position of the currently controlled ship to the tracked target (heading is 0°).

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

[0116] Step 23, this patent classifies and discusses two situations: the tracking target point is located near the rear of the target ship and the tracking target point is located on both sides of the target ship.

[0117] Step 231, when θ e ∈[160°,180°]∪(-180°,-160°], the tracking target point is considered to be directly behind the target ship. At this time, the tracking situation is divided as follows:

[0118] 1. When At this time, the controlled ship is located near the expected tracking point, and it is considered that the controlled ship is in the normal tracking stage. It can achieve continuous tracking navigation by controlling the speed, which is recorded as tracking status #0;

[0119] 2. When |y rel |≤k1·(L t +L j ) and x rel >0, it is considered that the controlled ship is on the future route of the tracked target and there is a risk of collision, which is recorded as tracking situation #1;

[0120] 3. When |y rel |>k1·(L t +L j ) and x rel >0, it is considered that the controlled ship is in front of the tracked target, but not on its future route. If the controlled ship is stationary, it will not pose a collision risk to the tracked target, which is recorded as tracking status #2;

[0121] 4. When ((θ rek ∈[160°,180°]∪(-180°,-160°] or |y rel |≤k1·(L t+L j )) and x g <0, it is considered that the controlled ship is located within 40° behind the tracked target, and the controlled ship is located behind the expected tracking point in the 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, it is considered that the controlled ship is within 40° 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, it is considered that the controlled ship is located behind the tracked target at this time, which is recorded as tracking situation #5;

[0124] Step 232, when θ e ∈(-160°,+160°), it is considered that the tracking target point is located on both sides of the tracked target. First, determine the relative position of the controlled ship (x rel ,y rel ) and the expected tracking point (x e ,y e ) is on the same side of the tracked target. e ·θ rel >0, or (θ rel ∈[160°,180°]∪(-180°,-160°], then the above two points are considered to be on the same side, and there is no need to guide the controlled ship to bypass the tail of the tracked target. The side change judgment variable b is recorded as cs is 0; otherwise, it is considered that the controlled ship and the expected tracking point are on different sides of the tracked target, and it is necessary to guide the controlled ship to bypass the tail of the tracked target to the other side, and the side change judgment variable b is recorded. cs is 1. At this time, the tracking situation is divided as follows:

[0125] 1. When At this time, the controlled ship is located near the expected tracking point, and it is considered that the controlled ship is in the normal tracking stage. It can achieve continuous tracking navigation by controlling the speed, which is recorded as tracking status #0.

[0126] 2. When |y rel |≤k1·(L t +L j ) and x rel >0, it is considered that the controlled ship is on the future route of the tracked target and there is a risk of collision, which is recorded as tracking situation #6.

[0127] 3. When b cs =1 and x rel ≥0, it is considered that the controlled ship is in front 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 <0, it is considered that the controlled ship is behind the tracking target and needs to change sides, which is recorded as tracking situation #8.

[0129] 5. When b cs = 0 and x g <0, it is considered that there is no need to change sides at this time, and the controlled ship is behind the expected 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 considered that there is no need to change sides at this time, and the controlled ship is slightly ahead of the expected tracking point, which is recorded as tracking situation #10.

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

[0132] Among them, L t is the scale of the tracked target, L j is the length of the accused ship, k1 is the collision distance coefficient, r t is the tracking error tolerance radius, x set is a preset amount that is considered to be slightly ahead of the expected tracking point, and C is the offset distance coefficient.

[0133] Step 3: Based on the current tracking situation, calculate the approach point to the expected tracking point or the target point to be tracked (only for tracking situation #0) for guiding the unmanned ship to navigate under the current situation, recorded as (x a ,y a ), the approach point calculation strategy and calculation formula under each tracking situation are as follows:

[0134] Tracking situation #0. Guide the controlled ship to keep tracking, and the target point is the expected tracking point.

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

[0136] Tracking situation #1. Guide the controlled ship to leave the forward route area of ​​the tracked target as soon as possible and enter tracking situation #2. At this time, the approach point is set to the side and front of the tracked target, and the side of this approach point is selected as the side to which the current bow of the controlled ship is pointing.

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

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

[0139] Tracking situation #2: Guide the controlled ship to sail from the side and front of the tracked target to the side and rear to enter tracking situation #5. At this time, the set approach point is located at the side and rear of the tracked target and maintains a certain safety distance from the tracked target.

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

[0141] Tracking situation #3. Guide the controlled ship to catch up with the expected tracking point from a long distance behind the tracked target and enter tracking situation #0. At this time, the set approach point is located at the expected tracking point (x e ,y e ) is the center of the circle, and the radius is the tracking error allowable radius r t within the circle.

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

[0143] Tracking situation #4. Guide the controlled ship to stop or reduce propulsion, keep the heading consistent with the tracked target, wait for the expected tracking point to catch up with the controlled ship, and enter tracking situation #0. At this time, the set approach point is located at the expected tracking point (x e ,y e ) is the center of the circle, and the radius is the tracking error allowable radius rt within the circle.

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

[0145] Tracking situation #5. Guide the controlled ship to sail from the side and rear of the tracked target to the rear of the desired tracking point and enter tracking situation #3. At this time, the approach point is set at the origin of the body coordinate system (0,0) as the center of the circle, and the radius is 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 ship to leave the forward route 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 and front of the side of the expected tracking point of the tracked target.

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

[0149] Tracking situation #7. Guide the controlled ship to sail from the side in front of the tracked target on a different side from the expected tracking point to the side behind, and enter tracking situation #8. At this time, the set approach point is located on the side behind the tracked target and maintains a certain safety distance from the tracked target.

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

[0151] Tracking situation #8. Guide the controlled ship to sail from the side and rear of the tracked target on a different side from the expected tracking point to directly behind the tracked target, and enter tracking situation #9. At this time, the set approach point is directly behind the tracked target and maintains a certain safety distance from the tracked target.

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

[0153] Tracking situation #9. Guide the controlled ship to sail from the side and rear of the same side of the tracked target as the expected tracking point to the expected tracking point and enter tracking situation #0. At this time, the set approach point is located at the expected tracking point (x e ,y e ) is the center of the circle, and the radius is the tracking error allowable radius r t within the circle.

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

[0155] Tracking situation #10. Guide the controlled ship to stop or reduce propulsion, keep the heading consistent with the tracked target, wait for the expected tracking point to catch up with the controlled ship, and enter tracking situation #0.

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

[0157] Tracking situation #11. Guide the controlled ship to sail from the side front to the side rear relative to the expected tracking point and enter tracking situation #9. At this time, the approach point is set at the origin of the body coordinate system (0,0) as the center of the circle, and the radius is 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] Among them, x a It indicates the horizontal coordinate of the approach point in the coordinate system of the tracked target, y a represents the ordinate of the approach point in the tracked target body coordinate system, sign represents the sign function, ψ t is the heading / course of the tracked target, ψ jis the bow direction of the controlled ship, k1 is the collision distance coefficient, and k2 is the safety distance coefficient.

[0160] Step 4: The approach point of the expected tracking point or the relative coordinates (x a ,y a ) can be converted into absolute longitude and latitude coordinates in the geodetic coordinate system (Lon a ,Lat a ), the absolute position of the current approach point can be obtained, and this position can be used for tracking navigation control of the controlled ship.

[0161] In the tracking navigation control stage, this patent first establishes a three-degree-of-freedom dynamic model of the controlled ship. This paper defines the symbols and meanings of each variable in the six-degree-of-freedom motion model of the controlled ship, 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 part of this patent assumes that the heave, roll and pitch motions are constrained, so the state variables of the controlled ship are defined as:

[0165]

[0166] v represents the velocity of the ship in the three-degree-of-freedom motion state space, It represents the speed of movement in the horizontal direction of the geodetic coordinate system. represents the velocity in the ordinate direction in the geodetic coordinate system, r represents the ship's bow angular velocity, x represents the abscissa of the ship in the geodetic coordinate system, y represents the ordinate of the ship in the geodetic coordinate system, ψ represents the ship's bow angle in the geodetic coordinate system, η represents the ship's three-degree-of-freedom motion state space, Represents the velocity in the ship's motion state space;

[0167] The motion control equation of the controlled ship is:

[0168]

[0169] Where M is 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, τ is the generalized control force, τ=[XYN] T .

[0170] The mass matrix M of the controlled ship during motion includes the mass matrix M of the controlled ship itself RB The additional mass matrix M of the surrounding fluid A .

[0171]

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

[0173] m represents the mass of the ship, y G Indicates that the center of gravity of the ship is located in the body coordinate system position y coordinate, x G Indicates that the center of gravity of the ship is located in the x-coordinate of the body coordinate system, I zz represents the ship's moment of inertia around the z-axis, It represents the additional mass coefficient of the additional longitudinal surge force on the longitudinal acceleration, represents the additional mass coefficient of the lateral acceleration due to the sway force, It represents the additional mass coefficient of the bow moment to the bow angular acceleration;

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

[0175]

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

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

[0178]

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

[0180] For a symmetrical controlled ship, the generalized restoring force is:

[0181]

[0182] For this article, 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 the controlled ship with electric propulsion as an example, considering the motor and propeller model, the control voltage V s The controlled ship dynamics model is the input. The propeller model can be simplified to generate thrust and torque simultaneously when rotating, and their magnitudes are:

[0187]

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

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

[0190] Consider the motor model again, the control equation of the motor:

[0191]

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

[0193]

[0194] R i represents the horizontal distance between the propeller and the center of gravity of the ship, α i represents the angle between the propeller and the x-axis of the body coordinate system, β i It indicates the angle between the reverse direction of propeller thrust and the horizontal line connecting the propeller and the center of gravity;

[0195] Thus, the generalized force matrix Π is obtained, and the relationship between the thrust of the propeller and the generalized force is:

[0196] τ=ΠT. (30)

[0197] T represents the propeller thrust vector;

[0198] In order to maintain the effectiveness and stability of the controlled ship's navigation control and prevent control chattering, this patent uses the sliding mode method to establish a ship dynamics controller under some tracking situations. a ,y a ) is firstly established based on the current state variable η0=[X rel Y rel ψ abs ] T Go to η a =[X a Y a ψ a ] T The feasible Dubins curve requires that the minimum curvature radius of the curve shall not be less than the turning radius of the controlled ship; after discretizing the curve according to the time step, the expected motion trajectory of the controlled ship running this section of the 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 reaching law

[0203] Also because

[0204]

[0205] The generalized control force can be obtained:

[0206]

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

[0208] The above are only specific steps of the present invention and do not constitute any limitation to the protection scope of the present invention; any technical solutions formed by equivalent transformation or equivalent replacement shall fall within the protection scope of the present invention; some parts not elaborated in detail in the present invention belong to the known technologies of those skilled in the art.

Claims

1. A method for tracking and planning navigation of a ship for a water moving target, characterized in that: The tracking navigation planning method for a ship to a water moving target comprises the following steps: Step 1, obtaining surface and underwater target information and tracking mission information around the controlled ship; Step 2, calculating the tracking situation based on the surface and underwater target information and the tracking task information; Step 3, according to the tracking situation, calculate the approach point to the expected tracking point or the target point to be tracked for guiding the unmanned ship to navigate under the current situation; Step 4, tracking navigation control of the ship based on the approach point of the expected tracking point or the coordinates of the target point to be tracked.

2. The method for tracking and navigating a ship to a water sports target according to claim 1, characterized in that: In step 1, the tracking task information includes the specified tracking target identification batch number, tracking distance, tracking relative azimuth and tracking duration; The identification batch number of the tracked target is the basis for the target tracking algorithm to distinguish and extract the tracked target information from the fused situation information; Tracking distance is the expected distance between the controlled ship and the tracked target during the tracking process; The tracking relative azimuth is the relative angle between the line connecting the position of the tracked target to the expected tracking point of the controlled ship during the tracking process and the heading of the tracked target, ranging from (-180°, +180°]; The tracking duration controls the duration of the entire tracking task. When the actual tracking execution time is longer than the given tracking duration of the task, the tracking task is considered to be completed and the tracking is stopped.

3. The method for tracking and navigating a ship to a water sports target according to claim 1, characterized in that: In step 2, the following steps are also included: Step 21, find the tracked target in the target situation information according to the tracked target identification batch number, and read the latitude and longitude, speed, and heading of the target; According to the tracking distance and the tracking relative azimuth in the tracking task information, the expected tracking point of the target tracking task at the current moment is calculated; Planning a route from the current position of the controlled ship to its tracking point so that the controlled ship approaches the desired tracking point and remains near the tracking point, and is considered to be tracking the target; When the time changes and the target situation information is updated, the latitude and longitude and speed and heading information of the tracked target are re-read, the expected tracking point is recalculated, and the control amount is updated; Step 22, the approach tracking method based on tracking situation recognition guides the controlled ship to approach the desired tracking point in a path that will not collide with the tracked target; Step 23, the tracking situation is calculated by taking 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 cases.

4. The method for tracking and navigating a ship to a water sports target according to claim 3, characterized in that: In step 22, the target tracking algorithm first reads the position, heading and position of the tracked target and the controlled ship, and calculates the relative position (x rel ,y rel ), the relative position of the expected tracking point relative to the tracked target (x e ,y e ), and the relative position of the controlled ship relative to the desired tracking point (x g ,y g ),in: θ rel Indicates the relative position of the currently controlled ship to the tracked target, r e represents the tracking distance, θ e Represents the tracking relative azimuth.

5. The method for tracking and navigating a ship to a water sports target according to claim 4, characterized in that: In step 23, when θ e ∈[160°,180°]∪(-180°,-160°], the tracking 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 in the normal tracking phase and is achieving continuous tracking navigation by controlling the speed; Tracking Situation #1: When |y rel |≤k1·(L t +L j ) and X rel >0, the controlled ship is on the future route of the tracked target and there is a risk of collision; Tracking Situation #2: When |y rel |>k1·(L t +L j ) and X rel >0, the controlled ship is in front of the tracked target. If the controlled ship is stationary, there is no collision risk to the tracked target; Tracking Situation #3: When ((θ rel ∈[160°,180°]∪(-180°,-160°] or |y rel |≤k1·(L t +L j )) and x g <0, the controlled ship is located within 40° behind the tracked target, and the controlled ship is behind the expected tracking point in the target body coordinate system; Tracking Situation #4: When ((θ rel ∈[160°,180°]∪(-180°,-160°] or |y rel |≤k1·(L t +L j )) and x g >0, the controlled ship is within 40° behind the tracked target and is ahead of the expected tracking point; Tracking situation #5: Other situations, the controlled ship is located to the side and rear of the tracked target; Among them, L t is the scale of the tracked target, L j is the length of the accused ship, k1 is the collision distance coefficient, r t is the tracking error tolerance radius.

6. The method for tracking and navigating a ship to a water sports target according to claim 3, characterized in that: In step 23, when θ e ∈(-160°,+160°), it is considered that the tracking target point is located on both sides of the tracked target, and the tracking situation is divided as follows: Tracking Situation #0: When The controlled ship is located near the expected tracking point and is considered to be in the normal tracking stage. The controlled ship can achieve continuous tracking navigation by controlling the speed. Tracking Situation #6: When |y rel |≤k1·(L t +L j ) and X rel >0, the controlled ship is on the future route of the tracked target and there is a risk of collision; Tracking Situation #7: When b cs =1 and x rel ≥0, the controlled ship is in front of the tracking target and needs to change sides; Tracking Situation #8: When b cs =1 and x rel <0, the controlled ship is behind the tracking target and needs to change sides; Tracking Situation #9: When b cs = 0 and x g <0, the controlled ship is behind the expected tracking point and does not need to change side; Tracking Situation #10: When b cs =0 and (0 <x g ≤x ser And|y g |≤C·(r t )), the controlled vessel is slightly ahead of the expected tracking point and does not need to change sides; Tracking Situation #11: When b cs =0, the controlled ship is in front of the expected tracking point and does not need to change sides; Among them, L t is the scale of the tracked target, L j is the length of the accused ship, k1 is the collision distance coefficient, r t is the tracking error tolerance radius, x set is the preset amount that is considered to be slightly ahead of the expected tracking point, C is the offset distance coefficient, and b cs To judge the variable for switching sides.

7. The method for tracking and navigating a target on water by a ship according to claim 5, characterized in that: In step 3, the approach point calculation strategy and method under each tracking situation are as follows: Tracking situation #0, guide the controlled ship to keep tracking, the target point is the expected tracking point: (x a ,and a )=(x e ,and e ) Tracking situation #1, guide the controlled ship to leave the forward route area of ​​the tracked target as soon as possible and enter tracking situation #2. The approach point is set to the side and front of the tracked target, and the side of the approach point is selected as the side to which the controlled ship's current heading is pointing: ψ rel =ψ j -ψ t ,ψ rel ∈(-180°,+180°] (x a ,y a )=(x rel ,sign(ψ rel )·[k1·(L t +L j )+50]) Tracking situation #2, guide the controlled ship to sail from the side and front of the tracked target to the side and rear of the tracked target to enter tracking situation #5, the set approach point is located at the side and rear of the tracked target, and maintains a certain safe distance from the tracked target: (x a ,y a )=(-50,sign(y rel )·[k2·(L t +L j )+50]) Tracking situation #3, guide the controlled ship to catch up with the expected tracking point from a long distance behind the tracked target, enter tracking situation #0, and set the approach point to be at the expected tracking point (x e ,y e ) is the center of the circle, and the radius is the tracking error allowable radius r t Inside the circle: (x a ,y a )=(x e -0.5·r t ,y e ) Tracking situation #4, guide the controlled ship to stop or reduce propulsion, keep the heading consistent with the tracked target, wait for the expected tracking point to catch up with the controlled ship, enter tracking situation #0, and set the approach point at the expected tracking point (x e ,y e ) is the center of the circle, and the radius is the tracking error allowable radius r t Inside the circle: (x a ,and a )=(x rel -5,and e ) Tracking situation #5, guide the controlled ship to sail from the side and rear of the tracked target to the rear of the desired tracking point, enter tracking situation #3, and set the approach point to be located at the origin of the body coordinate system (0,0) as the center of the circle, with a radius of 1.2 times the tracking distance r e On the circle: (x a ,y a )=(1.2r e ·sinθ e ,1.2r e ·cosθ e ) Among them, x a It indicates the horizontal coordinate of the approach point in the coordinate system of the tracked target, y a represents the ordinate of the approach point in the tracked target body coordinate system, sign represents the sign function, L t is the scale of the tracked target, L j is the captain of the accused vessel, ψ t is the heading / course of the tracked target, ψ j is the bow direction of the controlled ship, k1 is the collision distance coefficient, and k2 is the safety distance coefficient.

8. The method for tracking and navigating a ship to a water sports target according to claim 6, characterized in that: In step 3, the approach point calculation strategy and method under each tracking situation are as follows: Tracking situation #0, guide the controlled ship to keep tracking, the target point is the expected tracking point: (x a ,and a )=(x e ,and e ) Tracking situation #6, guide the controlled ship to leave the forward route area of ​​the tracked target as soon as possible, and enter tracking situation #11. The approach point is set to the side and front of the side of the expected tracking point of the tracked target: (x a ,y a )=(x rel ,sign(y e )·[k1·(L t +L j )+50]) Tracking situation #7, guide the controlled ship to sail from the side in front of the tracked target on a different side from the expected tracking point to the side behind, and enter tracking situation #8, the set approach point is located behind the tracked target and maintains a certain safety distance from the tracked target: (x a ,y a )=(-50,sign(y rel )·[k2·(L t +L j )+50]) Tracking situation #8, guide the controlled ship to sail from the side and rear of the tracked target on a different side from the expected tracking point to the rear of the tracked target, and enter tracking situation #9, the set approach point is located directly behind the tracked target and maintains a certain safety distance from the tracked target: (x a ,y a )=(min{-[k2·(L t +L j )+50],x e -20},0) Tracking situation #9, guide the controlled ship to sail from the side and rear of the same side of the tracked target and the expected tracking point to the expected tracking point, enter tracking situation #0, and the set approach point is located at the expected tracking point (x e ,y e ) is the center of the circle, and the radius is the tracking error allowable radius r t Inside the circle: (x a ,y a )=(x e -0.5·r t ,y e ) Tracking situation #10, guide the controlled ship to stop or reduce propulsion, keep the heading consistent with the tracked target, wait for the expected tracking point to catch up with the controlled ship, and enter tracking situation #0: (x a ,and a )=(x rel -5,and e ) Tracking situation #11, guide the controlled ship to sail from the side front to the side rear relative to the expected tracking point and enter tracking situation #9. The approach point is set at the origin of the body coordinate system (0,0) as the center of the circle, and the radius is 1.2 times the tracking distance r e On the circle: (x a ,y a )=(1.2r e ·sinθ e ,1.2r e ·cosθ e ) Among them, x a It indicates the horizontal coordinate of the approach point in the coordinate system of the tracked target, y a represents the ordinate of the approach point in the tracked target body coordinate system, sign represents the sign function, ψ t is the heading / course of the tracked target, ψ j is the bow direction of the controlled ship, k1 is the collision distance coefficient, and k2 is the safety distance coefficient.

9. The method for tracking and navigating a target on water by a ship according to claim 1, characterized in that: In step 4, define the state variables of the controlled ship: η=[xyψ] T , v represents the velocity of the ship in the three-degree-of-freedom motion state space, It represents the speed of movement in the horizontal direction of the geodetic coordinate system. represents the velocity in the ordinate direction in the geodetic coordinate system, r represents the ship's bow angular velocity, x represents the abscissa of the ship in the geodetic coordinate system, y represents the ordinate of the ship in the geodetic coordinate system, ψ represents the ship's bow angle in the geodetic coordinate system, η represents the ship's three-degree-of-freedom motion state space, Represents the velocity in the ship's motion state space; The acceleration is: represents the acceleration in the ship's motion state space, 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; The propeller model is: V ai =(1-w)V i Among them, T i is the propeller thrust, Q i is the propeller torque, ρ is the fluid density, D is the propeller diameter, α1, α2 are the propeller thrust characteristic parameters, β1, β2 are the propeller torque characteristic parameters, V ai Indicates propeller advance speed, V i represents the ship speed, w is the wake coefficient, and i is the propeller number; Considering the motor model again, the control equation of the motor is: Among them, J z is the rotor moment of inertia, L is the armature inductance, R a is the armature internal resistance, B is the damping coefficient, k e is the electromagnetic induction coefficient, k t is the electromagnetic torque coefficient, propeller torque Q i It can be obtained from the propeller model, ω i =2πn i , represents the rate of change of angular acceleration of thruster i, represents the angular acceleration of thruster i, ω i represents the angular velocity of propeller i; the thrust of each propeller T is obtained i With input voltage V si Ignoring the influence of the torque generated by the propeller rotation on general surface ships, the horizontal plane generalized control force generated by the propeller is: R i represents the horizontal distance between the propeller and the center of gravity of the ship, α i represents the angle between the propeller and the x-axis of the body coordinate system, β i It represents the angle between the reverse direction of propeller thrust and the horizontal line connecting the propeller and the center of gravity. X is the longitudinal force, Y is the transverse force, and N is the bow moment. The relationship between the propeller thrust and the generalized force is: τ=ΠT Π is the generalized force matrix, T represents the propeller thrust vector; Generalized control: τ c is the generalized control force, M is the mass matrix, J is the coordinate transformation matrix between the earth coordinates and the body coordinates, C is the offset distance coefficient, η d is the expected ship tracking motion navigation point, that is, the expected tracking point / approach point at the current moment, e = η d -η is the error term, is the first-order derivative of e with respect to time, is the second-order derivative of the desired ship tracking motion navigation point with respect to time, k is the control parameter of the sliding mode method, Based on the ship's state variables, propeller model, motor control equations, the speed of each propeller, the relationship between propeller thrust and generalized force, and the generalized control force, a control system is obtained which uses propeller motor voltage as input to control the ship's tracking navigation.

Images