CN109693774A - A kind of control method and system of submarine navigation device track - Google Patents

Abstract

The invention relates to a method and system for controlling the trajectory of an underwater vehicle. The control method includes: receiving a target signal, the target signal carrying target position information to be reached; determining a first state signal according to the current position information and the target position information, the first state signal carries the first direction angle and the first speed; the first filter component is used to process the first state signal to obtain the second direction angle and the second speed; the control force is determined according to the second speed, so that the underwater vehicle The sailing speed of the underwater vehicle reaches the target speed under the action of the control force; the control torque is determined according to the second direction angle, so that the sailing direction angle of the underwater vehicle reaches the target direction angle under the action of the control torque, suppressing high-frequency noise, external Interference, parameter uncertainty, etc., enhance the robustness of the system.

Description

A method and system for controlling the trajectory of an underwater vehicle

technical field

The invention relates to the field of aircraft, in particular to a method and system for controlling the trajectory of an underwater vehicle.

Background technique

With the development of electronic technology, measurement technology, control technology and other fields, it is easier for underwater vehicles to complete high-precision, high-quality and difficult tasks in harsh environments. Nowadays, underwater vehicles have been widely used in underwater detection, rescue, environmental monitoring, scientific research, military and national defense, etc., and the ability to accurately track the navigation trajectory is the necessary technical basis for the realization of the above applications. However, because such vehicles are usually underactuated, that is, the control quantity is less than the controlled quantity of the system, and the complex external environment has noise and strong interference, and there are modeling errors and uncertain parameters inside the system, which makes the underwater vehicle’s The design of the trajectory tracking controller has certain difficulties, and it cannot realize that the underwater vehicle can travel along a given trajectory, that is, it cannot meet the precise tracking of the trajectory of the underwater vehicle.

So we need to find new solutions to solve this problem.

SUMMARY OF THE INVENTION

The embodiments of the present invention provide a method and system for controlling the trajectory of an underwater vehicle, which satisfies the precise control of the trajectory of the underwater vehicle and realizes that the underwater vehicle travels along a given trajectory.

In a first aspect, a method for controlling the trajectory of an underwater vehicle is provided, the control method comprising:

Receive the target signal, the target signal carries the target position information to be reached;

Determine the first state signal according to the current position information and the target position information, and the first state signal carries the first direction angle and the first speed;

Using the first filter component to process the first state signal to obtain the second direction angle and the second speed;

Determine the control force according to the second speed, so that the sailing speed of the underwater vehicle reaches the target speed under the action of the control force;

The control torque is determined according to the second direction angle, so that the navigation direction angle of the underwater vehicle reaches the target direction angle under the action of the control torque.

In a possible implementation, using the first filtering component to process the first state signal to obtain the second direction angle and the second speed, including:

The second direction angle is obtained by setting the first time constant for the first filter component and processing the first direction angle by the first filter component;

The second speed is obtained by setting the second time constant to the first filter component and processing the first speed by the first filter component.

Further, in a possible implementation, the second direction angle and the first time constant, and the second speed and the second time constant respectively satisfy the following relationships:

where τ _1θ is the first time constant, and τ _1v is the second time constant; is the first direction angle, is the first velocity; θ _1d is the second directional angle, and v _1d is the second velocity.

In one possible implementation, determining the control force based on the second speed includes:

The control force is determined according to the current position information, the target position information, and the second speed.

In another possible implementation, the control force is determined according to the current position information, the target position information, and the second speed, including:

Calculate the position deviation between the current position information and the target position information;

Calculate the speed deviation between the current speed and the second speed;

The control force is determined according to the position deviation amount and the speed deviation amount.

Further, in a possible implementation, the second speed and the control force satisfy the following relationship:

Among them, F is the control force, m is the system mass, k ^1v is the first gain value, S _1v is the speed deviation, v _1d is the second speed, and S _1x is the current position information and the position of the target position information in the x direction Deviation, S _1y is the position deviation between the current position information and the target position information in the y direction, v is the current speed, θ ₁ is the current direction angle, is the first speed, is the first direction angle, C ₁ (v) is the Coriolis matrix; D ₁ (v) is the hydrodynamic damping matrix.

In a possible implementation, the control torque is determined according to the second direction angle, including:

Calculate the angle deviation between the current direction angle and the second direction angle;

The control torque is determined according to the second direction angle and the angle deviation.

In another possible implementation, the control torque is determined according to the second direction angle and the angle deviation, including:

Calculate a second state signal according to the second direction angle and the angle deviation, and the second state signal carries the first angular velocity;

Using the second filter component to process the second state signal to obtain the second angular velocity;

The control torque is determined according to the angular velocity deviation between the current angular velocity and the second angular velocity and the second angular velocity.

Further, in a possible implementation, the second speed and the control torque satisfy the following relationship:

Among them, τ is the control torque, I is the moment of inertia, k ^2θ is the second gain value, S _2θ is the angular velocity deviation, θ _2d is the second angular velocity, S _{1 θ} is the angular deviation, ω is the angular velocity of the underwater vehicle, C ₂ (ω) is the Coriolis matrix; D ₂ (v) is the hydrodynamic damping matrix.

In a second aspect, a system is provided, the system includes the first filtering component and the second filtering component in the first aspect, the first filtering component is configured to process the first state signal to obtain the second direction angle and the second speed , so that the target speed is determined according to the second speed, and the target direction angle is determined according to the second direction angle; the system is used to execute the method of the first aspect.

Based on the method and system for controlling the trajectory of an underwater vehicle provided by the embodiments of the present application, by adding a filter component to the system, the first state signal in the system is filtered and processed based on the filtered second speed and second direction. The angle determines the control force and control torque, so that under the action of the control force and control torque, the sailing speed of the underwater vehicle reaches the target speed, and the sailing direction angle reaches the target direction angle, which realizes the precise control of the trajectory of the underwater vehicle. , the underwater vehicle can travel along a given trajectory.

Description of drawings

In order to illustrate the technical solutions of the embodiments of the present invention more clearly, the following briefly introduces the accompanying drawings that need to be used in the embodiments of the present invention. Obviously, the drawings described below are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can also be obtained from these drawings without any creative effort.

1 is a schematic diagram of an underwater vehicle according to an embodiment of the present invention;

2 is a schematic flowchart of a method for controlling an underwater vehicle trajectory according to an embodiment of the present invention;

3 is a schematic diagram of a system framework provided by an embodiment of the present invention;

4 is a schematic diagram of an attitude tracking error provided by an embodiment of the present invention;

5 is a schematic diagram of a position tracking error provided by this embodiment;

6 is a schematic diagram of a plane trajectory provided by an embodiment of the present invention;

FIG. 7 is a schematic structural diagram of a system provided by an embodiment of the present invention.

Detailed ways

In order to realize the tracking of the trajectory of the underwater vehicle, the convergence of the system has been proved theoretically, but the designed control law is often very complicated and difficult to achieve. Among them, the control law is an algorithm for the system to form a control command, which describes the functional relationship between the controlled state variable and the system input signal. Secondly, without considering the difficulty of signal derivation, it is usually necessary to assume that the derivative of the internal signal of the system is easy to obtain, but in practice, the derivation of the signal is often approximate, and the method of derivation of the signal is often at intervals. Differentiation, when there is noise, uncertain parameters, and external interference, the derivative of the channel cannot be accurately obtained; and the use of differential derivation will also amplify the system noise.

Therefore, due to the harsh underwater environmental conditions, noise and strong interference, the usual derivation method is not applicable. In order to reduce the influence of signal derivation on system stability and make the control law easy to implement, the embodiment of the present invention provides a control method for trajectory tracking of an underwater vehicle, which provides a concise control input expression and is easy to apply in engineering; and By inputting the signal into the filter to replace the derivation of the current signal to suppress high-frequency noise, external interference, and parameter uncertainty, the robustness of the system is enhanced. certain performance characteristics.

It should be noted that, the system in this embodiment refers to the control system of the trajectory of the underwater vehicle. The underwater vehicle mentioned below may also be referred to as a vehicle.

The solutions of the embodiments of the present invention will be described below with reference to the accompanying drawings.

The underwater vehicle has a propeller in the x direction and the y direction respectively, which is driven by the propeller to generate inference, and the control torque is generated by adjusting the size of the thrust to achieve the purpose of steering. As shown in Figure 1, the heading angle of the underwater vehicle is defined. is θ, the position is P=[xy] ^T , the control input is q=[v ω] ^T , v and ω are the linear and angular velocities of the vehicle, respectively. In the description of the various embodiments below, v may also be referred to as velocity.

The available kinematics and dynamic equations are:

Where m is the mass of the underwater vehicle, I is the moment of inertia; F is the control force, τ is the control torque; C ₁ (v), C ₂ (ω) are Coriolis matrices; D ₁ (v), D ₂ ( v) is the hydrodynamic damping matrix. It can be seen from the above formula (1) that the model has 2 degrees of freedom, and the model output is 3 variables, which is an underactuated system, and can only achieve active tracking of 2 variables, and the remaining variables are in the follow-up or steady state. By designing the control force F and the control torque τ, the tracking of the mobile vehicle to the reference speed p _d and the follow-up of the included angle θ are realized.

FIG. 2 is a schematic flowchart of a method for controlling a trajectory of an underwater vehicle according to an embodiment of the present invention. The execution body of the method is an underwater vehicle, which is also called a system. As shown in Figure 2, the method may include the following steps:

S110: Receive a target signal, where the target signal carries target position information to be reached.

The system receives the target signal p _d =(x _1d , y _1d ) ^T at the desired target position (x _1d , y _1d ), as shown in FIG. 3 . Among them, p _d =(x _1d , y _1d ) ^T is used to describe the target position of the system navigation and the target motion state, wherein the target motion state includes the target speed of the system navigation, the target direction angle of the system navigation and other state information. FIG. 3 is a schematic diagram of a system framework provided by an embodiment of the present invention, and the schematic diagram of the system framework shows an internal implementation process of controlling the trajectory of an underwater vehicle.

S120: Determine a first state signal according to the current position information and the target position information, where the first state signal carries the first direction angle and the first speed.

The first direction angle is to determine the ideal direction angle according to the current position information (x, y) and the target position information (x _1d , y _1d ) Also called heading angle. The first velocity is the ideal linear velocity determined according to the current position information (x ₁ , y ₁ ) and the target position information (x _1d , y _1d )

In one embodiment, as shown in Figure 3, the system determines from (x, y) and (x _1d , y _1d ) and The specific process is as follows:

The system is based on the current position information (x, y) and the target position information (x _1d , y _1d ) carried in the target signal, the first state signal obtained by calculating the position dynamics model and the position deviation, the first state signal carry first speed and the first direction angle

Among them, the position dynamics model is used to describe the position and motion state of the system. Suppose x ₁ =x, y ₁ =y, θ ₁ =θ, θ ₂ =ω. The position dynamics model can be obtained from equation (1):

as well as,

Among them, the formula (2) can be called the position subsystem of the underwater vehicle, and the formula (3) can be called the attitude subsystem of the underwater vehicle. The position deviation can also be called the system position error, that is, the target signal p _d =(x _1d , y _1d ) ^T is the actual value of the target position information to be reached, and the current position information is the measured value, then it is considered that the current position information and the target The difference of the position information is the system position error, that is, the position deviation. The position continuous difference includes the position deviation in the x-direction and the position deviation in the y-direction, namely:

S _1x =x ₁ -x _1d

S _1y =y ₁ -y _1d (4)

Among them, S _1x is the position deviation amount of the current position information and the target position information in the x direction, and S _1y is the position deviation amount of the current position information and the target position information in the y direction.

From this, it can be seen that Substitute into formula (2) to get:

Because the system is an underactuated system, only designing v is unable to track the position information in both x and y directions at the same time, so the ideal angle information Also as a control variable to overcome the underactuated problem. Control target selection as tracking position and angle, position subsystem and attitude subsystem Indirectly control the position variable, design the control force F so that v tracks the target speed Design the control torque τ so that θ ₁ tracks the target angle

Therefore, the first Lyapunov function is designed as:

Available,

Define the first state signal as:

Wherein, k ^1x and k ^1y are preset control gain values respectively.

Substitute equation (8) into equation (5) to get:

in,

Combining formula (7), we can get:

first speed is the ideal speed during sailing, the first direction angle It is the ideal heading angle during sailing.

According to formula (8), it can be known that, Will The value range is limited to (-π/2, π/2), and the ideal attitude trajectory tracking is obtained. for:

that is obtained by the above formula is the ideal angle required by the position control law, if the actual angle θ ₁ of the underwater vehicle is different from the ideal angle equal, the ideal trajectory control law can be realized, but the actual θ ₁ and It is impossible to be completely consistent, especially in the initial stage of control, which will cause instability of the closed-loop tracking system.

If the usual sliding mode control method is used, equations (12) and (13) need to be derived to achieve trajectory tracking. Taking the derivation of formula (12) and formula (13) respectively, we can get:

Expanding equations (15) and (15), the number of terms is very complex; moreover, with the increase of the derivative order, the result will be more complicated, resulting in "differential explosion".

In order to solve the above problem, the embodiment of the present invention adds a filter component to the system, and the filter component may be a first-order low-pass filter, and the output of the filter is used to replace the original signal, so as to avoid the derivation of the signal. Will input to the low-pass filter, that is, execute S130. It should be noted that, in this embodiment, the filtering component may include multiple filtering components.

S130, using the first filter component to process the first state signal to obtain the second direction angle and the second speed.

Considering that the noise of the system will cause strong interference to the derivation process of the signal, causing large errors and affecting the stability of the system, a filter component is introduced to convert the intermediate signal. and Filtering is performed to obtain the second direction angle θ _1d and the second velocity v _1d of the output signal. The first time constant τ _1θ and the second time constant τ _1v are set in the AND filter assembly of FIG. 3 , and τ _1θ is used to adjust the intermediate signal for filtering, τ _1v is used for the intermediate signal Filter; the settings of τ _1θ and τ _1v will affect the filtering effect.

In one embodiment, the filter component satisfies the following relationship:

where τ _1θ and τ _1v are the time constants of the filter; is the input of the filter; θ _1d , v _1d are the output of the filter.

S140: Determine the control force according to the second speed, so that the sailing speed of the underwater vehicle reaches the target speed under the action of the control force.

Then use the filtered v _1d and the speed deviation S _1v between the current speed and the filtered v _1d to substitute into the designed control force F formula:

Among them, F is the control force, m is the system mass, k ^1v is the first gain value, S _1v is the speed deviation, v _1d is the second speed, and S _1x is the current position information and the position of the target position information in the x direction Deviation, S _1y is the position deviation between the current position information and the target position information in the y direction, v is the current speed, θ ₁ is the current direction angle, is the first speed, is the first direction angle, C ₁ (v) is the Coriolis matrix; D ₁ (v) is the hydrodynamic damping matrix. It should be noted that k ^1v here is a preset gain value, or is called a control gain value.

Obtain the control force F, and then according to the position dynamics model

Obtain v and x ₁ , y ₁ . The v and x ₁ , y ₁ are values that are constantly updated in the implementation of the present invention. After obtaining F according to the current speed v, obtain a new v according to F, and further integrate Obtain x ₁ and y ₁ , and update the current speed v to ensure that the vehicle can sail according to the preset v, so that the vehicle position coordinates x ₁ , y ₁ can track the target signal p _d =(x _1d , y _1d ) ^T .

S150: Determine the control torque according to the second direction angle, so that the sailing direction angle of the underwater vehicle reaches the target direction angle under the action of the control torque.

The system uses the filtered θ _1d and the angle deviation S _1θ between the current direction angle θ ₁ and the filtered θ _1d to determine the second state signal, and the second state signal carries the first angular velocity

Among them, k ^1θ is a preset control gain value. Then, the second state signal is processed by the second filter component to obtain the second angular velocity θ _2d , wherein, in the process of processing the second state signal by the second filter component, a time constant τ _2θ is set to satisfy the second filter component. The filtering effect of the component on the second state signal. Next, according to the current angular velocity θ ₂ =ω and the angular velocity deviation S _2θ of the second angular velocity θ _2d , and the second angular velocity θ _2d , determine the control torque τ, and specifically substitute these quantities into the designed control torque τ formula:

Among them, τ is the control torque, I is the moment of inertia, k ^2θ is the second gain value, S _{2 θ} is the angular velocity deviation, θ _2d is the second angular velocity, S _{1 θ} is the angular deviation, ω is the vehicle angular velocity, C ₂ ( ω) is the Coriolis matrix; D ₂ (v) is the hydrodynamic damping matrix. It should be noted that k ^2θ here is a preset gain value, or is called a control gain value. The control torque τ is obtained, and according to the formula:

to get θ ₁ . Since θ ₁ =θ, the sailing angle θ is obtained. In this embodiment, the sailing angle θ is also a value that is constantly updated. When the control torque τ is obtained according to the current θ, θ is obtained from the control torque τ to ensure that the sailing can follow the preset heading angle θ.

During the whole sailing process, the sailing is carried out according to the preset sailing speed v _d and the predetermined sailing angle θ _d , that is, the sailing according to the preset trajectory p _d =(x _1d , y _1d ) ^T is realized. Using the embodiment of the present invention to increase the filter element to filter the intermediate signal, avoid the amplification of the system noise caused by the search and derivation, and avoid the inability to accurately obtain the derivative of the signal under harsh conditions such as noise, parameter uncertainty, and external interference. It can suppress high-frequency noise, external interference, and parameter uncertainty, enhance the robustness of the system, improve the stability of the system, and make the control law easy to implement. And the relationship between the designed control force and the control torque is relatively simple, and it is easy to be used in engineering applications.

It should be noted that a filtering component is set in the system shown in FIG. 3, including a first filtering component and a second filtering component, and the first filtering component and the second filtering component may be filters, or devices with filtering functions . In one embodiment, the first filtering component and the second filtering component may be the same filtering component, or may be separate filtering components, which are not limited in this embodiment.

It should also be noted that, in this embodiment, "first" and "second" are only for distinguishing things, and do not limit the things themselves.

In Fig. 3, the designed control force F and control torque τ are directly used to realize the control of the trajectory of the underwater vehicle. The design process of the control force F and the control torque τ is described below.

First, define the position tracking error (or called velocity deviation) as:

S _1v = vv _1d (17)

Since the control objective is to make v track The position tracking error defined above is the error between v and v _1d , in other words, the deviation between v and v _1d . if then v track or v _1d is equivalent. definition The filtered boundary layer error with v _1d is:

If it can be guaranteed that S _1v → 0 and obviously

Design the second Lyapunov function as:

Guidance can be obtained:

From equation (16), it can be known that, Substitute into formula (20) and expand it to get:

The design position control force is:

Substitute equation (22) into equation (21) to get:

The above is the design of the control force F, and the design process of the control torque τ is described below:

The attitude tracking error (or called angle deviation) is defined as:

S _1θ = θ ₁ -θ _1d (24)

The control objective is to make θ ₁ track And the attitude tracking error defined above is to make θ ₁ track θ _1d , if Then theta ₁ tracks Both are equivalent to θ _1d . definition The filtered boundary layer error with θ _1d is:

have to Design the third Lyapunov function as:

Guidance can be obtained:

The virtual control law is designed as:

Will into the following low-pass filter,

where τ _2θ is the time constant of the filter; is the input of the filter; θ _2d is the output of the filter. Similar to Equation (24) and Equation (25), define the second set of attitude tracking error and filter boundary layer error:

S _2θ =θ ₂ -θ _2d

Available,

According to formula (16), it can be known that, According to equation (30) and combined with equation (28), it can be known that,

Substituting the above result into equation (27) can get:

The fourth Lyapunov function is designed as:

Guidance can be obtained:

According to formula (29), it can be known that,

According to formula (28) and formula (31), it can be known that,

Substitute equations (32) and (35) into equations (34) to get:

The design attitude control torque is:

The control force F and control torque τ are designed as described above, and the stability of the designed control force F and control torque τ is analyzed below:

Substitute equation (38) into equation (37) to get:

Substituting equation (23) into equation (39), we can get:

From the definition of the filter boundary layer error, we can get:

According to formula (8), formula (14) and formula (15), it can be known that there are non-negative continuous functions B _1v , B _1θ , B _2θ satisfying:

Assumption 1. The tracking target trajectory p _d is bounded, and there is a positive number χ ₁ such that holds, there is a positive number χ ₂ such that established.

Theorem 1: For the aircraft system described by formula (1), the control force and control torque designed by formula (22) and formula (38) are used. On the basis of Assumption 1, when the initial value of the system V ₄ (0)≤c, c is any positive constant. Then, the state signals of the aircraft can be semi-globally consistent by adjusting the control gains k ^1x , k ^1y , k ^1v , k ^1θ , k ^2θ ; functions B _1v , B _1θ , B _2θ ; time constants τ _1v , τ _1θ , τ _2θ Bounded, whose tracking error is confined to a small residual.

Proof: Using Young's inequality, equation (40) satisfies:

On the basis of the establishment of V ₄ (0)≤p, let Ω ₁ and Ω ₂ be compact sets, then they can be expressed as:

It can be seen that Ω ₁ ×Ω ₂ is also a compact set. When V≤c is established, B _1v , B _1θ , and B _2θ have maximum values on Ω ₁ ×Ω ₂ , which are denoted as M _1v , M _1θ , and M _2θ . therefore Satisfy:

also because

Substitute equation (46) into equation (45)

The control parameters of the selected system are as follows: k ^1θ ≥1+r, where r can set the final error of the system, then

When V ₄ =c, since B _* ≤ M _* (*=1v, 1θ, 2θ), we can get Therefore, it can be known that V ₄ ≤c is an invariant set of the system; in addition, when V ₄ (0)≤c, that is, V ₄ (t)≤c for any t>0.

Available

Solving (48) can get

Therefore all signals of a closed-loop system are bounded, and

It can be seen from the above formula that increasing the parameter r can adjust the control gains k ^1x , k ^1y , k ^1v , k ^1θ , k ^2θ ; functions B _1v , B _1θ , B _2θ ; time constants τ _1v , τ _1θ , τ _2θ To achieve, make V ₄ (t) converge to a residual set, the tracking error can be arbitrarily small, to meet the needs of engineering.

From the control force input formula (22), it can be known that the required signal needs to be and when After the input filter is satisfied input through filter The linear operation with the output v _1d avoids the derivation of the signal; through the control torque input equation (38), it can be seen that the required signal needs and when After the input filter is satisfied input through filter The linear operation with the output θ _2d avoids the derivation of the signal; it enhances the robustness of the system to noise interference and parameter uncertainty.

In a specific embodiment, for example, the mass of the underwater vehicle is m=10Kg, and the moment of inertia is I=4Kg·m ² . The control target is p _d =(-5+t, sin(0.5t)+1) ^T m; the control gain is: k ^1x =5, k ^1y =5, k ^1v =5, k ^1θ =5, k ^2θ = 5; The filter time constant is: τ _1v =0.02, τ _1θ =0.02, τ _2θ =0.02; the initial state is p(0)=(-6,-2) ^T m, θ(0)=0°, through The method for controlling the trajectory of the underwater vehicle provided by the embodiment of the present invention obtains the schematic diagrams in FIGS. 4 to 6 .

4 shows a schematic diagram of the attitude tracking error S _1θ , that is, a schematic diagram of the relationship between the angle (rad) of S _1θ and the time t(s). As shown in Figure 4, the angle value of S _1θ tends to zero in a very short time and remains stable, which proves that the actual sailing angle θ ₁ is equal to the angle of θ _1d , that is, the method can be used to track the underwater well. The sailing trajectory of the aircraft is guaranteed to sail according to the preset sailing angle θ.

Figure 5 shows a schematic diagram of the system position tracking error, that is, a schematic diagram of the relationship between the system position and time t(s). As shown in Figure 5, the position error S _1x in the x direction and the position error S _1y in the y direction can be achieved within about 3s. Both are zero, which means that the actual sailing position information is consistent with the set sailing position information. This method can well track the navigation trajectory of the underwater vehicle, that is, it can ensure the navigation according to the preset position.

Fig. 6 shows a schematic diagram of the plane trajectory of the navigating vehicle. As shown in Fig. 6, the actual navigating position P almost coincides with the expected navigating position P _d , which also shows that the underwater navigation can be well tracked using this method The position track of the underwater vehicle can be used to ensure that the underwater vehicle can navigate according to the set position.

An embodiment of the present invention further provides a control system for the trajectory of an underwater vehicle. As shown in FIG. 7 , the control system 700 includes:

The receiving module 710 is used for receiving a target signal, and the target signal carries the target position information to be reached;

A determination module 720, configured to determine a first state signal according to the current position information and the target position information, where the first state signal carries the first direction angle and the first speed;

a first filtering component 730, configured to process the first state signal to obtain a second direction angle and a second speed;

The determining unit 720 is further configured to determine the control force according to the second speed, so that the sailing speed of the underwater vehicle reaches the target speed under the action of the control force;

The determining unit 720 is further configured to determine the control torque according to the second direction angle, so that the navigation direction angle of the underwater vehicle reaches the target direction angle under the action of the control torque.

Optionally, in one embodiment, the first filtering component 730 is specifically configured to:

The second direction angle is obtained by setting the first time constant for the first filter component and processing the first direction angle by the first filter component;

The second speed is obtained by setting the second time constant to the first filter component and processing the first speed by the first filter component.

Optionally, in one embodiment, the second direction angle and the first time constant, and the second speed and the second time constant respectively satisfy the following relationships:

where τ _1θ is the first time constant, and τ _1v is the second time constant; is the first direction angle, is the first velocity; θ _1d is the second directional angle, and v _1d is the second velocity.

Optionally, in one embodiment, the determining unit 720 is specifically configured to:

The control force is determined according to the current position information, the target position information, and the second speed.

Optionally, in one embodiment, the determining unit 720 is specifically configured to:

Calculate the position deviation between the current position information and the target position information;

Calculate the speed deviation between the current speed and the second speed;

The control force is determined according to the position deviation amount and the speed deviation amount.

Further, in one embodiment, the second speed and the control force satisfy the following relationship:

Among them, F is the control force, m is the mass of the underwater vehicle, k ^1v is the first gain value, S _1v is the speed deviation, v _1d is the second speed, and S _1x is the current position information and target position information at The position deviation in the x direction, S _1y is the position deviation between the current position information and the target position information in the y direction, v is the current speed, θ ₁ is the current direction angle, is the first speed, is the first direction angle, C ₁ (v) is the Coriolis matrix; D ₁ (v) is the hydrodynamic damping matrix.

Optionally, in one embodiment, determining the control torque according to the second direction angle includes:

Calculate the angle deviation between the current direction angle and the second direction angle;

The control torque is determined according to the second direction angle and the angle deviation.

In another possible implementation, the control system further includes a second filtering component 740;

The determining unit 720 is configured to calculate a second state signal according to the second direction angle and the angle deviation, and the second state signal carries the first angular velocity;

The second filtering component 740 processes the second state signal to obtain the second angular velocity;

The determining unit 720 determines the control torque according to the angular velocity deviation between the current angular velocity and the second angular velocity, and the second angular velocity.

Further, in one embodiment, the second speed and the control torque satisfy the following relationship:

Among them, τ is the control torque, I is the moment of inertia, k ^2θ is the second gain value, S _{2 θ} is the angular velocity deviation, θ _2d is the second angular velocity, S _{1 θ} is the angular deviation, ω is the vehicle angular velocity, C ₂ ( ω) is the Coriolis matrix; D ₂ (v) is the hydrodynamic damping matrix.

The system 700 can implement the methods/steps shown in FIG. 2 and FIG. 3 , and can achieve the same technical effect, which is not repeated here for brevity.

It should be noted that, in an embodiment, the first filtering component and the second filtering component may be the same filtering component, or may be independent filtering components, which are not limited in this embodiment.

Professionals should be further aware that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. Interchangeability, the above description has generally described the components and steps of each example in terms of function. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Skilled artisans may implement the described functionality using different methods for each particular application, but such implementations should not be considered beyond the scope of the present invention.

The steps of a method or algorithm described in connection with the embodiments disclosed herein may be implemented in hardware, a software module executed by a processor, or a combination of the two. A software module can be placed in random access memory (RAM), internal memory, read only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, removable disk, CD-ROM, or any other in the technical field. in any other known form of storage medium.

The specific embodiments described above further describe the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above descriptions are only specific embodiments of the present invention, and are not intended to limit the scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims (10)

1. a control method of an underwater vehicle trajectory, is characterized in that, described control method comprises: receiving a target signal, the target signal carrying the target location information to be reached; Determine a first state signal according to the current position information and the target position information, where the first state signal carries a first direction angle and a first speed; Using the first filter component to process the first state signal to obtain a second direction angle and a second speed; determining a control force according to the second speed, so that the sailing speed of the underwater vehicle reaches a target speed under the action of the control force; The control torque is determined according to the second direction angle, so that the sailing direction angle of the underwater vehicle reaches the target direction angle under the action of the control torque. 2 . The method according to claim 1 , wherein the obtaining the second direction angle and the second speed by processing the first state signal by using the first filter component, comprising: 2 . The second direction angle is obtained by setting a first time constant for the first filter component, and processing the first direction angle by the first filter component; The second speed is obtained by setting a second time constant for the first filter component, and processing the first speed with the first filter component. 3. The method according to claim 2, wherein the second direction angle and the first time constant, and the second speed and the second time constant respectively satisfy the following relationships: where τ _1θ is the first time constant, and τ _1v is the second time constant; is the first direction angle, is the first speed; θ _1d is the second direction angle, v _1d is the second speed; Yes The zero initial state of ; θ _1d (0) is the zero initial state of θ _1d ; Yes The zero initial state of ; v _1d (0) is the zero initial state of v _1d . 4. The method according to claim 1, wherein the determining the control force according to the second speed comprises: The control force is determined based on the current position information, the target position information, and the second velocity. 5. The method according to claim 4, wherein the determining the control force according to the current position information, the target position information, and the second speed comprises: Calculate the position deviation between the current position information and the target position information; calculating the speed deviation between the current speed and the second speed; The control force is determined based on the position deviation amount and the speed deviation amount. 6. The method according to claim 4 or 5, wherein the second speed and the control force satisfy the following relationship: Among them, F is the control force, m is the mass of the underwater vehicle, k ^1v is the first gain value, S _1v is the speed deviation, v _1d is the second speed, and S _1x is the current position information and target position information at The position deviation in the x direction, S _1y is the position deviation between the current position information and the target position information in the y direction, v is the current speed, θ ₁ is the current direction angle, is the first speed, is the first direction angle, C ₁ (v) is the Coriolis matrix; D ₁ (v) is the hydrodynamic damping matrix. 7. The method according to claim 1, wherein the determining the control torque according to the second direction angle comprises: calculating the angular deviation between the current direction angle and the second direction angle; The control torque is determined according to the second direction angle and the angle deviation. 8. The method according to claim 7, wherein the determining the control torque according to the second direction angle and the angle deviation comprises: calculating a second state signal according to the second direction angle and the angle deviation, the second state signal carrying the first angular velocity; Using the second filter component to process the second state signal to obtain a second angular velocity; The control torque is determined according to the angular velocity deviation between the current angular velocity and the second angular velocity and the second angular velocity. 9. The method according to claim 7 or 8, wherein the second speed and the control torque satisfy the following relationship: Among them, τ is the control torque, I is the moment of inertia, k ^2θ is the second gain value, S _2θ is the angular velocity deviation, θ _2d is the second angular velocity, S _{1 θ} is the angular deviation, ω is the angular velocity of the underwater vehicle, C ₂ (ω) is the Coriolis matrix; D ₂ (v) is the hydrodynamic damping matrix. 10. A system, characterized in that the system comprises the first filtering component and the second filtering component described in claims 1 to 9, the first filtering component being used for processing the first state signal The second direction angle and the second speed are obtained, so that the target speed is determined according to the second speed, and the target direction angle is determined according to the second direction angle;