Autonomous underwater vehicle submarine topography tracking method based on preview control

Abstract

The invention relates to an autonomous underwater vehicle submarine topography tracking method based on preview control. The method comprises the following steps: establishing a trajectory tracking error model of an autonomous underwater vehicle in a vertical plane; the method comprises the following steps: acquiring topographic data of a seabed in front by using sonar, converting the topographic data into description of future disturbance of an autonomous underwater vehicle, and expanding the description into a trajectory tracking error model as preview compensation; aiming at the expanded trajectory tracking error model, establishing a tracking controller based on discrete time state feedback H2 preview control; and the tracking controller is linearized in different working areas, a segmented affine parameter dependence model is adopted to carry out approximation on a nonlinear preview controller, and gain scheduling is realized based on a D method. Compared with the prior art, the method has the advantages that the depth measurement preview data and nonlinear dynamic characteristic modeling of the autonomous underwater vehicle are effectively combined, and a theoretical basis is provided for accurate tracking of the autonomous underwater vehicle in a complex submarine topography environment.

Description

Technical Field

[0001] The present invention relates to the field of underwater vehicle control, and more particularly to a method for an autonomous underwater vehicle to track the seabed terrain based on preview control. Background Art

[0002] When an autonomous underwater vehicle (AUV) performs tasks such as underwater detection and seabed terrain mapping, it needs to maintain stable operation at a height close to the seabed. Since the complex and unknown seabed environment will seriously affect the motion control accuracy and safety of the AUV, the AUV must perceive the seabed terrain ahead in real time and perform control compensation in combination with its own motion state to ensure the navigation height and tracking performance. Using a forward-looking sonar is a common way for an AUV to perform active terrain perception. As an underwater acoustic imaging device, the forward-looking sonar can emit acoustic beams, receive the reflected echo signals, and calculate the seabed terrain contour within a certain range in the forward direction of the AUV. The AUV can adjust its motion state in advance according to the digital model fed back by the sonar, overcome the interference caused by the change of the seabed terrain, and thus achieve high-precision trajectory tracking under terrain matching. The control methods that use forward-looking information to improve the performance of the controlled object are collectively referred to as preview control. Preview feedback can effectively improve the dynamic characteristics of the system. However, the traditional optimal control theory does not consider preview compensation. Introducing feedforward compensation in the conventional state-space framework requires dimension expansion of the controlled object model. For linear objects, a robust preview controller can be easily designed through linear matrix inequalities (LMIs). However, for non-linear systems such as AUVs, there is currently a lack of a comprehensive method for directly applying preview controllers. Summary of the Invention

[0003] The purpose of the present invention is to provide a method for an autonomous underwater vehicle to track the seabed terrain based on preview control, realizing preview control for perceiving the seabed terrain using sonar, and being used for near-bottom autonomous navigation of an autonomous underwater vehicle.

[0004] The purpose of the present invention can be achieved by the following technical solutions:

[0005] A method for an autonomous underwater vehicle to track the seabed terrain based on preview control includes the following steps:

[0006] S1, establishing a trajectory tracking error model of the autonomous underwater vehicle in the vertical plane;

[0007] S2, obtaining the seabed terrain data ahead using sonar, converting it into a description of the future disturbance to the autonomous underwater vehicle, and expanding it as preview compensation into the trajectory tracking error model;

[0008] S3, establishing a tracking controller based on discrete-time state feedback H 2 preview control for the expanded trajectory tracking error model;

[0009] S4. Linearize the tracking controller in different working areas, approximate the non-linear preview controller using a piecewise affine parameter-dependent model, and implement gain scheduling based on the D method.

[0010] The S1 includes the following steps:

[0011] S11. Establish a continuous trajectory tracking error model for the autonomous underwater vehicle:

[0012]

