A ship track control method based on variable look-ahead distance (LOS) algorithm

Abstract

The application relates to a ship track control method based on a variable look-ahead distance LOS algorithm, which comprises the following steps: a ship tracks a target track generated by a series of track points, and position and motion information of the ship is obtained according to a navigation and sensing module arranged on the ship; a LOS algorithm based on a curvature variable look-ahead distance is designed; a speed control rate is designed according to the target track and the position of the ship, so that expected speed required by the ship at different positions is obtained, the look-ahead distance is updated based on the curvature of the track point, the expected heading angle is calculated in combination with the look-ahead distance, so that the error of the current speed and heading is obtained and is input into a PID controller; the PID controller is designed in combination with fuzzy processing, and the power parameters of the ship are updated; the process is iterated repeatedly, and finally the ship is controlled to travel along the target track. The application is designed based on the curvature variable look-ahead distance, and a time-varying expected longitudinal speed is set, so that the rapidity and stability of track tracking for various curvature tracks are ensured.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of ship track control, and particularly relates to a ship track control method based on a variable look-ahead distance LOS algorithm. BACKGROUND

[0002] As the most important waterway transportation tool, ships not only play an important role in ocean transportation and national defense strength, but also occupy an important position in the field of national economy and ocean development. In recent years, unmanned ship technology has developed rapidly and plays an increasingly important role in many fields such as ocean mapping, water sampling, environmental monitoring, hydrological detection, and marine search and rescue. As a new technology, unmanned ships cover a wide range of technical fields and have a wide range of development space and available scenarios.

[0003] In recent years, research teams have developed and utilized unmanned ships in various ways. By combining ship, communication, electronics, automation, big data, remote monitoring, environmental science, and robot systems, they have achieved various complex functions such as ship navigation, positioning, autonomous path planning, networked monitoring, path tracking, and intelligent collision avoidance. With the increasing intelligence and automation of ships in recent years, the motion control of unmanned ships in this field mainly includes speed and heading control, track control, intelligent planning, obstacle avoidance and navigation, and formation coordination. Track control is a prerequisite for unmanned ships to complete tasks autonomously and is one of the core of intelligent control of unmanned ships.

[0004] Track control refers to the process of a ship moving forward along a predetermined path at a certain longitudinal speed during operation or navigation. Track control is mainly used in oil pipeline laying and maintenance, dredger operations, and other applications. Ship track control technology has the advantages of high precision, no influence from subjective factors, and safety and reliability. As the work to be carried out at sea becomes increasingly complex and the demand for manpower increases, ship track control has become an inevitable trend in the 21st century. As a core technology of ship intelligence, ship track control is the foundation of automatic ship travel. It can not only reduce the labor intensity of the crew, but also reduce the occurrence of off-course and wrong-course situations, thereby improving the speed of navigation, reducing the time of navigation, improving the economic benefits of navigation, and greatly improving the safety of navigation.

[0005] The main methods of unmanned ship path control include line-of-sight (LOS) algorithm, backstepping sliding mode technology, cascade PID, neural network control, robust adaptive control and fuzzy control. At present, the most widely used is the LOS path control algorithm. In recent years, the LOS algorithm has developed rapidly and has been greatly improved. However, when tracking a curved path, the same set of parameters (look-ahead distance, longitudinal velocity, etc.) is often used for path tracking, so the look-ahead distance and longitudinal velocity used in the case of large curvature may cause the ship to have difficulty in adjusting the attitude to complete the turning operation in time, resulting in low tracking accuracy and low driving efficiency. At the same time, the traditional LOS algorithm uses a constant longitudinal velocity to approach the path, which may cause the ship to deviate from the path for a long time and reduce the economic benefit. SUMMARY

[0006] The technical problem to be solved by the present application is to provide a ship path control method based on a variable look-ahead distance LOS algorithm, which addresses the deficiencies of the prior art. The method is designed based on a curvature variable look-ahead distance and sets a time-varying expected longitudinal velocity to ensure the rapidity and stability of tracking various curvature paths. In combination with a PID control system and fuzzy processing, the method achieves good control effect, flexibility and stability in complex marine environments.

