CN106896361A - A kind of deep water robot multi-model EKF combined navigation devices and method - Google Patents

Abstract

It is used for deep water robot Combinated navigation method the present invention relates to deep water robotics, more particularly to one kind.Wherein the method relevant apparatus include rangefinder, acoustic marker, inertial navigation sensors and navigational computer.Method is：Under deep water robot after water, navigational computer carries out data acquisition to rangefinder, course transmitter, speed of a ship or plane sensor and depth transducer；The navigation stage of deep water robot is determined according to the quality and quantity of distance measuring data；When deep water robot is in the integrated navigation stage, multiple deep water robot kinematics' models are set up, and when distance measuring data update, determine the corresponding weight of each model, and then try to achieve the weighting output of all navigation models.This weighting output is the output valve of Integrated Navigation Algorithm.This method is reliable and stable, and deepwater environment navigation accuracy is high, has a wide range of application, and the apparatus and method for being used are simple, and inheritance is good.

Description

A kind of deep water robot multi-model EKF combined navigation devices and method

Technical field

The present invention relates to deep water robotics, more particularly to a kind of deep water robot multi-model EKF integrated navigations Device and method.

Background technology

Deep water robot navigation depth is up to thousands of rice.Under water navigate by water during, navigation error can with distance and when Between increase and gradually increase., it is necessary to be modified to its position after deep water one segment distance of robot navigation, to improve navigation Precision.

Currently the method that deep water robot carries out position correction is mainly：The positioning exported by acoustic positioning system is believed Breath is directly corrected to deep water robot location.But there is certain defect in this method：

1) position error that acoustic propagation time delay causes cannot be eliminated；

2) underwater robot is demarcated and there is deviation, can cause navigation deviation with accumulated time；

3) deep water robot navigation track is unsmooth；

4) cannot be efficiently against the interference of outlier.

The content of the invention

In order to overcome the shortcomings of existing method, the technical problem to be solved in the present invention is to provide a kind of deep water robot multimode Type EKF combined navigation devices and method so that ship trajectory is smooth and position error is small.

The technical scheme that is used to achieve the above object of the present invention is：A kind of deep water robot multi-model EKF combinations are led Boat device, including：

Rangefinder, is installed on deep water robot hull outside, the navigational computer of deep water robot interior is connected, using sound Distance of the signal measurement deep water robot to each beacon；

Beacon, at least three, seabed is laid in, coordinate is known；

Sensor, including course transmitter, speed of a ship or plane sensor and depth transducer, are mounted in deep water robot and connect Navigational computer is connect, is respectively used to measure course, the speed of a ship or plane and the residing depth of deep water robot.

Navigational computer, the output signal for gathering rangefinder and sensor, and using multi-model EKF integrated navigation sides Method calculates the position at deep water robot current time in real time.

The rangefinder is found range by the fixed cycle, and the cycle, metric data content included beacon between 3 seconds~60 seconds Numbering, signal round trip propagation time, and measuring time value is converted into distance measuring.

The navigational computer presses fixed cycle gathered data, and the cycle is between 0.2 second~2 seconds.

When navigational computer collects the distance measuring data of rangefinder, deep water robot is estimated using EKF algorithms Position；

Judge distance measuring data that navigational computer collects whether while the requirement met on quality and quantity；If Distance measuring data that navigational computer is collected while meet the requirement on quality and quantity, then by locus geometric solution Calculate the initial position for obtaining deep water robot, and deep water robot is set and be in the integrated navigation stage, and by this spatial algorithm position Install and be set to the reliable initial value of deep water robot；If being unsatisfactory for requiring, now the navigational state of deep water robot is in Integrated navigation initial stage, integrated navigation initial stage purpose is to resolve the initial bit for determining deep water robot by locus Put；

In the integrated navigation stage, whether effectively, if distance measuring data invalid, this is measured judging distance metric data It is outlier to be worth, and gives up the distance measuring data, passes back through the position step that dead reckoning method estimates deep water robot；If Distance measuring data effectively, then determine the Models Sets of deep water robot motion's system first, are then adopted according to distance measuring data On-line amending is carried out to the position of deep water robot with multi-model EKF algorithms.

The deep water robot is estimated according to course transmitter and the measuring value of speed of a ship or plane sensor by dead reckoning method The position of deep water robot, specially：

v_e=v_ksinθ_k (3)

v_n=v_kcosθ_k (4)

In formula, (x₁₀,x₂₀) it is the initial point of deep water robot, Δ t is the collection period of navigational computer, (x_1t,x_2t) be Dead reckoning, θ_kIt is course transmitter measuring value, v_kIt is speed of a ship or plane sensor measuring value, v_eIt is deep water robot speed, v eastwards_nFor Deep water robot northwards speed.

It is described rangefinder is collected when navigational computer distance measuring data when, using EKF algorithms estimate deep water machine The position of people, comprises the following steps：

