CN109782289A - A kind of submarine navigation device localization method based on the constraint of baseline geometry - Google Patents

Abstract

The present invention provides a kind of submarine navigation device localization methods based on the constraint of baseline geometry, the 3D being calculated between each beacon measures distance, calculate the distance between beacon two-by-two, construct equality constraint and inequality constraints condition that the 2D between beacon and submarine navigation device current location calculates distance, acquire prediction location information, the relative distance measurement equation between water surface beacon and submarine navigation device is established, and obtains submarine navigation device position, course and pitching estimated value by solving constrained optimization problem.The present invention realizes underwater inertia/dead reckoning localization method and combines with hydrolocation method, it is ensured that submarine navigation device is effectively accurately positioned for a long time；So that the range information between acoustic marker is efficiently used in position fixing process；By MAP estimation criterion and EKF filter, so that constraint condition effectively incorporates during Position-Solving, the raising of submarine navigation device positioning accuracy is realized.

Description

A kind of submarine navigation device localization method based on the constraint of baseline geometry

Technical field

The invention belongs to underwater sound navigation and positioning fields, and in particular to a kind of submarine navigation device localization method, equally Suitable for other all kinds of underwater autonomous target position fixing process.

Background technique

It is easily absorbed and rapid attenuation when being propagated under water due to radio signal, leads to land, aerial target navigation The systems such as common GPS, Beidou are difficult to be directly used in submarine navigation device accurate positioning.Submarine navigation device location technology is main at present It can be divided into three classes: (1) inertia/dead reckoning positioning；(2) geophysics positions；(3) hydrolocation.Inertia/dead reckoning is fixed The advantages of position technology, is can not depend on oracle and carries out round-the-clock, full region autonomous positioning, but each high-precision inertia Measurement sensor is at high price, volume is larger, and error is tended to spread out with time integral.And geophysics's location technology is then It is more combined with inertia/dead reckoning, forms underwater integrated navigation and location system, although can be to a certain extent Overcome inertia/dead reckoning position error divergence problem, but depend critically upon the prior informations such as landform, earth magnetism, depth, and easily It is influenced by complicated marine environment.Acoustic positioning technique then mainly by measurement underwater sound signal transmitter between receiver at a distance from And orientation, estimation is resolved by positioning target position information.Common hydrolocation method includes that Long baselines position, short baseline is determined Position, ultra-short baseline positioning etc..Long baselines positioning system acoustic marker basic matrix known to being arranged in the position of water-bed or the water surface forms, respectively Baseline length between beacon is similar to GPS positioning principle, can be determined using surface intersection method from rice up to a hundred to several kms etc. Relative position of the submarine navigation device relative to basic matrix.Acoustic marker basic matrix is arranged in fixation by short baseline and ultra short baseline locating system On carrier (such as surface vessel, job platform), the baseline length between beacon is shorter, and general short baseline length is no more than tens Rice, and ultra-short baseline length is then even more small to several centimetres of meeting, needs to measure between submarine navigation device and acoustic marker in position fixing process Distance and phase difference.In comparison, Long baselines positioning accuracy highest, good reliability, but system constitutes complexity, lays, Calibration and maintenance beacon basic matrix are both needed to a large amount of financial resource and material resource, and short baseline and ultra-short baseline positioning accuracy are fixed not as good as Long baselines Position system, the system that is advantageous in that is constituted simply, convenient for operation.

In current submarine navigation device position fixing process, if being limited to underwater navigation only with inertia/dead reckoning positioning mode The volume size and load capacity of device, the inertial sensor positioning accuracy being carried by is limited, needs submarine navigation device timing It emerges, carries out position correction using systems such as GPS, Beidous, influence task execution efficiency；And if only with acoustic beacon into Row positioning, then phenomena such as influenced by underwater complex environment, be easy to appear hydrolocation information time delay and packet loss, when causing part Carve positioning failure.In addition, the opposite exact position between multiple acoustic markers fails to be fully utilized with range information, acoustic marker baseline Between geometric relationship for distance measuring error effect of contraction also not by enough attention.

Summary of the invention

For overcome the deficiencies in the prior art, the present invention provides a kind of submarine navigation device based on the constraint of baseline geometry Localization method, using the geometric relationship between hydrolocation baseline, construct between acoustic marker and submarine navigation device distance measuring and Relevant constraint, and be applied in submarine navigation device position fixing process, realize mentioning for submarine navigation device autonomous positioning precision It is high.

The technical solution adopted by the present invention to solve the technical problems the following steps are included:

Step 1, three acoustic beacons with self-locating function broadcast self-position (x with Fixed Time Interval_i, y_i), i =1,2,3, submarine navigation device utilizes the underwater sound signal delay time Δ t received_iWith bulk sound velocity under regional waterProduct, 3D to be calculated between each beacon measures distanceIt is deep using the underwater aircraft of entrained depth gauge precise measurement 3D is measured distance l by angle value h_iIt is converted to plane 2D and measures distance

Step 2, it is known that beacon position calculates the distance L between beacon two-by-two_ij, that is, position baseline length, j=1,2,3 and i ≠j；Using the geometric relationship between positioning baseline, the 2D between construction beacon and submarine navigation device current location (x, y) is calculated DistanceEquality constraint S (r_i ²)=0 and inequality constraints condition h (r_i)≥J(r_i)；

Step 3, on the basis of -1 positioning result of kth, submarine navigation device utilizes the surveyed forward speed of inertial sensor v_k-1, course angular speed w_k-1And rate of pitch υ_k-1, each sensor positioning sampling period is set as δ t, to acquire kth time Predict location informationIncluding position, course angle and pitch angle, that is, have

It corresponds to state estimationU is inputted with control_k-1=[v_k-1 w_k-1 υ_k-1]^T Jacobian matrixWithRespectively

Utilize three received water surface acoustic marker position (x_{I, k}, y_{I, k}) and the obtained relative distance of measurementIt is ε in distance measuring error_kOn the basis of, it establishes opposite between water surface beacon and submarine navigation device Distance measuring equation

It corresponds to predictive informationJacobian matrix

MAP estimation method is selected, realizes equality constraintWith inequality constraints condition h (r_{I, k})≥J (r_I,) and relative distanceIt blends, and obtain underwater navigation by solving following constrained optimization problem Device position, course and pitching estimated value, that is, have

s.t. ∑_k=Diag (P_k/k-1, R_k)

h(r_{I, k})≥J(r_{I, k})

I=1,2,3

Wherein, R_kFor distance measuring error covariance, and the predicted value P of state estimation covariance_k/k-1Then by means of extension Kalman filter carries out Recursive Solution, i.e.,

The beneficial effects of the present invention are:

(1) it realizes underwater inertia/dead reckoning localization method to combine with hydrolocation method, it is ensured that underwater navigation Device is effectively accurately positioned for a long time；

(2) by the geometric relationship between positioning baseline, to construct apart from equality constraint and inequality constraints item Part, so that the range information between acoustic marker is efficiently used in position fixing process；

(3) by MAP estimation criterion and EKF filter, so that constraint condition effectively incorporates Position-Solving In the process, the raising of submarine navigation device positioning accuracy is realized.

Detailed description of the invention

Fig. 1 is the hydrolocation structural schematic diagram based on actual distance；

Fig. 2 is the hydrolocation structural schematic diagram measured based on actual range；

Fig. 3 is X-axis position error schematic diagram；

Fig. 4 is Y-axis position error schematic diagram.

Specific embodiment

Present invention will be further explained below with reference to the attached drawings and examples, and the present invention includes but are not limited to following implementations Example.

Technical solution of the present invention the following steps are included:

Step 1: three acoustic beacons with self-locating function broadcast self-position (x with Fixed Time Interval_i, y_i)(i =1,2,3), submarine navigation device then utilizes the underwater sound signal delay time Δ t received_iWith bulk sound velocity under regional waterMultiply Product, the 3D to be calculated between each beacon measure distanceUtilize the entrained underwater aircraft of depth gauge precise measurement 3D is measured distance l by depth value h_iIt is converted to plane 2D and measures distance

Step 2: the distance L of (i.e. positioning baseline) between beacon two-by-two is calculated by known beacon position_ij(i, j=1,2,3 and I ≠ j), using the geometric relationship between positioning baseline, the 2D between construction beacon and submarine navigation device current location (x, y) is counted Calculate distanceEquality constraint S (r_i ²)=0 and inequality constraints condition h (r_i)≥J(r_i)(J (r_i) and distance L_ijWithIt is related), as follows；

Step 3: being combined using MAP estimation criterion with EKF filter, distance will be measuredEquality constraint Condition S (r_i ²)=0 and inequality constraints condition h (r_i)≥J(r_i) effectively incorporate submarine navigation device it is self-positioning during, recursion Solution obtains submarine navigation device operational configuration information.

(1) on the basis of -1 positioning result of kth, submarine navigation device utilizes the surveyed forward speed v of inertial sensor_k-1, boat To angular speed w_k-1And rate of pitch υ_k-1, each sensor positioning sampling period is set as δ t, so as to acquire kth time prediction Location informationIncluding position, course angle and pitch angle, that is, have

It corresponds to state estimationU is inputted with control_k-1=[v_k-1 w_k-1 υ_k-1]^T Jacobian matrixWithRespectively

(2) three received water surface acoustic marker position (x are utilized_{I, k}, y_{I, k}) (i=1,2,3) and measurement obtain Relative distanceIt is ε in distance measuring error_kOn the basis of, water surface beacon and underwater navigation can be established Relative distance measurement equation between device, that is, have

It corresponds to predictive informationJacobian matrixThen it is

(3) selection MAP estimation method is, it can be achieved that equality constraintWith inequality constraints condition h (r_{I, k})≥J(r_I,) and relative distanceIt blends, and obtained by solving following constrained optimization problem Submarine navigation device position, course and pitching estimated value, that is, have

s.t. ∑_k=Diag (P_k/k-1, R_k)

h(r_{I, k})≥J(r_{I, k})

I=1,2,3

Wherein, R_kFor distance measuring error covariance.And the predicted value P of state estimation covariance_k/k-1It then can be by means of expanding It opens up Kalman filter and carries out Recursive Solution, i.e.,

In the present embodiment, three acoustic beacons are deployed on the water surface according to certain geometric configuration, are equipped with and are defended on beacon Star navigation and time service equipment and underwater sound communication module.Beacon position is accurate it is found that beacon is led by satellite by satellite navigation Time service realization of navigating is synchronous with UTC time, and according to Fixed Time Interval, utilizes underwater sound communication module broadcast transmission self-position Information.Submarine navigation device, can be with using entrained depth gauge, Doppler anemometer (DVL) and inertial measurement component (IMU) etc. Realize autonomous positioning, but position error dissipates at any time.In addition, submarine navigation device is by initial satellite navigation time service and is taken Band atomic clock is kept time module, realizes the time synchronization with UTC time and acoustic beacon, and on this basis, using entrained Underwater sound communication module receives acoustic beacon location information, and measures at a distance between beacon, assists itself autonomous positioning precision It improves.Detailed position fixing process is as follows:

Step 1: horizontal distance measures between acoustic beacon and submarine navigation device.

(1) in t_sMoment, three acoustic beacons obtain self-position (x by satellite navigation_i, y_i) (i=1,2,3), and lead to Cross the information such as underwater sound communication module broadcast transmission position, time.Submarine navigation device is in t_{R, i}After reception to underwater acoustic information packet, Then on the basis of time synchronization, the time difference Δ t between packets and the time of reception is utilized_i, solution obtain and the underwater sound believe 3D relative distance between mark, i.e.,

In formula,For underwater bulk sound velocity, generally desirable approximation

(2) can accurately measure submarine navigation device depth value h using entrained depth gauge, and by the 3D between beacon measure away from From l_iIt is converted to plane 2D and measures distance, i.e.,

Step 2: by positioning baseline geometric relationship structure constraint condition.

In kth time position fixing process, the water surface acoustic marker position that submarine navigation device receives is respectively (x_{I, k}, y_{I, k}) (i= 1,2,3), the actual distance between submarine navigation device and acoustic marker is r_{I, k}, and the distance measuring for utilizing step 1 the method to obtain ForIt is influenced by factors such as underwater sound signal multipath effect, Ray-tracing methods, underwater acoustic measurement distance is commonly greater than actual distance, i.e.,So as to construct following equation and inequality constraints condition.

(1) using the acoustic marker location information received, the distance of acoustic marker between any two, which can be calculated, is

In formula, i, j=1,2,3 and i ≠ j.If assuming, the angle between submarine navigation device and acoustic marker line is α_{Ij, k}, such as attached Shown in Fig. 1, so as to there is following relationship to set up

(cosα_{13, k})²+(cosα_{12, k})²+(cosα_{23, k})²=1+2cos α_{13, k}cosα_{12, k}cosα_{23, k} (4)

By equivalent transformation, above-mentioned two formula (4) and (5) can be expressed as to the form of matrix and multiplication of vectors, can then be obtained Obtain following equation constraint condition

In formula,

(2) based on actual amount ranging from position fixing process in, as shown in Fig. 2, centered on acoustic marker, with to underwater The measurement distance of aircraft is three circle intersections of radius, and intersection point is not unique, so that being limited by positioning submarine navigation device position In the range of a very little, set up so as to be able to lower inequality relational expression.

In formula,

Equivalent transformation is carried out to formula (7), following inequality constraints condition can be obtained

Step 3: carrying out positioning calculation using MAP estimation criterion and EKF filter.

(1) if in -1 position fixing process of kth,To estimate obtained underwater navigation Device operational configuration vector, whereinIndicate the real-time course angle of submarine navigation device,Indicate the real-time pitch angle of submarine navigation device,Expression submarine navigation device real time position, and keel depth h_k-1Generally obtained by depth gauge precise measurement, It can not have to be solved.Therefore, according to submarine navigation device kinematics model, can recursion to obtain the self-positioning result of its kth time as follows

In formula,δ t is self-positioning sampling period, u_k-1=[v_k-1 w_k-1 υ_k-1]^TControl input, v are navigated by water for submarine navigation device_k-1、w_k-1And υ_k-1The forward direction speed that respectively entrained inertial sensor measures Degree, course angular speed and rate of pitch, control input noise are ω_k-1~N (0, Q_k-1)。

Kinematical equation can be calculated separately corresponding to state estimationU is inputted with control_k-1Jacobian matrixWithIt is as follows:

(2) three water surface acoustic marker positions received by submarine navigation device are respectively (x_{I, k}, y_{I, k}) (i=1,2,3), and It corresponds, is then according to step 1 2D relative distance calculatedTherefore it can establish between water surface beacon and submarine navigation device Relative distance measurement equation it is as follows:

In formula,h(r_{I, k})=[r_{L, k} r_{2, k} r_{3, k}]^T.If distance measuring noise ε_kIt obeys high This distribution, then its corresponding covariance is R_k。

Distance measuring equation can be calculated corresponding to predicted stateJacobian matrixIt is as follows:

(3) optional in order to effectively incorporate relative distance measurement and its constraint condition in the position fixing process of submarine navigation device It selects through method that MAP estimation criterion is combined with EKF filter and designs corresponding location algorithm.And most Under big Posterior estimator criterion, submarine navigation device position fixing process can be seen as, utilize all distance measuring information Z obtained_1:k =(Z₁, Λ, Z_k) solve so that posterior probability density function p (X_k|Z_1:k) maximum submarine navigation device state estimationBy its With being represented mathematically as

In formula, p (Z_k|X_k) it is likelihood function, p (X_k|Z_1:k-1) it is priori probability density function.In control input noise ω_k-1With distance measuring noise ε_kIn the case where Gaussian distributed, then have

To be by formula (15) is equivalently represented according to formula (16) and (17)

In formula, ∑_k=Diag (P_k/k-1, R_k)。P_k/k-1For the predicted value of state estimation covariance, can be calculated it is refined can Compare matrixAndOn the basis of, it is using EKF filter method Recursive Solution

In formula, K_kFor filtering gain matrix, can be calculated as

Thus, can be by equality constraint (6), inequality constraints (8) and (15) phase in the case where considering baseline geometrical constraint In conjunction with obtaining submarine navigation device position, course and pitching estimated value by solving following constrained optimization problem, i.e.,

Note: the 1. predicted value P of state estimation covariance_k/k-1Solution, can be used it is other with extension kalman filtering it is similar Method, such as iteration EKF filter (IEKF)；

2. being used for predicted value P_k/k-1The Jacobian matrix of updateIt can also be in state estimationPlace is acquired, i.e.,

3. constrained optimization problem (22) may be selected to carry out using Ge Lang multiplier method, quadratic approximation method and halving method etc. It solves.

In addition, simulation analysis is carried out to submarine navigation device localization method disclosed in this invention in conjunction with attached drawing 3 and Fig. 4, point It is as follows to analyse result:

Assuming that submarine navigation device route speed is 4m/s, range rate error covariance is (0.5m/s)²White Gaussian noise, boat It is (0.001rad/s) to angular speed and rate of pitch measurement error covariance²White Gaussian noise, acoustic marker and underwater Range error covariance between aircraft is (4m)²White Gaussian noise add the constant error of 13m~15m, distance measuring updates Period is 10s, and positioning and optimizing problem can be solved by means of the lsqlin function carried in Matlab.

It will not consider the least square method (no constraint LS) of constraint condition and self-positioning information, do not consider the expansion of constraint condition It is fixed to open up three kinds of submarine navigation devices such as Kalman filter method (no constraint EKF) and presently disclosed location algorithm (constraint MAP) Position method is compared, and attached drawing 3 and Fig. 4 are respectively submarine navigation device X-axis and Y-axis position error.It can be found that with underwater The movement of underway position, no constraint LS algorithm positioning accuracy variation are obvious.And compared to commonly without constraint LS algorithm and nothing EKF algorithm is constrained, the effective use of geometry institute structure constraint condition between acoustic marker, presently disclosed localization method are passed through (constraint MAP) can obtain higher positioning accuracy.

Claims (1)

1. a kind of submarine navigation device localization method based on the constraint of baseline geometry, it is characterised in that include the following steps:

Step 1, three acoustic beacons with self-locating function broadcast self-position (x with Fixed Time Interval_i,y_i), i=1, 2,3, submarine navigation device utilizes the underwater sound signal delay time Δ t received_iWith bulk sound velocity under regional waterProduct, to count It calculates the 3D obtained between each beacon and measures distanceUtilize the underwater aircraft depth value of entrained depth gauge precise measurement 3D is measured distance l by h_iIt is converted to plane 2D and measures distance

Step 2, it is known that beacon position calculates the distance L between beacon two-by-two_ij, that is, position baseline length, j=1,2,3 and i ≠ j； Using the geometric relationship between positioning baseline, the 2D between construction beacon and submarine navigation device current location (x, y) calculates distanceEquality constraint S (r_i ²)=0 and inequality constraints condition h (r_i)≥J(r_i)；

Step 3, on the basis of -1 positioning result of kth, submarine navigation device utilizes the surveyed forward speed v of inertial sensor_k-1, boat To angular speed w_k-1And rate of pitch υ_k-1, each sensor positioning sampling period is set as δ_t, to acquire the pre- measurement of kth time Position informationIncluding position, course angle and pitch angle, that is, have

It corresponds to state estimationU is inputted with control_k-1=[v_k-1 w_k-1 υ_k-1]^TIt is refined Than matrixWithIt is respectively as follows:

Utilize three received water surface acoustic marker position (x_i,k,y_i,k) and the obtained relative distance of measurementIt is ε in distance measuring error_kOn the basis of, establish water surface beacon and the phase between submarine navigation device It adjusts the distance measurement equation

It corresponds to predictive informationJacobian matrix

MAP estimation method is selected, realizes equality constraintWith inequality constraints condition h (r_i,k)≥J(r_i,) With relative distanceIt blends, and obtain submarine navigation device position by solving following constrained optimization problem It sets, course and pitching estimated value, that is, has

s.t.Σ_k=Diag (P_k/k-1,R_k)

h(r_i,k)≥J(r_i,k)

I=1,2,3

Wherein, R_kFor distance measuring error covariance, and the predicted value P of state estimation covariance_k/k-1Then by means of extension Kalman filter carries out Recursive Solution, i.e.,