[0013] where: x e ∈R 5 represents the error space state vector, v e = [u e , w e T = v - R(γ T - θ)v r ∈R 2 , the variables u e and w e are the forward speed and the vertical speed respectively, v represents the speed along the reference path, R(θ) represents the rotational motion around the angle θ, γ T represents the moving path angle, θ represents the pitch angle, v r = [V r , 0] T represents the required linear velocity in the two-dimensional space, q e = q - q c , q represents the angular velocity, q c represents the reference angular velocity, θ C represents the pitch angle of the desired vehicle coordinate system {C} relative to the inertial coordinate system {I}, d T represents the derivative of the distance between the current position of the vehicle and the reference path, and the superscript · represents the derivative with respect to time;

[0014] S12. Introduce the output vector:

[0015] y e = v - R(γ T - θ)[V r - d T T

[0016] where: y e represents the output vector;

[0017] S13. Determine the linearized error model at the equilibrium point x e = 0:

[0018]

[0019] Where: A e (ζ), B e (ζ), C e (ζ) respectively represent the Jacobian matrices evaluated under the equilibrium condition of the parameterization ζ = [V r , γ T T . The constant input vector is u = [δ b , δ s , T], where δ b and δ s respectively represent the bow and stern plane deflections;

[0020] S14, determine the discrete form of the trajectory tracking error model of the autonomous underwater vehicle:

[0021] The discrete-time equivalent form of the linear continuous-time model is obtained by applying the zero-order hold technique to the input and setting the sampling time T, and introducing a discrete-time integral action in the output. Its eigenvalues are located at z = 1, resulting in the discrete trajectory tracking error model:

[0022] x d (k + 1) = A(ζ)x d (k) + B(ζ)u(k)

[0023] Where: x d (k) = [x e (k) T , x i (k) T T , x i (k) represents the variable related to the integral part of the error in the control system,

[0024] In S2 above, to incorporate future path disturbances into the discrete trajectory tracking error model, it is assumed that the autonomous underwater vehicle moves along a continuous reference path at a constant speed, and the path is composed of a series of straight lines spliced together. The following seabed disturbance vector is introduced as the disturbance to be previewed:

[0025]

[0026] Assume that there is a discontinuity in the slope of the reference path, which is caused by the connection of two straight lines, and the vehicle crosses the reference path at the time point t = t 0 . Rewrite the disturbance vector as:

[0027]

[0028] Where: δ(t - t 0 ) is the Dirac delta function, t​​0 Represents a specific point in time, respectively representing the time t 0 and the limiting moments before and after, used to describe the instantaneous change of the system state near the time point t 0 nearby.

[0029] According to the introduced perturbation vector, the linearized error model is rewritten as:

[0030]

[0031] Where: represents the injection matrix, and w represents the external disturbance.

[0032] The seabed perturbation vector observed by the sonar is modeled as s(t) = ∑ i s(t i )δ(t - t i ), where s(t i ) represents an intensity vector corresponding to the crossing time t i of the i-th connection point, and the discrete trajectory tracking error model considering the seabed perturbation is obtained:

[0033] x d (k + 1) = A(ζ)x d (k) + B(ζ)u(k) + B 1 (ζ)s(k)

[0034] Where: is obtained from the impulse-invariant discrete equivalent form of the injection matrix W(ζ);

[0035] Assume that the sampling period is small enough so that the change of the reference path is synchronized with the sampling time, that is:

[0036]

[0037] Where: respectively represent the limiting moments before and after the time t k to describe the instantaneous change of the system state near the time point t k nearby;

[0038] Assume that the preview length is p samples, and let x s (k) = [s(k) T , s(k + 1) T ,..., s(k + p) T T ∈R (s(p+1))×1 is the vector containing all preview inputs at time k, and the vector x s (k) is modeled as a first-in-first-out queue, and its update rule is given by the following formula:

[0039] x s (k + 1)= Dx s (k)+ B s s(k + p + 1)

[0040] Where: I represents the identity matrix.

[0041] Considering the discrete trajectory tracking error model of seabed disturbance and the update rule of vector x s (k), the expanded trajectory tracking error model is obtained, and its form is:

[0042]

[0043] Where: H = [B 1 , 0, 0,…, 0] represents the injection matrix for injecting the preview signal into the error dynamics; the preview information is obtained at p points selected on the path, and the obtained points are evenly distributed along the path d p = V t (k)T, the scalar V t (k) corresponds to the modulus of the projection of the velocity vector v of the autonomous underwater vehicle calculated at time k on the path, where V t = [1 0]R(θ - γ T )v.

[0044] The establishment process of the tracking controller based on discrete-time state feedback H 2 preview control is specifically as follows: Considering the expanded trajectory tracking error model, the controller is expressed as a generalized affine parameter-dependent extended closed-loop control system G(ζ), which depends on the varying parameter vector ζ, ζ belongs to a compact set Θ ∈ R q , and is a family composed of compact closed subsets Θ i , i = 1, 2…, N, where N represents the number of subsets, and the subsets cover the desired motion envelope of the autonomous underwater vehicle; in the i-th parameter region ζ ∈ Θ i , the dynamic behavior of the extended closed-loop control system is in the following form:

[0045]

[0046] Where: x(k) is the state vector, w(k) represents the input vector of the external signal, z(k) is the error output vector that needs to be reduced during the controller design process, u(k) is the vector of the execution signal, and the matrix C z (ζ) and E(ζ) are the parameter vectors ζ = [ζ 1 ,..., ζ q ​T affine function

[0047] The extended closed-loop control system G(ζ) includes a controlled object to be controlled and additional weights, where the weights are used to describe the characteristics of external signals and internal signals, as well as preview dynamic behavior; for a given parameter region Δ = Θ i , assume that the values of the elements constituting the parameter vector ζ are restricted to the interval Define Δ 0 as a set of m = 2 q vertices of the parameter-related region:

[0048]

[0049] where: Δ corresponds to the convex hull of Δ 0 , that is, Δ = co{Δ 0}, which is the smallest convex set containing all points in Δ 0 ;

[0050] For an extended closed-loop control system with a larger preview interval p > 50, by utilizing the specific structure of the extended preview dynamics, it is decomposed into a feedback controller and a feedforward controller, and the following method is used to calculate the feedforward gain matrix:

[0051] Let the closed-loop control system be defined on the region Δ and have m vertices in Δ 0 . By restricting the dependence of the parameters to the state equation, let represent the state-space matrices at points 1,..., m in Δ 0 ; according to the structure of the extended closed-loop control system, the matrix is partitioned. Assume that s belongs to the disturbance vector w, and the matrix contains the preview input matrix Also consider the center point of the region Δ and its corresponding state-space matrix Partition the matrix according to the structure of the extended system:

[0052]

[0053] Thus, the state-feedback H 2 controller is designed according to the affine parameter-related system without the preview part, and given the resulting feedback gain matrix K d , calculate the feedforward gain matrix K s to meet specific performance metrics evaluated at the center point of the region Δ

[0054] The design of the reference path includes the following steps:

[0055] Install two narrow - beam echo sounders below the autonomous underwater vehicle to scan the seabed along the forward direction of the autonomous underwater vehicle, obtain a set of reference point vectors represented in the inertial coordinate system, and calculate the complete preview vector x s (k);

[0056] The output of the echo sounder is converted into a representation in the inertial coordinate system {I} and stored as a vector sorted in ascending order of the x - coordinate value, and a height offset is added. The height offset is defined as the offset that the autonomous underwater vehicle should maintain above the seabed; the reference points are converted into a path formed by a series of straight - line connections. At each sampling moment, the previous path point visited by the autonomous underwater vehicle is used as the starting anchor point, and a fixed interval N w V T T generates a series of path points. The coordinates of each path point are the centroid of the data points within a sphere with a radius of N w V T T. The path points are approximated as the contour of the seabed by linear interpolation, where N w represents the number of samples, V T represents the linear velocity in the horizontal direction, and T represents the sampling time.

[0057] The specific implementation of gain scheduling based on the D method is as follows:

[0058] Under the framework of gain - scheduling control theory, a state - feedback matrix gain K i =[K di ,K si T is calculated for each working area. During the controller design stage, the working areas are defined as overlapping areas, and the disturbance - input matrix is set to while the state and control weight matrices and are set respectively to achieve the performance vector z(k)=[z 1 (k) T ,z 2 (k) T ,z 3 (k) T T , where:

[0059]

[0060] z 2 =[W 1 (z)δ n ,W 2 (z)δ s T ​​​

[0061]

[0062] Where: W j (z), j = 1, 2 is a second-order high-pass filter implemented by the strictly proper transfer function W j (z) = 90(z - 0.999) / (z - 1)(z - 0.9), which is used to weight the signals of the bow rudder and stern rudder control surfaces, and its goal is to limit the bandwidth of the corresponding actuators; x i1 , x i2 , x i3 represents the introduced integral term, and the third integral term x i3 corresponds to the discrete-time integral action on the bow rudder control surface δ b ;

[0063] By using the D method, all integrators are moved to the input of the controlled object, and differentiators are added at necessary positions to maintain the transfer function and stability characteristics of the closed-loop system.

[0064] Compared with the prior art, the present invention has the following beneficial effects:

[0065] (1) Aiming at the trajectory tracking control requirements of an AUV in an unknown seabed environment, the present invention proposes a preview control method based on a forward-looking sonar. By using the seabed terrain information detected by the sonar as preview compensation, it can better suppress the motion interference of terrain changes on the AUV and greatly improve the near-bottom tracking performance.

[0066] (2) The present invention proposes the design of a discrete-time preview controller based on a piecewise affine model, and realizes the automatic scheduling of the preview control gain within the AUV motion envelope based on the D method. Compared with the traditional direct augmentation method, the present invention adopts the method of independently solving state feedback and feedforward, which greatly reduces the solution complexity of the preview controller LMI. At the same time, it ensures the internal and external characteristics of the closed-loop system, and there is no need for feedforward compensation of the equilibrium point information. Brief Description of the Drawings

[0067] Figure 1 is the flowchart of the method of the present invention;

[0068] Figure 2 is the schematic diagram of the reference path under discontinuous slope;

[0069] Figure 3 is the schematic diagram of the interconnected feedback of the closed-loop control system;

[0070] Figure 4 is the schematic diagram of data point capture;

[0071] The reference numerals in the figure are: 1, autonomous underwater vehicle; 2, reference path; 3, offset; 4, sonar; 5, seabed terrain. Detailed Implementation Manner

[0072] The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments. This embodiment is implemented on the premise of the technical solution of the present invention, and gives a detailed implementation manner and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0073] The present invention provides a method for an autonomous underwater vehicle to track the seabed terrain based on preview control. Its main steps include: using an echo detector to obtain seabed terrain data to provide necessary environmental information for the control system. Defining a seabed terrain tracking controller to describe the state of the AUV in a specific error space, and transforming the seabed terrain tracking problem into a discrete-time path tracking control problem, thereby simplifying the problem-solving. By introducing bathymetric preview data, the error state space model of the system is supplemented to improve the accuracy and robustness of the system. A piecewise affine parameter-dependent model is used to approximate the nonlinear characteristics of the AUV. This model is used to describe the characteristics of the predefined operation area of the AUV under linearized error dynamics, ensuring that the controller design matches the actual dynamics. For each region, a state feedback control problem is designed to solve the affine parameter-dependent system, and it is solved by linear matrix inequalities. Finally, the function of the nonlinear controller is realized through a gain-scheduling controller, and the D method is used to complete the design and optimization of the controller. This method effectively combines bathymetric preview data and the modeling of the nonlinear dynamic characteristics of the AUV, providing a theoretical basis for the accurate tracking of the AUV in a complex seabed terrain environment.

[0074] Specifically, as Figure 1 shown, it includes the following steps:

[0075] S1. Establish a trajectory tracking error model of the autonomous underwater vehicle in the vertical plane.

[0076] S1 includes the following steps:

[0077] S11. Establish a continuous trajectory tracking error model of the autonomous underwater vehicle:

[0078]

[0079] Where: x e ∈R 5 represents the error space state vector, v e =[u e , w e T =v - R(γ T -θ)v r ∈R 2 , the variables u e and w eare the forward speed and the vertical speed respectively, v represents the speed along the reference path, R(θ) represents the rotational motion about the angle θ, γ T represents the path angle of movement, θ represents the pitch angle, v r = [V r , 0] T represents the required linear velocity in the two-dimensional space, q e = q - q c , q represents the angular velocity, q c represents the reference angular velocity, θ C represents the pitch angle of the desired vehicle body coordinate system {C} relative to the inertial coordinate system {I}, d T represents the derivative of the distance between the current position of the vehicle and the reference path, and the superscript · represents the derivative with respect to time.

[0080] S12, introduce the output vector:

[0081] y e = v - R(γ T - θ)[V r - d T T (2)

[0082] where: y e represents the output vector.

[0083] S13, determine the linearized error model at the equilibrium point x e = 0:

[0084]

[0085] where: A e (ζ), B e (ζ), C e respectively represent the Jacobian matrices evaluated under the equilibrium condition of the parameterization ζ = [V r , γ T T u = [δ b , δ s , T] represents the constant input vector, δ b and δ s respectively represent the deflections of the bow and stern planes;

[0086] S14, determine the discrete form of the trajectory tracking error model of the autonomous underwater vehicle:

[0087] The discrete-time equivalent form of the linear continuous-time model is obtained by adopting the zero-order hold technique for the input and setting the sampling time T, and a discrete-time integration action is introduced in the output, and its eigenvalues are located at z = 1, obtaining the discrete trajectory tracking error model: ​​

[0088] x d (k + 1)= A(ζ)x d (k)+ B(ζ)u(k) (4)

[0089] where: x d (k)= [x e (k) T , x i (k) T T , x i (k) represents the variable related to the integral part of the error in the control system,

[0090] S2. Obtain the seabed terrain data ahead using sonar, convert it into a description of the future disturbances to the autonomous underwater vehicle, and expand it as preview compensation into the trajectory tracking error model.

[0091] To incorporate future path disturbances into the discrete trajectory tracking error model (4), assume that the autonomous underwater vehicle moves along a continuous reference path at a constant speed, and this path is composed of a series of straight lines spliced together. Conduct a detailed analysis of the error dynamics (1), and thus introduce the following seabed disturbance vector as the disturbance to be previewed:

[0092]

[0093] Assume that there is a discontinuity in the slope of the reference path, as Figure 2 shown, this discontinuity is caused by the connection of two straight lines, and the vehicle crosses the reference path at time point t = t 0 . Rewrite the disturbance vector (5) as:

[0094]

[0095] where: δ(t - t 0 ) is the Dirac δ function, t 0 represents a specific time point, respectively represent the limiting moments after and before time t 0 , used to describe the instantaneous change of the system state near time point t 0 .

[0096] According to the introduced disturbance vector, combined with formula (3), rewrite the linearized error model as:

[0097]

[0098] where: represents the injection matrix, and w represents the external disturbance.

[0099] ​The seabed disturbance vector observed by sonar can be modeled as s(t) = ∑ i s(t i )δ(t - t i ), where s(t i ) represents an intensity vector corresponding to the crossing time t i of the i-th connection point, and the discrete trajectory tracking error model considering seabed disturbance is obtained:

[0100] x d (k + 1) = A(ζ)x d (k) + B(ζ)u(k) + B 1 (ζ)s(k) (8)

[0101] where: is obtained from the impulse-invariant discrete equivalent form of the injection matrix W(ζ).

[0102] Assume that the sampling period is small enough so that the change of the reference path is synchronized with the sampling time, i.e.:

[0103]

[0104] where: represent the limit moments after and before time t k respectively, and are used to describe the instantaneous change of the system state near time point t k .

[0105] Assume that the preview length is p samples, and let x s (k) = [s(k) T , s(k + 1) T ,..., s(k + p) T T ∈R (s(p+1))×1 be the vector containing all preview inputs at time k. The vector x s (k) is modeled as a first-in-first-out queue, and its update rule is given by the following formula:

[0106] x s (k + 1) = Dx s (k) + B s s(k + p + 1) (10)

[0107] where: I represents the identity matrix.

[0108] Considering the discrete trajectory tracking error model (8) with seabed disturbance and the update rule (10) of the vector x s (k), the augmented trajectory tracking error model is obtained, and its form is:

[0109] ​

[0110] Wherein: H = [B 1 , 0, 0, …, 0] represents the injection matrix for injecting the preview signal into the error dynamics. In this embodiment, the preview information is obtained at p points selected on the path, and the obtained points are evenly distributed along the path d p = V t (k)T, and the scalar V t (k) corresponds to the modulus of the projection of the velocity vector v of the autonomous underwater vehicle calculated at time k on the path, where V t = [1 0]R(θ - γ T ).

[0111] S3. For the augmented trajectory tracking error model, a tracking controller based on discrete-time state feedback H 2 preview control is established.

[0112] This embodiment proposes a solution to the discrete-time state feedback H 2 preview control problem, which is applicable to affine parameter-dependent systems, and develops a controller synthesis algorithm based on linear matrix inequalities (LMI). Considering the augmented trajectory tracking error model, the controller is represented as a generalized affine parameter-dependent extended closed-loop control system G(ζ), which depends on the varying parameter vector ζ, and ζ belongs to a compact set Θ ∈ R q , and can be divided into a family consisting of compact closed subsets Θ i , i = 1, 2, …, N, where N represents the number of subsets, and the subsets cover the desired motion envelope of the autonomous underwater vehicle; as Figure 3 shown, in the i-th parameter region ζ ∈ Θ i , the dynamic behavior of the extended closed-loop control system is in the following form:

[0113]

[0114] Wherein: x(k) is the state vector, w(k) represents the input vector of the external signal, z(k) is the error output vector that needs to be reduced during the controller design process, u(k) is the vector of the execution signal, and the matrix B w (ζ), C z (ζ) and E(ζ) are affine functions of the parameter vector ζ = [ζ 1 ,..., ζ q T .

[0115] ​S4. Linearize the tracking controller in different working regions, approximate the non-linear preview controller using a piecewise affine parameter-dependent model, and implement gain scheduling based on the D method.

[0116] The extended closed-loop control system G(ζ) includes the plant to be controlled and additional weights that are used to describe the characteristics of external and internal signals, as well as preview dynamic behavior. For a given parameter region Δ = Θ i , assume that the values of the elements constituting the parameter vector ζ are restricted to the interval Define Δ 0 as the set of m = 2 q vertices of the parameter-dependent region:

[0117]

[0118] where: Δ corresponds to the convex hull of Δ 0 , i.e., Δ = co{Δ 0}, which is the smallest convex set containing all the points in Δ 0 .

[0119] For an extended closed-loop control system with a large preview interval p > 50, the proposed discrete-time controller leads to LMI optimization problems involving a large number of variables, which cannot be easily solved by existing tools. In this embodiment, by exploiting the specific structure of the extended preview dynamics, it is decomposed into a feedback controller and a feedforward controller, and on this basis, an alternative algorithm for calculating the feedforward gain matrix is proposed.

[0120] First, consider an affine parameter-dependent system with the structure described by formula (10), which is defined on the region Δ and has m vertices in Δ 0 . By restricting the parameter dependence to the state equation, let denote the state-space matrices at points 1,..., m in Δ 0 . Partition the matrices according to the structure of the extended closed-loop control system. Assume that s belongs to the disturbance vector w, and the matrix contains the preview input matrix Also consider the center point of the region Δ and its corresponding state-space matrix Partition the matrices according to the structure of the extended system:

[0121]

[0122] Compared with directly calculating K d and K s , the following alternative is proposed for the extended system (11) in this embodiment.

[0123] (1) Design a state - feedback H 2 controller for the original affine - parameter - related system (without the preview part);

[0124] (2) Given the obtained feedback gain matrix K d , calculate the feed - forward gain matrix K s to meet specific performance metrics for evaluating the system at the center point of the region Δ.

[0125] The design of the reference path in this embodiment includes the following steps:

[0126] By using different distance - sensing technologies, the proposed preview - control - based tracking controller can be applied to the seabed terrain tracking task of an AUV. As Figure 4 shown, in this embodiment, two narrow - beam echo sounders are installed below the autonomous underwater vehicle to scan the seabed along the forward direction of the autonomous underwater vehicle, and a set of reference - point vectors represented in the inertial coordinate system can be obtained, and the complete preview vector x s (k) is calculated at each sampling moment.

[0127] The output of the echo sounder is converted into a representation in the inertial coordinate system {I} and stored as a vector sorted in ascending order of the x - coordinate value, while adding a height offset (defined as the offset that the autonomous underwater vehicle should maintain above the seabed). These reference points are converted into a path connected by a series of straight lines. At each sampling moment, the path point last visited by the autonomous underwater vehicle is used as the starting anchor point, and a series of path points are generated at a fixed interval N w V T T. The coordinates of each path point are the centroid of the data points within a sphere of radius N w V T T. These path points are approximated as the seabed contour by linear interpolation, where N w represents the number of samples, V T represents the linear velocity in the horizontal direction, and T represents the sampling time.

[0128] By using the centroid to calculate the path points, sudden changes in the reference path between sampling moments caused by adding new data points can be effectively avoided. In addition, this simple reference - path construction method has a high anti - interference ability to sonar sensor noise. If there are more available data points, the calculation of the centroid provides a certain smoothing inertia for the inclusion of new data points, so that within a reasonable parameter N w range, the path points are always located at or very close to the data - point cloud.

[0129] In the application introduced in this embodiment, the AUV is expected to move in a vertical plane along a reference path composed of straight-line connections. During the controller design phase, the considered AUV maneuvering envelope is parameterized as ζ = [V r ,γ T T (equivalent to [u, w] T ). For each operating region, the elements of the discrete-time state-space matrix are obtained by linearizing the error dynamics on a dense grid of operating points, and approximated as an affine function of ζ using a least-squares fitting method.

[0130] Under the framework of gain-scheduling control theory, this embodiment calculates a state-feedback matrix gain K i = [K di ,K si T for each operating region. During the controller design phase, to avoid rapid switching between controllers, these operating regions are defined as overlapping regions, and the disturbance input matrix is set to while the state and control weight matrices and are set to achieve the performance vector z(k) = [z 1 (k) T ,z 2 (k) T ,z 3 (k) T T , where:

[0131]

[0132] where: W j (z), j = 1, 2 are second-order high-pass filters implemented by the strictly proper transfer function W j (z) = 90(z - 0.999) / (z - 1)(z - 0.9), which are used to weight the signals of the bow and stern control surfaces, and the goal is to limit the bandwidth of the corresponding actuators; x i1 , x i2 , x i3 represent the introduced integral terms, and the third integral term x i3 corresponds to the discrete-time integral action on the bow control surface δ b .

[0133] ​​​By using the D method, all integrators are moved to the input of the controlled plant, and differentiators are added at necessary positions to maintain the transfer function and stability characteristics of the closed-loop system. In the gain-scheduling setup, the linear controller is designed to handle disturbances in the input and output of the controlled plant around the equilibrium point, which is achieved by differentiating some measured outputs and then feeding them back to the gain-scheduling controller. To maintain the input-output behavior of the feedback system, integral action is provided at the input of the controlled plant. Integrators are the key to the success of this method because they naturally "charge" to reach the input values required to trim the controlled plant.

[0134] The preferred specific embodiments of the present invention have been described in detail above. It should be understood that those of ordinary skill in the art can make many modifications and variations based on the concept of the present invention without creative work. Therefore, all technical solutions that can be obtained by those skilled in the art in the technical field based on the concept of the present invention through logical analysis, reasoning, or limited experiments on the basis of the prior art should fall within the protection scope determined by the claims.

Claims

1. A method for tracking seabed terrain of an autonomous underwater vehicle based on preview control, characterized in that: The following steps are involved: S1, establish the trajectory tracking error model of the autonomous underwater vehicle in the vertical plane; S2, using sonar to obtain the forward seabed topography data, converting it into a description of the future disturbance of the autonomous underwater vehicle, and expanding it into the trajectory tracking error model as a preview compensation; S3, for the expanded trajectory tracking error model, a tracking controller based on discrete-time state feedback H2 preview control is established; S4, linearizing the tracking controller in different working regions, using a piecewise affine parameter dependency model to approximate the nonlinear preview controller, and implementing gain scheduling based on the D method.

2. The method for tracking seabed terrain of an autonomous underwater vehicle based on preview control according to claim 1, characterized in that: The S1 comprises the following steps: S11, establish a continuous trajectory tracking error model for autonomous underwater vehicles: Where: x e ∈R 5 represents the error space state vector, v e =[u e ,w e ] T =vR(γ T -θ)v r ∈R 2 , variable u e and w e are the forward speed and vertical speed respectively, v represents the speed along the reference path, R(θ) represents the rotational motion around the angle θ, γ T represents the moving path angle, θ represents the pitch angle, v r =[V r ,0] T represents the required linear velocity in two-dimensional space, q e =qq c , q represents the angular velocity, q c represents the reference angular velocity, θ C represents the desired pitch angle of the carrier coordinate system {C} relative to the inertial coordinate system {I}, d T represents the derivative of the distance between the vehicle's current position and the reference path, and the superscript · represents the derivative with respect to time; S12, introduce the output vector: y e =vR(γ T -θ)[V r -d T ] T Where: y e represents the output vector; S13, determine the equilibrium point x e = 0 linear error model: Among them: A e (ζ), B e (ζ), C e (ζ) respectively represent the parameterization ζ=[V r ,γ T ] T The Jacobian matrix evaluated under equilibrium conditions, u = [δ b ,δ s ,T] represents a constant input vector, δ b and δ s deflections of the bow and stern planes, respectively; S14, determine the discrete form of the trajectory tracking error model of the autonomous underwater vehicle: The discrete-time equivalent form of the linear continuous-time model is obtained by applying a zero-order hold technique to the input and setting the sampling time T, and introducing a discrete-time integral action in the output, whose eigenvalue is located at z = 1, to obtain the discrete trajectory tracking error model: x d (k+1)=A(ζ)x d (k)+B(ζ)u(k) Where: x d (k) = [x e (k) T ,x i (k) T ] T , x i (k) represents the variable associated with the integral part of the error in the control system, 3. The method for tracking seabed terrain of an autonomous underwater vehicle based on preview control according to claim 2, characterized in that: In S2, in order to include future path disturbances into the discrete trajectory tracking error model, it is assumed that the autonomous underwater vehicle moves at a constant speed along a continuous reference path, and the path is composed of a series of straight lines. The following seabed disturbance vector is introduced as the disturbance that needs to be previewed: Assuming that there is a discontinuity in the slope of the reference path, the discontinuity is caused by the connection of two straight lines, and the vehicle crosses the reference path at time t=t0, the perturbation vector is rewritten as: Where: δ(t-t0) is the Dirac delta function, t0 represents a specific time point, They represent the extreme moments after and before time t0, respectively, and are used to describe the instantaneous change of the system state near time point t0.

4. The method for tracking seabed terrain of an autonomous underwater vehicle based on preview control according to claim 3, characterized in that: According to the introduced disturbance vector, the linearized error model is rewritten as: in: represents the injection matrix, and w represents external interference.

5. The method for tracking seabed terrain of an autonomous underwater vehicle based on preview control according to claim 4, characterized in that: The seabed disturbance vector observed by sonar is modeled as s(t) = ∑ i s(t i )δ(tt i ), where s(t i ) represents an intensity vector corresponding to the crossing time t of the i-th connection point i , the discrete trajectory tracking error model considering seabed disturbance is obtained: x d (k+1)=A(ζ)x d (k)+B(ζ)u(k)+B1(ζ)s(k) in: is obtained from the pulse-invariant discrete equivalent of the injection matrix W(ζ); Assume that the sampling period is small enough to synchronize the change of the reference path with the sampling time, that is: in: Respectively represent time t k The limit moments after and before are used to describe the k Instantaneous changes in the state of nearby systems; Assume the preview length is p samples, let x s (k) = [s(k) T ,s(k+1) T ,...,s(k+p) T ] T ∈R (s(p+1))×1 is a vector containing all preview inputs at time k, and the vector x s (k) is modeled as a first-in, first-out queue, and its update rule is given by the following formula: x s (k+1)=Dx s (k)+B s s(k+p+1) in: I represents the identity matrix.

6. The method for tracking seabed terrain of an autonomous underwater vehicle based on preview control according to claim 5, characterized in that: Discrete trajectory tracking error model and vector x considering seabed disturbance s The update rule of (k) is used to obtain the expanded trajectory tracking error model, which is in the form of: in: H = [B1, 0, 0, ..., 0] represents the injection matrix for injecting the preview signal into the error dynamics; the preview information is obtained at p points selected on the path, and the points obtained are along the path d p =V t (k)T is evenly spaced, scalar V t (k) corresponds to the magnitude of the projection of the velocity vector v of the autonomous underwater vehicle calculated at time k onto the path, where V t =[1 0]R(θ-γ T )v.

7. The method for tracking seabed terrain of an autonomous underwater vehicle based on preview control according to claim 6, characterized in that: The establishment process of the tracking controller based on discrete-time state feedback H2 preview control is as follows: Considering the expanded trajectory tracking error model, its controller is represented as a generalized affine parameter-dependent extended closed-loop control system G(ζ), which depends on the variable parameter vector ζ, which belongs to a compact set Θ∈R q , is a compact closed subset Θ i , i = 1, 2…, N, where N is the number of subsets covering the desired autonomous underwater vehicle motion envelope; in the i-th parameter region ζ∈Θ i In the extended closed-loop control system, the dynamic behavior is as follows: Where: x(k) is the state vector, w(k) represents the input vector of external signals, z(k) is the error output vector that needs to be reduced during the controller design process, u(k) is the vector of execution signals, and the matrix B w (ζ), C z (ζ) and E(ζ) are parameter vectors ζ = [ζ1, ..., ζ q ] T The affine function of .

8. The method for tracking seabed terrain of an autonomous underwater vehicle based on preview control according to claim 7, characterized in that: The extended closed-loop control system G(ζ) includes a controlled object to be controlled and additional weights, which are used to describe the characteristics of external and internal signals and preview dynamic behavior; for a given parameter region Δ=Θ i , assuming that the values ​​of the elements constituting the parameter vector ζ are restricted to the interval Define Δ0 as the parameter-dependent region m=2 q A collection of vertices: Among them: Δ corresponds to the convex hull of Δ0, that is, Δ = co{Δ0}, which is the smallest convex set containing all points in Δ0; For the extended closed-loop control system with a large preview interval p>50, by exploiting the specific structure of the extended preview dynamics, it is decomposed into a feedback controller and a feedforward controller, and the feedforward gain matrix is ​​calculated as follows: Assume that the closed-loop control system is defined on the region Δ and has m vertices in Δ0. By restricting the dependence of the parameters to the state equation, let Represents the state space matrix of points 1,…,m in Δ0; divide the matrix according to the structure of the extended closed-loop control system, assuming that s belongs to the disturbance vector w, the matrix Contains preview input matrix Also consider the center point of the region Δ and its corresponding state space matrix Divide the matrix according to the structure of the extended system: Thus, the state feedback H2 controller is designed according to the affine parameter correlation system without the preview part, and the feedback gain matrix K is given. d , calculate the feedforward gain matrix K s , to meet the specific performance indicators of the system evaluated at the center point of the area Δ.

9. The method for tracking seabed terrain of an autonomous underwater vehicle based on preview control according to claim 8, characterized in that: The design of the reference path includes the following steps: Two narrow-beam echo sounders are installed under the autonomous underwater vehicle to scan the seabed along the forward direction of the autonomous underwater vehicle to obtain a set of reference point vectors expressed in an inertial coordinate system and calculate the complete preview vector x at each sampling moment. s (k); The output of the echo sounder is converted to an inertial coordinate system {I} and stored as a vector sorted in ascending order by x-coordinate value, with a height offset added, which is defined as the offset that the autonomous underwater vehicle should keep above the seabed; the reference point is converted into a path connected by a series of straight lines, with the path point last visited by the autonomous underwater vehicle as the starting anchor point at each sampling moment, and the path point is connected at a fixed interval N. w V T T generates a series of path points, the coordinates of each path point are located within a radius of N w V T The path points are approximated as the contour of the seabed by straight line interpolation, where N w Indicates the number of samples, V T represents the horizontal linear velocity, and T represents the sampling time.

10. The method for tracking seabed terrain of an autonomous underwater vehicle based on preview control according to claim 9, characterized in that: The gain scheduling based on the D method is specifically implemented as follows: Under the framework of gain scheduling control theory, a state feedback matrix gain K is calculated for each working area. i =[K di ,K si ] T , in the controller design phase, the working region is defined as the overlap region, and the disturbance input matrix is set to The state and control weight matrices and are respectively set to achieve the performance vector z(k)=[z1(k) T ,z2(k) T ,z3(k) T ] T ,in: z2=[W1(z)δ b ,W2(z)δ s ] T Where: W j (z), j = 1, 2 is the strict regular transfer function W j A second-order high-pass filter implemented by (z) = 90(z-0.999) / (z-1)(z-0.9) is used to weight the signals of the bow and stern control surfaces, with the goal of limiting the bandwidth of the corresponding actuators; x i1 , x i2 , x i3 represents the introduced integral term, the third integral term x i3 Corresponding to the bow rudder control surface δ b Discrete-time integral action on ; By using the D method, all integrators are moved to the input of the plant and differentiators are added where necessary to maintain the transfer function and stability characteristics of the closed-loop system.

Images