Unmanned underwater vehicle preset time trajectory tracking control system with obstacle avoidance function

Abstract

The invention provides an unmanned underwater vehicle preset time trajectory tracking control system with an obstacle avoidance function so as to improve trajectory tracking precision in an obstacle environment. The method comprises the following steps: establishing an unmanned underwater vehicle model; designing a preset time function; a novel artificial potential field function is designed, so that the obstacle avoidance response speed is increased, and tracking errors are reduced; a preset time obstacle performance function is designed to serve as an error limiting boundary for trajectory tracking under the obstacle condition, and the function can dynamically adjust the error limiting boundary according to error changes; designing a preset time extended state observer for estimating model uncertainty and external interference; an auxiliary system with a smooth switching function is adopted to process an input saturation problem; and finally deducing a trajectory tracking control scheme. A simulation result shows that the absolute error integral of the unmanned underwater vehicle in the advancing direction, the yawing direction and the course angle within 100 seconds is reduced by 56.37%, 43.21% and 7.20%; estimation absolute error integral of longitudinal, swaying and yaw angular velocity is reduced by 25.44%, 8.86% and 35.24%.

Description

Technical Field

[0001] The present invention relates to the field of marine engineering detection and control equipment, in particular to unmanned systems and robot control, and specifically to a preset time trajectory tracking control system for an unmanned underwater vehicle with an obstacle avoidance function. Background Art

[0002] Unmanned submersibles play a key role in ocean exploration, environmental monitoring, marine water quality and ecological assessment, scientific research, and military missions. With the increasing attention to marine resources and the increasing demand for underwater rescue equipment, the application demand for unmanned submersibles continues to expand, and the requirements for their motion control technology are also getting higher and higher. The marine environment is complex and changeable. Factors such as submarine reefs, marine life, and non-cooperative ships will affect the motion control of unmanned submersibles, which also brings about the problem of obstacle avoidance.

[0003] In the trajectory tracking control of unmanned underwater vehicles, achieving high-precision and stable control is crucial to ensure the success of the mission. In recent years, researchers have continuously explored controller design, the development of intelligent perception and control equipment, and the innovation of control systems in order to improve the trajectory tracking capability of unmanned underwater vehicles. Preset time control technology has gradually become one of the research focuses. This technology allows users to preset the time to reach the control target. Considering that the unmanned underwater vehicle may deviate from the reference trajectory during the obstacle avoidance process, a performance function designed based on the distance between the unmanned underwater vehicle and the obstacle is introduced to constrain the error, thereby improving the performance of the unmanned underwater vehicle in complex underwater environments. This method is of great significance for improving the mission performance of unmanned underwater vehicles in complex underwater environments.

[0004] Compared with the patent "An unmanned submersible trajectory tracking control system based on preset time and performance", the present invention further improves the intelligence and adaptability of the system in design, not only realizing high-precision trajectory tracking within the preset time, but also introducing obstacle avoidance capability and model parameter uncertainty processing mechanism. In practical applications, unmanned submersibles may face complex marine environments, and traditional trajectory tracking methods are prone to tracking deviations due to obstacles or uncertainty in model parameters. The present invention adds an obstacle avoidance function so that the unmanned submersible can adjust its course in time when encountering obstacles to ensure navigation safety. At the same time, the present invention takes into account the uncertainty of model parameters, thereby enhancing the robustness and adaptability of the system in unknown environments. This improvement significantly improves the practical application value of the system, making it more suitable for complex and changeable marine environments with higher reliability and safety. In general, the present invention comprehensively considers obstacle avoidance, model parameter uncertainty, error convergence speed, and observer speed recoverability, and comprehensively designs an unmanned submersible preset time trajectory tracking control system with obstacle avoidance function.

[0005] (1) In the paper "Obstacle Avoidance Strategy for Submersible Vehicles Based on Improved Artificial Potential Field Method", an obstacle avoidance strategy based on improved artificial potential field method is proposed. By introducing random perturbation mechanism and target adjustment factor, the obstacle avoidance capability of underwater submersible vehicles in complex environments is significantly improved. However, its algorithm steps are relatively complex and require high accuracy of environmental modeling. If the environmental model is inaccurate or incomplete, the path planning result may be invalid.

