An underwater vehicle guidance method based on improved line-of-sight method

Abstract

The present application relates to underwater robot technology, aiming at providing a kind of underwater vehicle guidance method based on improved line-of-sight method. Including: AUV path tracking problem is divided into geometric tracking and direction tracking, traditional line-of-sight method guidance law is improved into target separation type line-of-sight method guidance law;Synthetically consider AUV path tracking accuracy, motion stability and motion energy consumption, construct performance index function, calculate the optimal control parameters of PID controller by optimizing performance index function, and output the force and torque required for motion control to driving mechanism;Based on the updated motion state data, repeat execution is implemented, and the guidance of AUV in the course of route is realized.The present application is simple in steps, low in cost, stable and reliable, and strong in robustness;The present application has wide application range, can be applied to the planar path tracking control of autonomous underwater vehicle of various shapes and various paths simultaneously, and can be extended to corresponding three-dimensional path tracking control.

Description

Technical Field

[0001] This invention relates to the field of underwater robot technology, and in particular to an underwater vehicle guidance method based on an improved line-of-sight method. Background Technology

[0002] In recent years, various intelligent equipment have played a significant role in marine development, including marine data observation and acquisition, seabed topography scanning, and subsea pipeline detection. These devices can adapt to complex and ever-changing marine environments and complete specific underwater tasks. Autonomous underwater vehicles (AUVs), with their unique advantages such as compact and simple structure, high flexibility, and superior control performance, have attracted widespread attention from researchers.

[0003] In the application of autonomous underwater vehicles (AUVs), high-precision path tracking control is required to complete specific tasks, making AUV path tracking control a research hotspot. Traditional guidance methods, such as the line-of-sight (LOS) guidance law and L1 guidance law, only consider the geometric error between the AUV's trajectory and the desired path, without taking into account factors such as AUV motion energy consumption and motion stability as objects of consideration for the control system. Furthermore, problems such as low tracking accuracy at path curves, strong uncertainty in control system model parameters, and poor robustness of PID guidance controllers lead to poor overall path tracking performance, increasing the difficulty of control.

[0004] Therefore, reducing AUV motion energy consumption, improving motion stability, enhancing the robustness of the control system, and improving AUV path tracking accuracy are of great research significance and application value. Summary of the Invention

[0005] The technical problem to be solved by the present invention is to overcome the shortcomings of the prior art and provide an underwater vehicle guidance method based on an improved line-of-sight method.

[0006] To solve the technical problem, the solution of the present invention is:

[0007] A guidance method for underwater vehicles based on an improved line-of-sight method is provided, comprising the following steps:

[0008] (1) The target separation line of sight (TSLOS) method is adopted to divide the AUV path tracking problem into two parts: geometric tracking and velocity direction tracking. Based on the motion state defined by the current coordinates and attitude of the AUV, a virtual tracking point is selected on the tracking path of the AUV. The virtual tracking point is used as the geometric tracking target of the AUV at the next moment, and the direction angle of the tracking path is used as the velocity direction tracking target of the AUV at the next moment.

[0009] (2) Based on the distance error, motion energy consumption, and the angle error between the velocity direction and the desired path during the AUV path tracking process, a performance index function is constructed by combining the control logic of the target separation line-of-sight guidance law.

[0010] (3) Calculate the optimal control parameters of the PID controller by optimizing the performance index function, and output the force and torque required for motion control to the AUV drive mechanism;

[0011] (4) Repeat steps (1)-(3) based on the updated AUV motion status data to achieve guidance of the underwater vehicle during the course of the route.

[0012] The present invention further provides an underwater vehicle guidance system based on an improved line-of-sight method, characterized in that it includes a TSLOS guidance module, a performance index function construction module, and a PID controller; which are respectively used to execute the contents of steps (1)-(3) in the aforementioned method.

[0013] Compared with the prior art, the beneficial effects of the present invention are:

[0014] 1. The present invention has simple steps and low cost.