Step 1：The original state for setting AUV is X_0/0It is P with initial covariance matrix_0/0, it is defaulted as deep water robot firm Just start longitude and latitude position when execution task；

Step 2：Time updates：By following time update equation, the state X of deep water robot is updated_k+1/kAnd covariance Matrix P_k+1/k：

X_k+1/k=AX_k/k+B·u_k (5)

P_k+1/k=AP_k/k·A^T+B·Q_k·B^T (6)

Wherein, u_kIt is by course transmitter measuring value θ_kWith speed of a ship or plane sensor measuring value v_kCalculate, u_k=[v_e v_n]^T, A is state-transition matrix, A^TIt is the transposition of matrix A, B is control input matrix, B^TIt is the transposition of B, X_k/kAfter being updated for measurement The state of deep water robot, P_k/kThe state covariance matrix of deep water robot, X after being updated for measurement_k+1/kIt is deep water robot One-step prediction state, P_k+1/kIt is the one-step prediction state covariance matrix of deep water robot；

Step 3：Measure and update：When deep water robot obtains the metric data of rangefinder, then using measurement renewal equation The state X of correction deep water robot_k+1/kWith covariance matrix P_k+1/k；

K_k+1=P_k+1/k·H_k+1 ^T·(H_k+1·P_k+1/k·H_k+1 ^T+R_k+1)^-1 (7)

X_k+1/k+1=X_k+1/k+K_k+1·(y-y_k+1) (8)

P_k+1/k+1=(I-K_k+1·H_k+1)·P_k+1/k (9)

Wherein, P_k+1/kIt is the one-step prediction state covariance matrix of deep water robot, H_k+1It is measurement matrix, R_k+1To measure Covariance matrix, K_k+1It is kalman gain matrix, y is actual measuring value, y_k+1It is prediction measuring value, I is unit battle array, X_k+1/kFor The one-step prediction state of deep water robot, X_k+1/k+1It is k+1 moment state updated value, P_k+1/k+1It is k+1 moment state covariance squares Battle array updated value.

It is described whether to judge distance measuring data that navigational computer collects while the requirement met on quality and quantity, Specially：

It is right when the ranging data quantity in a range finding cycle is more than or equal to 3 when arriving in next range finding cycle Deep water robot water plane coordinates is resolved, and solution formula is as follows：

Ax=v (10)

In formula,

It is No. i-th three-dimensional coordinate of beacon, (x₁,x₂,x₃) it is deep water robot water plane coordinates And depth,It is the level interval of No. i-th beacon to coordinate origin,For No. i-th beacon to deep water robot horizontal range, wherein R_iRepresent No. i-th one-way only propagation distance of beacon；

Deep water robot water plane coordinates is poor with the dead reckoning, root-mean-square error is then asked for, i.e.,：

If e≤Bias, metric data quality reaches requirement, and makes locus resolve indexed variable N=N+1, N's Initial value is 0, and when N >=4, then metric data quality and quantity reaches requirement simultaneously, then it is group to set now navigational state Navigation stage is closed, and determines that the initial position of current deep water robot water plane is (x₁,x₂)；If e ＞ Bias and N ＜ 4, N=0 is then set, wherein, Bias scopes are 1 meter~100 meters, and Bias is preset value.

It is described in the integrated navigation stage, judging distance metric data whether effectively, specially：

V (k+1)=y-y_k+1/k (12)

v(k+1)^T·S_k ^-1·v(k+1)≤γ (13)

Wherein, y is actual measuring value, y_k+1/kIt is prediction measuring value, v (k+1) is new breath, S_kIt is newly to cease covariance matrix, γ is threshold value, and the recommended value of γ is 9.2.

Whether judgment formula (13) is set up, if set up, uses current measurement value y；Otherwise, this measuring value is outlier, It is invalid, give and give up.

The Models Sets for determining deep water robot motion's system, according to the scope of systematic procedure noise Q, it is determined that reasonably Process noise minimum covariance matrix Q_minWith process noise maximum covariance matrix Q_max, Q_min≤Q≤Q_max, take N groups motion mould Type, the determination method of Models Sets is：

It is each Q_iAn EKF wave filter is set up, the state of each wave filter independent estimations deep water robot, i tables Representation model is indexed, and N is preset value, represents the number of model, when the maximal rate of underwater robot is less than or equal to 3m/s, N Value is 10.

It is described that on-line amending is carried out to the position of deep water robot using multi-model EKF algorithms according to distance measuring data, Comprise the following steps：

Step 1：Time renewal is carried out to the corresponding wave filter of each model, X is obtained_k+1/k,

X_i,k+1/k=AX_i,k/k+B·u_k (15)

P_i,k+1/k=AP_i,k/k·A^T+B·Q_i,k·B^T (16)

y_i,k+1/k=h (X_i,k+1/k) (17)

