CN104076688A - Master-slave type cooperative control method for autonomous underwater vehicles - Google Patents

Abstract

The invention discloses a master-slave type cooperative control method for autonomous underwater vehicles. The method comprises the first step of regarding an external ocean current as external disturbance and conducting mathematical modeling on the external ocean current with regard to influences of the external ocean current, the second step of establishing an inertial coordinate system of a system and conducting vector resolution on relative speeds of the two underwater vehicles in the direction of a connecting line between the centroids of the two underwater vehicles and the direction perpendicular to the connecting line, and establishing a cooperative control motion model of the system, the third step of converting an original system motion model into a linear system model with the external disturbance through a feedback linearization method, and the fourth step of designing an optimal feedforward-feedback controller of the system motion model after conversion in order to suppress influences of the external disturbance. Through the master-slave type cooperative control method, one autonomous underwater vehicle can be controlled with high tracking accuracy and low driving energy to track the other autonomous underwater vehicle at a certain angle and distance on the condition of existence of the ocean current and other external disturbance, so cooperative control over the autonomous underwater vehicles can be achieved.

Description

A kind of Autonomous Underwater Vehicle master-slave mode cooperative control method

Technical field

The present invention relates to a kind of control method, particularly relate to a kind of Autonomous Underwater Vehicle master-slave mode cooperative control method.

Background technology

The Collaborative Control of many Autonomous Underwater Vehicles (AUVs) is significant for oceanographic survey and ocean development etc.Many AUVs assist Collaborative Control can significantly improve the ability that comprises ocean sampling, imaging, supervision and numerous application aspect of communicating by letter of AUV.Compare with land multirobot (or multiple agent) Collaborative Control, many AUVs Collaborative Control is difficulty especially.Although many AUVs Collaborative Control problem has been subject to paying attention to widely in recent years, the research of many AUVs Collaborative Control is ripe unlike land multirobot (or multiple agent) Collaborative Control.Correlative study about many AUVs Collaborative Control is at the early-stage, and its result of study is mainly to have used for reference land multirobot (or multiple agent) coordination control strategy.In document <Multi-AUV control and adaptive sampling in Monterey Bay>, Fiorelli etc. has carried out the Collaborative Control of many AUVs and the experiment of adaptively sampled research in Monterey bay, it has used the fast running of the many AUVs of modularization based on cable and has kept away barrier and control, and is limited to the length of cable not completely from main control.In document <High precision formation control of mobile robots using virtual structure approach>, adopted centralized virtual architecture cooperative control method, adopted virtual architecture formation framework to realize the Collaborative Control of robot, but virtual architecture is imagination to be existed, in reality, do not exist, limited practical engineering application.Do has studied the Collaborative Control problem of land robot in the limited situation of communication in document <Formation tracking control of unicycle-type mobile robots with limited sensing ranges>, but it does not consider the disturbing influence of ocean current to system under water.

Summary of the invention

Technical matters to be solved by this invention is to provide a kind of Autonomous Underwater Vehicle master-slave mode cooperative control method, it can be under disturbance outside ocean current etc. exists, with higher tracking accuracy and less driving-energy, control an Autonomous Underwater Vehicle and follow the tracks of another Autonomous Underwater Vehicle with certain angle and distance, can realize the Collaborative Control of Autonomous Underwater Vehicle.

The present invention solves above-mentioned technical matters by following technical proposals: a kind of Autonomous Underwater Vehicle master-slave mode cooperative control method, it is characterized in that, it comprises the following steps: consider the impact of outside ocean current, outside ocean current is considered as to external disturbance and it is carried out to mathematical modeling; Set up system inertia coordinate system, the relative velocity between two submarine navigation devices is carried out to resolution of vectors along line between their centres of form and vertical join line direction, set up the Collaborative Control motion model of system; By feedback linearization method, original system motion model is converted into the Linear system model with external disturbance; In order to suppress the impact of external disturbance, designed the feedforward and Feedback Optimal Control device that transforms rear system motion model.

Preferably, the mathematical modeling of described outside ocean current disturbance is the external system model with persistent disturbances, by formula describe, wherein G ∈ R ^{2 * 2}for known scalar matrix, by ocean current disturbance characteristic, determined.So ocean current disturbance under water can be described as meeting the disturbance of following condition: all eigenwerts of matrix G meet Re[λ _i(G)] <0, i=1,2; The starting condition of outer disturbance is unknown.

Preferably, the described concrete steps of setting up Collaborative Control motion model under system inertia coordinate system are as follows: the relative velocity between two submarine navigation devices is carried out to resolution of vectors along line between their centres of form and vertical join line direction, the Collaborative Control motion model of setting up system, is specially: note A _ifor main robot, A _jwei Cong robot, A _jat a certain distance and angle follow the tracks of A _i, by designing suitable cooperative control method, make it to form the master-slave mode formation between Liang Ge robot; A _iand A _jcoordinate under inertial coordinates system is expressed as p _i=[x _iy _iθ _i] ^tand p _j=[x _jy _jθ _j] ^t; v _iand v _jbe respectively A _iand A _jactual speed, l _ijfor A _jto A _idistance, for A _ithe angle of velocity reversal and the two line, d is that reference point arrives A _jthe distance of center of gravity, f ₁and f ₂for disturbance component under water in plane; Relative velocity is decomposed along line direction and vertical join line direction respectively:

Convenient for subsequent treatment, above formula is written as to vector form, definition u _i=[v _iω _i] ^t, f=[f ₁f ₂] ^t, u _j=[v _jω _j] ^t, β _ij=θ _i-θ _j, wherein $G_1=\left[\begin{array}{cccc}c& {\mathrm{\&gamma;}}_{i}o& s_{j}d& \mathrm{\&gamma;}\\ -s& {\mathrm{\&gamma;}}_{i}i/l& n_j& d_i\mathrm{\&gamma;}/j\end{array}\right],$ $D_1=\left[\begin{array}{cc}{d}_{11}& d_{12}\\ d_{21}& d_{22}\end{array}\right].$

Positive progressive effect of the present invention is: the present invention can be under disturbance outside ocean current etc. exists, with higher tracking accuracy and less driving-energy, control an Autonomous Underwater Vehicle and follow the tracks of another Autonomous Underwater Vehicle with certain angle and distance, can realize the Collaborative Control of Autonomous Underwater Vehicle.

Accompanying drawing explanation

Fig. 1 is the schematic diagram of two aircraft cooperative control system models proposing of patent of the present invention;

Fig. 2 is the master-slave mode cooperative control method analogous diagram that can effectively suppress outer disturbance;

Fig. 3 is distance l between two aircraft _ijthe schematic diagram of variation tendency;

Fig. 4 is between two aircraft the schematic diagram of variation tendency;

Fig. 5 is aircraft A _jthe schematic diagram of controller input quantity.

Embodiment

Below in conjunction with accompanying drawing, provide preferred embodiment of the present invention, to describe technical scheme of the present invention in detail.

The present invention includes following steps: consider the impact of outside ocean current, outside ocean current is considered as to external disturbance and it is carried out to mathematical modeling; Set up system inertia coordinate system, the relative velocity between two submarine navigation devices is carried out to resolution of vectors along line between their centres of form and vertical join line direction, set up the Collaborative Control motion model of system; By feedback linearization method, original system motion model is converted into the Linear system model with external disturbance; In order to suppress the impact of external disturbance, designed the feedforward and Feedback Optimal Control device that transforms rear system motion model.

The mathematical modeling of described outside ocean current disturbance is the external system model with persistent disturbances, by formula describe, wherein G ∈ R ^{2 * 2}for known scalar matrix, by ocean current disturbance characteristic, determined.So ocean current disturbance under water can be described as meeting the disturbance of following condition: all eigenwerts of matrix G meet Re[λ _i(G)] <0, i=1; The starting condition of outer disturbance is unknown.Because underwater robot energy storage is limited, consider system tracking error and driving-energy consumption problem, the optimize performance index of choosing coordinated control system is by solving Riccati matrix equation, obtain feedforward and feedback optimal control law.

As shown in Figure 1, the present invention will be further described for example below, and concrete steps are:

(1) considering the movement characteristic of ocean current, is the external system model with persistent disturbances by outside ocean current interference modeling.By formula describe, wherein G ∈ R ^{2 * 2}for known scalar matrix, by ocean current disturbance characteristic, determined.

(2) set up inertial coordinates system, the relative velocity between two submarine navigation devices is carried out to resolution of vectors along line between their centres of form and vertical join line direction, set up the Collaborative Control motion model of system.Be specially: note A _ifor main robot, A _jwei Cong robot.A _jat a certain distance and angle follow the tracks of A _i, by designing suitable cooperative control method, make it to form the master-slave mode formation between Liang Ge robot.A _iand A _jcoordinate under inertial coordinates system is expressed as p _i=[x _iy _iθ _i] ^tand p _j=[x _jy _jθ _j] ^t; v _iand v _jbe respectively A _iand A _jactual speed, l _ijfor A _jto A _idistance, for A _ithe angle of velocity reversal and the two line, d is that reference point arrives A _jthe distance of center of gravity, f ₁and f ₂for disturbance component under water in plane.Relative velocity is decomposed along line direction and vertical join line direction respectively, must be as shown in the formula (1):

Convenient for subsequent treatment, above formula is written as to vector form.We define u _i=[v _iω _i] ^t, f=[f ₁f ₂] ^t, u _j=[v _jω _j] ^t, β _ij=θ _i-θ _j, ${\stackrel{\&CenterDot;}{\mathrm{\&beta;}}}_{\mathrm{ij}}={\mathrm{\&omega;}}_i-{\mathrm{\&omega;}}_j,$ wherein $D_1=\left[\begin{array}{cc}{d}_{11}& d_{12}\\ d_{21}& d_{22}\end{array}\right].$

(3) cooperative motion model (1) is adopted to I/O linearization method, by auxiliary control inputs and substitution of variable method are set, make cooperative motion model (1) be converted into linear system, as shown in the formula (2):

$\stackrel{\&CenterDot;}{x}=\mathrm{Ax}+\mathrm{Bu}+\mathrm{Df}\&CenterDot;\&CenterDot;\&CenterDot;\left(2\right)$

X=[x wherein ₁x ₂] ^t, u=[v w] ^t, here $A=\left[\begin{array}{cc}{-k}_1& 0\\ 0& {-k}_2\end{array}\right],$ $B=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],$ $D=\left[\begin{array}{cc}{-d}_{11}& {-d}_{12}\\ {-d}_{21}& {-d}_{22}\end{array}\right],$ F is for perturbation vector and its dynamic perfromance are under water by formula g ∈ R is described ^{2 * 2}for known scalar matrix.

(4) because underwater robot energy storage is limited, consider system tracking error and driving-energy consumption problem, the optimize performance index of choosing coordinated control system is by solving Riccati matrix equation, obtaining feedforward and feedback optimal control law is as shown in the formula (3):

$u^{*}\left(t\right)=-R^{-1}{B}^T[\mathrm{Px}\left(t\right)+\stackrel{\_}{P}f]\&CenterDot;\&CenterDot;\&CenterDot;\left(3\right)$

Wherein P is Riccati matrix equation as shown in the formula (4):

A ^TP+PA-PSP+Q＝0…………………………………………………(4)

Unique steady-state solution, for matrix equation unique solution, S=BR wherein ^-1b ^t.

The control strategy of realizing Collaborative Control is A _iby acceleration transducer and gyroscope, detect the speed v of self _i, angular velocity omega _i, drive towards angle θ _jand relatively drive towards angle information, and pass through underwater sound communication mode to A _jsend data a _jthe l measuring in conjunction with self sonar sensor _ijand the γ that calculates gained _ijcontrol, so just can realize A _jwith desired distance and angle follow the tracks of A _jthereby, realize the two master-slave mode Collaborative Control.

Instantiation of the present invention is as follows:

Its initial position of various sensor measurements and the attitude state that pass through self of main aircraft under inertial coordinates system.Original state is p _i(0)=[x _i(0) y _i(0) θ _i(0)] ^t=[2m 1m π/6rad] ^t, by acceleration and its speed of gyroscope survey and angular velocity, be respectively v=3.5m/s, ω=0rad/s and remaining unchanged.From aircraft, by self its initial position of various sensor measurements and attitude state, its original state is p _j(0)=[x _j(0) y _j(0) θ _j(0)] ^t=[1.8m-1.2m 0rad] ^t, desired spacing angle is followed the tracks of in expectation ? disturbance correlation matrix is as shown in the formula (5) under water:

$D=\left[\begin{array}{cc}-0.5& 0\\ 1& 1\end{array}\right],G=\left[\begin{array}{cc}-1& 10\\ -10& -2\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\left(5\right)$

Performance index correlation matrix is as shown in the formula (6):

$Q=\left[\begin{array}{cc}2& 0\\ 0& 1\end{array}\right],R=\left[\begin{array}{cc}4& 0\\ 0& 2\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\left(6\right)$

Simulation step length τ=0.05s, simulation time T=200s; k ₁=1, k ₂=1, d=0.2m.

By having carried out emulation comparison with traditional optimum control, show that the optimum control based on feedforward and feedback can suppress the impact of outer disturbance on system, simulation result is as mistake! Do not find Reference source.-mistake! Do not find Reference source.Shown in.

Above-mentioned explanation is not limitation of the present invention, and the present invention is not limited in above-mentioned giving an example, and the variation that those skilled in the art make in essential scope of the present invention, remodeling, interpolation or replacement, also should belong to protection scope of the present invention.

Claims (3)

1. an Autonomous Underwater Vehicle master-slave mode cooperative control method, is characterized in that, it comprises the following steps: consider the impact of outside ocean current, outside ocean current is considered as to external disturbance and it is carried out to mathematical modeling; Set up system inertia coordinate system, the relative velocity between two submarine navigation devices is carried out to resolution of vectors along line between their centres of form and vertical join line direction, set up the Collaborative Control motion model of system; By feedback linearization method, original system motion model is converted into the Linear system model with external disturbance; In order to suppress the impact of external disturbance, designed the feedforward and Feedback Optimal Control device that transforms rear system motion model.

2. Autonomous Underwater Vehicle master-slave mode cooperative control method as claimed in claim 1, is characterized in that, the mathematical modeling of described outside ocean current disturbance is the external system model with persistent disturbances, by formula describe, wherein G ∈ R ^{2 * 2}for known scalar matrix, by ocean current disturbance characteristic, determined.So ocean current disturbance under water can be described as meeting the disturbance of following condition: all eigenwerts of matrix G meet Re[λ _i(G)] <0, i=1; The starting condition of outer disturbance is unknown.

3. Autonomous Underwater Vehicle master-slave mode cooperative control method as claimed in claim 1, it is characterized in that, the described concrete steps of setting up the Collaborative Control motion model under system inertia coordinate system are as follows: the relative velocity between two submarine navigation devices is carried out to resolution of vectors along line between their centres of form and vertical join line direction, the Collaborative Control motion model of setting up system, is specially: note A _ifor main robot, A _jwei Cong robot, A _jat a certain distance and angle follow the tracks of A _i, by designing suitable cooperative control method, make it to form the master-slave mode formation between Liang Ge robot; A _iand A _jcoordinate under inertial coordinates system is expressed as p _i=[x _iy _iθ _i] ^tand p _j=[x _jy _jθ _j] ^t; v _iand v _jbe respectively A _iand A _jactual speed, l _ijfor A _jto A _idistance, for A _ithe angle of velocity reversal and the two line, d is A _jfront end reference point is to A _jthe distance of center of gravity, f ₁and f ₂for disturbance component under water in plane; Relative velocity is decomposed along line direction and vertical join line direction respectively:

Convenient for subsequent treatment, above formula is written as to vector form, definition u _i=[v _iω _i] ^t, f=[f ₁f ₂] ^t, u _j=[v _jω _j] ^t, β _ij=θ _i-θ _j, wherein $G_1=\left[\begin{array}{cccc}c& {\mathrm{\&gamma;}}_{i}o& s_{j}d& \mathrm{\&gamma;}\\ -s& {\mathrm{\&gamma;}}_{i}i/l& n_j& d_i\mathrm{\&gamma;}/j\end{array}\right],$ $D_1=\left[\begin{array}{cc}{d}_{11}& d_{12}\\ d_{21}& d_{22}\end{array}\right].$