[0015] This invention addresses the problems of high energy consumption, poor stability, and low path tracking accuracy at turns in traditional AUV guidance methods. It divides the AUV path tracking problem into geometric tracking and directional tracking, and improves the traditional line-of-sight guidance law into a target-separated line-of-sight guidance law. It also constructs a performance index function that comprehensively considers AUV path tracking accuracy, motion stability, and motion energy consumption.

[0016] 2. This invention is stable, reliable, and robust.

[0017] This invention addresses the problems of poor robustness and difficulty in determining parameters of traditional PID controllers as path tracking guiding laws by combining PID controllers with guiding laws. By introducing directional angle tracking, selecting tracking points and tracking angles as controlled objects, and optimizing performance index functions, the model parameters of the PID control system are calculated.

[0018] 3. This invention has a wide range of applications.

[0019] This invention can be applied to planar path tracking control of autonomous underwater vehicles of various shapes and paths, and can also be extended to corresponding three-dimensional path tracking control. Attached Figure Description

[0020] Figure 1 This is a schematic diagram of the system control flow of the method of the present invention.

[0021] Figure 2 This is a schematic diagram illustrating the principle of the Target Separation Line-of-Sight (TSLOS) guidance algorithm.

[0022] Figure 3 This is a schematic diagram illustrating the principle of the Target Separation Line-of-Sight Guidance (TSLOS) algorithm at path turning points. Detailed Implementation

[0023] First, it should be noted that this invention relates to underwater robot technology, hydrodynamic modeling technology, control technology, and path tracking technology. The applicant believes that, after carefully reading the application documents and accurately understanding the implementation principles and objectives of this invention, and in conjunction with existing known technologies, those skilled in the art can fully utilize their software programming skills and algorithm modifications to implement this invention. The implementation of this invention involves the application of underwater robots; underwater robots include, but are not limited to: underwater robots, underwater vehicles, submersibles, underwater vehicles, deep-sea submersibles, and underwater self-propelled vehicles; optimization methods for performance index functions include, but are not limited to: gradient descent, Newton's method, quasi-Newton method, and conjugate gradient method; controllers used include, but are not limited to: PID controllers, MPC controllers, LQR controllers, and sliding mode controllers, etc. All controllers mentioned in this application document fall within this scope, and the applicant will not list them all.

[0024] The implementation process of the present invention will now be described in detail with reference to the accompanying drawings.

[0025] Figure 1 This is a schematic diagram of the control flow of the method of the present invention. The underwater vehicle guidance system based on the improved line-of-sight method includes a TSLOS guidance module, a performance index function construction module, and a PID controller. The functions of the three main modules are as follows:

[0026] The TSLOS guidance module selects a virtual tracking point on the AUV's tracking path based on the motion state defined by the AUV's current coordinates and attitude; it uses this virtual tracking point as the geometric tracking target of the AUV in the next moment, and takes the direction angle of the tracking path as the velocity direction tracking target of the AUV in the next moment.

[0027] The performance index function construction module constructs the performance index function based on the distance error, the angle error between the velocity direction and the desired path, and the motion energy consumption during the AUV path tracking process, combined with the control logic of the target separation line-of-sight guidance law.

[0028] The PID controller, combining AUV kinematics and kinematic models, calculates the optimal control parameters of the PID controller by optimizing the performance index function, and outputs the force and torque required for motion control to the AUV drive mechanism.

[0029] Specifically, the underwater vehicle guidance method based on the improved line-of-sight method in this invention includes the following steps:

[0030] Step 1: Target Separation Line-of-Sight (TSLOS) Guidance

[0031] The target separation line-of-sight method is adopted to divide the AUV path tracking problem into two parts: geometric tracking and velocity direction tracking. Based on the motion state defined by the current coordinates and attitude of the AUV, a virtual tracking point is selected on the tracking path of the AUV. This virtual tracking point is used as the geometric tracking target of the AUV in the next moment, and the direction angle of the tracking path is taken as the velocity direction tracking target of the AUV in the next moment.