X_i,k+1/k+1=X_i,k+1/k+K_i,k+1·(y-y_i,k+1/k) (19)

P_i,k+1/k+1=(I-K_i,k+1·H_i,k+1)·P_i,k+1/k (20)

Wherein, A is state-transition matrix, X_i,k/kIt is the corresponding state of i-th model, B is speed control input matrix, u_k It is velocity, X_i,k+1/kRepresent the corresponding predicted state of i-th model, P_i,k+1/kRepresent the corresponding predicted state of i-th model Covariance matrix, P_i,k/kRepresent measure update after the corresponding state covariance matrix of i-th model, Q_i,kRepresent i-th model Corresponding process noise covariance matrix, h () represents measurement equation, y_i,k+1/kRepresent that i-th corresponding prediction of model measures Value, K_i,k+1Represent the corresponding kalman gain of i-th model, H_i,k+1Represent and measure square after the corresponding linearisation of i-th model Battle array, R_k+1It is to measure covariance matrix, y is measuring value, X_i,k+1/k+1Represent the corresponding state updated value of i-th model, P_i,k+1/k+1 The corresponding state covariance matrix updated value of i-th model is represented, I represents unit matrix；

Step 2：When distance measuring information is detected, deep water robot to the distance of beacon is calculated；And according to measurement, Calculate the corresponding weight of each model：

v_i(k+1)=y-y_i,k+1/k, i=1 ..., N (21)

S_k+1=HP_k+1/k·H^T+R_k+1 (22)

Wherein, y is measuring value, y_i,k+1/kRepresent the corresponding prediction measuring value of i-th model, v_i(k+1) i-th mould is represented The corresponding new breath of type, R_k+1It is to measure covariance matrix, H represents the measurement matrix after linearisation, S_k+1Represent new breath covariance Matrix, P_k+1/kIt is predicted state covariance matrix, e (i) is weight, and β (i) is normalized weight；

Step 3：Update the state and covariance matrix of deep water robot：

X_k+1/k+1=∑ β (i) X_i,k+1/k+1 (25)

P_k+1/k+1=∑ β (i) P_i,k+1/k+1 (26)

Wherein, β (i) is normalized weight, X_i,k+1/k+1It is the corresponding state updated value of i-th model measurement, P_i,k+1/k+1 It is the corresponding state covariance matrix updated value of i-th model measurement, X_k+1/k+1It is the state of estimation required for us, P_k+1/k+1 It is the state covariance matrix of estimation required for us；

Then X_k+1/k+1It is required deep water robot state at this very moment, P_k+1/k+1It is corresponding covariance matrix.

The present invention has advantages below：

1. the present invention is reliable and stable, and correction result is accurate.The present invention devises determination initial position algorithm and range finding ripple door Algorithm, it is ensured that initial value and measured value it is reliable, effective, location estimation can stable convergence, as a result accurately.

2. device needed for the present invention is simple, and inheritance is good.Device needed for the method is only several beacons, a rangefinder With a computer, without other servicing units, rangefinder simple installation, correction algorithm portability of program is good, can conveniently transplant To each deep water robot.

3. the present invention has a wide range of application.The present invention can be applied not only to deep water robot, can be also used for other oceans Relevant device, is applicable to the deep navigation in full sea.

Brief description of the drawings

Fig. 1 is deep water robot navigation sensor configuration figure of the invention；

Fig. 2 Integrated Navigation Algorithm flow charts；

Fig. 3 rangefinders obtain metric data schematic diagram；

Fig. 4 multi-model EKF algorithm flow charts.

Specific embodiment

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

The present invention is made up of the beacon no less than 3 and deep water robot.Deep water robot navigation sensor configuration such as Fig. 1 It is shown, it is made up of rangefinder, course transmitter, speed of a ship or plane sensor, depth transducer.

Rangefinder, is installed on deep water robot hull outside, the navigational computer of deep water robot interior is connected, using sound Distance of the signal measurement deep water robot to each beacon；

Beacon, at least three, seabed is laid in, coordinate is known；

Sensor, including course transmitter, speed of a ship or plane sensor and depth transducer, are mounted in deep water robot and connect Navigational computer is connect, is respectively used to measure course, the speed of a ship or plane and the residing depth of deep water robot.

Navigational computer, the output signal for gathering rangefinder and sensor, and using multi-model EKF integrated navigation sides Method calculates the position at deep water robot current time in real time.

The rangefinder is found range by the fixed cycle, and the cycle, metric data content included beacon between 3 seconds~60 seconds Numbering, signal round trip propagation time, and measuring time value is converted into distance measuring.

The navigational computer presses fixed cycle gathered data, and the cycle is between 0.2 second~2 seconds.

Multi-model EKF Integrated Navigation Algorithms are as shown in Fig. 2 under deep water robot after water, navigational computer is to rangefinder, boat Data acquisition is carried out to sensor, speed of a ship or plane sensor and depth transducer；Deep water robot is according to course transmitter and boat first The measuring value of fast sensor, by the initial position of dead reckoning method estimation deep water robot, (initial position is inaccurate not to be had Relation, in subsequent filtering, its navigation accuracy is improved constantly).

Integrated navigation system is then turned on, deep water robot is according to inertial navigation system and dead reckoning algorithm constantly to certainly Body is positioned；Simultaneously by long-base-line system periodically with broadcast mode to surrounding transmission signal.

Deep water robot estimates deep water according to course transmitter and the measuring value of speed of a ship or plane sensor by dead reckoning method The position of robot；When navigational computer collects the distance measuring data of rangefinder, deep water machine is estimated using EKF algorithms The position of people；

If deep water robot does not receive range finding measurement signal, continue according to inertial navigation system carry out boat position push away Calculate；If receiving range finding measurement signal, into integrated navigation initial stage.Additionally, when measurement signal is detected, it is necessary to will Metric data is modified, as shown in figure 3, because generally deep water robot is being moved, then deep water robot is at it It is not in same position, it is therefore desirable to which it is corrected when transmission signal is with signal is detected.Modification method is as follows：

Assuming that deep water robot is in t₀Moment transmission signal, in t₁Moment acoustic marker m detects signal, and gives deep water machine One signal of people's feedback, deep water robot is in t₂Moment detects the signal from beacon m, then

Judge distance measuring data that navigational computer collects whether while the requirement met on quality and quantity；If Distance measuring data that navigational computer is collected while meet the requirement on quality and quantity, then by locus geometric solution Calculate the initial position for obtaining deep water robot, and deep water robot is set and be in the integrated navigation stage, and by this spatial algorithm position Install and be set to the reliable initial value of deep water robot；If being unsatisfactory for requiring, now the navigational state of deep water robot is in Integrated navigation initial stage, integrated navigation initial stage purpose is to resolve the initial bit for determining deep water robot by locus Put；

In integrated navigation initial stage, on the one hand carrying out dead reckoning estimates the position of itself for deep water robot, on the one hand Then calculate number and quality that locus resolves.If found range in the sampling period at one first, there is measurement to update, then use EKF updates the position of deep water robot.If found range in the sampling period at one, have more than or equal to 3 measurement signals, then Can be resolved by locus, solve the position of deep water robot.If found range in the sampling periods at continuous 4, can be with Obtain the spatial algorithm position of deep water robot, then deep water robot is then set and is in the integrated navigation stage.

In the integrated navigation stage, deep water robot predicts the position of itself by dead reckoning, after obtaining metric data, adopts The position of itself is corrected with multi-model EKF algorithms, navigation accuracy is improved；Locus is recorded simultaneously and resolves point, for offline point Analysis.Multi-model EKF algorithm flow charts are as shown in Figure 4.

In the integrated navigation stage, whether effectively, if distance measuring data invalid, this is measured judging distance metric data It is outlier to be worth, and gives up the distance measuring data, passes back through the position step that dead reckoning method estimates deep water robot；If Distance measuring data effectively, then determine the Models Sets of deep water robot motion's system first, are then adopted according to distance measuring data On-line amending is carried out to the position of deep water robot with multi-model EKF algorithms.

The deep water robot is estimated according to course transmitter and the measuring value of speed of a ship or plane sensor by dead reckoning method The position of deep water robot, specially：

v_e=v_ksinθ_k (3)

v_n=v_kcosθ_k (4)

In formula, (x₁₀,x₂₀) it is the initial point of deep water robot, Δ t is the collection period of navigational computer, (x_1t,x_2t) be Dead reckoning, θ_kIt is course transmitter measuring value, v_kIt is speed of a ship or plane sensor measuring value, v_eIt is deep water robot speed, v eastwards_nFor Deep water robot northwards speed.

It is described rangefinder is collected when navigational computer distance measuring data when, using EKF algorithms estimate deep water machine The position of people, comprises the following steps：

Step 1：The original state for setting AUV is X_0/0It is P with initial covariance matrix_0/0, it is defaulted as deep water robot firm Just start longitude and latitude position when execution task；

Step 2：Time updates：By following time update equation, the state X of deep water robot is updated_k+1/kAnd covariance Matrix P_k+1/k：

X_k+1/k=AX_k/k+B·u_k (5)

P_k+1/k=AP_k/k·A^T+B·Q_k·B^T (6)

Wherein, u_kIt is by course transmitter measuring value θ_kWith speed of a ship or plane sensor measuring value v_kCalculate, u_k=[v_e v_n]^T, A is state-transition matrix, A^TIt is the transposition of matrix A, B is control input matrix, B^TIt is the transposition of B, X_k/kAfter being updated for measurement The state of deep water robot, P_k/kThe state covariance matrix of deep water robot, X after being updated for measurement_k+1/kIt is deep water robot One-step prediction state, P_k+1/kIt is the one-step prediction state covariance matrix of deep water robot；

Step 3：Measure and update：When deep water robot obtains the metric data of rangefinder, then using measurement renewal equation The state X of correction deep water robot_k+1/kWith covariance matrix P_k+1/k；

X_k+1/k+1=X_k+1/k+K_k+1·(y-y_k+1) (8)

P_k+1/k+1=(I-K_k+1·H_k+1)·P_k+1/k(9) wherein, P_k+1/kFor the one-step prediction state of deep water robot is assisted Variance matrix, H_k+1It is measurement matrix, R_k+1To measure covariance matrix, K_k+1It is kalman gain matrix, y is actual measuring value, y_k+1It is prediction measuring value, I is unit battle array, X_k+1/kIt is the one-step prediction state of deep water robot, X_k+1/k+1It is k+1 moment states Updated value, P_k+1/k+1It is k+1 moment state covariance matrix updated value.

It is described whether to judge distance measuring data that navigational computer collects while the requirement met on quality and quantity, Specially：

It is right when the ranging data quantity in a range finding cycle is more than or equal to 3 when arriving in next range finding cycle Deep water robot water plane coordinates is resolved, and solution formula is as follows：

Ax=v (10)

In formula,

It is No. i-th three-dimensional coordinate of beacon, (x₁,x₂,x₃) it is deep water robot water plane coordinates And depth,It is the level interval of No. i-th beacon to coordinate origin,For No. i-th beacon to deep water robot horizontal range, wherein R_iRepresent No. i-th one-way only propagation distance of beacon；

Deep water robot water plane coordinates is poor with the dead reckoning, root-mean-square error is then asked for, i.e.,：

If e≤Bias, metric data quality reaches requirement, and makes locus resolve indexed variable N=N+1, N's Initial value is 0, and when N >=4, then metric data quality and quantity reaches requirement simultaneously, then it is group to set now navigational state Navigation stage is closed, and determines that the initial position of current deep water robot water plane is (x₁,x₂)；If e ＞ Bias and N ＜ 4, N=0 is then set, wherein, Bias scopes are 1 meter~100 meters, and Bias is preset value.

It is described in the integrated navigation stage, judging distance metric data whether effectively, specially：

V (k+1)=y-y_k+1/k (12)

v(k+1)^T·S_k ^-1·v(k+1)≤γ (13)

Wherein, y is actual measuring value, y_k+1/kIt is prediction measuring value, v (k+1) is new breath, S_kIt is newly to cease covariance matrix, γ is threshold value, and the recommended value of γ is 9.2.

Whether judgment formula (13) is set up, if set up, uses current measurement value y；Otherwise, this measuring value is outlier, It is invalid, give and give up.

The Models Sets for determining deep water robot motion's system, according to the scope of systematic procedure noise Q, it is determined that reasonably Process noise minimum covariance matrix Q_minWith process noise maximum covariance matrix Q_max, Q_min≤Q≤Q_max, take N groups motion mould Type, the determination method of Models Sets is：

It is each Q_iAn EKF wave filter is set up, the state of each wave filter independent estimations deep water robot, i tables Representation model is indexed, and N is preset value, represents the number of model, when the maximal rate of underwater robot is less than or equal to 3m/s, N Value is 10.

It is described that on-line amending is carried out to the position of deep water robot using multi-model EKF algorithms according to distance measuring data, Comprise the following steps：

Step 1：Time renewal is carried out to the corresponding wave filter of each model, X is obtained_k+1/k,

X_i,k+1/k=AX_i,k/k+B·u_k (15)

P_i,k+1/k=AP_i,k/k·A^T+B·Q_i,k·B^T (16)

y_i,k+1/k=h (X_i,k+1/k) (17)

X_i,k+1/k+1=X_i,k+1/k+K_i,k+1·(y-y_i,k+1/k) (19)

P_i,k+1/k+1=(I-K_i,k+1·H_i,k+1)·P_i,k+1/k (20)

Wherein, A is state-transition matrix, X_i,k/kIt is the corresponding state of i-th model, B is speed control input matrix, u_k It is velocity, X_i,k+1/kRepresent the corresponding predicted state of i-th model, P_i,k+1/kRepresent the corresponding predicted state of i-th model Covariance matrix, P_i,k/kRepresent measure update after the corresponding state covariance matrix of i-th model, Q_i,kRepresent i-th model Corresponding process noise covariance matrix, h () represents measurement equation, y_i,k+1/kRepresent that i-th corresponding prediction of model measures Value, K_i,k+1Represent the corresponding kalman gain of i-th model, H_i,k+1Represent and measure square after the corresponding linearisation of i-th model Battle array, R_k+1It is to measure covariance matrix, y is measuring value, X_i,k+1/k+1Represent the corresponding state updated value of i-th model, P_i,k+1/k+1 The corresponding state covariance matrix updated value of i-th model is represented, I represents unit matrix；

Step 2：When distance measuring information is detected, deep water robot to the distance of beacon is calculated；And according to measurement, Calculate the corresponding weight of each model：

v_i(k+1)=y-y_i,k+1/k, i=1 ..., N (21)

S_k+1=HP_k+1/k·H^T+R_k+1 (22)

Wherein, y is measuring value, y_i,k+1/kRepresent the corresponding prediction measuring value of i-th model, v_i(k+1) i-th mould is represented The corresponding new breath of type, R_k+1It is to measure covariance matrix, H represents the measurement matrix after linearisation, S_k+1Represent new breath covariance Matrix, P_k+1/kIt is predicted state covariance matrix, e (i) is weight, and β (i) is normalized weight；

Step 3：Update the state and covariance matrix of deep water robot：

X_k+1/k+1=∑ β (i) X_i,k+1/k+1 (25)

P_k+1/k+1=∑ β (i) P_i,k+1/k+1 (26)

Wherein, β (i) is normalized weight, X_i,k+1/k+1It is the corresponding state updated value of i-th model measurement, P_i,k+1/k+1 It is the corresponding state covariance matrix updated value of i-th model measurement, X_k+1/k+1It is the state of estimation required for us, P_k+1/k+1 It is the state covariance matrix of estimation required for us；

Then X_k+1/k+1It is required deep water robot state at this very moment, P_k+1/k+1It is corresponding covariance matrix.

The variable of table 1 and variable introduction

Claims (10)

1. a kind of deep water robot multi-model EKF combined navigation devices, it is characterised in that including：

Rangefinder, is installed on deep water robot hull outside, the navigational computer of deep water robot interior is connected, using acoustical signal Distance of the measurement deep water robot to each beacon；

Beacon, at least three, seabed is laid in, coordinate is known；

Sensor, including course transmitter, speed of a ship or plane sensor and depth transducer, are mounted in deep water robot and connect to lead Boat computer, is respectively used to measure course, the speed of a ship or plane and the residing depth of deep water robot.

Navigational computer, the output signal for gathering rangefinder and sensor, and use multi-model EKF Combinated navigation method realities When calculate deep water robot current time position.

2. a kind of deep water robot multi-model EKF combined navigation devices according to claim 1, it is characterised in that described Rangefinder is found range by the fixed cycle, and the cycle, metric data content included the numbering of beacon, signal between 3 seconds~60 seconds The round trip propagation time, and measuring time value is converted into distance measuring.

3. a kind of deep water robot multi-model EKF combined navigation devices according to claim 1, it is characterised in that described Navigational computer presses fixed cycle gathered data, and the cycle is between 0.2 second~2 seconds.

4. a kind of deep water robot multi-model EKF Combinated navigation methods, it is characterised in that comprise the following steps：

Under deep water robot after water, navigational computer is carried out to rangefinder, course transmitter, speed of a ship or plane sensor and depth transducer Data acquisition；

Deep water robot estimates deep water machine according to course transmitter and the measuring value of speed of a ship or plane sensor by dead reckoning method The position of people；When navigational computer collects the distance measuring data of rangefinder, deep water robot is estimated using EKF algorithms Position；

Judge distance measuring data that navigational computer collects whether while the requirement met on quality and quantity；If navigation Computer acquisition to distance measuring data meet requirement on quality and quantity simultaneously, then resolved by locus geometry and obtained The initial position of deep water robot is obtained, and deep water robot is set and be in the integrated navigation stage, and this spatial algorithm position is set It is set to the reliable initial value of deep water robot；If being unsatisfactory for requiring, now the navigational state of deep water robot is in combination Navigation initial stage, integrated navigation initial stage purpose is to resolve the initial position for determining deep water robot by locus；

In the integrated navigation stage, whether effectively, if distance measuring data invalid, this measuring value is judging distance metric data Outlier, gives up the distance measuring data, passes back through the position step that dead reckoning method estimates deep water robot；If distance Metric data effectively, then determines the Models Sets of deep water robot motion's system first, then according to distance measuring data using many Model E KF algorithms carry out on-line amending to the position of deep water robot.

5. a kind of deep water robot multi-model EKF Combinated navigation methods according to claim 4, it is characterised in that described Deep water robot estimates deep water robot according to course transmitter and the measuring value of speed of a ship or plane sensor by dead reckoning method Position, specially：

$x_{1t}=x_{10}+\underset{k=1}{\Σ}{v}_e\·\mathrm{\Δ}t---\left(1\right)$

$x_{2t}=x_{20}+\underset{k=1}{\Σ}{v}_n\·\mathrm{\Δ}t---\left(2\right)$

v_e=v_ksinθ_k (3)

v_n=v_kcosθ_k (4)

In formula, (x₁₀,x₂₀) it is the initial point of deep water robot, Δ t is the collection period of navigational computer, (x_1t,x_2t) it is reckoning Boat position, θ_kIt is course transmitter measuring value, v_kIt is speed of a ship or plane sensor measuring value, v_eIt is deep water robot speed, v eastwards_nIt is deep water Robot northwards speed.

6. a kind of deep water robot multi-model EKF Combinated navigation methods according to claim 4, it is characterised in that described When navigational computer collects the distance measuring data of rangefinder, the position of deep water robot is estimated using EKF algorithms, including Following steps：

Step 1：The original state for setting AUV is X_0/0It is P with initial covariance matrix_0/0, it is defaulted as deep water robot and just opens Longitude and latitude position when beginning execution task；

Step 2：Time updates：By following time update equation, the state X of deep water robot is updated_k+1/kAnd covariance matrix P_k+1/k：

X_k+1/k=AX_k/k+B·u_k (5)

P_k+1/k=AP_k/k·A^T+B·Q_k·B^T (6)

Wherein, u_kIt is by course transmitter measuring value θ_kWith speed of a ship or plane sensor measuring value v_kCalculate, u_k=[v_e v_n]^T, A is shape State transfer matrix, A^TIt is the transposition of matrix A, B is control input matrix, B^TIt is the transposition of B, X_k/kDeep water machine after being updated for measurement The state of device people, P_k/kThe state covariance matrix of deep water robot, X after being updated for measurement_k+1/kIt is a step of deep water robot Predicted state, P_k+1/kIt is the one-step prediction state covariance matrix of deep water robot；

Step 3：Measure and update：When deep water robot obtains the metric data of rangefinder, then using measurement renewal equation correction The state X of deep water robot_k+1/kWith covariance matrix P_k+1/k；

K_k+1=P_k+1/k·H_k+1 ^T·(H_k+1·P_k+1/k·H_k+1 ^T+R_k+1)^-1 (7)

X_k+1/k+1=X_k+1/k+K_k+1·(y-y_k+1) (8)

P_k+1/k+1=(I-K_k+1·H_k+1)·P_k+1/k (9)

Wherein, P_k+1/kIt is the one-step prediction state covariance matrix of deep water robot, H_k+1It is measurement matrix, R_k+1To measure association side Difference matrix, K_k+1It is kalman gain matrix, y is actual measuring value, y_k+1It is prediction measuring value, I is unit battle array, X_k+1/kIt is deep water The one-step prediction state of robot, X_k+1/k+1It is k+1 moment state updated value, P_k+1/k+1For k+1 moment state covariance matrix more New value.

7. a kind of deep water robot multi-model EKF Combinated navigation methods according to claim 4, it is characterised in that described Whether distance measuring data that navigational computer collects are judged while the requirement met on quality and quantity, specially：

When arriving in next range finding cycle, when the ranging data quantity in a range finding cycle is more than or equal to 3, to deep water Robot water plane coordinates is resolved, and solution formula is as follows：

Ax=v (10)

In formula,

$A=\left[\begin{array}{cc}(x_1^2-x_1^1)& (x_2^2-x_2^1)\\ (x_1^3-x_1^1)& (x_2^3-x_2^1)\end{array}\right],$ $x=\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right],$ $v=\left[\begin{array}{c}\frac{1}{2}\[(r_1^2-r_2^2)-(d_1^2-d_2^2)\]-(x_3^2-x_3^1)\·x_3\\ \frac{1}{2}\[(r_1^2-r_3^2)-(d_1^2-d_3^2)\]-(x_3^3-x_3^1)\·x_3\end{array}\right]$

It is No. i-th three-dimensional coordinate of beacon, (x₁,x₂,x₃) it is deep water robot water plane coordinates and depth Degree,It is the level interval of No. i-th beacon to coordinate origin,It is i-th Number beacon to deep water robot horizontal range, wherein R_iRepresent No. i-th one-way only propagation distance of beacon；

Deep water robot water plane coordinates is poor with the dead reckoning, root-mean-square error is then asked for, i.e.,：

$e=\sqrt{{(x_1-x_{1t})}^2+{(x_2-x_{2t})}^2}---\left(11\right)$

If e≤Bias, metric data quality reaches requirement, and makes locus resolve the initial of indexed variable N=N+1, N It is 0 to be worth, and when N >=4, then metric data quality and quantity reaches requirement simultaneously, then set now navigational state for combination is led The boat stage, and determine that the initial position of current deep water robot water plane is (x₁,x₂)；If e ＞ Bias and N ＜ 4, set N=0 is put, wherein, Bias scopes are 1 meter~100 meters, and Bias is preset value.

8. a kind of deep water robot multi-model EKF Combinated navigation methods according to claim 4, it is characterised in that described In the integrated navigation stage, whether judging distance metric data is effective, specially：

V (k+1)=y-y_k+1/k (12)

v(k+1)^T·S_k ^-1·v(k+1)≤γ (13)

