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

A kind of control method and system of submarine navigation device track Download PDF

Info

Publication number
CN109693774A
CN109693774A CN201811641485.7A CN201811641485A CN109693774A CN 109693774 A CN109693774 A CN 109693774A CN 201811641485 A CN201811641485 A CN 201811641485A CN 109693774 A CN109693774 A CN 109693774A
Authority
CN
China
Prior art keywords
speed
direction angle
angle
target
control
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201811641485.7A
Other languages
Chinese (zh)
Other versions
CN109693774B (en
Inventor
王锐
鄢社锋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Institute of Acoustics CAS
Original Assignee
Institute of Acoustics CAS
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Institute of Acoustics CAS filed Critical Institute of Acoustics CAS
Priority to CN201811641485.7A priority Critical patent/CN109693774B/en
Publication of CN109693774A publication Critical patent/CN109693774A/en
Application granted granted Critical
Publication of CN109693774B publication Critical patent/CN109693774B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
    • B63G—OFFENSIVE OR DEFENSIVE ARRANGEMENTS ON VESSELS; MINE-LAYING; MINE-SWEEPING; SUBMARINES; AIRCRAFT CARRIERS
    • B63G8/00—Underwater vessels, e.g. submarines; Equipment specially adapted therefor
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
    • B63C—LAUNCHING, HAULING-OUT, OR DRY-DOCKING OF VESSELS; LIFE-SAVING IN WATER; EQUIPMENT FOR DWELLING OR WORKING UNDER WATER; MEANS FOR SALVAGING OR SEARCHING FOR UNDERWATER OBJECTS
    • B63C11/00—Equipment for dwelling or working underwater; Means for searching for underwater objects
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions

Landscapes

  • Engineering & Computer Science (AREA)
  • Mechanical Engineering (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Ocean & Marine Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Feedback Control In General (AREA)

Abstract

The present invention relates to the control method and system of a kind of submarine navigation device track, which includes: reception echo signal, and echo signal carries the target position information to be reached；Determine that first state signal, first state signal carry first direction angle and First Speed according to current location information and target position information；First state signal is handled using the first filtering unit to obtain second direction angle and second speed；Control force is determined according to second speed, so that the route speed of submarine navigation device reaches target velocity under the action of control force；Control moment is determined according to second direction angle, so that the navigation orientation angle of submarine navigation device reaches target direction angle under the action of control moment, it is suppressed that situations such as high-frequency noise, external interference, Parameter uncertainties, enhance the robustness of system.

Description

Method and system for controlling track of underwater vehicle
Technical Field
The invention relates to the field of aircrafts, in particular to a method and a system for controlling the track of an underwater aircraft.
Background
With the development of the fields of electronic technology, measurement technology, control technology and the like, the underwater vehicle can more easily complete tasks with high precision, high quality and high difficulty in a severe environment. Nowadays, underwater vehicles have been widely applied in the fields of underwater exploration, rescue, environmental monitoring, scientific research, military and national defense, etc., and the accurate tracking capability of navigation tracks is the necessary technical basis for realizing the above application. However, the vehicles are usually under-actuated, that is, the control quantity is smaller than the controlled quantity of the system, noise and strong interference exist in the external complex environment, and modeling errors and uncertain parameters exist in the system, so that the design of the track tracking controller of the underwater vehicle has certain difficulty, and the underwater vehicle cannot run along a given track, that is, the accurate tracking of the track of the underwater vehicle cannot be met.
New solutions need to be found to solve this problem.
Disclosure of Invention
The embodiment of the invention provides a method and a system for controlling the track of an underwater vehicle, which meet the requirement of accurately controlling the track of the underwater vehicle and realize that the underwater vehicle runs along a given track.
In a first aspect, a method for controlling an underwater vehicle trajectory is provided, the method comprising:
receiving a target signal, wherein the target signal carries target position information to be reached;
determining a first state signal according to the current position information and the target position information, wherein the first state signal carries a first direction angle and a first speed;
processing the first state signal by using a first filtering component to obtain a second direction angle and a second speed;
determining a control force according to the second speed so that the navigation speed of the underwater vehicle reaches a target speed under the action of the control force;
and determining a control moment according to the second direction angle, so that the navigation direction angle of the underwater vehicle reaches a target direction angle under the action of the control moment.
In one possible implementation, the processing the first state signal by the first filtering component to obtain the second direction angle and the second speed includes:
setting a first time constant for the first filtering component, and processing the first direction angle by using the first filtering component to obtain a second direction angle;
and setting a second time constant for the first filtering component, and processing the first speed by using the first filtering component to obtain a second speed.
Further, in one possible implementation, the second direction angle and the first time constant, and the second speed and the second time constant satisfy the following relationships, respectively:
wherein, tau1θIs a first time constant, τ1vIs a second time constant;is the angle of the first direction and,is a first speed; theta1dIs a second direction angle, v1dIs the second speed.
In one possible implementation, determining the control force based on the second speed includes:
the control force is determined based on the current position information, the target position information, and the second velocity.
In another possible implementation, determining the control force based on the current position information, the target position information, and the second velocity includes:
calculating the position deviation amount of the current position information and the target position information;
calculating a speed deviation amount of the current speed and the second speed;
the control force is determined based on the position deviation amount and the speed deviation amount.
Further, in one possible implementation, the second speed and the control force satisfy the following relationship:
where F is the control force, m is the system mass, k1vIs a first gain value, S1vIs the amount of velocity deviation, v1dIs the second speed, S1xIs the amount of positional deviation in the x direction of the current positional information and the target positional information, S1yIs the amount of positional deviation in the y-direction between the current positional information and the target positional information, v is the current velocity, θ1Is the angle of the current direction and,is the first speed of the motor vehicle,is a first direction angle, C1(v) Is a Coriolis matrix; d1(v) Is a hydrodynamic damping matrix.
In one possible implementation, determining the control moment according to the second direction angle includes:
calculating the angle deviation amount of the current direction angle and the second direction angle;
and determining the control moment according to the second direction angle and the angle deviation amount.
In another possible implementation, determining the control torque according to the second direction angle and the angular deviation includes:
calculating a second state signal according to the second direction angle and the angle deviation amount, wherein the second state signal carries the first angular speed;
processing the second state signal by using a second filtering component to obtain a second angular velocity;
the control torque is determined based on the angular velocity deviation amount of the current angular velocity from the second angular velocity, and the second angular velocity.
Further, in one possible implementation, the second speed and the control torque satisfy the following relationship:
where τ is the control moment, I is the moment of inertia, k2θIs the second gain value, S2θIs the amount of angular velocity deviation, θ2dIs the second angular velocity, S1θIs the angular deviation, ω is the angular velocity of the underwater vehicle, C2(ω) is a Coriolis matrix; d2(v) Is a hydrodynamic damping matrix.
In a second aspect, a system is provided, where the system includes the first filtering component and the second filtering component in the first aspect, and the first filtering component is configured to process the first state signal to obtain a second direction angle and a second speed, so as to determine a target speed according to the second speed and determine a target direction angle according to the second direction angle; the system is configured to perform the method of the first aspect.
According to the method and the system for controlling the underwater vehicle track, the filtering assembly is added in the system to filter the first state signal in the system, and the control force and the control torque are determined based on the filtered second speed and the filtered second direction angle, so that the underwater vehicle can navigate at the target speed and the navigation direction angle can be reached under the action of the control force and the control torque, the accurate control of the underwater vehicle track is realized, and the underwater vehicle can travel along the given track.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings needed to be used in the embodiments of the present invention will be briefly described below, and it is obvious that the drawings described below are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
FIG. 1 is a schematic illustration of an underwater vehicle provided by an embodiment of the present invention;
fig. 2 is a schematic flow chart of a method for controlling the trajectory of an underwater vehicle according to an embodiment of the present invention;
FIG. 3 is a system framework diagram provided by an embodiment of the invention;
FIG. 4 is a schematic diagram of an attitude tracking error provided by an embodiment of the present invention;
FIG. 5 is a schematic diagram of a position tracking error provided by the present embodiment;
FIG. 6 is a schematic plan view of a track provided by an embodiment of the present invention;
fig. 7 is a schematic structural diagram of a system according to an embodiment of the present invention.
Detailed Description
In order to realize the tracking of the track of the underwater vehicle, the convergence of the system is proved theoretically at present, but the designed control law is complex and difficult to realize. The control law is an algorithm for forming a control command by the system and describes a functional relation between a controlled state variable and a system input signal. Secondly, the difficult realization of signal derivation is not considered, it is generally required to assume that the derivative of the signal in the system is easy to obtain, and in practice, the signal derivation is often approximate, and the common method of signal derivation is to differentiate the signal at intervals, and when noise, uncertain parameters and external interference exist, the derivative of the channel cannot be accurately obtained; but the use of differential derivation also amplifies system noise.
Therefore, the conventional derivation method is not suitable because of the harsh underwater environment and the existence of noise and strong interference. In order to reduce the influence of signal derivation on system stability and enable the control law to be easy to realize, the embodiment of the invention provides a control method for tracking the track of an underwater vehicle, and the provided control input expression is simple and is easy for engineering application; the signal is input into a filter to replace the derivation of the current signal, so that high-frequency noise, external interference and uncertain parameters are suppressed, and the robustness of the system is enhanced, wherein the robustness refers to the characteristic that the system maintains certain other performances under certain parameter variation.
It should be noted that the system in this embodiment refers to a control system for the trajectory of an underwater vehicle. The underwater vehicle mentioned below may also be referred to as a vehicle.
The following describes aspects of embodiments of the present invention with reference to the drawings.
Underwater vehicle in xThe direction and the y direction are respectively provided with a propeller, the propeller drives the propellers to generate reasoning, the control moment is generated by adjusting the thrust, and the purpose of steering is achieved, as shown in fig. 1, the heading angle of the underwater vehicle is defined as theta, and the position is P ═ x y]TThe control input is q ═ v ω]TAnd v and ω are the linear and angular velocity of the aircraft, respectively. In the description of the various embodiments below, v may also be referred to as velocity.
The available kinematic and kinetic equations are:
wherein m is the mass of the underwater vehicle, and I is the moment of inertia; f is control force, and tau is control moment; c1(v)，C2(ω) is a Coriolis matrix; d1(v)，D2(v) Is a hydrodynamic damping matrix. As can be seen from the above formula (1), the model has 2 degrees of freedom, the output of the model is 3 variables, the model is an under-actuated system, active tracking of only 2 variables can be realized, and the remaining variables are in a follow-up or stable state. Reference speed p of mobile aircraft is realized by designing control force F and control moment taudTracking of angle theta.
Fig. 2 is a schematic flow chart of an underwater vehicle trajectory control method according to an embodiment of the present invention. The execution subject of the method is an underwater vehicle, also called system, which, as shown in fig. 2, may comprise the following steps:
s110, receiving a target signal, wherein the target signal carries target position information to be reached.
The system receives a desired target position (x)1d,y1d) Target signal p ofd＝(x1d,y1d)TAs shown in fig. 3. Wherein p isd＝(x1d,y1d)TThe system is used for describing the target position of the system navigation and the target motion state, wherein the target motion state comprises state information such as target speed of the system navigation, target direction angle of the system navigation and the like. Fig. 3 is a system block diagram illustrating an internal implementation process for controlling the trajectory of an underwater vehicle in accordance with an embodiment of the present invention.
And S120, determining a first state signal according to the current position information and the target position information, wherein the first state signal carries a first direction angle and a first speed.
The first direction angle is based on the current position information (x, y) and the target position information (x)1d,y1d) Determining the angle of the ideal directionAlso known as the heading angle. The first speed is based on the current position information (x)1,y1) And target location information (x)1d,y1d) Determined ideal linear velocity
In one embodiment, as shown in FIG. 3, the system is based on (x, y) and (x)1d,y1d) DeterminingAndthe specific process is as follows:
the system is based on the current position information (x, y) and the target position information (x) carried in the target signal1d,y1d) A first state signal obtained by calculating a position dynamic model and a position deviation value, wherein the first state signal carries a first speedAnd a first direction angle
Wherein the position dynamics model is used to describe the position and motion state of the system. Let x be1＝x，y1＝y，θ1＝θ，θ2ω. A position kinetic model can be obtained from equation (1):
and the number of the first and second groups,
wherein, the formula (2) can be called as a position subsystem of the underwater vehicle, and the formula (3) can be called as an attitude subsystem of the underwater vehicle. The amount of positional deviation may also be referred to as the system position error, i.e. the target signal pd＝(x1d,y1d)TIf the actual value of the target position information to be achieved and the current position information are measured values, the difference between the current position information and the target position information is considered as a system position error, i.e., a position deviation amount. The position continuous difference amount includes a position deviation amount in the x direction and a position deviation amount in the y direction, that is:
S1x＝x1-x1d
S1y＝y1-y1d(4)
wherein S is1xIs the amount of positional deviation in the x direction of the current positional information and the target positional information, S1yIs the amount of positional deviation in the y direction of the current positional information and the target positional information.
Thus, it can be seen thatSubstituting formula (2) can obtain:
because the system is an under-actuated system, the position information in the x and y directions cannot be tracked simultaneously only by designing v, so that the ideal angle informationAlso as a control quantity to overcome the underactuation problem. Control target is selected as tracking position and angle, position subsystemAnd attitude subsystemIndirectly controlling the position variable, designing the control force F such that v tracks the target velocityThe control moment tau is designed so that theta1Tracking target angle
Therefore, the first Lyapunov function is designed as:
it is possible to obtain,
defining the first status signal as:
wherein k is1x，k1yRespectively, are preset control gain values.
The formula (8) may be substituted for the formula (5):
wherein,
the binding formula (7) can be obtained:
first speedIs an ideal speed and a first direction angle in the sailing processIs an ideal course angle in the navigation process.
As can be seen from the formula (8),will be provided withThe value range is limited to (-pi/2, pi/2), and the ideal posture trajectory tracking is satisfiedComprises the following steps:
obtained by the above formulaIdeal angle required for position control law if the actual angle θ of the underwater vehicle1From the ideal angleEqual, then the ideal trajectory control law can be achieved, but the actual θ1Andcomplete agreement is not possible, especially in the initial stages of control, which can cause instability in closed loop tracking systems.
If a general sliding mode control method is adopted, the derivation of the formula (12) and the formula (13) is required to realize the track tracking. The following can be obtained by deriving equations (12) and (13):
expanding the formula (15) and the formula (15), wherein the terms are quite complex; moreover, the results obtained with increasing derivative order are more complex, causing a "differential explosion".
In order to solve the above problems, the embodiments of the present invention add a filtering component in the system, where the filtering component may be a first-order low-pass filter, and use the output of the filter to replace the original signal, thereby avoiding the derivation of the signal. Will be provided withInput to a low-pass filter, i.e.Line S130. It should be noted that, in this embodiment, the filtering component may include a plurality of filtering components.
S130, the first state signal is processed by the first filtering component to obtain a second direction angle and a second speed.
Considering that the noise of the system can generate strong interference on the derivation process of the signal, great error is caused, the stability of the system is influenced, a filtering component is introduced, and the intermediate signal is transmittedAndfiltering to obtain a second direction angle theta of the output signal1dAnd a second speed v1d. Setting a first time constant τ in the AND filter assembly of FIG. 31θAnd a second time constant τ1v，τ1θFor the intermediate signalPerforming a filtering operation, tau1vFor the intermediate signalFiltering is carried out; tau is1θAnd τ1vThe setting of the values may affect the filtering effect.
In one embodiment, the filtering component satisfies the following relationship:
wherein tau is1θ，τ1vIs the time constant of the filter;is the input of the filter; theta1d，v1dIs the output of the filter.
And S140, determining a control force according to the second speed so that the navigation speed of the underwater vehicle reaches a target speed under the action of the control force.
Then using the filtered v1dAnd the current velocity and the filtered v1dSpeed deviation amount S1vSubstituting the designed control force F formula:
where F is the control force, m is the system mass, k1vIs a first gain value, S1vIs the amount of velocity deviation, v1dIs the second speed, S1xIs the amount of positional deviation in the x direction of the current positional information and the target positional information, S1yIs the amount of positional deviation in the y-direction between the current positional information and the target positional information, v is the current velocity, θ1Is the angle of the current direction and,is the first speed of the motor vehicle,is a first direction angle, C1(v) Is a Coriolis matrix; d1(v) Is a hydrodynamic damping matrix. It should be noted that k here1vIs a preset gain value, otherwise referred to as a control gain value.
The control force F is obtained and then the dynamic model of the position is used
Obtaining v and x1，y1V and x1，y1In the implementation of the invention, the value is continuously updated, after F is obtained according to the current speed v, new v is obtained according to F, and x is obtained by further integrating1And y1And updating the current velocity v to ensure that the vehicle can navigate according to the preset v, thereby enabling the position coordinate x of the vehicle to be1，y1Capable of tracking a target signal pd＝(x1d,y1d)T。
And S150, determining a control moment according to the second direction angle, so that the navigation direction angle of the underwater vehicle reaches a target direction angle under the action of the control moment.
The system uses the theta obtained after filtering1dAnd current direction angle theta1And theta obtained after filtering1dAmount of angular deviation S of1θDetermining a second state signal, the second state signal carrying a first angular velocity
Wherein k is1θIs a preset control gain value. Then using the second filter component pairThe second state signal is processed to obtain a second angular velocity theta2dWherein, in the process of processing the second state signal by the second filter component, the time constant tau is set2θSo as to satisfy the filtering effect of the second filtering component on the second state signal. Then, according to the current angular velocity theta2ω and the second angular velocity θ2dAngular velocity deviation amount S of2θAnd a second angular velocity theta2dDetermining the control moment tau, and particularly substituting the quantities into a designed control moment tau formula:
where τ is the control moment, I is the moment of inertia, k2θIs the second gain value, S2θIs the amount of angular velocity deviation, θ2dIs the second angular velocity, S1θIs the angular deviation, ω is the aircraft angular velocity, C2(ω) is a Coriolis matrix; d2(v) Is a hydrodynamic damping matrix. It should be noted that k here2θIs a preset gain value, otherwise referred to as a control gain value. Obtaining a control moment tau, and according to a formula:
to obtain theta1. Because of theta1And obtaining the navigation angle theta. In the embodiment, the navigation angle theta is also an updated value, and when the control torque tau is obtained according to the current theta, the theta is obtained according to the control torque tau so as to ensure that the navigation can be carried out according to the preset heading angle theta.
According to the preset navigation speed v in the whole navigation processdAnd a preset navigation angle thetadNavigation deviceThat is, according to the preset track pd＝(x1d,y1d)TAnd (5) running. By adopting the embodiment of the invention, the filtering device is additionally arranged to carry out filtering processing on the intermediate signal, the amplification of system noise caused by searching and derivation is avoided, the problem that the derivative of the signal cannot be accurately obtained under the severe conditions of noise, uncertain parameters, external interference and the like is avoided, high-frequency noise, external interference and uncertain parameters are inhibited, the robustness of the system is enhanced, the stability of the system is improved, and the control law is easy to realize. And the relation between the control force and the control moment is simple and easy to apply to engineering.
It should be noted that, in the system shown in fig. 3, a filtering component is provided, and includes a first filtering component and a second filtering component, and the first filtering component and the second filtering component may be filters or devices having a filtering function. In an embodiment, the first filtering component and the second filtering component may be the same filtering component, or may be separate filtering components, which is not limited in this embodiment.
It should be noted that, in this embodiment, "first" and "second" are merely used to distinguish the objects, and do not limit the objects themselves.
In fig. 3, the control of the underwater vehicle trajectory is directly realized by using the designed control force F and control moment τ, and the design process of the control force F and control moment τ is described below.
First, the position tracking error (or called speed deviation) is defined as:
S1v＝v-v1d(17)
since the control target is to make v trackAnd the position tracking errors defined above are v and v1dThe error between, in other words, v and v1dThe amount of deviation therebetween. If it is notThen v tracksOr v1dAre equivalent. Definition ofAnd v1dThe filtered boundary layer error between is:
if S can be guaranteed1v→ 0 andit is obvious that
A second Lyapunov function was designed as:
the derivation can be:
as can be seen from the formula (16),the substitution formula (20) can be developed:
the design position control force is as follows:
the formula (22) can be substituted for the formula (21):
the above is the design of the control force F, and the following describes the design process of the control moment τ:
the attitude tracking error (or called angular deviation) is defined as:
S1θ＝θ1-θ1d(24)
the control target is such that1TrackingThe attitude tracking error is defined so that θ1Tracking theta1dIf, ifThen theta1TrackingAnd theta1dBoth are equivalent. Definition ofAnd theta1dThe filtered boundary layer error between is:
to obtainA third Lyapunov function was designed as:
the derivation can be:
the virtual control law is designed as follows:
will be provided withThe input is to a low-pass filter,
wherein tau is2θIs the time constant of the filter;is the input of the filter; theta2dIs the output of the filter. Similar to equations (24) and (25), a second set of attitude tracking errors and filtered boundary layer errors is defined:
S2θ＝θ2-θ2d
it is possible to obtain,
as can be seen from the formula (16),further, according to the combination of the formula (30) and the formula (28),
the above results can be obtained by substituting the above results into formula (27):
the fourth Lyapunov function was designed as:
the derivation can be:
as can be seen from the formula (29),
as can be seen from the formulae (28) and (31),
by substituting formula (32) and formula (35) for formula (34):
the design attitude control moment is:
the control force F and the control moment τ are designed as described above, and the stability of the designed control force F and the control moment τ is analyzed as follows:
formula (38) can be substituted for formula (37):
the formula (23) is substituted into the formula (39) to obtain:
from the definition of the filtered boundary layer error we can derive:
from the formulas (8), (14) and (15), it can be seen thatIn the presence of a non-negative continuous function B1v、B1θ、B2θSatisfies the following conditions:
suppose 1, track target trajectory pdIs bounded, has a positive number χ1So thatIs true, has a positive number χ2So thatThis is true.
Theorem 1: for the aircraft system described by equation (1), the control forces and control moments designed by equations (22) and (38) are used. On the basis of assumption 1, when the system is initialized to the value V4(0) C is less than or equal to c, and c is any normal number. Then the control gain k can be adjusted1x、k1y、k1v、k1θ、k2θ(ii) a Function B1v、B1θ、 B2θ(ii) a Time constant τ1v、τ1θ、τ2θThe state signal of the aircraft is semi-globally and consistently bounded, and the tracking error is limited to a small residual set.
Evidence: using the young's inequality, equation (40) satisfies:
at V4(0) On the basis of p being less than or equal to p, omega is set1And Ω2For the compact set, they are expressed as:
can know omega1×Ω2And is also compact. When V is less than or equal to c, B1v、B1θ、B2θAt omega1×Ω2Above has a maximum value, noted: m1v、M1θ、M2θ. Thus, it is possible to provideSatisfies the following conditions:
and because of
Substituting formula (46) into formula (45)
The control parameters of the selected system are as follows:k1θ≥1+r，where r can set the final error of the system, then
When V is4When c, B is*≤M*(. 1v,1 θ,2 θ), available asThus, it is known that V4C is not more than c is an invariant set of the system; in addition, when V4(0) C is less than or equal to c, namely V is present for any t more than 04(t)≤c。
Can obtain the product
The solution (48) can be obtained
Therefore, all signals of the closed loop system are bounded, and
as can be seen from the above formula, increasing the parameter r can control the gain k by adjusting1x、k1y、k1v、k1θ、k2θ(ii) a Function B1v、B1θ、B2θ(ii) a Time constant τ1v、τ1θ、τ2θIs realized such that V4And (t) convergence to a residual set, the tracking error can be arbitrarily small, and the engineering requirement is met.
By controlling the force input (22), the required signal is requiredWhen inInput filter post-satisfactionInput through a filterAnd output v1dThe linear operation of (3) avoids the derivation of the signal; by controlling the torque input (38), the required signal is requiredWhen inInput filter post-satisfactionInput through a filterAnd an output theta2dThe linear operation of (3) avoids the derivation of the signal; the robustness of the system to noise interference and parameter uncertainty is enhanced.
In one particular embodiment, for example, the mass of the underwater vehicle is 10Kg and the moment of inertia is 4Kg · m2. The control target is pd＝(-5+t,sin(0.5t)+1)Tm; the control gain is:k1x＝5，k1y＝5， k1v＝5，k1θ＝5，k2θ(ii) 5; the filter time constant is: tau is1v＝0.02，τ1θ＝0.02，τ2θ0.02; initial state p (0) ═ 6, -2)TThe diagrams of fig. 4 to 6 are obtained by the method for controlling the trajectory of the underwater vehicle according to the embodiment of the present invention, where m and θ (0) are 0 °.
Wherein FIG. 4 shows the attitude tracking error S1θIs a schematic view of (A), i.e. S1θThe relationship between the angle (rad) and the time t(s) is shown schematically. As shown in FIG. 4, S1θThe angle value of the navigation angle tends to zero in a short time and keeps stable, and the navigation angle theta of the actual navigation is proved1And theta1dThe angles are equal, namely the navigation track of the underwater vehicle can be well tracked by using the method, namely navigation according to the preset navigation angle theta is guaranteed.
Fig. 5 shows a schematic diagram of the tracking error of the system position, i.e. a schematic diagram of the system position with time t(s). As shown in FIG. 5, the x-direction position error S can be achieved within about 3S1xAnd y-direction position error S1yIf the navigation position information is zero, the fact that the actual navigation position information is consistent with the set navigation position information shows that the navigation track of the underwater vehicle can be well tracked by using the method, namely, the navigation is guaranteed to be carried out according to the preset position.
FIG. 6 is a schematic plan view showing who navigates by an aircraft, and as shown in FIG. 6, the actual navigation position P and the desired navigation position PdAlmost coinciding, also showing that the position track navigated by the underwater vehicle can be well tracked by using the method, and ensuring that the underwater vehicle navigates according to the set position.
An embodiment of the present invention further provides a control system for an underwater vehicle trajectory, as shown in fig. 7, the control system 700 includes:
a receiving module 710, configured to receive a target signal, where the target signal carries target location information to be reached;
a determining module 720, configured to determine a first status signal according to the current location information and the target location information, where the first status signal carries a first direction angle and a first speed;
the first filtering component 730 is used for processing the first state signal to obtain a second direction angle and a second speed;
the determining unit 720 is further configured to determine a control force according to the second speed, so that the navigation 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 a 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 an embodiment, the first filtering component 730 is specifically configured to:
setting a first time constant for the first filtering component, and processing the first direction angle by using the first filtering component to obtain a second direction angle;
and setting a second time constant for the first filtering component, and processing the first speed by using the first filtering component to obtain a second speed.
Optionally, in an embodiment, the second direction angle and the first time constant, and the second speed and the second time constant respectively satisfy the following relationships:
wherein, tau1θIs a first time constant, τ1vIs a second time constant;is the angle of the first direction and,is a first speed; theta1dIs a second direction angle, v1dIs the second speed.
Optionally, in an embodiment, the determining unit 720 is specifically configured to:
the control force is determined based on the current position information, the target position information, and the second velocity.
Optionally, in an embodiment, the determining unit 720 is specifically configured to:
calculating the position deviation amount of the current position information and the target position information;
calculating a speed deviation amount of the current speed and the second speed;
the control force is determined based on the position deviation amount and the speed deviation amount.
Further, in one embodiment, the second speed and the control force satisfy the following relationship:
where F is the control force, m is the mass of the underwater vehicle, k1vIs a first gain value, S1vIs the amount of velocity deviation, v1dIs the second speed, S1xIs the amount of positional deviation in the x direction of the current positional information and the target positional information, S1yIs the amount of positional deviation in the y-direction between the current positional information and the target positional information, v is the current velocity, θ1Is the angle of the current direction and,is the first speed of the motor vehicle,is a first direction angle, C1(v) Is a Coriolis matrix; d1(v) Is a hydrodynamic damping matrix.
Optionally, in one embodiment, determining the control moment according to the second direction angle includes:
calculating the angle deviation amount of the current direction angle and the second direction angle;
and determining the control moment according to the second direction angle and the angle deviation amount.
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, where the second state signal carries the first angular velocity;
the second filtering component 740 processes the second state signal to obtain a second angular velocity;
the determination unit 720 determines the control torque based on the angular velocity deviation amount of the current angular velocity from the second angular velocity, and the second angular velocity.
Further, in one embodiment, the second speed and the control torque satisfy the following relationship:
where τ is the control moment, I is the moment of inertia, k2θIs the second gain value, S2θIs the amount of angular velocity deviation, θ2dIs the second angular velocity, S1θIs the angular deviation, omega is the aircraftAngular velocity, C2(ω) is a Coriolis matrix; d2(v) Is a hydrodynamic damping matrix.
The system 700 can implement the methods/steps in fig. 2 and fig. 3, and achieve the same technical effects, and for brevity, the description is not repeated herein.
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 separate filtering components, which is not limited in this embodiment.
Those of skill would further appreciate that the various illustrative components and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that the various illustrative components and steps have been described above generally in terms of their functionality in order to clearly illustrate this interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
The steps of a method or algorithm described in connection with the embodiments disclosed herein may be embodied in hardware, a software module executed by a processor, or a combination of the two. A software module may reside in Random Access Memory (RAM), memory, Read Only Memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.
The above-mentioned embodiments are intended to illustrate the objects, technical solutions and advantages of the present invention in further detail, and it should be understood that the above-mentioned embodiments are merely exemplary embodiments of the present invention, and are not intended to limit the scope of the present invention, and any modifications, equivalent substitutions, improvements and the like made within the spirit and principle of the present invention should be included in the scope of the present invention.