[0032] Figure 2 This is a schematic diagram illustrating the principle of the target separation line-of-sight guidance algorithm. i-1 s i This is the AUV's current desired tracking path, P t (x t y t The coordinates of the AUV at the current moment are shown below. First, the spatial coordinates in the ground coordinate system are measured using the sensors on the AUV. Then, a line segment perpendicular to the current tracking path is drawn through this coordinate point, intersecting at point R. t Then from the intersection point R t Starting from point Q, move forward a distance l along the tracking path. t (x Qt y Qt This point is used as the virtual tracking point, i.e., the geometric tracking target of the AUV at the next moment. According to P... t Calculate Q from coordinates t Coordinates, i.e. (x Qt y Qt )=f(x t y t , S). Current tracking path s i-1 s i Direction angle in Earth coordinate system As the target for velocity and direction tracking in the next moment of the AUV.

[0033] When the virtual tracking point is outside the critical radius, the current tracking path is used as the reference to select the velocity direction to track the target; when the virtual tracking point is within the critical radius, the next tracking path is used as the reference to select the velocity direction to track the target, and the tracking path is switched to the next path.

[0034] At the turning points of the tracking path, the selection of virtual tracking points is based on the current path, while the selection of the path direction angle is based on the next path. Figure 3 This is a schematic diagram illustrating the principle of the target separation line-of-sight guidance algorithm at path turning points. i- 1s i This is the current tracking path, s i s i+1 This is the next tracking path, virtual tracking point Q.t The selection rules are the same as above. When the target point is within the critical radius, the direction angle of the next path in the Earth coordinate system is selected. As the target for velocity direction tracking in the next moment of the AUV, and the tracking path of the AUV in the next moment is changed from s i-1 s i Convert to s i s i+1 .

[0035] Step 2: Construct the performance index function

[0036] Based on the distance error, motion energy consumption, and angular error between the velocity direction and the desired path during AUV path tracking, a performance index function is constructed using the control logic of the target separation line-of-sight guidance law. Specifically, this includes:

[0037] 1. Obtain the AUV's yaw angle at time t and the angle Δψ between it and the virtual tracking point. t Tracking path direction The superscript 's' indicates the tracking path;

[0038] Calculate the yaw angle of the AUV at time t and the angle Δψ with the virtual tracking point using the following formula. t :

[0039]

[0040] In equation (1), ψ t It is the yaw angle of the AUV in the Earth coordinate system at time t. is a directed line segment P t Q t Direction angle in the Earth coordinate system;

[0041] 2. Construct the performance index function as shown in the following formula:

[0042]

[0043] In equation (2), Γ t The performance index function represents the value at time t, u is the system input, R is the coefficient matrix, and ΔS t+1 This represents the predicted position of the AUV and the virtual tracking point Q at time t+1. t Distance error, V t Let Δt represent the velocity vector of the AUV at time t, where Δt is the step size, and ΔΨ is the velocity vector of the AUV at time t. t+1 τ represents the difference vector between the attitude angle and the tracking path direction of the AUV in the ground coordinate system at time t+1, where α, β, and γ are weighting coefficients; τ represents any time between t and t+1; t≤T≤t+Δt;

[0044] V t =[ut v t w t p t q t r t ] T (3)

[0045] In equation (3), u t v t w t p t q t r t These represent the velocity components of the six degrees of freedom of the AUV at time t;

[0046]

[0047] In equation (4), for planar path tracking, Ψ t+1 =ψ t+1 , is the heading angle of the AUV in the Earth coordinate system at time t+1; It is the direction angle of the tracking path in the Earth coordinate system at time t;

[0048] β=[β1, β2, β3, β4, β5, β6] T (5)

[0049] In equation (5), β i (i = 1, 2, 3, 4, 5, 6) are the weighting coefficients of the velocity components in the six degrees of freedom of the AUV.

[0050] Step 3: Optimize the performance index function and calculate the optimal control parameters of the PID controller.

[0051] The optimal control parameters of the PID controller are calculated by optimizing the performance index function, and the force and torque required for motion control are output to the AUV drive mechanism. Specifically, the following steps are included:

[0052] 1. For the motion of the AUV in the xOy plane, the sway, heave, roll and pitch velocity components in the body coordinate system are all 0, that is, v=w=p=q=0, and the sway velocity is set to a constant value u=u0;

