AUV close-range docking method based on sliding mode adaptive control under ocean current interference.

Abstract

This invention relates to an AUV close-range docking method based on sliding mode adaptive control under ocean current interference. The controller design eliminates decoupling of the AUV's motion and incorporates an integral term in the sliding mode adaptive controller design, employing Integral Sliding Mode Adaptive Control (ISMAC) to minimize chattering during the AUV's approach motion. Finally, in designing the control parameters, overshoot is minimized at the expense of settling time to ensure successful docking. Furthermore, the estimation of ocean current velocity gives the system a certain degree of anti-interference capability, demonstrating its robustness.

Description

Technical Field

[0001] This invention belongs to the field of unmanned underwater vehicle technology, and relates to a method for close-range docking of AUVs based on sliding mode adaptive control under ocean current interference. Specifically, it relates to a docking control system and control method for fully driven underwater vehicles under ocean current interference. Background Technology

[0002] Unmanned underwater vehicles (UUVs) have important and wide applications in fields such as marine research, ecological monitoring, and military reconnaissance. Tracking and control of UUVs is fundamental for them to complete their pre-set tasks. To meet the needs of underwater operations, the UUV control system is often required to track the desired trajectory from any initial state with rapid convergence.

[0003] When tracking underwater vehicles at close range, acoustic sensors are not accurate enough, while visual sensors have a clear advantage in short-range detection, with recognition accuracy down to the centimeter level. Moreover, they are small in size, easy to carry and install, and are more suitable for close-range underwater tracking.

[0004] When underwater vehicles are tracking at close range, numerous constraints prevent the decoupling of the fully driven vehicle's motion to the horizontal and vertical planes. For sliding mode control, under the influence of model inaccuracies and external disturbances, the control law will inevitably be discontinuous when crossing the sliding surface s=0, leading to chattering. To mitigate chattering, the first step is to consider parameter uncertainty and estimate parameter values ​​online through adaptive control, bringing the parameter estimation error to zero, thereby reducing the impact of parameter uncertainty on the system. The second step is to find a way to appropriately smooth the control input to achieve the optimal trade-off between control bandwidth and tracking accuracy. Summary of the Invention

[0005] Technical problems to be solved

[0006] To overcome the shortcomings of existing technologies, this invention proposes a close-range docking method for AUVs based on sliding mode adaptive control under ocean current interference. It aims to provide a tracking control system and method for fully driven vehicles, taking into account ocean current interference. The following vehicle obtains relative pose information through visual sensors, and path tracking is achieved by designing a sliding mode controller.

[0007] Technical solution

[0008] A method for close-range docking of AUVs based on sliding mode adaptive control under ocean current interference is characterized by: its application in a multi-submarine vehicle cooperative system, wherein the multi-submarine vehicle cooperative system includes at least one parent AUV and one follower AUV; the parent AUV is equipped with a docking mechanism at its rear end, and the follower AUV is equipped with a monocular camera at its front end; the parent AUV is equipped with a visual sensor that uses a light source beacon and ArucoMarker codes to provide visual information to the follower AUV for sensing the relative pose of the two vehicles; the docking steps are as follows:

[0009] Step 1: Establish a dynamic model of the AUV under ocean current disturbance;

[0010]

[0011] Where, η=[xyz ψ θ] T To connect to the motion state of the AUV, which is also the system output of the kinematic equations, v r =[u r v r w r qr] T The control input for the kinematic equations is also the velocity of the docking AUV relative to the water flow, v. f =[v fx v fy v fz 0 0] T Let be the water flow velocity in the ground coordinate system, and n be the external dynamic disturbances experienced by the AUV underwater. M is the inertia matrix; C(v) is the Coriolis force and centripetal force matrix; D(v) is the damping matrix; J(ψ,θ) is the transformation matrix and includes the following parameters: θ is the pitch angle, ψ is the heading angle; τ T η is the control torque generated by the AUV during its motion, and g(η) is the restoring torque under the load system.

[0012] Step 2: Design a sliding mode controller for the dynamic model of the AUV under ocean current disturbance. The control input is:

[0013]

[0014] The output is the force of the vehicle's propulsion system;

[0015] Where: L represents the straight-line distance from the corresponding thruster to the geometric center of the AUV, and is expressed as:

[0016]

[0017] s is the sliding mode switching function, e B This refers to the observation error between the target AUV and the target AUV in the volume coordinate system. Given a function matrix containing the following parameters: α is the desired stabilization function. Here, θ is an estimate of the derivative of the stabilization function, ψ is the pitch angle, and ψ is the heading angle. ε represents the estimated values ​​of the unknown parameters of the system; k and ε are selected positive definite diagonal matrices; C i For the integral term coefficient within the controller, g(e B ) is a piecewise function and has the form:

[0018]

[0019] The adaptation rate is:

[0020]

[0021] in: Let Γ and F be constant positive definite symmetric matrices, and M be a symmetric positive definite matrix, representing the estimation error of unknown system parameters. C represents the error in the estimation of ocean current velocity. d This is the proportional coefficient within the controller.

[0022] Step 3: When the distance between the parent AUV and the docking follower AUV is 10 to 6 meters, it is a close-range docking; the monocular camera at the front of the docking follower AUV acquires the visual image of the four-source beacon at the rear of the parent AUV, and the visual image of the four-source beacon is analyzed to obtain the relative pose information between the parent AUV and the docking follower AUV; the relative pose information is input into the sliding mode controller, and the docking follower AUV's power system adjusts its pose and propels it toward the parent AUV under the control of the sliding mode controller;

[0023] Step 4: When the distance between the parent AUV and the docking follower AUV is less than 2 meters, the monocular camera at the front of the docking follower AUV acquires the Aruco Marker code at the rear of the parent AUV, and parses the Aruco Marker code to obtain the relative pose information between the parent AUV and the docking follower AUV; the relative pose information is input into the sliding mode controller, and the docking follower AUV's power system precisely adjusts its pose under the control of the sliding mode controller to advance towards the parent AUV and complete the docking.

[0024] The following AUV is equipped with an orientation and attitude measurement system and a Doppler velocimeter, which can obtain the AUV's real-time angular velocity, angle information, and speed information.

[0025] The docking mechanism includes several docking rings arranged sequentially along the axis of the parent AUV and an inner limiting plate.

[0026] Beneficial effects

[0027] This invention proposes a method for close-range docking of AUVs under ocean current interference based on sliding mode adaptive control. The controller design eliminates the need for decoupling of the AUV's motion. Furthermore, an integral term is added to the sliding mode adaptive controller design, employing Integral Sliding Mode Adaptive Control (ISMAC) to minimize chattering during the AUV's approach motion. Finally, in designing the control parameters, overshoot is minimized at the expense of settling time, ensuring successful docking. Simultaneously, the estimation of ocean current velocity gives the system a certain degree of anti-interference capability, demonstrating its robustness. Attached Figure Description

[0028] Figure 1 Schematic diagram of the docking mechanism of the present invention

[0029] Figure 2 Close-range docking process

[0030] Figure 3 AUV Target State Tracking Control Framework Diagram Figure 4 Target AUV and docking AUV operating trajectories Figure 5 Docking error curve

[0031] Figure 6 thrust variation Figure 2 Close-range docking process

[0032] Figure 7 Water flow velocity estimation curve Detailed Implementation

[0033] The present invention will now be further described in conjunction with the embodiments and accompanying drawings:

[0034] The technical solution of this invention is: a close-range docking method for AUVs based on sliding mode adaptive control, specifically including the following steps:

[0035] S1: Establish a dynamic model of AUV under ocean current disturbance;

[0036]

[0037] Where, η=[xyz ψ θ] T To connect to the motion state of the AUV, which is also the system output of the kinematic equations, v r =[u r v r w r qr] T The control input for the kinematic equations is also the velocity of the docking AUV relative to the water flow, v. f =[v fxv fy v fz 0 0] T Let be the water flow velocity in the ground coordinate system, and n be the external dynamic disturbances experienced by the AUV underwater. M is the inertia matrix, C(v) is the Coriolis force and centripetal force matrix, D(v) is the damping matrix, and J(ψ,θ) is the transformation matrix.

[0038] S2: Based on the established aircraft model, the relative pose of the tracking aircraft and the main aircraft is obtained through information obtained from the visual sensor;

[0039] S3. Design a sliding mode controller based on the established dynamic model of the AUV under ocean current disturbance. The steps include:

[0040] S3-1: Design the integral sliding surface based on the aircraft's operating status.

[0041] For sliding mode control, under the influence of model inaccuracies and external disturbances, the control law will inevitably be discontinuous when crossing the sliding surface s=0. In practice, the switching of the corresponding control law is bound to be imperfect; for example, the switching of the control law is not instantaneous in practice, and the value of s cannot be infinitely precise. This inevitably leads to chattering in the control law.

[0042] To reduce chattering and improve integral control, a function of the sliding mode switching method, in the form of... The variable can be used to redesign the sliding mode switching function as follows:

[0043]

[0044] To reduce chattering and improve integral control, a function of the sliding mode switching method, in the form of... The variable can be used to redesign the sliding mode switching function as follows:

[0045]

[0046] By choosing an appropriate constant, s(t=0)=0 can be made, then we have

[0047]

[0048] When the initial system error is large, the added integral term can make it difficult for the system to reach a stable state for a long time. In this case, the integral separation method can be used to limit the time during which the integral takes effect, thereby achieving better control. The newly designed sliding mode function is as follows:

[0049]

[0050] in,

[0051]

[0052] Taking the derivative with respect to s, we get

[0053] First, define the control output vector of the docked AUV as η=[xyz ψ θ] T The target's motion state change curve is η d =[x d y d z d ψ d θ d ] T Then the tracking error of the system in the ground coordinate system can be defined as:

[0054] e = η - η d

[0055] Taking its derivative, we can obtain

[0056]

[0057] During the docking process, the observation error between the docking AUV and the target AUV in its body coordinate system can be directly obtained through measurement, and its expression is:

[0058] e B =J -1 e = J -1 (η-η d )

[0059] In the formula, J = J(ψ,θ), and J is an orthogonal matrix. -1 =J T .

[0060] Here, by combining the kinematic equations of the docking AUV, the error derivative equation based on the docking AUV's body coordinate system can be obtained.

[0061]

[0062] e B This represents the error observed by the AUV in the volume coordinate system.

[0063] S3-2 changes the design sliding surface switching function according to the operating status of the aircraft;

[0064] For ease of description, let's choose a desired stabilization function as...

[0065]

[0066] in, Let be the estimated value of the ocean current velocity, and define the estimation error as . C d These are control parameters, and they are diagonal positive definite matrices.

[0067] Differentiating with respect to α, we get

[0068]

[0069] because This includes errors in water flow velocity estimation. This uncertain term is then defined. for The estimated value is of size

[0070]

[0071] Therefore, there is

[0072]

[0073] To ensure good parameter estimation performance for the system given the parameters, we assume here that the system's dynamic model (4-4) has a linearly parameterized form, i.e.

[0074]

[0075] Where, Θ∈R l×1 For unknown parameter vector matrix, Given a matrix of functions.

[0076] The docking process requires more precise control to improve the success rate. Therefore, an integral sliding surface is designed to mitigate the impact of chattering and eliminate static errors. The sliding surface switching function is designed as follows:

[0077]

[0078] In the formula, g(e) is a diagonal matrix with size g(e) = diag(g(e)). x ),g(e y ),g(e z ),g(e ψ ),g(e θ C i These are control parameters, and are positive definite diagonal matrices.

[0079] Differentiating it, we have

[0080]

[0081] S3-3 Design Controller Input;

[0082] To obtain a suitable control input, since M is a symmetric positive definite matrix, the first Lyapunov function can be designed as follows:

[0083]

[0084] Taking its derivative, we get

[0085]

[0086] in, The system parameters are estimated values, and the parameter estimation error is defined.

[0087] To ensure that the system can reach the sliding surface quickly enough under any condition and with minimal chattering, an exponential reaching law is chosen. Where k and ε are positive definite diagonal matrices, i.e., k = diag(k i ),k i >0, ε=diag(ε i ),ε i >0. Therefore, the control input can be designed as

[0088]

[0089] The S3-4 design incorporates further parameter adaptation laws and ocean current velocity adaptation laws to improve the docking performance of AUVs under ocean current interference.

[0090] To obtain further adaptive laws for parameters and ocean current velocity, a second Lyapunov function is designed.

[0091]

[0092] Where Γ and F are constant positive definite symmetric matrices.

[0093] Differentiating it, we have

[0094]

[0095] Therefore, the adaptive law of parameters and ocean current velocity can be obtained as follows:

[0096]

[0097] Finally, after eliminating the influence of ocean currents and parameter estimation errors, we can obtain...

[0098]

[0099] When the term ks+εsgns is large enough to eliminate the dynamic error interference n, we have Therefore, the overall stability of the system can be guaranteed.

[0100] To achieve better adaptive performance, the adaptive rate was modified to improve the docking performance of AUVs under ocean current interference.

[0101]

[0102] To verify the effectiveness of the above guidance method, the following embodiments are also provided in this invention.

[0103] Example 1:

[0104] To verify the control effect of the designed integral sliding mode adaptive controller in tracking the state of the target AUV, the simulation assumes that the maximum thrust output of each thruster is 20N, the maximum thrust change rate is 20N / s, the distance between the two vertical thrusters and the geometric center of the docking AUV is L2=L3=0.40m, and the distance between the two lateral thrusters and the geometric center is L4=L5=0.55m.

[0105] In the simulation, the ocean current velocity in the kinematic model is assumed to be...

[0106] v f =[-0.4 0.1 -0.2 0 0 0] T

[0107] The magnitude of the disturbance force for each degree of freedom is

[0108]

[0109] Where U(-1,+1) represents a uniform distribution between (-1,+1).

[0110] The selected control parameters and adaptive parameters are as follows:

[0111] C d =I5,C i =I5, k=10I5, ε=0.02I5, Γ=9I5, F=diag(4.5,9,5,0,0), g=0.7

[0112] Given the initial state of the docking AUV as η = [0 -2 0 0 0.1] T The simulation time is set to 120 seconds, and the sampling period is 0.1 seconds. The initial state η of the target is given. d =[5 -3 0 0 0] T Its equation of motion is

[0113]

[0114] The final AUV trajectory is as follows Figure 4 As shown.

[0115] Docking error curve as shown Figure 5As shown.

[0116] The thrust of the thruster changes as follows Figure 6 As shown.

[0117] The water flow velocity estimation curve is as follows: Figure 7 As shown.

[0118] Figures 4 to 7 The simulation demonstrated the tracking effect of a target moving along a curved path, with a total simulation time of 120 seconds. From Figure 4 The docking AUV trajectory curve in the middle and Figure 5 As can be seen from the angle error and position error curves, the docking AUV achieved a good fitting and tracking effect on the curve. Due to the large velocity error between the docking AUV and the target AUV under the initial simulation conditions, the angle error and position error fluctuated slightly in the initial stage of the simulation (0s~5s). However, under the control, the error quickly converged to zero.

[0119] from Figure 6 The thrust variation curve shows that during the initial adjustment process, the thruster has a large thrust output, which provides a high speed for the docking AUV, enabling the docking AUV to quickly track the target AUV. After entering the stable phase, even under unpredictable disturbances from external forces, the thrust output tends to stabilize. This demonstrates the good anti-shaking effect and anti-interference capability of the integral sliding mode controller.

[0120] In addition, from Figure 7 The estimation curves of ocean current velocity show that, when the target is moving at a variable speed, the adaptive control also has a good estimation effect on ocean current velocity.

[0121] In summary, the simulation results of controlling and tracking the target under two different motions demonstrate that the integral sliding mode adaptive controller proposed in this chapter has a good effect on tracking the state of the moving target and can achieve the expected docking performance. Furthermore, the estimation results of the ocean current velocity also show that the adaptive control has a good estimation effect on the ocean current velocity under different target motion conditions.

[0122] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention without departing from the principles and spirit of the present invention.

Claims

1. An AUV close-range docking method based on sliding mode adaptive control under ocean current disturbance, characterized in that: For a multi-underwater vehicle cooperative system, the multi-underwater vehicle cooperative system at least includes a mother AUV and a following AUV; the rear end of the mother AUV is provided with a docking mechanism, and the front end of the following AUV is provided with a monocular camera; the mother AUV is provided with a light source beacon and an ArucoMarker code for the following AUV to provide visual information for a visual sensor for sensing the relative pose of the two; the docking steps are as follows: Step 1: a dynamic model of the AUV under the interference of sea currents is established; where, η = [x y z ψ θ] T is the motion state of the docking AUV, and is the system output of the kinematic equation, v r = [u r v r w r q r] T is the control input of the kinematic equation, and is the velocity of the docking AUV relative to the water flow, v f = [v fx v fy v fz 0 0] T is the water flow velocity in the ground coordinate system, n is the external dynamic disturbance received by the AUV underwater; M is the inertia matrix; C(v) is the Coriolis force and centripetal force matrix; D(v) is the damping matrix; J(ψ,θ) is the conversion matrix and contains the following parameters θ is the pitch angle, and ψ is the heading angle; τ T is the control torque generated by the AUV during the motion process, and g(η) is the restoring torque in the load system; Step 2: a sliding mode controller of the dynamic model of the AUV under the interference of sea currents is designed, and the control input is: The output is the force of the vehicle propeller; Wherein L is the straight line distance from the corresponding propeller to the geometric center of the AUV and is represented as: s is a sliding mode switching function, e B is the observation error between the target AUV in the body coordinate system, is a known function matrix and contains the following parameters: a is the desired stabilizing function, is the estimated value of the derivative of the stabilizing function, θ is the pitch angle, and ψ is the heading angle; is the estimated value of the unknown parameters of the system; k and ε are selected positive definite diagonal matrices; C i is the coefficient of the integral term in the controller, g(e B ) is a piecewise function: The adaptive rate is: wherein: is the estimation error of the unknown parameters of the system, Γ, F are constant positive definite symmetric matrices, M is a symmetric positive definite matrix, is the estimation error of the current velocity, C d is the coefficient of the proportional term within the controller; Step 3: when the distance between the mother AUV and the docking following AUV is 10-6 meters, it is a close-range docking; the monocular camera at the front end of the docking following AUV obtains the visual image of the four light source beacons at the rear end of the mother AUV, and the relative pose information between the mother AUV and the docking following AUV is obtained by analyzing the visual image of the four light source beacons; the relative pose information is input into the sliding mode controller, and the power system of the docking following AUV adjusts the pose to the mother AUV under the control of the sliding mode controller; Step 4: when the distance between the mother AUV and the docking following AUV is less than 2 meters, the monocular camera at the front end of the docking following AUV obtains the Aruco Marker code at the rear end of the mother AUV, and the relative pose information between the mother AUV and the docking following AUV is obtained by analyzing the Aruco Marker code; the relative pose information is input into the sliding mode controller, and the power system of the docking following AUV adjusts the pose to the mother AUV under the control of the sliding mode controller and completes the docking.

2. The AUV close proximity docking method based on sliding mode adaptive control under ocean current disturbance according to claim 1, characterized in that: The following AUV is provided with an azimuth attitude measurement system and a Doppler speedometer, which can obtain real-time angular velocity and angle information and speed information of the AUV.

3. The AUV close proximity docking method based on sliding mode adaptive control under ocean current disturbance according to claim 1, characterized in that: The docking mechanism includes a plurality of docking rings and an inner limiting plate arranged in sequence along the axial direction of the mother AUV.

Images