[0006] (2) The paper "Output Feedback Tracking Control with Collision Avoidance for Dynamic Positioning Vessel under Input Constraint" proposes a trajectory tracking control strategy that combines an artificial potential field function, backstepping method, and extended state observer to ensure that the ship can safely navigate along a smooth trajectory and avoid collisions. The paper "Research on Path Planning and Formation Method of Unmanned Underwater Vehicles Based on Hybrid Potential Field Method" proposes a hybrid potential field path planning and obstacle avoidance method that combines obstacle avoidance, heading keeping, and path tracking algorithms and is suitable for underactuated underwater unmanned submersibles. However, the artificial potential field function in the above two methods uses square decay, which makes the force of the obstacle change more slowly with distance. This square decay causes the potential field strength to weaken more slowly when moving away from the obstacle, so that the system's speed is less suppressed before approaching the obstacle boundary.

[0007] (3) The paper “Ship Trajectory Tracking Sliding Mode Control Strategy Considering Obstacle Avoidance” proposed a trajectory tracking strategy based on sliding mode control. This strategy combines backstepping and integral sliding mode control to control the longitudinal velocity and heading angle respectively, and uses the artificial potential field method to avoid collision with obstacles. However, the obstacle avoidance method adopted in this paper is to avoid obstacles by generating a new reference trajectory instead of circumventing the original reference trajectory. Regenerating the reference trajectory requires continuous updating and recalculation, which increases the computational burden. In addition, generating a new reference trajectory in real time may cause computational delays, thereby affecting the control response speed.

[0008] (4) The paper "Research on Ship Formation Control and Obstacle Avoidance Algorithm" proposed a trajectory tracking controller and anti-interference controller based on the backstepping method, and designed an improved artificial potential field obstacle avoidance algorithm. However, the design of its extended state observer did not consider the convergence time problem, which would lead to larger observation errors and affect the accuracy of the system. Summary of the invention

[0009] In response to the above problems, the present invention adopts an artificial potential field function in obstacle avoidance to ensure that the system can effectively avoid obstacles; for the estimation of model uncertainty and external interference, a preset time extended state observer is designed to accelerate the convergence of observation errors; when dealing with input saturation problems, an auxiliary dynamic system with a smooth switching function is introduced to deal with system oscillations caused by saturation. The preset time control part introduces a time-varying gain function so that the convergence time can be flexibly set to meet different task requirements. For error constraints, the present invention defines a performance function based on the distance between the unmanned underwater vehicle and the obstacle to ensure the safety and accuracy of the system when approaching obstacles.

[0010] To achieve the above-mentioned purpose, the present invention proposes a preset time trajectory tracking control system for an unmanned underwater vehicle with an obstacle avoidance function, the system comprising: an artificial potential field function, a performance function based on the distance between the unmanned underwater vehicle and the obstacle, an auxiliary dynamic system with a smooth switching function, a preset time extended state observer, a controller and an error conversion part.

[0011] S1: Establish the unmanned underwater vehicle model:

[0012] S1.1: The kinematic and dynamic models of the unmanned underwater vehicle are:

[0013]

[0014] Where: η=[x,y,ψ] T is the position vector in the earth-fixed coordinate system, x, y and ψ represent the position of the unmanned underwater vehicle in the forward direction, the position in the yaw direction and the heading angle respectively; v = [u,υ,r] T represents the velocity in the fixed coordinate system, u, υ and r represent the longitudinal, sway and yaw angular velocities respectively; τ = [τ1, τ2, τ3] T represents the actual control input of the unmanned underwater vehicle, g(η) is the restoring force and torque generated by gravity / buoyancy, and d(t) = [d1, d2, d3] T Represents unknown external disturbance. M represents the inertia matrix, C(v) represents the Coriolis matrix, D(v) represents the damping matrix, and R(ψ) represents the rotation matrix. Their specific expressions are as follows:

[0015]

[0016] Where: m 11 , m 22 , m 33 , m 23 , m 32 Represents the inertial mass of the unmanned underwater vehicle including the additional mass. 11 , d 22 , d33 , d 23 , d 32 is the hydrodynamic parameter, covering linear and nonlinear damping terms. The rotation matrix R(ψ) is used to transform the body solid system to the ground solid system. The Coriolis centrifugal term satisfies the following relationship: 13 =-c 31 =m 22 υ-0.5(m 23 +m 32 )r,c 23 =-c 32 =m 11 u,m 23 =m 32 .

[0017] The matrix R(ψ) satisfies the following properties:

[0018]

[0019] Where: S is defined as:

[0020]

[0021] Due to the physical limitations of the actuator, in the presence of input saturation, the dynamic equation of the unmanned underwater vehicle can be expressed as:

[0022]

[0023] Where: τ c =[τ c1 ,τ c2 ,τ c3 ] T represents the control input calculated by the control system, Δτ=τ-τ c Represents τ and τ c The difference between τ and τ c The relationship between can be described as:

[0024]

[0025] Where: τ max i is the maximum allowable control input of the UUV.

[0026] S1.2: Introduce a time-varying function used in the performance function, the mathematical form is as follows:

[0027]

[0028] In the formula: m∈(0,ln(2) / T f ], T f represents the maximum allowable setting time, in t∈[0,Tf ) ρ (t) from the initial value exp[mT f ]-1 decays to a positive number close to zero, and then at t∈[T f ,∞) increases monotonically, which means μ ρ (t) is always a positive number.

[0029] The preset time barrier performance function is designed as:

[0030]

[0031] Where: a, δ are design parameters; μ p μ p Simplification of (t), p L Represents the distance between the unmanned underwater vehicle and the obstacle, R z is the collision avoidance range relative to the obstacle; k ρ 、c ρ is a positive definite design matrix, p is the artificial potential field function in the subsequent design, As shown below:

[0032]

[0033] Where: T m For transition time, is the initial value of the boundary function, f ρk As shown below:

[0034]

[0035] Where: S2: Collision avoidance strategy:

[0036] The collision avoidance capability of the control system plays an important role in collision avoidance and navigation safety, and the artificial potential field function is used to achieve collision avoidance. The path planning of the unmanned underwater vehicle is designed here, which contains a repulsion function. When the unmanned underwater vehicle is far enough away from the obstacle, the artificial potential field function will not take effect; but when the unmanned underwater vehicle approaches the obstacle, the artificial potential field function will repel it, thereby maintaining a safe distance from the obstacle. The obstacle is modeled as a circular object, and the expression of the artificial potential field function is as follows:

[0037]

[0038] Where: p L =||p x -p z || represents the distance between the unmanned underwater vehicle and the obstacle, p x =[x,y] Tis the position of the unmanned underwater vehicle, p z =[x z ,y z ] T is the center of the obstacle, γ, k are design parameters.

[0039] In order to avoid collision with obstacles, the path planning of the unmanned underwater vehicle is introduced into the control objectives. The control objectives are as follows:

[0040]

[0041] Where: η d is the reference trajectory, η0=[p,p,0] T , c z is a small positive constant.

[0042] When the distance between the unmanned underwater vehicle and the obstacle is far enough, the artificial potential field function does not take effect, that is, when p L >R z When η0=[0,0,0] T At this time, the control goal is simplified to the trajectory tracking task. When the unmanned underwater vehicle approaches an obstacle, the artificial potential field function begins to play a role, pushing the unmanned underwater vehicle to avoid the obstacle. The design of η0 ensures that the unmanned underwater vehicle can smoothly bypass the obstacle.

[0043] S3: Preset time extended state observer design:

[0044] The preset time-extended state observer is used to estimate the unknown terms including the environmental disturbance d(t), the damping matrix D(v), the restoring force and torque g(η) generated by gravity / buoyancy, and the Coriolis and centripetal force terms C(v). The mathematical model of the unmanned underwater vehicle can be rewritten as:

[0045]

[0046] Where: F = M -1 (-C(v)ν-D(v)ν-g(η)+d(t)).

[0047] The preset time extended state observer is as follows, which is used to estimate the uncertainty and disturbance of the model, and is expressed as:

[0048]

[0049] Where: are the estimated values ​​of x, y, ψ, u, υ, r, F1, F2, F3. is the observer gain matrix, μ is a simplification of μ(t). m∈(0,ln(2) / T], T is the maximum allowable setting time.

[0050] S4: Controller Design:

[0051] S4.1: Obstacle Performance Constrained Design:

[0052] Tracking error is defined as e = [e1, e2, e3] T =η-η d -η0, the present invention aims to design an appropriate controller so that the tracking error always meets the requirements, that is, to limit the transient tracking error within the user-defined boundaries:

[0053] -δ 1k ρ k (t)<e i (t)<δ 2k ρ k (t),t>0 (20)

[0054] Where: 1k ρ k (t),δ 2k ρ k (t) represent the upper and lower boundaries of the tracking error respectively; δ 1k , δ 2k is the design parameter, e i (t) represents the tracking error, ρ k (t)=[ρ1(t),ρ2(t),ρ3(t)] T is a bounded strictly positive decreasing smooth function.

[0055] Define s1 = [s 11 ,s 12 ,s 13 ] T , Ψ=diag(Ψ1,Ψ2,Ψ3), Φ=diag(Φ1,Φ2,Φ3), where Ψ is a diagonal matrix, Ψ i Non-zero means that the matrix has full rank, so Ψ is an invertible matrix.

[0056]

[0057] α is used as a virtual intermediate signal, and the present invention selects the following virtual control law:

[0058]

[0059] Where: K1 and C1 are positive definite design matrices, and C1 satisfies