[0053] At this point, equation (2) degenerates into

[0054]

[0055] In equation (6), r t+1 ψ is the theoretical angular velocity of the AUV at time t+1. t+1 It is the theoretical yaw angle of the AUV at time t+1.

[0056] 2. Within the range of motion step length Δt, assuming the force and torque input to the AUV drive system remain constant, the performance index function is transformed into:

[0057]

[0058] 3. For planar path tracking, set the input torque of the AUV to u = N; based on the yaw rate r of the AUV at time t measured by the sensor... t The system input at time t is calculated using the dynamic equations. t =N t The yaw rate r at time T T , where t≤T≤t+Δt.

[0059] Assuming the AUV's center of gravity is at the origin of the body coordinate system, and the center of buoyancy is directly below the center of gravity, while neglecting the influence of nonlinear drag terms on yaw motion, the dynamic equations for calculating AUV yaw are obtained as follows:

[0060]

[0061] In equation (8), I z It is the moment of inertia of the AUV in the z-axis direction. It is the added mass, N t The linear drag coefficient is N, the input torque is r, and the yaw rate of the AUV is r. It is the yaw acceleration of the AUV;

[0062] Solving the differential equation of equation (8), we obtain the yaw rate r at time T. T :

[0063]

[0064] Where t≤T≤t+Δt;

[0065] 4. Receives input torque u at time t t =N t During the time interval from t to t+Δt, the velocity vector of the AUV in the volume coordinate system is V. T =[u0, r T ] T The velocity of the AUV at time T in the Earth coordinate system was calculated based on the kinematic equations.

[0066]

[0067] 5. Obtain the yaw angle ψ of the AUV at time T using the yaw angle update equation. T :

[0068]

[0069] Where t≤T≤t+Δt;

[0070] 6. Approximate equation (10) so that Δt approaches 0;

[0071]

[0072] At the same time, the quadratic term (Tt) 2 Further approximation The AUV velocity update equation is obtained as follows:

[0073]

[0074] In equation (13):

[0075]

[0076] 7. Calculate the theoretical coordinates η of the AUV at time t+1 using the coordinate update equation. t+1 =[x t+1 y t+1 , ψ t+1 ] T :

[0077]

[0078] In equation (15):

[0079]

[0080] Let the step length Δt approach 0, equation (15) simplifies to:

[0081]

[0082] 8. Take the coefficient matrix R as the identity matrix, and the performance index function with respect to the input u t =N t The complete expression is as follows:

[0083]

[0084] Applying the performance index function (18) to N t Differentiation yields:

[0085]

[0086] In the formula, Γ(N) t The expression indicates that the performance index function at time t is about the torque N. t The function.

[0087] 9. For planar path tracking, construct the incremental PID control expression for the AUV:

[0088] N t =k p (Δψ t -Δψ t-1 )+k i Δψ t +k d (Δψ t -2Δψ t-1 +Δψ t-2 )+N t-1 (20)

[0089] In the formula, N t-1 and N t K represents the output torque of the PID controller at time t-1 and time t, respectively; p k i k d ] T These are parameters of the PID controller, Δψ t-1 and Δψ t-2 These are the included angles output by the TSLOS guidance module at times t-1 and t-2, respectively.

[0090] 10. Based on equation (20), the derivatives of the three PID parameters are obtained respectively. Specifically as follows:

[0091]

[0092] 11. Calculate the derivative of the performance index function with respect to the PID controller parameters according to equations (19) and (21):

[0093] When the result exceeds the threshold κ, the performance index function Γ(N) is optimized. t The PID controller parameters are calculated to obtain the minimum value of the performance index function. The parameter update equation is as follows:

[0094]

[0095] In equation (22), K k This represents the result of the k-th iteration of the PID controller parameters, and K... 0 Set the parameters for the previous PID controller; μ = [μ p μ i μ d ] T This indicates the update step size.

[0096] 12. Substitute the controller parameters obtained from equation (22) into equation (20) to calculate the torque N. t This is used as the output of the PID controller at time t.

[0097] Step 4: TSLOS Guidance