Claims (10)

1. A method of controlling an underwater vehicle trajectory, the method comprising:
receiving a target signal, wherein the target signal carries target position information to be reached;
determining a first state signal according to the current position information and the target position information, wherein the first state signal carries a first direction angle and a first speed;
processing the first state signal by using a first filtering component to obtain a second direction angle and a second speed;
determining a control force according to the second speed so that the navigation speed of the underwater vehicle reaches a target speed under the action of the control force;
and determining a control moment according to the second direction angle, so that the navigation direction angle of the underwater vehicle reaches a target direction angle under the action of the control moment.
2. The method of claim 1, wherein processing the first status signal with the first filter component to obtain a second direction angle and a second velocity comprises:
setting a first time constant for the first filtering component, and processing the first direction angle by using the first filtering component to obtain a second direction angle;
and setting a second time constant for the first filtering component, and processing the first speed by using the first filtering component to obtain the second speed.
3. The method of claim 2, wherein the second directional angle and the first time constant, and the second velocity and the second time constant, respectively, satisfy the following relationships:
wherein, tau1θIs a first time constant, τ1vIs a second time constant;is the angle of the first direction and,is a first speed; theta1dIs a second direction angle, v1dIs a second speed;is thatZero initial state of (a); theta1d(0) Is theta1dZero initial state of (a);is thatZero initial state of (a); v. of1d(0) Is v1dZero initial state of (c).
4. The method of claim 1, wherein determining a control force based on the second speed comprises:
and determining the control force according to the current position information, the target position information and the second speed.
5. The method of claim 4, wherein said determining the control force based on the current position information, the target position information, and the second velocity comprises:
calculating a position deviation amount of the current position information and the target position information;
calculating a speed deviation amount of the current speed from the second speed;
determining the control force 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:
where F is the control force, m is the mass of the underwater vehicle, k1vIs a first gain value, S1vIs the amount of velocity deviation, v1dIs the second speed, S1xIs the amount of positional deviation in the x direction of the current positional information and the target positional information, S1yIs the amount of positional deviation in the y-direction between the current positional information and the target positional information, v is the current velocity, θ1Is the angle of the current direction and,is the first speed of the motor vehicle,is a first direction angle, C1(v) Is a Coriolis matrix; d1(v) Is a hydrodynamic damping matrix.
7. The method of claim 1, wherein determining a control torque based on the second directional angle comprises:
calculating the angle deviation amount of the current direction angle and the second direction angle;
and determining the control moment according to the second direction angle and the angle deviation amount.
8. The method of claim 7, wherein determining the control torque based on the second directional angle and the angular deviation comprises:
calculating a second state signal according to the second direction angle and the angle deviation amount, wherein the second state signal carries a first angular speed;
processing the second state signal by using a second filtering component to obtain a second angular velocity;
the control torque is determined based on the angular velocity deviation amount of the current angular velocity from the second angular velocity, and the second angular velocity.
9. The method according to claim 7 or 8, characterized in that the second speed and the control torque satisfy the following relation:
where τ is the control moment, I is the moment of inertia, k2θIs the second gain value, S2θIs the amount of angular velocity deviation, θ2dIs the second angular velocity, S1θIs the angular deviation, ω is the angular velocity of the underwater vehicle, C2(ω) is a Coriolis matrix; d2(v) Is a hydrodynamic damping matrix.
10. A system, characterized in that the system comprises a first filtering component and a second filtering component as claimed in claims 1 to 9, the first filtering component is configured to process the first status signal to obtain a second direction angle and a second speed, so as to determine a target speed according to the second speed and a target direction angle according to the second direction angle; the system is configured to perform the method of claims 1-9.
CN201811641485.7A 2018-12-29 2018-12-29 Method and system for controlling track of underwater vehicle Active CN109693774B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811641485.7A CN109693774B (en) 2018-12-29 2018-12-29 Method and system for controlling track of underwater vehicle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811641485.7A CN109693774B (en) 2018-12-29 2018-12-29 Method and system for controlling track of underwater vehicle