[0060] Next, define the velocity error vector as:

[0061]

[0062] The preset time-assisted dynamic system with smooth switching function is:

[0063]

[0064] Where K2, C2, σ1 and σ2 are design parameters. is a smooth switching function.

[0065] In order to achieve the control objective, the following control law is designed:

[0066]

[0067] In the formula, K3, C3 and K β is a design parameter.

[0068] The present invention has the following beneficial effects:

[0069] (1) The present invention designs a performance function based on the distance between the unmanned underwater vehicle and the obstacle, which is more adaptable than the traditional preset performance function. In the process of avoiding obstacles, the unmanned underwater vehicle will temporarily deviate from the reference trajectory and produce a large tracking error. The performance function can dynamically adjust the constraint boundary according to the error change: when the tracking error increases, the function will temporarily relax the constraint to ensure that the error does not exceed the preset range; after the obstacle avoidance is completed, the unmanned underwater vehicle returns to the reference trajectory, the tracking error is reduced, and the constraint conditions become more stringent, thereby meeting the preset performance requirements. The function can flexibly adjust the boundary shape to achieve more precise control goals.

[0070] (2) Compared with the traditional artificial potential field function, the artificial potential field function adopted by the present invention introduces a growth rate adjustment factor. By adjusting this factor, the value of the artificial potential field function can be rapidly increased, which makes the artificial potential field function designed by the present invention have a faster response speed than the traditional artificial potential field function, and can get closer to obstacles and effectively avoid obstacles while ensuring safety. This improvement not only helps to reduce tracking errors, but also significantly improves tracking accuracy. In the simulation of the present invention, after 100 seconds of operation, the absolute error integral of the unmanned underwater vehicle in the forward direction, yaw direction and heading angle was reduced by about 56.37%, 43.21% and 7.20%; at the same time, the time-weighted absolute error integral was also reduced by 51.85%, 51.48% and 49.42% respectively. These results show that the application effect of the improved artificial potential field function in complex environments is significant, which helps to improve the overall performance and operating efficiency of the unmanned underwater vehicle.

[0071] (3) The present invention designs a preset time extended state observer to effectively deal with the estimation problem of model uncertainty and time-varying environmental interference. Compared with the traditional extended state observer, the significant advantage of this observer is that the gain can be adjusted dynamically over time. When the observer has not converged, the gain of the preset time part gradually increases to accelerate the convergence of the observation error; and after convergence, it will become a constant value, thereby ensuring rapid convergence while maintaining excellent performance and achieving higher estimation accuracy. In the simulation of the present invention, after 100 seconds of operation, the absolute error integral of the longitudinal, sway and yaw angular velocity of the unmanned underwater vehicle was reduced by 25.44%, 8.86% and 35.24%; at the same time, the time-weighted absolute error integral was also significantly reduced by 44.49%, 23.66% and 50.29%. These results show that the preset time extended state observer has obvious advantages in dealing with the complexity in the dynamic marine environment and can significantly improve the performance and control accuracy of the unmanned underwater vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 It is a block diagram of the preset time trajectory tracking control system of an unmanned underwater vehicle with obstacle avoidance function;

[0073] Figure 2 It is the trajectory tracking result diagram;

[0074] Figure 3 It is the distance diagram between the unmanned underwater vehicle and obstacles;

[0075] Figure 4 is the tracking error and boundary condition diagram;

[0076] Figure 5 It is a schematic diagram of the trajectory tracking component results;

[0077] Figure 6 is a plot of model parameters versus disturbance estimates;

[0078] Figure 7 is the velocity estimation graph;

[0079] Figure 8 is the control input map;

[0080] Fig. 9 It is a trajectory tracking comparison chart;

[0081] Fig.10 It is a comparison chart of the distance to obstacles;

[0082] Fig.11 is a comparison chart of model parameters and disturbance estimates;

[0083] Fig.12 It is a comparison chart of trajectory tracking components;

[0084] Fig.13It is a comparison chart of trajectory tracking error. DETAILED DESCRIPTION

[0085] The technical solutions in the embodiments of the present invention are described in detail below in conjunction with the accompanying drawings. It should be noted that the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0086] Embodiment: The present invention is further described below in conjunction with the accompanying drawings;

[0087] The specific steps are as follows:

[0088] S1: Figure 1 As shown, Figure 1 The present invention is a structural block diagram of a preset time trajectory tracking control system for an unmanned underwater vehicle with obstacle avoidance function, including: artificial potential field function, performance function based on the distance between the unmanned underwater vehicle and the obstacle, auxiliary dynamic system with smooth switching function, preset time extended state observer, controller and error conversion.

[0089] S1: Establish the unmanned underwater vehicle model:

[0090] S1.1: The kinematic and dynamic models of the unmanned underwater vehicle are:

[0091]

[0092] Where: η=[x,y,ψ] T is the position vector in the earth-fixed coordinate system, x, y and ψ represent the position of the unmanned underwater vehicle in the forward direction, the position in the yaw direction and the heading angle respectively; v = [u,υ,r] T represents the velocity in the fixed coordinate system, u, υ and r represent the longitudinal, sway and yaw angular velocities respectively; τ = [τ1, τ2, τ3] T represents the actual control input of the unmanned underwater vehicle, g(η) is the restoring force and torque generated by gravity / buoyancy, and d(t) = [d1, d2, d3] T Represents unknown external disturbance. M represents the inertia matrix, C(v) represents the Coriolis matrix, D(v) represents the damping matrix, and R(ψ) represents the rotation matrix. Their specific expressions are as follows:

[0093]

[0094] Where: m 11 , m 22 , m 33 , m 23 , m 32Represents the inertial mass of the unmanned underwater vehicle including the additional mass. 11 , d 22 , d 33 , d 23 , d 32 is the hydrodynamic parameter, covering linear and nonlinear damping terms. The rotation matrix R(ψ) is used to transform the body solid system to the ground solid system. The Coriolis centrifugal term satisfies the following relationship: 13 =-c 31 =m 22 υ-0.5(m 23 +m 32 )r,c 23 =-c 32 =m 11 u,m 23 =m 32 .

[0095] The matrix R(ψ) satisfies the following properties:

[0096]

[0097] Where: S is defined as:

[0098]

[0099] Due to the physical limitations of the actuator, in the presence of input saturation, the dynamic equation of the unmanned underwater vehicle can be expressed as:

[0100]

[0101] Where: τ c =[τ c1 ,τ c2 ,τ c3 ] T represents the control input calculated by the control system, Δτ=τ-τ c Represents τ and τ c The difference between τ and τ c The relationship between can be described as:

[0102]

[0103] Where: τ max i is the maximum allowable control input of the UUV.

[0104] S1.2: Introduce a time-varying function used in the performance function, the mathematical form is as follows:

[0105]

[0106] In the formula: m∈(0,ln(2) / T f ], T f represents the maximum allowable setting time, in t∈[0,T f ) ρ (t) from the initial value exp[mT f ]-1 decays to a positive number close to zero, and then at t∈[T f ,∞) increases monotonically, which means μ ρ (t) is always a positive number.

[0107] The preset time barrier performance function is designed as:

[0108]

[0109] Where: a, δ are design parameters; μ p μ p Simplification of (t), p L Represents the distance between the unmanned underwater vehicle and the obstacle, R z is the collision avoidance range relative to the obstacle; k ρ 、c ρ is a positive definite design matrix, p is the artificial potential field function in the subsequent design, As shown below:

[0110]

[0111] Where: T m For transition time, is the initial value of the boundary function, f ρk As shown below:

[0112] Where: S2: Collision avoidance strategy:

[0113] The collision avoidance capability of the control system plays an important role in collision avoidance and navigation safety, and the artificial potential field function is used to achieve collision avoidance. The path planning of the unmanned underwater vehicle is designed here, which contains a repulsion function. When the unmanned underwater vehicle is far enough away from the obstacle, the artificial potential field function will not take effect; but when the unmanned underwater vehicle approaches the obstacle, the artificial potential field function will repel it, thereby maintaining a safe distance from the obstacle. The obstacle is modeled as a circular object, and the expression of the artificial potential field function is as follows:

[0114]

[0115] Where: p L =||p x -p z|| represents the distance between the unmanned underwater vehicle and the obstacle, p x =[x,y] T is the position of the unmanned underwater vehicle, p z =[x z ,y z ] T is the center of the obstacle, γ, k are design parameters.

[0116] In order to avoid collision with obstacles, the path planning of the unmanned underwater vehicle is introduced into the control objectives. The control objectives are as follows:

[0117]

[0118] Where: η d is the reference trajectory, η0=[p,p,0] T , c z is a small positive constant.

[0119] When the distance between the unmanned underwater vehicle and the obstacle is far enough, the artificial potential field function does not take effect, that is, when p L >R z When η0=[0,0,0] T At this time, the control goal is simplified to the trajectory tracking task. When the unmanned underwater vehicle approaches an obstacle, the artificial potential field function begins to play a role, pushing the unmanned underwater vehicle to avoid the obstacle. The design of η0 ensures that the unmanned underwater vehicle can smoothly bypass the obstacle.

[0120] S3: Preset time extended state observer design:

[0121] The preset time-extended state observer is used to estimate the unknown terms including the environmental disturbance d(t), the restoring force and torque g(η) generated by gravity / buoyancy, the damping matrix D(v), and the Coriolis and centripetal force terms C(v). The mathematical model of the unmanned underwater vehicle can be rewritten as:

[0122]

[0123] Where: F = M -1 (-C(v)ν-D(v)ν-g(η)+d(t)).

[0124] The preset time extended state observer is as follows, which is used to estimate the uncertainty and disturbance of the model, and is expressed as:

[0125]

[0126] Where: are the estimated values ​​of x, y, ψ, u, υ, r, F1, F2, F3. is the observer gain matrix, μ is the simplification of μ(t), m∈(0,ln(2) / T], and T represents the maximum allowable setting time.

[0127] S4: Controller Design:

[0128] S4.1: Obstacle Performance Constrained Design:

[0129] Tracking error is defined as e = [e1, e2, e3] T =η-η d -η0, the present invention aims to design an appropriate controller so that the tracking error always meets the requirements, that is, to limit the transient tracking error within the user-defined boundaries:

[0130] -δ 1k ρ k (t)<e i (t)<δ 2k ρ k (t),t>0 (49)

[0131] Where: 1k ρ k (t),δ 2k ρ k (t) represent the upper and lower boundaries of the tracking error respectively; δ 1k , δ 2k is the design parameter, e i (t) represents the tracking error, ρ k (t)=[ρ1(t),ρ2(t),ρ3(t)] T is a bounded strictly positive decreasing smooth function.

[0132] Define s1 = [s 11 ,s 12 ,s 13 ] T , Ψ=diag(Ψ1,Ψ2,Ψ3), Φ=diag(Φ1,Φ2,Φ3), where Ψ is a diagonal matrix, Ψ i Non-zero means that the matrix has full rank, so Ψ is an invertible matrix.

[0133]

[0134] α is used as a virtual intermediate signal, and the present invention selects the following virtual control law:

[0135]

[0136] Where: K1 and C1 are positive definite design matrices, and C1 satisfies

[0137] Next, define the velocity error vector as:

[0138]

[0139] The preset time-assisted dynamic system with smooth switching function is:

[0140]

[0141]

[0142] Where K2, C2, σ1 and σ2 are design parameters. is a smooth switching function.

[0143] In order to achieve the control objective, the following control law is designed:

[0144]

[0145] In the formula, K3, C3 and K β is a design parameter.

[0146] The following is an offline simulation verification of the unmanned underwater vehicle and the preset time trajectory tracking control system of the present invention. The simulation parameters are set as follows: reference signal η d is used to generate the desired trajectory, and the control force and torque constraints are τ max =[20N,20N,15Nm] T , environmental interference is d = [sin (0.3t), sin (0.4t), sin (0.3t)] T The observer parameters are: K 01 =15I3,K 02 =15I3,K 03 =15I3,C 01 =I3,C 02 =I3,C 03 =I3, controller parameters are: K1 = 5I3, K2 = 10I3, K3 = 5I3, C1 = I3, C2 = I3, C3 = I3, artificial potential field function parameter γ = 0.1, k = 3, safe collision avoidance radius is 1.5m, relative to the collision avoidance range R z =2.6, T=20, m=0.03, T m =15, k ρ =1,δ 1k =1, c ρ =2,δ 2k = 1. σ1 = 1, σ2 = 2, a = 1, δ = 1, the position of the obstacle P z =(10m,-1m), the initial position of the unmanned underwater vehicle, the initial position of the reference trajectory, and the speed of the reference trajectory are: η d(0) = [0m, 0m, 0rad] T v d =[0.2m / s,0m / s,0rad / m] T ,η(0)=[-1m,3m,0rad] T .

[0147] Figure 2 The video demonstrated the trajectory tracking control effect of the unmanned underwater vehicle successfully overtaking an obstacle on the port side and bypassing the obstacle during the obstacle avoidance process. Figure 3 The distance relationship between the unmanned underwater vehicle and obstacles is presented. The results show that all distances are greater than the safe collision avoidance radius, which means that there is no safety risk during the navigation process and the unmanned underwater vehicle effectively avoids collision with obstacles. Figure 4 It can be seen that the obstacle performance function designed based on the distance between the unmanned submersible and the obstacle effectively limits the tracking error within the preset boundaries. During the period from 0 to 20 seconds, the tracking error gradually converged to the vicinity of zero within the preset time. At this time, the artificial potential field function did not participate in the trajectory tracking control. Subsequently, between 38 and 62 seconds, the distance between the unmanned submersible and the obstacle gradually approached, and the artificial potential field function began to play a role. Due to the deviation of the trajectory caused by the unmanned submersible in order to avoid obstacles, the error increased, and therefore the value of the obstacle performance function also increased. After 62 seconds, the unmanned submersible returned to the predetermined trajectory. At this time, the main task of the system was only to track the trajectory. The unmanned submersible maintained a safe distance from the obstacle, and the artificial potential field function no longer played a role. From Figure 5 It can be seen that the UUV has good tracking effect in all directions. However, at 38 seconds, due to the need to avoid obstacles, the UUV bypassed the obstacle from the port side, which resulted in a large tracking error in the y direction.