[0098] Based on the latest coordinates and attitude of the AUV, update the AUV's motion status data. Repeat steps one through three to achieve guidance of the underwater vehicle during its course.

Claims

1. A guidance method for underwater vehicles based on an improved line-of-sight method, characterized in that, Includes the following steps: (1) The target separation line-of-sight method is adopted to divide the AUV path tracking problem into two parts: geometric tracking and velocity direction tracking; virtual tracking points are selected on the tracking path of the AUV according to the motion state defined by the current coordinates and attitude of the AUV. The virtual tracking point is used as the geometric tracking target of the AUV in the next moment, and the direction angle of the tracking path is taken as the velocity direction tracking target of the AUV in the next moment. (2) Based on the distance error, motion energy consumption, and the angle error between the velocity direction and the desired path during the AUV path tracking process, a performance index function is constructed by combining the control logic of the target separation line-of-sight guidance law. (3) Calculate the optimal control parameters of the PID controller by optimizing the performance index function, and output the force and torque required for motion control to the AUV drive mechanism; (4) Repeat steps (1)-(3) based on the updated AUV motion status data to achieve guidance of the underwater vehicle during the course of the route.

2. The method according to claim 1, characterized in that, In step (1), when the virtual tracking point is outside the critical radius, the speed direction is selected to track the target based on the current tracking path; when the virtual tracking point is within the critical radius, the speed direction is selected to track the target based on the next tracking path, and the tracking path is switched to the next path.

3. The method according to claim 1, characterized in that, In step (1), at the turning point of the tracking path, the selection of the virtual tracking point is based on the current path, while the selection of the path direction angle is based on the next path.

4. The method according to claim 1, characterized in that, In step (2), the performance index function is obtained according to the following steps: (2.1) Obtain the yaw angle of the AUV at time t and the angle Δψ between the AUV and the virtual tracking point. t Tracking path direction The superscript 's' indicates the tracking path; Calculate the yaw angle of the AUV at time t and the angle Δψ with the virtual tracking point using the following formula. t : In equation (1), ψ t It is the yaw angle of the AUV in the Earth coordinate system at time t. is a directed line segment P t Q t Direction angle in the Earth coordinate system; (2.2) Construct the performance index function as shown in the following formula: In equation (2), Γ t The value of the performance index function at time t is represented; u is the system input; R is the coefficient matrix; ΔS t+1 This represents the predicted position of the AUV and the virtual tracking point Q at time t+1. t Distance error; V t Δt represents the velocity vector of the AUV at time t; Δt is the step size; ΔΨ t+1 α represents the difference vector between the attitude angle and the tracking path direction of the AUV in the ground coordinate system at time t+1; α, β, and γ are weighting coefficients; τ represents any time between t and t+1; t≤T≤t+Δt; V t =[u t ,υ t ,w t ,p t ,q t ,r t ] T (3) In equation (3), u t υ t w t p t q t r t These represent the velocity components of the AUV in the six degrees of freedom at time t; In equation (4), for planar path tracking, Ψ t+1 =ψ t+1 , is the heading angle of the AUV in the Earth coordinate system at time t+1; It is the direction angle of the tracking path in the Earth coordinate system at time t; β＝[β1,β2,β3,β4,β5,β6] T (5) In equation (5), β1-β6 are the weighting coefficients of the velocity components of the AUV in the six degrees of freedom.

5. The method according to claim 4, characterized in that, In step (3), the performance index function is optimized and calculated according to the following steps: (3.1) For the motion of the AUV in the xOy plane, the sway, heave, roll and pitch velocity components in the body coordinate system are all 0, that is, υ=w=p=q=0, and the pitch velocity is set to a constant value u=u0; Equation (2) degenerates into: In equation (6), r t+1 ψ is the theoretical angular velocity of the AUV at time t+1. t+1 It is the theoretical yaw angle of the AUV at time t+1; (3.2) Within the range of motion step length Δ, assuming the force and torque input to the AUV drive system remain constant, the performance index function is transformed into: (3.3) For planar path tracking, set the input torque of the AUV to u = N; based on the yaw rate r of the AUV at time t measured by the sensor... t The torque u at time t is calculated using the dynamic equation. t =N t The yaw rate r at time T T ; Assuming the AUV's center of gravity is at the origin of the body coordinate system, and the center of buoyancy is directly below the center of gravity, while neglecting the influence of nonlinear drag terms on yaw motion, the dynamic equations for calculating AUV yaw are obtained as follows: In equation (8), I z It is the moment of inertia of the AUV in the z-axis direction. It is the added mass, N r N is the linear drag coefficient, and N is the input torque. r is the yaw rate of the AUV. It is the yaw acceleration of the AUV; Solving the differential equation of equation (8), we obtain the yaw rate r at time T. T : (3.4) Receives input torque u at time t t =N t During the time interval from t to t+Δt, the velocity vector of the AUV in the volume coordinate system is V. T =[u0, r T ] T The velocity of the AUV at time T in the Earth coordinate system was calculated based on the kinematic equations. (3.5) The yaw angle ψ of the AUV at time T is obtained using the yaw angle update equation. T : (3.6) Approximating equation (10), Δt approaches 0; The quadratic term (Tt) 2 Approximately The AUV velocity update equation is obtained as follows: In equation (13): (3.7) The theoretical coordinates η of the AUV at time t+1 are calculated using the coordinate update equation. t+1 =[x t+1 y t+1 , ψ t+1 ] T : In equation (15): Let the step length Δt approach 0, and simplify equation (15) as follows: (3.8) Taking the coefficient matrix R as the identity matrix, the performance index function with respect to the input u t =N t The complete expression is as follows: Applying the performance index function (18) to N t Taking the derivative, we get: In the formula, Γ(N) t The expression indicates that the performance index function at time t is about the torque N. t The function.

6. The method according to claim 5, characterized in that, In step (3), the parameters of the PID controller are calculated according to the following steps: (1) For planar path tracking, construct the incremental PID control expression for the AUV: N t =k p (Dp t -Dpψ t-1 )+k i Dp t +k d (Dp t -2Dψ t-1 +Dψ t-2 )+N t-1 (20) In the formula, N t-1 and N t K represents the output torque of the PID controller at time t-1 and time t, respectively; p k i k d ] T These are parameters of the PID controller; Δψ t-1 and Δψ t-2 These are the angles output by the TSLOS guidance module at times t-1 and t-2, respectively. (2) Based on equation (20), the derivatives of the three PID parameters are obtained respectively. Specifically as follows: (3) Calculate the derivative of the performance index function with respect to the PID controller parameters according to equations (19) and (21); When the result exceeds the threshold κ, the performance index function Γ(N) is optimized. t The PID controller parameters are obtained by calculating the minimum performance index function, and the parameter update equation is: In equation (22), K k This represents the result of the k-th iteration of the PID controller parameters, and K... 0 Set the parameters for the previous PID controller; μ = [μ p μ i μ d ] T Indicates the update step size; (4) Solve the controller parameters according to the parameter update equation (22), and substitute them into equation (20) to calculate the torque N. t This is used as the output of the PID controller at time t.

7. A guidance system for underwater vehicles based on an improved line-of-sight method, characterized in that, Includes a TSLOS guidance module, a performance index function construction module, and a PID controller; The TSLOS guidance module is used to execute step (1) of the method described in claim 1, and select a virtual tracking point on the tracking path of the AUV according to the motion state defined by the current coordinates and attitude of the AUV. The virtual tracking point is used as the geometric tracking target of the AUV in the next moment, and the direction angle of the tracking path is taken as the velocity direction tracking target of the AUV in the next moment. The performance index function construction module is used to execute the content of step (2) in the method of claim 1, and construct the performance index function based on the distance error, the angle error between the speed direction and the desired path and the motion energy consumption in the AUV path tracking process, combined with the control logic of the target separation line-of-sight guidance law. The PID controller is used to execute the content of step (3) in the method of claim 1, and calculates the optimal control parameters of the PID controller by combining the kinematics and kinematic model of AUV through the optimization performance index function, and outputs the force and torque required for motion control to the AUV drive mechanism.

Images