Publications (2)

Publication Number Publication Date
CN109693774A true CN109693774A (en) 2019-04-30
CN109693774B CN109693774B (en) 2020-11-03

Family

ID=66233113

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811641485.7A Active CN109693774B (en) 2018-12-29 2018-12-29 Method and system for controlling track of underwater vehicle

Country Status (1)

Country Link
CN (1) CN109693774B (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111781938A (en) * 2020-06-23 2020-10-16 中国科学院声学研究所 Under-actuated underwater vehicle and stabilizing method and device thereof
CN117553787A (en) * 2024-01-09 2024-02-13 湖南大学无锡智能控制研究院 Collaborative navigation method, device and system of underwater unmanned aircraft

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB1133658A (en) * 1965-05-28 1968-11-13 Litton Industries Inc A method and apparatus for guiding a vehicle automatically and asymptotically into a predetermined path
JP2006313087A (en) * 2005-05-06 2006-11-16 Mitsui Eng & Shipbuild Co Ltd Method and system for correcting position of detection of underwater vehicle
CN103576693A (en) * 2013-11-11 2014-02-12 哈尔滨工程大学 Underwater robot three-dimensional path tracking control method based on second-order filter
CN103699007A (en) * 2014-01-10 2014-04-02 大连海事大学 Ship dynamic positioning system and design method thereof
CN104076821A (en) * 2014-06-19 2014-10-01 哈尔滨工程大学 Under-actuated surface vehicle trajectory tracking control system based on self-adaptive fuzzy observer
CN105807789A (en) * 2016-03-24 2016-07-27 哈尔滨工程大学 UUV control method based on T-S fuzzy observer compensation
CN106444806A (en) * 2016-09-27 2017-02-22 哈尔滨工程大学 Under-actuated AUV (autonomous underwater vehicle) three-dimensional trajectory tracking control method based on biological speed regulation
CN106873628A (en) * 2017-04-12 2017-06-20 北京理工大学 A kind of multiple no-manned plane tracks the collaboration paths planning method of many maneuvering targets
CN107168369A (en) * 2017-06-13 2017-09-15 中国科学院声学研究所 The formation control method and system of a kind of drive lacking unmanned vehicle

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB1133658A (en) * 1965-05-28 1968-11-13 Litton Industries Inc A method and apparatus for guiding a vehicle automatically and asymptotically into a predetermined path
JP2006313087A (en) * 2005-05-06 2006-11-16 Mitsui Eng & Shipbuild Co Ltd Method and system for correcting position of detection of underwater vehicle
CN103576693A (en) * 2013-11-11 2014-02-12 哈尔滨工程大学 Underwater robot three-dimensional path tracking control method based on second-order filter
CN103699007A (en) * 2014-01-10 2014-04-02 大连海事大学 Ship dynamic positioning system and design method thereof
CN104076821A (en) * 2014-06-19 2014-10-01 哈尔滨工程大学 Under-actuated surface vehicle trajectory tracking control system based on self-adaptive fuzzy observer
CN105807789A (en) * 2016-03-24 2016-07-27 哈尔滨工程大学 UUV control method based on T-S fuzzy observer compensation
CN106444806A (en) * 2016-09-27 2017-02-22 哈尔滨工程大学 Under-actuated AUV (autonomous underwater vehicle) three-dimensional trajectory tracking control method based on biological speed regulation
CN106873628A (en) * 2017-04-12 2017-06-20 北京理工大学 A kind of multiple no-manned plane tracks the collaboration paths planning method of many maneuvering targets
CN107168369A (en) * 2017-06-13 2017-09-15 中国科学院声学研究所 The formation control method and system of a kind of drive lacking unmanned vehicle

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111781938A (en) * 2020-06-23 2020-10-16 中国科学院声学研究所 Under-actuated underwater vehicle and stabilizing method and device thereof
CN117553787A (en) * 2024-01-09 2024-02-13 湖南大学无锡智能控制研究院 Collaborative navigation method, device and system of underwater unmanned aircraft
CN117553787B (en) * 2024-01-09 2024-03-26 湖南大学无锡智能控制研究院 Collaborative navigation method, device and system of underwater unmanned aircraft

Also Published As

Publication number Publication date
CN109693774B (en) 2020-11-03

Similar Documents

Publication Publication Date Title
CN106773713B (en) High-precision nonlinear path tracking control method for under-actuated marine vehicle
CN105929842B (en) A kind of drive lacking UUV planar obit simulation tracking and controlling method adjusted based on dynamic speed
CN106406325B (en) Feedback linearization control method of quad-rotor unmanned aerial vehicle based on fuzzy extended state observer
CN106444806B (en) The drive lacking AUV three-dimensional track tracking and controlling method adjusted based on biological speed
Tseng et al. Motion and attitude estimation using inertial measurements with complementary filter
Choi et al. UAV guidance using a monocular-vision sensor for aerial target tracking
CN105807789B (en) UUV control methods based on the compensation of T-S Fuzzy Observers
Lekkas et al. Minimization of cross-track and along-track errors for path tracking of marine underactuated vehicles
CN111061286B (en) Method for realizing lateral overload control of aircraft by providing damping through filtering differentiation
CN117389312B (en) Model-based anti-roll three-dimensional tracking control method for underwater vehicle
Soares et al. Triangular formation control using range measurements: An application to marine robotic vehicles
CN114047744B (en) Self-adaptive backstepping sliding mode multi-unmanned ship formation control method based on sampling communication
Wiig et al. A 3D reactive collision avoidance algorithm for underactuated underwater vehicles
CN113467231A (en) Unmanned ship path tracking method based on sideslip compensation ILOS guidance law
CN109693774B (en) Method and system for controlling track of underwater vehicle
EP1678460B1 (en) System and method with adaptive angle-of-attack autopilot
CN116185024A (en) Unmanned ship heading anti-interference control method
Xu et al. Event-triggered adaptive target tracking control for an underactuated autonomous underwater vehicle with actuator faults
CN112034869B (en) Design method and application of variable parameter neurodynamics controller of unmanned aerial vehicle
Ko et al. Attitude estimation and DVL based navigation using low-cost MEMS AHRS for UUVs
Jensen et al. Basics and guidelines of complementary filters for small UAS navigation
CN112904719B (en) Annular area tracking control method suitable for underwater robot position
Peng et al. Vision based target tracking/following and estimation of target motion
Ma Two modified unscented Kalman filter and acceleration information in unmanned surface vehicle estimation
CN113050683A (en) Fixed-time four-rotor aircraft control method based on terminal sliding mode control

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant