CN104679008B - A kind of AUV independently seeks bottom control method - Google Patents

Abstract

The present invention discloses a kind of AUV and independently seeks bottom control method, and the invention is applied to the latent device of unmanned untethered under water（AUV）Bottom control is sought during dive.Carrier dive procedure decomposition is two stages of unpowered dive and power dive by the invention, and by the fusion of multiple sensing datas, realizes that vectors safe is reached in predetermined height and smoothly enters detect operation.The present invention is simple to operate, safe and practical, can effectively realize the reliabilities seeking bottom task, improve AUV system practical application of the AUV during dive.

Description

A kind of AUV independently seeks bottom control method

Technical field

The present invention relates to underwater robot technical field, more particularly to a kind of AUV independently seeks bottom control method.

Background technology

Dive, seabed navigation operation will be typically experienced when AUV performs task under water, three groundworks such as load floating are thrown Stage, present invention is generally directed to the dive course of work of AUV.Under normal circumstances, AUV from sea with 45 ° to 50 ° of inclination angle without dynamic Power dive, after certain depth is reached, determines whether to reach predetermined altitude by the parameter of ranging sonar acquisition, if arrived Dive foundary weight is carried up to just throwing, and to determine height mode navigation operation.As the underwater error of depth gauge adds with the increase of depth Greatly, make a reservation for throw and carry depth and carry depth and there is certain deviation with actual throwing, at the same ranging sonar act in practical work process away from From limited and easily produce false-alarm, this is likely to cause carrier near Sea Bottom throws to carry or throw away from seabed and carries, and the former is to carrier Speech is quite dangerous, and the latter then causes carrier to seek bottom mission failure.Therefore, traditional bottom method its safety and reliability sought Effectively cannot ensure.

The content of the invention

To solve problems of the prior art, the problem to be solved in the present invention is to provide a kind of AUV and independently seeks bottom control Method processed so that AUV can it is safe and stable, be reliably completed dive and seek bottom process.

The technical scheme that adopted for achieving the above object of the present invention is：A kind of AUV independently seeks bottom control method, including with Lower step：

Dive that AUV is unpowered is to desired depth, and throws load dive foundary weight；

AUV hovers in desired depth, adjusts AUV attitudes according to the information of attitude transducer；

AUV is proceeded by from desired depth and vertically seek bottom operation；

After AUV reaches predetermined altitude, start fixed high navigation.

During dive that the AUV is unpowered, judge whether to reach desired depth according to depth transducer.

The AUV determines hovering time according to gesture feedback information in desired depth.

The vertical velocity of the AUV is calculated by depth offset differential and vertical acceleration information score.

It is described vertically seek bottom operation adopt adaptive filter algorithm, specially：

The state equation of discrete system is：

s_k=As_k+Bu_k+w_k（2）

z_k=Hs_k+v_k

Initialization：

P_0|0=10^-6*I

Prediction：

P_k|k-1=AP_k-1|k-1A^T+BQ_k-1B^T

Update：

K_k=P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1

P_k|k=(I-K_k)HP_k|k-1

Wherein, s_k=h_k, represent that k moment carrier is located to bottom height, its initialization combines Hai Shen on the spot by depth information Estimation is obtained；u_k=V_kΔ t, V_kThe vertical velocity of k moment carriers is represented, is obtained by integrated acceleration, Δ t represents speed interval Time；Q_k=diag (w_k ²), R_k=diag (v_k ²), I is unit matrix；z_kIt is the survey bottom height value of k moment Doppler；w_kIt was Journey drives moving noise, v_kFor observation noise, Q_k、R_kThe respectively covariance matrix of process noise and observation noise.

During AUV independently seeks bottom, monitoring state of each sensor to AUV, its alarm procedure include state Monitoring mechanism, i.e.,

Wherein v₀Vertical velocity when being carrier power dive, σ_kIt is estimator deviation of the carrier from bottom height, h_kWhen being k Carve the survey bottom distance value of Doppler, h_k-1It is the survey bottom distance value of k-1 moment Doppler,It is to survey estimating for bottom distance value at the k moment Evaluation, Δ t are the time intervals surveyed between bottom twice, if（1）Formula is set up, then effectively, AUV throws and carries for the warning that sensor is produced Float；Otherwise, it is considered as false-alarm.

The present invention has advantages below and beneficial effect：

1. process is succinct, be easy to execution.In the present invention, AUV first realizes depthkeeping and then independently seeks bottom again, by original AUV dives Process carry out Discrete control, process is simple, clear logic is easy to engineer applied.

2. safety and stability.The present invention remotely starts from bottom off sea with the Mode normal that hovers during AUV independently seeks bottom Bottom is sought, while real-time monitoring, the validity of differentiation bottom tracking altitude information, so as to ensure that AUV seeks the safety and stability of bottom process.

3. reliability is high.The present invention has evaded the impact of false-alarm problem during ranging sonar use, improves AUV systems The reliability of practical application.

Description of the drawings

Fig. 1 be the present invention seek bottom process schematic；

Fig. 2 is that carrier seeks bottom sensor composition schematic diagram in the present invention；

Fig. 3 be the present invention independently seek bottom flow chart.

Specific embodiment

Below in conjunction with the accompanying drawings and embodiment the present invention is described in further detail.

Present invention is generally directed to AUV is carried out independently from unpowered dive, power dive to fixed high navigation this stage of operation Bottom control is sought, as shown in Figure 1.Control computer that AUV is arrived used in the process of seeking bottom and sensor as shown in Figure 2, mainly By automatic Pilot computer, attitude transducer (TCM5), doppler sensor (DVL), acceleration transducer, depth transducer with And jettison system composition.

AUV installs an automatic Pilot computer, for real-time monitoring AUV posture, vertical velocity, depth, height Etc. status information, AUV self-positions, control and the scheduling AUV dive courses of work are calculated；AUV is provided with attitude transducer （TCM5）, for periodic measurement posture data；AUV is provided with depth gauge, accelerometer, Doppler and load rejection mechanism, For the various status informations of periodicity acquisition system.Automatic Pilot computer is thrown according to Fusion result, control Mounted mechanism is performed throws load action.

The present invention by carrier dive process subdivision be two stages of unpowered dive and power dive, the unpowered dive stage Main Basiss depth parameter determines whether to reach throws load depth, and the power dive stage completes independently to seek basic skill or training's work, and under seeking During latent, according to the fusion of multi-sensor data, using status monitoring of the present invention and adaptive filter algorithm, vectors safe is made Predetermined altitude is reliably reached, next stage detect operation is smoothly entered.

The whole dive process of system is acted on using the state monitoring mechanism of present invention design, to DVL systems during AUV dives Bottom tracking information and posture information carry out comprehensive descision, when warning message is effective AUV throw carry float, be otherwise considered as False-alarm.

Power dive stage during system dive, control are acted on using the adaptive filter algorithm of present invention design What AUV completed stability and high efficiency seeks bottom process.

Described state monitoring mechanism is：

Wherein v₀Vertical velocity when being carrier power dive, σ_kIt is estimator deviation of the carrier from bottom height, if (1) Formula is set up, then the warning is effective；Otherwise, it is considered as false-alarm.

Described adaptive filter algorithm is：

If the state equation of discrete system is as follows

s_k=As_k+Bu_k+w_k（2）

z_k=Hs_k+v_k

Wherein

s_k=h_k, represent the k moment carrier be located to bottom height；u_k=V_kΔ t, V_kThe vertical velocity of k moment carriers is represented, Δ t represents the speed sampling time；z_kIt is the survey bottom distance of k moment sensors；w_kIt is proceduredriven noise, v_kFor observation noise.

If Q_k、R_kThe respectively covariance matrix of process noise and observation noise, using height of the following filtering algorithm to AUV Angle value is filtered：

1) initialize

P_0|0=10^-6*I

2) predict

P_k|k-1=AP_k-1|k-1A^T+BQ_k-1B^T

3) update

K_k=P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1

P_k|k=(I-K_k)HP_k|k-1

Wherein s_kInitialization deep reckoning the in sea on the spot combined by depth gauge information obtain, Q_k=diag (w_k ²), R_k=diag (v_k ²), I is unit matrix.

The present invention use operating procedure be：

1. carrier preset first is thrown when dive to terrain clearance is 200m and carries dive foundary weight, and this process is exported with depth gauge Information is defined；

2., after carrier reaches the desired depth that shows of depth gauge and throws load, just switch to depthkeeping and hover mode of operation；

3. depthkeeping is adjusted after attitude of carrier is steady, is switched to self adaptation hovering mode of operation and is started to seek bottom；

4. bottom detecting function is sought by DVL, while being aided with the monitoring carrier dive of the output informations such as accelerometer and TCM5, lead to Cross adaptive filter algorithm to merge each information and made corresponding control decision；

5. when carrier reaches desired from bottom height（80 meters）, just enter fixed high sail mode and start detect operation.

Claims (5)

1. a kind of AUV independently seeks bottom control method, it is characterised in that comprise the following steps：

Dive that AUV is unpowered is to desired depth, and throws load dive foundary weight；

AUV hovers in desired depth, adjusts AUV attitudes according to the information of attitude transducer；

AUV is proceeded by from desired depth and vertically seek bottom operation；

After AUV reaches predetermined altitude, start fixed high navigation；

It is described vertically seek bottom operation adopt adaptive filter algorithm, specially：

The state equation of discrete system is：

$\begin{array}{c}{s}_k={\mathrm{As}}_k+{\mathrm{Bu}}_k+w_k\\ z_k={\mathrm{Hs}}_k+v_k\end{array}---\left(2\right)$

Initialization：

$s_{0|0}={\stackrel{\‾}{h}}_0$

P_0|0=10^-6*I

Prediction：

${\hat{s}}_{k|k-1}=A{\hat{s}}_{k-1|k-1}+{\mathrm{Bu}}_k$

P_k|k-1=AP_k-1|k-1A^T+BQ_k-1B^T

Update：

K_k=P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1

${\hat{s}}_{k|k}={\hat{s}}_{k|k-1}+K_k\[z_k-H{\hat{s}}_{k|k-1}\]$

P_k|k=(I-K_k)HP_k|k-1

Wherein, s_k=h_k, represent that k moment carrier is located to bottom height, its initialization is combined by depth information that sea on the spot is deep to be estimated Obtain；u_k=V_kΔ t, V_kThe vertical velocity of k moment carriers is represented, is obtained by integrated acceleration, Δ t represents the speed interval time； Q_k=diag (w_k ²), R_k=diag (v_k ²), I is unit matrix；z_kIt is the survey bottom height value of k moment Doppler；w_kIt is proceduredriven Noise, v_kFor observation noise, Q_k、R_kThe respectively covariance matrix of process noise and observation noise.

2. a kind of AUV according to claim 1 independently seeks bottom control method, it is characterised in that dive that the AUV is unpowered During, judge whether to reach desired depth according to depth transducer.

3. a kind of AUV according to claim 1 independently seeks bottom control method, it is characterised in that the AUV is in desired depth The hovering time is determined according to gesture feedback information.

4. a kind of AUV according to claim 1 independently seeks bottom control method, it is characterised in that the vertical velocity of the AUV Calculated by depth offset differential and vertical acceleration information score.

5. a kind of AUV according to claim 1 independently seeks bottom control method, it is characterised in that independently seek bottom process in AUV In, monitoring state of each sensor to AUV, its alarm procedure include state monitoring mechanism, i.e.,

$\begin{array}{c}{\hat{h}}_k=h_{k-1}+v_0\mathrm{\&Delta;}t\\ |h_k-{\hat{h}}_k|<{\mathrm{\&sigma;}}_k\end{array}---\left(1\right)$

Wherein v₀Vertical velocity when being carrier power dive, σ_kIt is estimator deviation of the carrier from bottom height, h_kIt is that the k moment is more The survey bottom distance value of general Le, h_k-1It is the survey bottom distance value of k-1 moment Doppler,It is estimate that the k moment surveys bottom distance value, Δ t is the time interval surveyed between bottom twice, if (1) formula is set up, effectively, AUV throws to carry and floats for the warning that sensor is produced； Otherwise, it is considered as false-alarm.