Unmanned surface vehicle path tracking method based on improved line-of-sight navigation method and limiting amplitude PID

Abstract

The application provides an unmanned ship path tracking control method based on a mobile circle variable look-ahead distance LOS guidance combined with a PID course and speed controller. The mobile circle variable look-ahead distance LOS guidance method changes the look-ahead distance according to the relative position relationship between the mobile circle and the expected path. When the lateral error of the unmanned ship and the expected path is large, the smaller look-ahead distance enables the unmanned ship to quickly approach the expected path. Conversely, when the lateral error of the unmanned ship and the expected path is small, the larger look-ahead distance can ensure that the unmanned ship stably and non-oscillatingly tracks the expected path. Finally, a limiting PI controller is used to regulate the basic control quantity of the propeller to realize speed control, and another limiting PI controller is used to regulate the control deviation of the left and right propellers, so as to realize differential steering. The radius of the mobile circle should be adjusted according to the experiment, and theoretically, it is slightly larger than the fixed look-ahead distance, so as to obtain a better anti-oscillation effect.

Description

TECHNICAL FIELD

[0001] The application relates to a line-of-sight navigation method and an unmanned ship path tracking technology and belongs to the navigation technical field. BACKGROUND

[0002] The navigation principle of the line-of-sight method is embodied in its intuitive understanding of the steering of a pilot and ship movement. The principle considers that if the heading of a controlled ship is kept aligned with a line-of-sight angle (LOS angle), the controlled ship can reach a desired position and achieve the effect of track following through proper control. Moreover, the LOS algorithm can reduce the traditional control quantity from three degrees of freedom of ship position and heading angle to two degrees of freedom of ship heading angle and sailing speed, which is particularly important for the control of under-actuated ships. The control method of the line-of-sight navigation is to guide an unmanned surface vehicle to track a target navigation point by selecting a suitable navigation point on a target path, so that the unmanned surface vehicle sails along the target path finally.

[0003] In engineering practice, the most widely used regulator control law is proportional, integral, derivative control, referred to as PID control, also known as PID regulation. PID controller has been in existence for nearly 70 years, and it has become one of the main technologies of industrial control because of its simple structure, good stability, reliable operation and convenient adjustment. When the structure and parameters of the controlled object cannot be completely mastered or an accurate mathematical model cannot be obtained, other control technologies are difficult to use, and the structure and parameters of the system controller must be determined by experience and on-site debugging. At this time, it is most convenient to apply PID control technology. Proportional control is the simplest control method. The output of the controller is proportional to the input error signal. When there is only proportional control, there is a steady-state error in the system output. In integral control, the output of the controller is proportional to the integral of the input error signal. For an automatic control system, if there is a steady-state error after entering the steady state, the control system is said to have a steady-state error or simply a difference system. In order to eliminate the steady-state error, an "integral term" must be introduced into the controller. The integral term depends on the integral of the error over time, and as time increases, the integral term will increase. Thus, even if the error is small, the integral term will increase with time, which will increase the output of the controller to further reduce the steady-state error to near zero. Therefore, the proportional + integral (PI) controller can make the system almost have no steady-state error after entering the steady state. In differential control, the output of the controller is proportional to the derivative of the input error signal (i.e. the rate of change of error). Automatic control systems may oscillate or even lose stability in the process of adjusting to overcome errors. The reason is that there are large inertia components (links) or delay components that have the effect of suppressing errors, and their changes always lag behind the changes in errors. The solution is to make the changes in the error suppression effect "ahead of time", that is, when the error is close to zero, the error suppression effect should be zero. This means that introducing only a "proportional" term in the controller is often not enough, and the proportional term only amplifies the amplitude of the error, and what needs to be added is the "derivative term", which can predict the trend of error change. In this way, the proportional + differential controller can make the control effect of suppressing errors equal to zero or even negative in advance, thereby avoiding serious overshoot of the controlled quantity. Therefore, for controlled objects with large inertia or delay, the proportional + differential (PD) controller can improve the dynamic characteristics of the system during the adjustment process. SUMMARY

[0004] TECHNICAL PROBLEM

[0005] In the traditional LOS guidance method, the look-ahead distance Δ is a constant value according to artificial experience. In the initial stage of path tracking, the vertical distance h (lateral tracking error) between the unmanned surface vehicle and the desired path is large, and the too large look-ahead distance Δ cannot ensure the unmanned surface vehicle to change direction quickly to approach the desired path, and the convergence is slow, thus leading to a large tracking error. When the unmanned surface vehicle approaches the desired path, the value of the lateral tracking error becomes small, and the too small look-ahead distance Δ leads to the frequent and unstable change of the heading of the unmanned surface vehicle, thus leading to the oscillation of the track on the left and right of the tracking path.

[0006] If the visible distance can be reasonably and continuously adjusted according to the actual motion state of the ship, the control effect will be greatly improved. The application provides an unmanned surface vehicle path tracking control method based on a mobile circle variable look-ahead distance LOS guidance combined with a PID heading and speed controller. The mobile circle variable look-ahead distance LOS guidance method changes the look-ahead distance according to the relative position relationship between the mobile circle and the desired path. When the lateral error between the unmanned surface vehicle and the desired path is large, the small look-ahead distance enables the unmanned surface vehicle to quickly approach the desired path, and vice versa. When the lateral error between the unmanned surface vehicle and the desired path is small, the large look-ahead distance can ensure that the unmanned surface vehicle stably tracks the desired path. Finally, the amplitude-limited PI controller is used to regulate the basic control quantity of the propeller to realize speed control, and another amplitude-limited PI controller is used to regulate the control deviation of the left and right propellers, so as to realize differential steering. The radius of the mobile circle should be adjusted according to experiments, and is slightly larger than the fixed look-ahead distance in theory, so as to obtain better anti-oscillation effect.

[0007] Technical scheme:

[0008] In the application, the following parameters are defined: A (x A , y A ) is the starting point of the target route; B (x B , y B ) is the end point of the target route; the current position of the unmanned surface vehicle is (x, y); h is the vertical distance between the unmanned surface vehicle and the target route, representing the lateral tracking error; α k is the inclination angle of the target route; α φ is the expected heading of the unmanned surface vehicle under the traditional LOS method; α φ ' is the improved expected heading; Δ is the look-ahead distance of the unmanned surface vehicle under the traditional LOS method, which is the distance between the projection point of the unmanned surface vehicle on the path and the navigation point; Δ' is the improved variable look-ahead distance; the heading angle of the unmanned surface vehicle is the angular velocity w, and the minimum turning radius R0.

[0009] Each variable can be calculated by the following formula:

[0010] α k = arctan2 (y B -yA x B -x A )

[0011] h = -(x - x A ) sin α k + (y - y A ) cos α k

[0012]

[0013] The improved variable look-ahead distance Δ' is calculated as follows:

[0014]

[0015] The improved desired heading α φ ' is obtained from the following equation:

[0016]

[0017] According to the LOS principle, by controlling the unmanned vehicle to adjust the heading angle to the desired heading, the lateral tracking error and the heading error tend to be zero. In addition, when the unmanned vehicle moves to the vicinity of the target point B, a new target straight line should be switched as the tracking path in the next stage.

[0018] When the lateral error of the unmanned vehicle to the desired path is large, i.e. h > R0, the moving circle has no intersection with the desired path, and the improved look-ahead distance is 0, i.e. the navigation point coincides with the projection point T0 of the unmanned vehicle on the desired path. At this time, the desired heading becomes the direction perpendicular to the desired path, and the unmanned vehicle will quickly adjust the heading to approach the desired path. The navigation point under the fixed look-ahead distance Δ is los0, the heading error is err0, the improved navigation point coincides with T0, and the heading error is err0'. Since err0' is much larger than err0, the control amount of changing the heading is larger, and the unmanned vehicle can approach the desired path more quickly.

[0019] As the lateral error of the unmanned vehicle to the desired path becomes smaller, i.e. h < R0, the moving circle has an intersection M1 with the desired path, and the improved look-ahead distance is the distance between the projection point T1 of the unmanned vehicle on the desired path and the intersection M1, i.e. the length of the line segment T1M1. The navigation point under the fixed look-ahead distance Δ is los1, the heading error is err1, the improved navigation point is M1, and the heading error is err1'. Since err1' is slightly smaller than err1, the unmanned vehicle is continuously guided to approach the path quickly, and the heading is adjusted to tend to the desired heading. As the unmanned vehicle gradually approaches the desired path, the improved look-ahead distance gradually becomes longer, the heading angle of the unmanned vehicle gradually tends to the desired heading, and the tendency of the unmanned vehicle to approach the desired path gradually slows down.

[0020] When the lateral error between the unmanned surface vessel and the desired path is sufficiently small, the length of the improved forward look-ahead distance is approximately equal to the radius of the moving circle and slightly larger than the fixed forward look-ahead distance. The navigation point under the fixed forward look-ahead distance Δ is los2, and the heading error is err2. The improved navigation point is M1, and the heading error is err2'. err2' is slightly smaller than err2, thus achieving a more stable angle adjustment and reducing the possibility of path oscillation.

[0021] For speed control, a limit-based PID control method is adopted. The calculation formula for the speed control quantity in the i-th step is as follows:

[0022] err v，i =v g -v r，i

[0023] I v，i =I v，i-1 +K v，I ×err v，i

[0024] du v，i =K v，P ×err v，i +I v，i

[0025] Among them, v g Expected speed, v r，i err v，i du v，i U 0，i These represent the unmanned surface vessel's speed, speed error, speed control deviation, and basic speed control quantity for the i-th step, respectively. v，i I v，i-1 These are the integral terms for the i-th step and the (i-1)-th step, respectively, and K v，I K is the coefficient for integral control. v，P Min is the coefficient for proportional control. v Max v These are the minimum speed control amount and the maximum speed control amount, respectively.

[0026] For heading control, a limit-limit PID control method is adopted. The formula for calculating the heading control quantity in the i-th step is as follows:

[0027]

[0028] I α，i =I α，i-1 +K α，I ×err α，i

[0029] du α，i =K α，P ×errα，i +I α，i

[0030]

[0031]

[0032] where, Δ i is the look-ahead distance of the i-th step, α k，i are the desired heading (based on improved LOS) and the straight path direction of the i-th step unmanned surface vehicle respectively, err α，i , du α，i , U 0，i are the i-th step unmanned surface vehicle heading, heading error, control deviation, speed basic control respectively, α，i , I α，i-1 are the i-th step, the i-1 step integral term respectively, K α，I is the integral control coefficient, K α，P is the proportional control coefficient, Min u , Max u are the minimum control, maximum control respectively, U L , U R are the left motor control, right motor control respectively.

[0033] Beneficial effects:

[0034] The present application is based on the variable look-ahead distance LOS guidance method of the moving circle, which changes the look-ahead distance according to the relative position relationship between the moving circle and the desired path. When the lateral error of the unmanned surface vehicle and the desired path is large, the smaller look-ahead distance enables the unmanned surface vehicle to approach the desired path faster. Conversely, when the lateral error of the unmanned surface vehicle and the desired path is small, the larger look-ahead distance can ensure that the unmanned surface vehicle tracks the desired path smoothly and without oscillation. Finally, the basic control amount of the propeller is regulated by a limiting PI controller to achieve speed control, and the control deviation of the left and right propellers is regulated by another limiting PI controller to achieve differential steering. The radius of the moving circle should be adjusted according to the experiment, which is slightly larger than the fixed look-ahead distance to obtain better anti-oscillation effect. BRIEF DESCRIPTION OF DRAWINGS

[0035] Figure 1 is a schematic diagram of the traditional LOS guidance method with too large look-ahead distance.

[0036] Figure 2 is a schematic diagram of the traditional LOS guidance method with too small look-ahead distance.

[0037] Figure 3 is a schematic diagram of the improved LOS (with large lateral error).

[0038] Figure 4 To improve the LOS (large lateral error) schematic diagram.

[0039] Figure 5 To improve the LOS (small lateral error) schematic diagram. DETAILED DESCRIPTION

[0040] The application will be further described in details in connection with the drawings and specific embodiments, but it should be understood that the protection scope of the application is not limited by the specific embodiments.

[0041] In the traditional LOS guidance method, the look-ahead distance Δ is a constant value according to artificial experience. In the initial stage of path tracking, the vertical distance h (lateral tracking error) between the unmanned ship and the desired path is large, and the too large look-ahead distance Δ cannot ensure the unmanned ship to change direction quickly to approach the desired path, and the convergence is slow, thus leading to a large tracking error, as shown in FIG. 1. Figure 1 When the unmanned ship approaches the desired path, the lateral tracking error value becomes small, and at this time, the too small look-ahead distance Δ will lead to frequent and unstable changes of the unmanned ship heading, thus leading to the oscillation of the track on the left and right of the tracking path, as shown in FIG. 2. Figure 2

[0042] If the visible distance can be reasonably and continuously adjusted according to the actual motion state of the ship, the control effect will be greatly improved. The application proposes an unmanned ship path tracking control method based on a mobile circle variable look-ahead distance LOS guidance combined with a PID heading and speed controller. The mobile circle variable look-ahead distance LOS guidance method changes the look-ahead distance according to the relative position relationship between the mobile circle and the desired path. When the lateral error between the unmanned ship and the desired path is large, the smaller look-ahead distance enables the unmanned ship to quickly approach the desired path, and vice versa. When the lateral error between the unmanned ship and the desired path is small, the larger look-ahead distance can ensure the unmanned ship to track the desired path stably without oscillation. Finally, the amplitude-limited PI controller is used to regulate the basic control quantity of the propeller to realize speed control, and another amplitude-limited PI controller is used to regulate the control deviation of the left and right propellers, so as to realize differential steering. The radius of the mobile circle should be adjusted according to the experiment, which is slightly larger than the fixed look-ahead distance in theory, so as to obtain better anti-oscillation effect.

[0043] In the application, the following parameters are defined: A (x A , y A ) is the starting point of the target route; B (x B , y B ) is the end point of the target route; the current position of the unmanned ship is (x, y); h is the vertical distance between the unmanned ship and the target route, representing the lateral tracking error; α k is the inclination angle of the target route; α φ is the desired heading of the unmanned ship under the traditional LOS method; α φ ​is the improved desired heading; Δ is the traditional LOS method's forward-looking distance of the unmanned surface vehicle, which is the distance between the projection point of the unmanned vehicle on the path and the navigation point; Δ' is the improved variable forward-looking distance; the heading angle of the unmanned vehicle Angular velocity w, minimum turning radius R0.

[0044] Each variable can be calculated by the following formula:

[0045] α k = arctan2 (y B -y A , x B -x A )

[0046] h = -(x-x A ) sin α k + (y-y A ) cos α k

[0047]

[0048] The improved variable forward-looking distance Δ' is calculated as follows:

[0049]

[0050] The improved desired heading α φ ' is obtained from the following formula:

[0051]

[0052] According to the LOS principle, by controlling the unmanned vehicle to adjust the heading angle to reach the desired heading, the lateral tracking error and the heading error tend to be 0. In addition, when the unmanned vehicle moves to the vicinity of the target point B, a new target straight line should be switched as the tracking path in the next stage.

[0053] When the lateral error of the unmanned vehicle and the desired path is large, i.e. h > R0, the moving circle has no intersection with the desired path, and the improved forward-looking distance is 0, i.e. the navigation point coincides with the projection point T0 of the unmanned vehicle on the desired path. At this time, the desired heading becomes the direction perpendicular to the desired path, and the unmanned vehicle will quickly adjust the heading to approach the desired path. As shown in Figure 3 , the navigation point under the fixed forward-looking distance Δ is los0, the heading error is err0, the improved navigation point coincides with T0, and the heading error is err0'. Since err0' is much larger than err0, the control amount of changing the heading can be larger, and the unmanned vehicle can approach the desired path more quickly.

[0054] As the lateral error between the unmanned vehicle and the desired path becomes smaller, i.e. h < R0, the moving circle intersects the desired path at M1, and the improved look-ahead distance is the distance between the projection point T1 of the unmanned vehicle on the desired path and the intersection point M1, i.e. the length of the line segment T1M1. As shown in Fig. 2, the navigation point under the fixed look-ahead distance Δ is los1, the heading error is err1, and the improved navigation point is M1, the heading error is err1', which is slightly smaller than err1, so as to continue to guide the unmanned vehicle to approach the path quickly, while adjusting the heading to the desired heading. As the unmanned vehicle gradually approaches the desired path, the improved look-ahead distance gradually becomes longer, the heading angle of the unmanned vehicle gradually approaches the desired heading, and the tendency of the unmanned vehicle to approach the desired path gradually slows down. Figure 4 As shown in Fig. 2, the navigation point under the fixed look-ahead distance Δ is los1, the heading error is err1, and the improved navigation point is M1, the heading error is err1', which is slightly smaller than err1, so as to continue to guide the unmanned vehicle to approach the path quickly, while adjusting the heading to the desired heading. As the unmanned vehicle gradually approaches the desired path, the improved look-ahead distance gradually becomes longer, the heading angle of the unmanned vehicle gradually approaches the desired heading, and the tendency of the unmanned vehicle to approach the desired path gradually slows down.

[0055] When the lateral error between the unmanned vehicle and the desired path is small enough, the length of the improved look-ahead distance is approximately equal to the length of the radius of the moving circle and slightly larger than the fixed look-ahead distance, as shown in Fig. 3. Figure 5 As shown in Fig. 3, the navigation point under the fixed look-ahead distance Δ is los2, the heading error is err2, and the improved navigation point is M1, the heading error is err2', which is slightly smaller than err2, so as to achieve a smooth angle adjustment and reduce the possibility of path oscillation.

[0056] For speed control, a limit PID control method is adopted, and the calculation formula of the speed control quantity of the i-th step is as follows:

[0057] err v，i = v g - v r，i I v，i = I v，i-1 + K v，I × err v，i

[0058] du v，i = K v，P × err v，i + I v，i

[0059]

[0060] wherein v g is the desired speed, v r，i , err v，i , du v，i , U 0，i are the unmanned vehicle speed, speed error, speed control quantity deviation, and speed basic control quantity of the i-th step, respectively, I v，i , I v，i-1 are the integral terms of the i-th step and the i-1-th step, respectively, K v，I is the coefficient of integral control, and K v，P is the coefficient of proportional control, Minv , Max v are minimum control amount and maximum control amount of speed respectively.

[0061] For the heading control, the amplitude-limited PID control method is adopted, and the calculation formula of the heading control amount of the i th step is as follows:

[0062]

[0063] I α，i = I α，i-1 + K α，I × err α，i

[0064] du α，i = K α，P × err α，i + I α，i

[0065]

[0066]

[0067] Wherein, Δ i is the look-ahead distance of the i th step, α k，i are the expected heading (based on improved LOS) and the direction of straight path of the i th step respectively, err α，i , du α，i , U 0，i are the heading, heading error, control amount deviation and speed basic control amount of the i th step respectively, I α，i , I α，i-1 are the integral terms of the i th step and the i-1 th step respectively, K α，I is the coefficient of integral control, K α，P is the coefficient of proportional control, Min u , Max u are minimum control amount and maximum control amount respectively, U L , U R are the left motor control amount and the right motor control amount respectively.

[0068] The technical means disclosed in the present application scheme is not limited to the technical means disclosed in the above-mentioned embodiments, and also includes the technical scheme composed of any combination of the above technical features. It should be pointed out that, for ordinary skilled in the art, without departing from the principle of the present application, a number of improvements and refinements can also be considered as the protection scope of the present application.

Claims

1. An unmanned surface vehicle path following method based on improved line-of-sight navigation method and amplitude-limited PID, characterized in that, Specifically comprising the following steps: S1: defining basic elements; S2: calculating real-time lateral error h; S3: calculating improved variable foresight distance Δ'; S4: Calculate the improved desired heading a φ '; S5: calculating the speed control amount of the i-th step based on the limiting PID; S6: transmitting the control amount to the unmanned ship motor drive; The step S1 of defining basic elements specifically is: A(x A , y A ) is the starting point of the target route; B(x B , y B ) is the end point of the target route; the current position of the unmanned ship is (x, y); h is the vertical distance between the unmanned ship and the target route, representing the lateral tracking error; α k is the inclination angle of the target route; α φ is the expected heading of the unmanned ship under the traditional LOS method; α φ ' is the improved expected heading; Δ is the forward-looking distance of the unmanned surface ship under the traditional LOS method, which is the distance between the projection point T of the unmanned ship on the path and the navigation point; Δ' is the improved variable forward-looking distance; the heading angle of the unmanned ship is the angular velocity w, and the minimum turning radius R0; The step S2 of calculating real-time lateral error specifically is: α k = arctan 2(y B -y A , x B -x A ) h = -(x - x A ) sin α k + (y - y A ) cos α k The step S3 of calculating improved variable foresight distance Δ' specifically is: The step S4 calculates the improved desired heading a φ Specifically:

2. The improved line-of-sight navigation method and limited amplitude PID based path tracking method for an unmanned surface vehicle according to claim 1, wherein, The step S5 of calculating the speed control amount of the i-th step based on the limiting PID specifically is: for speed control, the calculation formula of the speed control amount of the i-th step is as follows: err v,i = v g -v r，i I v,i = I v，i-1 + K v，I × err v，i du v，i = K v，P × err v，i + I v，i wherein v g desired speed, v r，i err v，i du v，i U 0，i are the i-th step speed of the unmanned surface vehicle, speed error, deviation of speed control amount, and speed basic control amount, respectively v，i I v,i-1 are the i-th step and i-1-th step integral terms, respectively v,I K v，P is the coefficient of integral control, K v Min v are the minimum control amount of speed and the maximum control amount of speed, respectively.

3. The improved line-of-sight navigation method and limited amplitude PID based path tracking method for an unmanned surface vehicle according to claim 2, wherein, The step S5 of calculating the heading control amount of the i-th step based on the limiting PID specifically is: for heading control, the calculation formula of the heading control amount of the i-th step is as follows: I α，i = I α,i-1 + K α，I × err α，i du α，i = K α，P × err α，i + I α，i wherein, Δ i is the look-ahead distance of the i-th step, α k，i is the desired heading of the i-th step and the straight path direction, respectively, err α，i , du α，i , U 0，i are the i-th step heading, heading error, control amount deviation, and speed basic control amount, respectively, α，i , K α，i-1 are the i-th step and the i-1 step integral term, respectively, α，I is the coefficient of integral control, L α，P is the coefficient of proportional control, Min u , Max u are the minimum control amount and the maximum control amount, respectively, L , U R are the left motor control amount and the right motor control amount, respectively.

Images