[0007] The technical solution adopted by the present application to solve the above technical problems is as follows:

[0008] A ship path control method based on a variable look-ahead distance LOS algorithm, comprising the following steps:

[0009] Step 1: The ship tracks a target path generated by a series of path points, and obtains the position and motion information of the ship according to the navigation and sensing modules equipped on the ship.

[0010] Step 2: Design a LOS algorithm based on a curvature variable look-ahead distance: design a speed control rate according to the target path and the position of the ship, so as to obtain the expected speed required by the ship at different positions, update the look-ahead distance based on the curvature of the path point, and calculate the expected heading angle based on the look-ahead distance to obtain the error of the current speed and heading, and input it into the PID controller.

[0011] The LOS algorithm based on a curvature variable look-ahead distance specifically comprises the following steps:

[0012] Step 21: Divide the geometric model of the ship during path tracking into a fast approximation zone and a curve tracking zone.

[0013] Step 22: In the fast approximation zone, design a speed control rate to update the expected speed so that the ship can quickly approach the path.

[0014] Step 23, in the curve tracking area, first calculate the curvature of the currently tracked track point, update the look-ahead distance according to the curvature, and then calculate the expected heading angle;

[0015] Step 3, design a PID controller combined with fuzzy processing, update the ship's power parameters;

[0016] Step 4, according to the update of the ship's power state in step 3, the ship's motion state changes, return to step 1 to obtain the current position and motion parameters of the ship, and then use the LOS algorithm in step 2 to obtain the speed and heading error again, input into the PID controller in step 3, and repeatedly iterate until the ship is finally controlled to travel along the target track.

[0017] In the above scheme, step 1 specifically includes the following steps:

[0018] The target track followed by the ship is composed of a series of track points, assuming P i (x i ,y i ), P i+1 (x i+1 ,y i+1 ) are two adjacent track points, i represents the i-th track point of the target track, and P i is considered as the current target track point. In the tracking process, the position information (x, y) of the ship at this time, the speed u and the heading

[0019] In the above scheme, in step 22, the following variable speed design is performed to enable the ship to quickly approach the track:

[0020]

[0021] Where u LOS is the expected speed of the ship; y e represents the lateral deviation of the ship at the current position to the target track point P i ; y set is the maximum lateral distance to the target track point, y min is the lateral distance to the target track point at the intersection of the two track areas, u max and u min are two threshold values of the longitudinal speed of the ship, and γ>0 represents the convergence rate of the speed change.

[0022] In the above scheme, the expected heading angle is: is the angle between the target track and the Y-axis of the geodetic coordinate system, and has:

[0023]

[0024] where y=f(x) is the target track curve equation, denotes the first derivative of the target track curve;

[0025] is the intermediate heading angle calculated according to the look-ahead distance, y e denotes the lateral deviation of the ship at the current position to the target track point, and the lateral deviation y e , longitudinal deviation x e is defined as:

[0026]

[0027] where x, y are the coordinates of the ship; x i , y i are the coordinates of the target track point P i ;

[0028] Δ is the look-ahead distance, which refers to a distance of the vertical projection point Q of the ship on the current target track line to the forward extension, and the extension is to the desired track point P LOS . In the improved LOS guidance law, Δ is a variable adjusted according to the curvature at the target track point on the target track.

[0029] In the above scheme, the curvature at the target track point on the target track is K, and the change formula of the look-ahead distance Δ with the curvature is:

[0030]

[0031] where Δ max , Δ min are the set maximum and minimum threshold values, e is the natural constant, and λ>0 represents the convergence speed of the change law.

[0032] In the above scheme, the calculation method of the curvature K at the target track point on the target track is as follows:

[0033] where y=f(x) is the target track curve equation, and there is a corresponding curvature circle at a target track point on the track, and the curvature radius is r. The first and second derivatives of the curve at a point are known, and the curvature circle radius at the point can be calculated:

[0034]

[0035] After obtaining the curvature circle radius, for the target track point P i (x i ,y i ) tracked on the current curve, the curvature K i of the corresponding target track point is:

[0036]

[0037] The corresponding forward distance Δ can be calculated by substituting formula (6) into formula (4).

[0038] In the above scheme, step 3 specifically comprises the following steps:

[0039] Step 31, based on the position PID control principle, a control equation is designed, and initial parameters are determined according to simulation or empirical formula to obtain a preliminary controller of the ship power parameter:

[0040]

[0041] In the formula, out(k) is the controller output, e(k) is the current error, j is the iteration number, k is the current iteration number, The cumulative error sum from the 0th to the kth iteration is represented as e(k-1), the previous error, T is the iteration period, K p is the proportional coefficient, K I is the integral coefficient, and K D is the differential coefficient.

[0042] Step 32, the preliminary controller is fuzzy processed by using a fuzzy controller combined with a database and a rule base;

[0043] Step 33, the controller gain coefficient is updated according to the fuzzy controller to obtain a final controller equation, and the ship power response parameter out(k) is output:

[0044]

[0045] In the formula, ΔK P , ΔK I , and ΔK D are the change amounts of K p , K I , and K D , respectively.

[0046] In the above scheme, step 4 specifically comprises the following steps:

[0047] Step 41, the power parameters including the rudder angle and the thrust of the ship are updated according to the PID controller output in step 3;

[0048] Step 42, the ship performs power response within the set iteration time, and returns to step 1 to update the position and motion information of the ship;

[0049] Step 43: Based on the updated position and motion information, first determine whether it is necessary to switch the tracking point. Use the improved LOS algorithm in step 2 to calculate the expected speed and expected heading angle at the next moment, then enter step 3 again for control, and loop steps 1-4 to control the ship to travel along the expected route.

[0050] In the above solution, in step 43, the basis for determining whether tracking point switching is required is as follows:

[0051] The next track point P of the current tracked target track point i+1 (x i+1 ,y i+1 ) is the center of the circle. Assuming a transition circle with a radius of R0, when the ship enters the range of this circle, it is considered that the ship has arrived at this track point and starts tracking this point. The conditions for reaching the vicinity of this point are:

[0052]

[0053] After the conditions are met, the operation of switching the track point is executed to make i=i+1, and then the expected heading angle corresponding to the next track point is calculated.

[0054] The beneficial effects of the present invention are:

[0055] 1. The traditional LOS method uses a fixed look-ahead distance. When dealing with different lateral errors and different curved tracks, it cannot guarantee that the tracking system will give a suitable expected heading angle, resulting in poor tracking effect. The method of the present invention uses a look-ahead distance that varies with the curvature in the curved tracking area, which can flexibly respond to tracks with different curvatures, obtain a reasonable expected heading angle, and improve the accuracy of track tracking.

[0056] 2. Design a fast approach area and flexibly adjust the desired speed according to the lateral error during track tracking, which reduces the time it takes for the ship to approach the track and improves the efficiency and reliability of track tracking.

[0057] 3. Taking into account the mutation and complexity of the marine environment, fuzzy processing is performed on the ship parameters, which greatly improves the practical ability of the PID controller, reduces the impact of environmental changes on track tracking, and improves the robustness and flexibility of the method. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] The present invention will be further described below with reference to the accompanying drawings and embodiments, in which:

[0059] Figure 1 Flowchart of the ship track control method based on the variable foresight distance LOS algorithm of the present invention;

[0060] Figure 2A step flow chart for the LOS algorithm with variable look-ahead distance in the embodiment of the present application;

[0061] Figure 3 A motion model representing the principle of the LOS guidance law in the embodiment of the present application;

[0062] Figure 4 A model for dividing the navigation area when the ship is navigating in the embodiment of the present application;

[0063] Figure 5 A ship guidance model when the target track is a curve with a relatively large curvature in the embodiment of the present application;