[0148] from Figure 6 It can be seen that the preset time state observer can effectively estimate uncertainty and interference within the set time, and the initial estimate is set to 0. In many practical applications, direct measurement of speed is often limited by various factors, such as sensor noise, cost issues, or environmental interference. These factors may lead to inaccurate measurement data, thus affecting the overall performance and stability of the system. To overcome these challenges, the use of preset time extended state observer is an effective solution. The preset time extended state observer can indirectly infer the speed through a comprehensive estimate of the system state without the need to directly measure the speed. This approach not only improves the observability of the system, but also enables systems operating in complex environments to better cope with uncertainty and interference.

[0149] Figure 7The speed recovery performance of the observer is demonstrated. Between 38 seconds and 62 seconds, the speed of the unmanned underwater vehicle changes due to the influence of the artificial potential field function. Figure 8 The case where the system input signal is subject to input saturation constraints is shown. Between 38 seconds and 62 seconds, in order to ensure the safe navigation of the unmanned underwater vehicle, the artificial potential field function begins to play a role and affects the input signal, resulting in obvious oscillation and saturation of the input signal. Compared with the traditional auxiliary dynamic system to deal with input saturation, the present invention adopts a preset time auxiliary dynamic system with a smooth switching function, thereby effectively solving the instability problem caused by input signal oscillation and saturation.

[0150] from Fig. 9 and Fig.10 It can be seen that under the same conditions, the control law designed by the present invention can avoid obstacles more closely while ensuring safety, and the deviation from the reference trajectory is also smaller. Fig.10 It can be clearly seen that the unmanned underwater vehicle reacts more quickly to avoiding obstacles, which further demonstrates the advantages of the designed new artificial potential field function. Fig.11 It can be seen that the observer designed by the present invention can effectively estimate the parameter uncertainty and interference of the model. Fig.12 and Fig.13 It can be seen that the control method designed by the present invention can effectively control the movement of the unmanned underwater vehicle in the process of avoiding obstacles, so that it maintains a high consistency with the reference trajectory and has a smaller tracking error.

[0151] The data in Table 1 show that compared with the scheme in the comparative experiment, the absolute error integral of the speed estimation error of the proposed scheme within 100 seconds is reduced by 25.44%, 8.86% and 35.24% respectively; at the same time, the time-weighted absolute error integral of the speed estimation error is reduced by 44.49%, 23.66% and 50.29% respectively within 100 seconds. Compared with the extended state observer, the preset time extended state observer designed by the present invention shows a significant improvement in estimation accuracy. According to the position error data in Table 2, compared with the comparative experiment, the absolute error integral within 100 seconds is reduced by about 56.37%, 43.21% and 7.20% respectively; the time-weighted absolute error integral is reduced by 51.85%, 51.48% and 49.42% respectively.

[0152] Table 1 Speed ​​observation error

[0153]

[0154] Table 2 Path tracking performance of different methods

[0155]

Claims