Wherein, y is actual measuring value, y_k+1/kIt is prediction measuring value, v (k+1) is new breath, S_kIt is newly to cease covariance matrix, γ is Threshold value, the recommended value of γ is 9.2.

Whether judgment formula (13) is set up, if set up, uses current measurement value y；Otherwise, this measuring value is outlier, invalid, Give and give up.

9. a kind of deep water robot multi-model EKF Combinated navigation methods according to claim 4, it is characterised in that described The Models Sets of deep water robot motion's system are determined, according to the scope of systematic procedure noise Q, it is determined that rational process noise is minimum Covariance matrix Q_minWith process noise maximum covariance matrix Q_max, Q_min≤Q≤Q_max, N group motion models are taken, Models Sets are really The method of determining is：

$Q_i=\frac{N+1-i}{N}{Q}_{min}+\frac{i-1}{N}{Q}_{max},i=1,2,...,N---\left(1\right)$

It is each Q_iAn EKF wave filter is set up, the state of each wave filter independent estimations deep water robot, i represents model Index, N is preset value, represents the number of model, and when the maximal rate of underwater robot is less than or equal to 3m/s, N values are 10。

10. a kind of deep water robot multi-model EKF Combinated navigation methods according to claim 4, it is characterised in that described On-line amending is carried out to the position of deep water robot using multi-model EKF algorithms according to distance measuring data, is comprised the following steps：

Step 1：Time renewal is carried out to the corresponding wave filter of each model, X is obtained_k+1/k,

X_i,k+1/k=AX_i,k/k+B·u_k (15)

P_i,k+1/k=AP_i,k/k·A^T+B·Q_i,k·B^T (16)

y_i,k+1/k=h (X_i,k+1/k) (17)

$K_{i,k+1}=P_{i,k+1/k}\·H_{i,k+1}^T\·{(H_{i,k+1}\·P_{i,k+1/k}\·H_{i,k+1}^T+R_{k+1})}^{-1}---\left(18\right)$

X_i,k+1/k+1=X_i,k+1/k+K_i,k+1·(y-y_i,k+1/k) (19)

P_i,k+1/k+1=(I-K_i,k+1·H_i,k+1)·P_i,k+1/k (20)

Wherein, A is state-transition matrix, X_i,k/kIt is the corresponding state of i-th model, B is speed control input matrix, u_kIt is speed Degree vector, X_i,k+1/kRepresent the corresponding predicted state of i-th model, P_i,k+1/kRepresent the corresponding predicted state association side of i-th model Difference matrix, P_i,k/kRepresent measure update after the corresponding state covariance matrix of i-th model, Q_i,kRepresent i-th model correspondence Process noise covariance matrix, h () represent measurement equation, y_i,k+1/kThe corresponding prediction measuring value of i-th model is represented, K_i,k+1Represent the corresponding kalman gain of i-th model, H_i,k+1Measurement matrix after the corresponding linearisation of i-th model is represented, R_k+1It is to measure covariance matrix, y is measuring value, X_i,k+1/k+1Represent the corresponding state updated value of i-th model, P_i,k+1/k+1Table Show the corresponding state covariance matrix updated value of i-th model, I represents unit matrix；

Step 2：When distance measuring information is detected, deep water robot to the distance of beacon is calculated；And according to measuring, calculate The corresponding weight of each model：

v_i(k+1)=y-y_i,k+1/k, i=1 ..., N (21)

S_k+1=HP_k+1/k·H^T+R_k+1 (22)

$e\left(i\right)=\exp \{-\frac{1}{2}{v}_i{(k+1)}^T{S_{k+1}}^{-1}{v}_i(k+1)\}---\left(23\right)$

$\mathrm{\β}\left(i\right)=\frac{e\left(i\right)}{\mathrm{\Σ}e\left(i\right)}---\left(24\right)$

Wherein, y is measuring value, y_i,k+1/kRepresent the corresponding prediction measuring value of i-th model, v_i(k+1) i-th model pair is represented The new breath answered, R_k+1It is to measure covariance matrix, H represents the measurement matrix after linearisation, S_k+1New breath covariance matrix is represented, P_k+1/kIt is predicted state covariance matrix, e (i) is weight, and β (i) is normalized weight；

Step 3：Update the state and covariance matrix of deep water robot：

X_k+1/k+1=Σ β (i) X_i,k+1/k+1 (25)

P_k+1/k+1=Σ β (i) P_i,k+1/k+1 (26)

Wherein, β (i) is normalized weight, X_i,k+1/k+1It is the corresponding state updated value of i-th model measurement, P_i,k+1/k+1It is i-th The corresponding state covariance matrix updated value of individual model measurement, X_k+1/k+1It is the state of estimation required for us, P_k+1/k+1It is us The state covariance matrix of required estimation；

Then X_k+1/k+1It is required deep water robot state at this very moment, P_k+1/k+1It is corresponding covariance matrix.