[0064] Figure 6 A ship guidance model when the target track is a curve with a relatively small curvature in the embodiment of the present application;

[0065] Figure 7 A position-based PID total control flow based on fuzzy control in the embodiment of the present application;

[0066] Figure 8 A flow for fuzzy processing of data based on fuzzy theory in the embodiment of the present application;

[0067] Figure 9 A multi-curvature track tracking simulation result graph in the embodiment of the present application;

[0068] Figure 10 A lateral error versus time graph of the simulation result in the embodiment of the present application. DETAILED DESCRIPTION

[0069] In order to have a clearer understanding of the technical features, objectives and effects of the present application, the specific embodiments of the present application will be described in detail with reference to the accompanying drawings.

[0070] As shown in Figure 1 , the present application proposes a ship track control method based on a LOS algorithm with variable look-ahead distance, including the following steps:

[0071] Step 1, the ship tracks a target track generated by a series of track points, and obtains the position and motion information of the ship according to the navigation and sensing modules equipped on the ship.

[0072] The target track tracked by the ship is composed of a series of track points, assuming P i (x i ,y i ), P i+1 (x i+1 ,y i+1 ) are two adjacent track points, i represents the i-th track point constituting the target track, and P iFor the current target track point, the position information (x, y) of the ship at this time, the speed u and the heading of the ship are obtained by the navigation and sensing equipment equipped on the ship during the tracking process

[0073] Step 2, design the LOS algorithm based on the curvature variable look-ahead distance: according to the target track and the position of the ship, the speed control rate is designed, so as to obtain the expected speed required by the ship at different positions, and the look-ahead distance is updated based on the curvature of the track point, and the expected heading angle is calculated combined with the look-ahead distance, so as to obtain the error of the speed and the heading at this time, which is input into the PID controller.

[0074] In order to make up for the lack of flexibility in tracking different curvature curves in the ship track tracking problem, and at the same time take into account the need for faster approach to the track when the lateral distance of the ship is too large, the improved LOS method is proposed, as shown in Figure 2 The LOS algorithm based on the curvature variable look-ahead distance specifically includes the following steps:

[0075] Step 21, the geometric model for track tracking of the ship is divided into a fast approximation area and a curve tracking area, as shown in Figure 3 .

[0076] Step 22, in the fast approximation area, the speed control rate is designed to update the expected speed, so that the ship can quickly approach the track. When in the fast approximation area, the ship is often unsafe to travel far from the target track, and cannot work normally, and long time in this area will cause great loss of economic benefit, therefore, in order to make the ship quickly approach the track in this area, the following variable speed design is carried out:

[0077]

[0078] Wherein, u LOS is the expected speed of the ship; y e represents the lateral deviation of the ship at the current position to the target track point P i ; y set is the maximum lateral distance to the target track point, y min is the lateral distance to the target track point at the junction of the two track areas, u max and u min are two threshold values of the longitudinal speed of the ship, γ>0, indicating the convergence rate of the speed change.

[0079] The variable speed design can make the ship keep high speed approaching when far away from the target track, and start reasonable deceleration at the appropriate position to ensure stable tracking when approaching the target track. According to the improved speed control rate, the expected speed required by the ship at different positions can be obtained, and after entering the curve tracking area, the ship keeps tracking at a suitable speed.

[0080] Step 23, in the curve tracking area, when the ship is close to the target track, firstly, the curvature of the currently tracked track point is calculated, the look-ahead distance is updated according to the curvature, and then the expected heading angle is calculated.

[0081] As shown in the formula (1), the expected heading angle is Figure 4 which can be expressed as: is the angle between the current target track and the Y axis of the geodetic coordinate system, and has:

[0082]

[0083] In the formula, the target track curve equation is y=f(x), which represents the first derivative of the target track curve.

[0084] is the intermediate heading angle calculated according to the look-ahead distance, and has: y e represents the lateral deviation of the ship to the target track point at the current position, and the lateral deviation y e , the longitudinal deviation x e is defined as:

[0085]

[0086] In the formula, x, y are the coordinates of the ship; x i , y i are the coordinates of the target track point P i ;

[0087] Δ is the look-ahead distance, which refers to a distance of the vertical projection point Q of the ship on the current target track line to the expected track point P LOS . In the traditional LOS algorithm, Δ is often taken as a certain value, while in the curve tracking, the curvature changes. In order to obtain good tracking effect, in the improved LOS guidance law of the present application, Δ is a variable adjusted according to the curvature of the target track point on the target track, and Δ value is adjusted according to the curvature of different track points, and the expected heading angle is solved.

[0088] Supposing that the curvature of the target track point on the target track is K, then the change formula of the look-ahead distance Δ with the curvature is:

[0089] Δ=(Δ max -Δ min )e -λK +Δ min (22)

[0090] In the formula, Δ max , Δ​min are the maximum and minimum thresholds set respectively, e is a natural constant, λ>0, which indicates the convergence speed of the change law.

[0091] The curvature K at the target track point on the target track is calculated as follows:

[0092] Assume that the target track curve equation is y = f(x). There is a corresponding curvature circle at a target track point on the track. Let its curvature radius be r. If the first and second order derivatives of the curve at a certain point are known, the radius of the curvature circle at this point can be calculated:

[0093]

[0094] After obtaining the radius of the curvature circle, for the target track point P currently tracked on the curve i (x i ,y i ), the curvature K corresponding to the target track point i Expressed as:

[0095]

[0096] Substituting formula (6) into formula (4) can calculate the corresponding foresight distance Δ.

[0097] See also Figure 5 For target tracks with large curvature, the improved LOS guidance rate of the present invention is used. At this time, the forward sight distance is small, and the tracking point is close to the target route that is changing direction sharply. This can reduce the control error under long-term control, improve the track control accuracy, and make the hull stay close to the route.

[0098] See also Figure 6 For target tracks with small curvature, the improved LOS guidance rate of the present invention is used. At this time, the forward sight distance is large and the approach trend is slowed down, which can ensure that the ship smoothly approaches the curve to avoid control overshoot caused by approaching the track too quickly, while also ensuring the control accuracy of track tracking.

[0099] Step 3: Design a PID controller in combination with fuzzy processing to update the power parameters of the ship.

[0100] Figure 7 This is the principle diagram of the PID controller process. The specific implementation steps are as follows:

[0101] Step 31: Design the control equation based on the position PID control principle, and determine the initial parameters according to simulation or empirical formulas to obtain a preliminary controller for the ship's power parameters:

[0102]

[0103] Wherein, out(k) is the controller output, e(k) is the current error, j is the iteration number, k is the current iteration number, represents the cumulative error from the 0th to the kth iteration, e(k-1) is the previous error, T is the iteration period, K p is the proportional coefficient, K I is the integral coefficient, K D is the differential coefficient;

[0104] Step 32, using the fuzzy controller to combine the database and the rule base to perform fuzzy processing on the preliminary controller.

[0105] As Figure 8 is the flowchart of the fuzzy controller, the fuzzy controller mainly converts the input accurate quantity into a fuzzy quantity, uses the knowledge base to determine the fuzzy quantity processing method, and the rule base is used to store the fuzzy control rules made according to expert knowledge or experience to determine the relationship between the output quantity and the input quantity.

[0106] The fuzzy controller takes the ship motion parameter error value e and its change rate ec as the input quantity, combines the corresponding database and rule base to perform fuzzy reasoning and defuzzification, and finally outputs the change amount ΔK P , ΔK I , ΔK D .

[0107] Step 33, updating the controller gain coefficient according to the fuzzy controller to obtain the final controller equation and output the ship power response parameter out(k):

[0108]

[0109] Wherein, ΔK p , ΔK I , ΔK D are the change amounts of K P , K I , K D .

[0110] Step 4, updating the ship power state according to step 3, the ship motion state changes, returning to step 1 to obtain the current position and motion parameters of the ship, and using the LOS algorithm of step 2 to obtain the speed and heading error again, inputting into the PID controller of step 3, repeatedly iterating until the ship is finally controlled to travel along the target track. Specifically, the following implementation steps are included:

[0111] Step 41, updating the power parameters including the rudder angle and thrust of the ship according to the PID controller output in step 3;

[0112] Step 42, the ship performs power response in the set iteration time, returns to step 1, and updates the position and motion information of the ship;

[0113] Step 43, according to the updated position and motion information, firstly, it is judged whether the tracking point switching is needed, the expected speed and expected heading angle of the next time are calculated by the improved LOS algorithm in step 2, and then step 3 is entered again to control, and the steps 1-4 are cycled to control the ship to sail along the expected route.

[0114] The basis for judging whether the tracking point switching is needed is as follows:

[0115] The next track point P i+1 (x i+1 ,y i+1 ) of the current tracked target track point is taken as the center of a transition circle with a radius R0, when the ship sails into the range of the circle, the ship is considered to have reached the track point, and the tracking of the point is started, and the condition for reaching the vicinity of the point is satisfied:

[0116]

[0117] After the condition is satisfied, the operation of switching the track point is performed, i is set to i+1, and then the expected heading angle corresponding to the next track point is calculated.

[0118] Figures 9-10 The track control simulation performed according to the method of the present application is shown in Fig. 4. Figure 9 As shown in the figure, the ship can effectively track the target route with continuous change of curvature, and the effectiveness of the variable forward distance LOS algorithm in track control for different curvature routes is verified. Figure 10 The change of the lateral deviation with the running time is shown in Fig. 5, and it can be seen that the ship can be quickly guided to approach the expected route in the initial stage according to the present application; the maximum lateral error in the whole tracking process appears at x=100m, i.e. the turning bow with the maximum curvature, and is only 1.2m. In summary, the improved LOS guidance law is further proved to be reasonable and effective.

[0119] The embodiments of the present application are described above in combination with the drawings, but the present application is not limited to the above specific embodiments, and the above specific embodiments are only illustrative but not limiting, and those skilled in the art can make many forms under the inspiration of the present application without departing from the purpose of the present application and the scope protected by the claims, and these all belong to the protection of the present application.

Claims

1. A ship track control method based on variable foresight distance LOS algorithm, characterized in that: The following steps are involved: Step 1: The ship tracks the target track generated by a series of track points and obtains the ship's position and motion information based on the navigation and sensor modules equipped on the ship; Step 2: Design an LOS algorithm based on curvature-variable foresight distance: Design a speed control rate based on the target track and ship position to obtain the desired speed required by the ship at different positions. Update the foresight distance based on the track point curvature and calculate the desired heading angle based on the foresight distance to obtain the speed and heading errors at this time, which are input into the PID controller. The LOS algorithm based on curvature-variable foresight distance specifically includes the following steps: Step 21: Divide the geometric model of the ship during track tracking into a fast approach area and a curve tracking area; Step 22: In the fast approaching area, design the speed control rate and update the desired speed so that the ship can quickly approach the track; Step 23: In the curve tracking area, first calculate the curvature of the currently tracked track point, and update the foresight distance based on the curvature to calculate the desired heading angle; Step 3: Design a PID controller in combination with fuzzy processing to update the power parameters of the ship; specifically, the following steps are included: Step 31: Design the control equation based on the position PID control principle, and determine the initial parameters according to simulation or empirical formulas to obtain a preliminary controller for the ship's power parameters: (7) Where out(k) is the controller output, e(k) is the current error, j is the iteration number, k is the current iteration number, represents the cumulative error sum from the 0th to the kth iteration, is the previous error, T is the iteration period, is the proportionality coefficient, is the integration coefficient, is the differential coefficient; Step 32: fuzzy processing is performed on the preliminary controller using the fuzzy controller in combination with the database and the rule base; Step 33: Update the controller gain coefficient according to the fuzzy controller to obtain the final controller equation and output the ship dynamic response parameters : (8) Where, 、 、 They are 、 、 The amount of change; Step 4: Update the ship's power state according to step 3. The ship's motion state changes. Return to step 1 to obtain the ship's current position and motion parameters. Use the LOS algorithm in step 2 again to obtain the speed and heading errors. Input them into the PID controller in step 3, iterate repeatedly, and finally control the ship to travel along the target track.

2. The ship track control method based on variable foresight distance LOS algorithm according to claim 1 is characterized in that: Step 1 specifically includes the following steps: The target track tracked by the ship consists of a series of track points. Assume 、 are two adjacent track points, Indicates the first track points, and consider The current target track point, during the tracking process, the navigation and sensor equipment equipped by the ship obtains the ship's current position information (x, y), speed and heading .

3. The ship track control method based on variable foresight distance LOS algorithm according to claim 1 is characterized in that: In step 22, the following speed change design is performed to enable the ship to quickly approach the track: (1) in, is the expected speed of the ship; Indicates the ship's current position relative to the target track point lateral deviation; is the maximum lateral distance to the target track point, is the lateral distance between the boundary of the two navigation areas and the target track point, and are two thresholds for the given longitudinal speed of the ship, >0, indicating the rate of convergence of velocity changes.

4. The ship track control method based on variable foresight distance LOS algorithm according to claim 1 is characterized in that: Desired heading angle for: , is the angle between the target track and the Y axis of the geodetic coordinate system, and is: (2) In the formula, the target track curve equation is , Represents the first derivative of the target track curve; is the intermediate heading angle calculated based on the foresight distance, , Indicates the lateral deviation of the ship from the target track point at the current position. Defines the lateral deviation , longitudinal deviation for: (3) Where, 、 are the coordinates of the ship respectively; 、 Target track points coordinates; Foresight distance refers to the distance from the vertical projection point Q of the ship on the current target track line to the expected track point. , in the improved LOS guidance law, is a variable adjusted for the curvature of the target track at the target track point.

5. The ship track control method based on variable foresight distance LOS algorithm according to claim 4 is characterized in that: Assume that the curvature of the target track point on the target track is K, then the forward distance The variation with curvature is: (4) Where, are the maximum and minimum thresholds set respectively, e is a natural constant, , which represents the convergence speed of the change law.

6. The ship track control method based on variable foresight distance LOS algorithm according to claim 5 is characterized in that: The curvature K at the target track point on the target track is calculated as follows: Assume that the target track curve equation is , there is a corresponding curvature circle at a target track point on the track, and its curvature radius is , given the first-order and second-order derivatives of the curve at a certain point, the radius of the curvature circle can be calculated: (5) After obtaining the radius of the curvature circle, for the target track point P currently tracked on the curve i , the curvature of the corresponding target track point is Expressed as: (6) Substituting formula (6) into formula (4) can calculate the corresponding foresight distance .

7. The ship track control method based on variable foresight distance LOS algorithm according to claim 1 is characterized in that: Step 4 specifically includes the following steps: Step 41: Update the power parameters including the rudder angle and thrust of the ship according to the output of the PID controller in step 3; Step 42: The ship performs dynamic response within the set iteration time, returns to step 1, and updates the position and motion information of the ship; Step 43: Based on the updated position and motion information, first determine whether it is necessary to switch the tracking point. Use the improved LOS algorithm in step 2 to calculate the expected speed and expected heading angle at the next moment, then enter step 3 again for control, and loop steps 1-4 to control the ship to travel along the expected route.

8. The ship track control method based on variable foresight distance LOS algorithm according to claim 7 is characterized in that: In step 43, whether the tracking point switching is required is determined based on the following: The next track point P of the current tracked target track point i+1 Assume that the center of the circle is When a ship enters the transition circle, it is considered that the ship has arrived at this track point and starts tracking this point. The conditions for reaching the vicinity of this point are: (9) After the conditions are met, execute the operation of switching the track point to make , and then find the expected heading angle corresponding to the next track point.

Images