1. A preset time trajectory tracking control system for an unmanned underwater vehicle with obstacle avoidance function, characterized in that: The following steps are involved: S1: Establish the unmanned underwater vehicle model: S1.1: The kinematic and dynamic models of the unmanned underwater vehicle are: Where: η=[x,y,ψ] T is the position vector in the earth-fixed coordinate system, x, y and ψ represent the position of the unmanned underwater vehicle in the forward direction, the position in the yaw direction and the heading angle respectively; v = [u,υ,r] T represents the velocity in the fixed coordinate system, u, υ and r represent the longitudinal, sway and yaw angular velocities respectively; τ = [τ1, τ2, τ3] T represents the actual control input of the unmanned underwater vehicle, g(η) is the restoring force and torque generated by gravity / buoyancy, and d(t) = [d1, d2, d3] T represents unknown external disturbance; M represents the inertia matrix, C(v) represents the Coriolis matrix, D(v) represents the damping matrix, and R(ψ) represents the rotation matrix. Their specific expressions are as follows: Where: m 11 , m 22 , m 33 , m 23 , m 32 represents the inertial mass of the unmanned underwater vehicle including the additional mass; d 11 , d 22 , d 33 , d 23 , d 32 is the hydrodynamic parameter, covering linear and nonlinear damping terms; the rotation matrix R(ψ) is used to transform the body solid system to the ground solid system, and the Coriolis centrifugal term satisfies the following relationship: c 13 =-c 31 =m 22 υ-0.5(m 23 +m 32 )r,c 23 =-c 32 =m 11 u,m 23 =m 32 ; The matrix R(ψ) satisfies the following properties: Where: S is defined as: Due to the physical limitations of the actuator, in the presence of input saturation, the dynamic equation of the unmanned underwater vehicle can be expressed as: Where: τ c =[τ c1 ,τ c2 ,τ c3 ] T represents the control input calculated by the control system, Δτ=τ-τ c Represents τ and τ c The difference between τ and τ c The relationship between can be described as: Where: τ maxi is the maximum allowable control input for the UUV; S1.2: Introduce a time-varying function used in the performance function, the mathematical form is as follows: In the formula: m∈(0,ln(2) / T f ], T f represents the maximum allowable setting time, in t∈[0,T f ) ρ (t) from the initial value exp[mT f ]-1 decays to a positive number close to zero, and then at t∈[T f ,∞) increases monotonically, which means μ ρ (t) is always a positive number; The preset time barrier performance function is designed as: Where: a, δ are design parameters; μ ρ μ ρ Simplification of (t), p L Represents the distance between the unmanned underwater vehicle and the obstacle, R z is the collision avoidance range relative to the obstacle; k ρ 、c ρ is a positive definite design matrix, p is the artificial potential field function in the subsequent design, As shown below: Where: T m For transition time, ρ k (0) is the initial value of the boundary function, f ρk As shown below: Where: S2: Collision avoidance strategy: The obstacle is modeled as a circular object, and the artificial potential field function expression is as follows: Where: p L =||p x -p z || represents the distance between the unmanned underwater vehicle and the obstacle, p x =[x,y] T is the position of the unmanned underwater vehicle, p z =[x z ,y z ] T is the center of the obstacle, γ and k are design parameters; The control objectives are as follows: Where: η d is the reference trajectory, η0=[p,p,0] T , c z is a small positive constant; S3: Preset time extended state observer design: The preset time-extended state observer is used to estimate the unknown terms including the environmental disturbance d(t), the damping matrix D(v), the restoring force and torque g(η) generated by gravity / buoyancy, and the Coriolis and centripetal force terms C(v). The mathematical model of the unmanned underwater vehicle can be rewritten as: Where: F = M -1 (-C(v)ν - D(v)ν - g(η) + d(t)); The preset time extended state observer is as follows, which is used to estimate the uncertainty and disturbance of the model, and is expressed as: Where: are the estimated values ​​of x, y, ψ, u, υ, r, F1, F2, F3; is the observer gain matrix, μ is a simplification of μ(t), m∈(0,ln(2) / T], T represents the maximum allowable setting time; S4: Controller Design: S4.1: Obstacle Performance Constrained Design: Tracking error is defined as e = [e1, e2, e3] T =η-η d -η0, the present invention aims to design an appropriate controller so that the tracking error always meets the requirements, that is, the transient tracking error is limited to the user-defined boundaries: -d 1k r k (t)<e i (t)<δ 2k r k (t),t>0 (20) Where: 1k ρ k (t),δ 2k ρ k (t) represent the upper and lower boundaries of the tracking error respectively; δ 1k , δ 2k is the design parameter, e i (t) represents the tracking error, ρ k (t)=[ρ1(t),ρ2(t),ρ3(t)] T is a bounded strictly positive decreasing smooth function; Define s1 = [s 11 ,s 12 ,s 13 ] T , Ψ=diag(Ψ1,Ψ2,Ψ3), Φ=diag(Φ1,Φ2,Φ3), where Ψ is a diagonal matrix, Ψ i Non-zero means that the matrix has full rank, so Ψ is an invertible matrix; P i =1 / 2(e i +d 1k r k )-1 / 2(e i -d 2k r k ) (23) α is used as a virtual intermediate signal, and the present invention selects the following virtual control law: Where: K1 and C1 are positive definite design matrices, and C1 satisfies Next, define the velocity error vector as: The preset time-assisted dynamic system with smooth switching function is: Where K2, C2, σ1, and σ2 are design parameters. is a smooth switching function; In order to achieve the control objective, the following control law is designed: In the formula, K3, C3 and K β is a design parameter.

Images