KR20170111582A - Method and system for estimating position and velocity of underwater vehicle using doppler beacon - Google Patents

Abstract

The present invention relates to a method and system for estimating the position and velocity of a moving object in a water vehicle, and more particularly, to a method and system for estimating the position and velocity of a moving object in the water using a receiving frequency of the moving object in a condition of knowing a plurality of Doppler beacon positions and an originating frequency of each Doppler beacon And more particularly, to a method and system for estimating the position and velocity of a moving object using a Doppler beacon capable of calculating position and velocity. In order to achieve the above object, according to the present invention, there is provided a method for estimating a position and a velocity of a moving object in a water vehicle, comprising: a plurality of beacons outputting acoustic signals having different frequencies; A method of estimating position and velocity as a state, comprising: providing a plurality of beacon positions and an originating frequency of an acoustic signal; measuring frequency of an acoustic signal received by the underwater vehicle from the plurality of beacons And estimating the state of the underwater vehicle using the plurality of beacon position information, the frequency information of the plurality of acoustic signals, and the reception frequency information of the underwater vehicle.

Description

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method and system for estimating the position and velocity of a moving object using a Doppler beacon,

The present invention relates to a method and system for estimating the position and velocity of a moving object in a water vehicle, and more particularly, to a method and system for estimating the position and velocity of a moving object in the water using a receiving frequency of the moving object in a condition of knowing a plurality of Doppler beacon positions and an originating frequency of each Doppler beacon And more particularly, to a method and system for estimating the position and velocity of a moving object using a Doppler beacon capable of calculating position and velocity.

There are dead reckoning and distance-based location estimation methods in conventional methods for locating underwater vehicles.

Estimation navigation uses DVL (Doppler Velocity Log) or IMU (Inertia Measurement Unit) to determine the linear velocity and angular velocity of the underwater vehicle. In this case, since the velocity is obtained from the sensor coordinate system, calibration is required to convert it to the velocity in the absolute coordinate system.

However, there is a problem that the estimation navigation is fundamentally accumulating error according to the movement of the moving vehicle and the time flow. In order to mitigate these problems, it is possible to compensate accumulated errors by using GPS position information on the surface of water, or to reduce the error accumulation by adding sensors for measuring distances or directions, but the system is complicated, Can not be precisely eliminated.

The method of estimating the distance based position is to measure the distance between the acoustic beacon and the underwater mobile body and obtain the position of the underwater mobile body. A transponder is needed to receive and transmit acoustic signals for distance measurements.

However, in the TOA (Time of Arrival) method for distance measurement, since it is necessary to know the transmission time and reception time of the accurate sound signal, this method can be used only when the synchronization between the transmitter and the receiver can be performed. However, since the beacon and the receiver must be connected by wire between the beacon and the receiver for time synchronization, their use is limited in practice.

(Prior Art 1) Patent Registration No. 10-0953921

SUMMARY OF THE INVENTION It is an object of the present invention to provide a position and velocity estimation method of a moving object in which a position error does not occur without using another sensor.

It is also an object of the present invention to provide a position and velocity estimation method of an underwater mobile object that can estimate the position and velocity of a moving object without requiring time synchronization between a transmitter and a receiver.

It is also an object of the present invention to provide a method and apparatus for estimating the position and velocity of a moving object in a water vehicle in which the structure of the apparatus is simple and the risk of malfunction and malfunction is small.

In order to achieve the above and / or other aspects of the present invention, there is provided a method for estimating a position and a velocity of a moving object in a water vehicle, comprising the steps of: receiving a plurality of beacons outputting acoustic signals having different frequencies; A method for estimating a position and a velocity as a state,

Calculating a predicted state by predicting a current state of the underwater mobile body from a state of the underwater mobile body at a previous time point; calculating a predicted state uncertainty at a current time point from a state uncertainty at a previous time point; Calculating a predicted measurement by predicting a measurement as a frequency of an acoustic signal received at a current time by the underwater vehicle; calculating a predicted measurement uncertainty at the present time using the predicted state uncertainty; Calculating a correction gain by using the mutual error dependency and the predicted measurement uncertainty; calculating a state of the current time using the prediction state and the correction gain; , Using the predicted state uncertainty and the correction gain And a step of calculating a state of uncertainty material point.

A method for estimating the position and velocity of a moving object in a water according to the present invention is a method for estimating a position and a velocity of a moving object in a system comprising a plurality of beacons for outputting acoustic signals having different frequencies and an underwater moving object for receiving acoustic signals from the plurality of beacons A method for estimating a position and a velocity as a state,

The method comprising the steps of: providing the plurality of beacon locations and an originating frequency of an acoustic signal; measuring frequency of an acoustic signal received by the underwater vehicle from the plurality of beacons; And estimating a state of the underwater vehicle using the frequency information of the underwater vehicle and the frequency information of the underwater vehicle.

A method for estimating the position and velocity of a moving object in a water according to the present invention is a method for estimating a position and a velocity of a moving object in a water comprising a plurality of beacons for outputting acoustic signals having different frequencies and an underwater moving object for receiving acoustic signals from the plurality of beacons A method for estimating a position and a velocity as a state,

The method comprising the steps of: measuring a frequency of an acoustic signal received at the current time by the underwater mobile body; calculating a predictive state by predicting the state of the underwater mobile body at the current time from the state of the underwater mobile body at the previous time; Calculating a predicted measurement by estimating a measurement as a frequency of an acoustic signal received at a current time by the underwater vehicle; calculating a correction gain for correcting the predicted state; And calculating the state of the current point of time using the predicted measurement and the correction gain.

Further, the underwater vehicle according to the present invention is an underwater vehicle for receiving an acoustic signal having a different frequency from a plurality of beacons and estimating the position and velocity as a state of the underwater vehicle,

Wherein the underwater vehicle calculates a predicted state by predicting a state of a moving object in the water at a current time from a state of a moving object in the water at a previous time, calculates a predicted state uncertainty at a current time from a state uncertainty at a previous time, Calculating an expected measurement by estimating a measurement as a frequency of an acoustic signal received at a current time by the underwater vehicle, calculating an expected measurement uncertainty at the present time using the predicted state uncertainty, Calculating a correction gain using the mutual error dependency and the estimated measurement uncertainty, calculating a state of a current point of time by using the predicted state and the correction gain, and comparing the predicted state uncertainty with the estimated state uncertainty, Obtain the current state uncertainty using the correction gain And it characterized in that.

According to another aspect of the present invention, there is provided a system for estimating the position and velocity of a moving object, comprising: a plurality of beacons for outputting acoustic signals having different frequencies; and a controller for measuring frequencies of the acoustic signals received by receiving the acoustic signals from the plurality of beacons And an estimating device for estimating a position and a velocity as a state of the mobile body using the plurality of beacon positions, the transmission frequency of the plurality of acoustic signals, and the measured reception frequency.

According to another aspect of the present invention, there is provided an apparatus for estimating the position and velocity of a moving object, comprising: an acoustic signal receiving unit for receiving acoustic signals having different frequencies from a plurality of beacons; a frequency measurement unit for measuring a frequency of the acoustic signal received by the acoustic receiver; And estimating a position and a velocity of the underwater mobile body at the current time based on the position and velocity of the underwater mobile body at the previous time point and calculating the predicted position and velocity using the predicted position and velocity, Calculating a predicted frequency by predicting the frequency of one acoustic signal, calculating a correction gain for correcting the predicted position and velocity, and calculating a correction gain by using the predicted position and velocity, the estimated frequency, And a control unit for calculating the position and speed of the moving object in the water at the time.

As described above, the method according to the present invention can estimate the position and velocity of a moving object in the water by measuring only the frequency of the acoustic signal received by the moving object under the condition of knowing the location and frequency of the beacon. Accordingly, only the acoustic receiver is required, so that the structure of the estimating device is simplified and the structure is simple, so that the possibility of device failure and malfunction is small.

Also, the present invention has the effect of not causing cumulative error according to the movement of the moving object and the time flow, unlike the conventional navigation method using the speed sensor.

In addition, since the present invention does not require time synchronization or sound signal arrival time detection between an acoustic transmitter and a receiver, the present invention can be used in an environment where time synchronization and arrival time detection are difficult.

The method according to the present invention can be used for ROV (Remotely Operated Vehicle) position estimation, which is often limited to a working area, and can be used for estimating the position of a USV (Unmanned Surface Vehicle) or an AUV (Autonomous Underwater Vehicle) .

In particular, the method according to the present invention can be used as an alternative navigation method for GPS jamming. That is, although GPS or GPS / INS composite navigation method is used for estimating the position of USV, since the navigation function is lost during GPS jamming, the method according to the present invention can be widely used as an alternative navigation method.

BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is a schematic block diagram of a position and velocity estimation system for an underwater vehicle according to the present invention; FIG.
FIG. 2 is a diagram illustrating a state estimation process according to an embodiment of the present invention. FIG.
3 is a flowchart illustrating a state estimation process according to the present invention.
4 is a flowchart showing a correction gain calculation process;
5 is a schematic internal configuration diagram of an apparatus for estimating the position and velocity of an underwater vehicle according to the present invention.
6 is a diagram showing conditions used in a simulation;
7 is a view showing a simulation result.
8 is a graph showing the position estimation result of the xy plane.
9 is a graph showing the result of position estimation of the xz plane.

Hereinafter, embodiments of the present invention will be described in detail with reference to the accompanying drawings. The configuration of the present invention and the operation and effect thereof will be clearly understood through the following detailed description.

 Before describing the invention in detail, the same components are denoted by the same reference numerals even if they are shown in different drawings, and a detailed description thereof will be omitted when it is judged that the gist of the present invention may be blurred to a known configuration .

1 schematically shows a position and velocity estimation system for an underwater vehicle according to the present invention.

As shown in FIG. 1, a plurality of Doppler beacons 100 are installed on the water surface or underwater, and the position and speed of the underwater vehicle 200 are estimated in real time while the underwater vehicle 200 is operated under water. The position and velocity estimation of the underwater vehicle 200 may be directly performed by the underwater vehicle 200 or may be performed by an external device (not shown).

The Doppler beacon 100 is installed at a predetermined position and outputs an acoustic signal of a specific frequency. The underwater mobile 200 receives the acoustic signal output from the beacon 100. The Doppler beacon is a beacon using the Doppler effect, and will be collectively referred to as a beacon hereinafter for convenience of explanation.

The system for estimating the position and velocity of a moving object in the underwater according to the present invention can estimate the position and velocity of the moving object in the absolute coordinate system using the position of the beacon 100, the frequency of the acoustic signal output from the beacon 100, The position and speed of the moving object 200 can be obtained.

The frequency of the acoustic signal output from the beacon 100 is changed by the Doppler effect when the acoustic signal is received by the underwater mobile 200 moving fast, so that the output frequencies of the beacons 100 should be sufficiently spaced from each other. That is, even if the underwater mobile 200 moves at the maximum speed, the differences in the frequencies emitted from the beacons 100 are increased in order to know the frequencies transmitted from the received frequencies.

For example, when the underwater vehicle 200 moves to v (t) m / s, if a frequency shift of Δf (t) Hz occurs, the frequency interval may be set to 2Δf (t) Hz or more.

The position and velocity estimation system of the underwater vehicle according to the present invention estimates the position and velocity of the moving object 200 at the current receiving time from the optimum position and velocity of the moving object 200 at the time of receiving the previous frequency, The position and speed of the mobile terminal 200 are corrected using the frequency of the acoustic signal currently received by the mobile 200 so as to calculate the optimal position and speed of the mobile terminal 200 at the present time. This process is repeated every time the receiving frequency is measured.

Hereinafter, the parameters necessary for estimating the position and velocity of the moving object in accordance with the present invention and the corresponding equations will be described in detail.

The location of the beacon 100 installed on the water surface or underwater is represented by Equation (1).

b _k denotes the position of the k-th beacon. b _k is a known vector value.

x _k is the x-direction value of the k-th beacon, y _k is the y-direction value of the k-th beacon, and z _k is the z-direction value of the k-th beacon.

There are a total of M beacons (M = 8 in the present embodiment). The frequency of the acoustic signal generated and output from the k-th beacon is denoted by f _{t, k} as a fixed value. The frequency of the acoustic signal measured when the underwater vehicle receives the acoustic signal of the k-th beacon at time t = t _i is denoted by f _{r, k} (t _i ).

The position of the moving object 200 in the water is expressed by Equation (2).

v (t _i ) means the position of the mobile underwater at time t = t _i .

x (t _i) is the time t = t _i x direction value, y (t _i) of the underwater movable body position from the time t = in t _i y direction value of the underwater movable body position, z (t _i) is the time t = t _i represents the z direction value of the position of the moving object in the water.

Status of Underwater Vehicle

The position and speed of the underwater vehicle are collectively referred to as a state.

The present invention aims at obtaining the state of a moving vehicle at time t = t _i .

The state of the moving object in the water is expressed by Equation (3).

x (t _i) denotes the status of the water moving object at time t = t _i and is referred to as state or state vector at time t = t _i.

v (t _i ) is the position of the moving object in the water at time t = t _i , and v ' (t _i ) is the velocity of the moving object at time t = t _i .

x '(t _i) is x (t _i) x direction speed differentials of the underwater moving object at time t = t _i a, y' (t _i) is the time t = t _i a differential value of y (t _i) Z '(t _i ) is the derivative of z (t _i ) and represents the z-direction velocity of the underwater vehicle at time t = t _i .

That is, x (t _i ) is a vector formed by grouping the positions and velocity variables in the x, y, and z directions into six columns.

State transformation model of underwater mobile

The state transformation shows how the state at t _{i -1} changes from t _i to a certain value. That is, the state transition model is a mathematical expression of the movement characteristics of the underwater vehicle.

In general, the state transformation model is expressed as a nonlinear expression g (·), but it is used linearly so that it can be easily applied to an algorithm. The linearized result is represented by a state transformation matrix G (t _i ).

Since the underwater vehicle does not change its velocity or direction very suddenly, we model that the velocity at t _{i -1} is equal to the velocity at t _i . It is assumed that there is no speed change because there is a short time between t _{i - 1} and t _i, which is on the order of tens or hundreds of milliseconds.

Under this assumption, the state of the underwater vehicle at t _i can be obtained as shown in Equation (4).

The state of the underwater vehicle can not be expressed mathematically without errors due to various environmental changes and uncertainties, movement control performance of the underwater vehicle itself, noise and uncertainty. Therefore, we need to add a term that takes into account these uncertainties and noise factors to the state transformation model.

To this end, each of the position and velocity, wherein _{ν x (t i), ν} y (t i), ν z (t i), ν x '(t i), ν y' (t i), ν z '(t _i ) is added.

ν (t _i ) represents the uncertainty and noise of the state transformation, and Q (t _i ) is the error covariance. Q (t _i ) is a matrix expressing the degree of error from the actual state transformation of the state transformation model, and the larger the value is, the larger the state error is. Q (t _i ) is modeled as having a Gaussian distribution.

Mathematical expression 4 can be expressed by the following equation (5).

As a result, the state transformation model is determined as shown in Equation (6), and the state transformation matrix G (t _i ) can be obtained as shown in Equation (7).

The state transformation matrix G (t _i ) shows that the state at t _{i -1} has a linear relationship with the state at t _i .

Measure

Measurement is the information required to obtain the state at t _i , which is the measurement information or measurement vector. The measurement consists of the frequencies of the acoustic signal measured and received by the underwater vehicle. The measurement is obtained from M beacons and output from the sound signal, so M variables are obtained. Therefore, the measurement is represented by Equation (8).

z (t _i ) is the frequency vector of the sound signal emitted from the M beacons and measured at time t _i in the underwater vehicle. f _{r, k} (t _i ) represents the frequency when the acoustic signal generated by the k-th beacon is received at time t _i in the underwater vehicle.

Measurement model

The measurement model is a mathematical model that indicates how the measurement vector z (t _i ) is determined by the state x (t _i ). Given x (t _i ), the measured value z (t _i ) can be determined by the measurement model function h (·). The measured value does not represent the actual value due to the inaccuracy of the measurement sensor, instability of the physical quantity to be measured, error of the measurement process, and the like. Therefore, measurement models should be sought to reflect the uncertainty and noise of these measurements.

The measurement model is expressed by Equation (9).

Here, ω _k (t _i ) represents the uncertainty and noise of the acoustic signal measurement model generated and output from the k-th beacon. The uncertainty and noise factors for the M beacons are summed together and denoted by the column vector ω (t _i ).

R (t _i ) is an error covariance matrix representing the degree of error of the measurement and is modeled as having a Gaussian distribution and expressed as Equation (10).

In Equation 10, it means that the diagonal elements of the values _{R (t i) ω jj (} t i) is large, a measurement error ω _j (t _i) is greater. And the non-diagonal elements means that the values of _{R (t i) ω jk (} t i) is high correlation between the large measurement error ω _j (t _i) and ω _k (t _i).

In Equation (9), f _{r, k} (t _i ) represents a frequency when the acoustic signal generated by the k-th beacon is received at time t _i in the underwater vehicle. f _{r, k} (t _i ) is a function of the coordinate values (x, y, z) indicating the position of the moving object in the water and the velocity (x ', y', z ') in each direction. Therefore _, (x, y, z) and (x ', y', z ') can be obtained by measuring f _{r, k} (t _i ).

f _{r, k} (t _i ) is derived as follows.

First, v _k (t _i ) means a velocity component in the k-th beacon direction of the underwater vehicle, and can be expressed by Equation (11).

Then, f _{r, k} (t _i ) can be obtained from Equation (11) as Equation (12).

Where v is the acoustic velocity in water and all state variables can be obtained at time t _i . The larger the velocity component v _k (t _i ) in the k-th beacon direction of the underwater mobile body, the greater the difference between the frequency of the acoustic signal received by the underwater vehicle and the frequency of the acoustic signal output from the beacon.

The measurement model function h (·) shown in Equation (9) is a nonlinear function, so it is linearized into a matrix H (t _i ).

H (t _i ) is a Jacobian matrix, and columns 1, 2 and 3 are partial derivatives of h (·) with respect to the position coordinates (x, y, z) , And column 6 is a component obtained by partial differentiation of h (·) with respect to the velocity variable (x ', y', z ') of the underwater vehicle.

Accordingly, the matrix H (t _i ) is obtained as shown in Equation (13).

The columns 1, 2, and 3 of the matrix H (t _i ) are derived through Equations 14, 15, and 16, respectively.

Also, the columns 4, 5, and 6 of the matrix H (t _i ) are derived through Equations 17, 18, and 19, respectively.

이고, 모든 상태 변수는 시각 t_i에서 추정된 값들이다.In Equations (14) to (19)

, And all the state variables are estimated at time t _i .

The procedure for obtaining the position and velocity, which is the state of the moving object in the water, will be described in detail using the results derived above.

State estimation process

The position and velocity estimation method of the underwater vehicle according to the present invention is to obtain the state x (t _i ) of the underwater vehicle at time t _i . Also, the uncertainty (error covariance) Σ (t _i ) for the state x (t _i ) is obtained at the same time. The state x (t _i ) and the uncertainty Σ (t _i ) are used as input values at the next time t _{i +1} .

FIG. 2 illustrates a state estimation process according to an embodiment of the present invention. FIG. 2 is a flowchart illustrating the state estimation process.

State estimation process state x (t _i) and a state of uncertainty Σ (t _i) of the process of obtaining, at time t _{i -1} state x of the underwater movable body obtained in (t _{i -1),} the uncertainty condition Σ (t _{i - 1} ), and the measurement z (t _i ) at time t _i is used.

FIG. 3 is a flowchart illustrating a state estimation process according to the present invention.

The state estimation process will be described in detail with reference to FIG. 2 and FIG.

First, in step S10, the state x ^- (t _i ) of the mobile body is predicted using the state transformation model. That is, the underwater mobile state x predicts (t _i) (see line 1 in Fig. 2) ^- _- state x by using the (t _{i 1)} to the state transformation matrix G (t _i). The value calculated in step S10 is the predicted state x ^- (t _i ).

In step S20, the uncertainty Σ ^- (t _i ) for the predicted state x ^- (t _i ) is obtained. The uncertainty Σ ^- (t _i) is the state transformation matrix G (t _i), the error covariance Q (t _i) of the state transition model, the state uncertainty Σ _- is predicted by using the (t _{i 1)} (line in Fig. 22, see ). The larger the error covariance Q (t _i ) of the state transformation model, the greater the uncertainty with respect to the predicted state. Also, the larger the state uncertainty Σ (t _{i - 1} ), the greater the uncertainty for the predicted state. The value calculated in step 2 is the predicted state uncertainty Σ ^- (t _i ).

In step S30, the predicted measurement z ^- (t _i ) is obtained. z ^- (t _i ) is obtained by applying the measurement model function h (·) assuming that the underwater vehicle is in the predicted state x ^- (t _i ) (see line 3 in FIG. 2).

In step S40, the correction gain K (t _i ) for correction is obtained. The correction gain K (t _i ) is used to correct the predicted state x ^- (t _i ) and the predicted state uncertainty Σ ^- (t _i ).

4 is a flowchart showing a correction gain calculation process.

Referring to Fig. 4, first in step S42, the uncertainty S (t _i ) for the estimated measurement z ^- (t _i ) is obtained. The uncertainty S (t _i ) is predicted using the predicted state uncertainty Σ ^- (t _i ), the measurement model matrix H (t _i ) and the error covariance R (t _i ) of the measurement model (see line 4 in FIG. The larger the error covariance R (t _i ) of the measurement model, the greater the uncertainty in the predicted measurement. Also, the larger the uncertainty of prediction state Σ ^- (t _i ), the greater the uncertainty about the predicted state. The value calculated in step S42 is the estimated measurement uncertainty S (t _i ).

In the next step S44, the predicted state x ^- mutual dependence error (cross-covariance) of (t _i) Σ ^{- -} (t _i) and the estimated measurement z calculate the _xz (t _i). The mutual error dependence is obtained using the predicted state uncertainty Σ ^- (t _i ) and the measurement model matrix H (t _i ) (see line 5 in FIG. 2). If the error of the predicted measure z ^- (t _i ) becomes large when the error of the predicted state x ^- (t _i ) becomes large, the value of the mutual error dependence Σ ^- _xz (t _i ) becomes large.

Finally, in step S46, the correction gain is calculated using the mutual error dependence and the predicted measurement uncertainty.

The larger the predicted state uncertainty Σ ^- (t _i ), the greater the correction by the measurement information z (t _i ). In addition, the smaller the predicted measurement uncertainty S (t _i ), the larger the correction by the measurement information z (t _i ). Thus, the correction gain K (t _i ) is designed to be proportional to the predicted state uncertainty Σ ^- (t _i ) and inversely proportional to the expected measurement uncertainty S (t _i ).

Referring to such a correction gain K (t _i) by using a cross-error dependence of the step S44, the correction gain K (t _i) is cross-error dependence Σ ^- _xz (t _i) in proportion to expected measurement uncertainty S (t _i) in (See line 6 in Fig. 2).

Subsequently, in step S50, the optimum state x (t _i ) is obtained by correcting the predicted state x ^- (t _i ). The optimum state x (t _i ) is obtained by multiplying the correction gain K (t _i ) by the difference between the actual measurement and the predicted measurement to obtain a correction value and adding the predicted state x ^- (t _i ) to the correction value Line 7).

Expected measurement z ^- (t _i) is predicted state x ^- obtain from the (t _i), the actual measurement z (t _i) is because to obtain by measuring the frequency of the actual sound signal is determined by the actual state x (t _i). Thus, the larger the difference between the predicted state x ^- (t _i ) and the actual state x (t _i ), the greater the difference between the predicted measurement z ^- (t _i ) and the actual measurement z (t _i ). The larger the difference between the predicted measurement z ^- (t _i ) and the actual measurement z (t _i ) and the larger the correction gain K (t _i ), the larger the correction is made.

In step S60, the optimal state uncertainty Σ (t _i ) is obtained by correcting the predicted state uncertainty Σ ^- (t _i ). As in the case of the state correction in the step S50, the larger the correction gain K (t _i ), the larger the correction is made.

Optimal state uncertainty Σ (t _i) is the correction gain K (t _i) the measurement model matrix H (t _i) and the predicted state uncertainty in the Σ ^- (t _i) the predicted state uncertainty obtain the correction value by multiplying Σ ^- (t _i ) (See line 8 in Fig. 2). The optimal state uncertainty Σ (t _i ) is less than the predicted state uncertainty Σ ^- (t _i ) since the correction value is subtracted from the predicted state uncertainty Σ ^- (t _i ).

After the state x (t _i ) and the state uncertainty Σ (t _i ) are calculated, if the next time passes through the step S70, the frequency received at the time t _{i +1} is measured in the step S80 and the state x _a measurement z (t _{i + 1} ) is used in the estimation process at time t _{i + 1} (see line 9 in FIG. 2) with the state uncertainty Σ (t _i )

5 is a schematic internal structure of an apparatus for estimating the position and velocity of an underwater vehicle according to the present invention. The configuration according to FIG. 5 is provided in the underwater mobile 200, but in another embodiment, the controller 30 may be installed in a separate external device (not shown). In this case, equipment for communication between the moving vehicle 200 and an external device may be further needed.

Referring to FIG. 5, the acoustic signal receiving unit 10 receives acoustic signals output from a plurality of beacons 100.

The frequency measuring unit 20 measures the frequency of the sound signal received by the sound signal receiving unit 10. [ When the underwater mobile 200 moves, the frequency of the beacon 100 and the frequency of the underwater mobile 200 are different due to the Doppler effect.

The controller 30 controls the overall operation of the estimating device. The controller 30 includes a calculating module for calculating the position and velocity of the moving object. The computing module may be implemented in software or hardware.

The memory 40 stores location information of a plurality of beacons, an originating frequency, and initial position information of a moving object in the water, and stores various data generated by the arithmetic processing of the controller 30. [ In addition, the memory 40 stores a program for estimating the state of the underwater vehicle.

The control unit 30 executes a program to calculate a state by time from the initial state of the moving vehicle using the position and frequency of the previously stored beacons and the frequency of the acoustic signal measured and received at each time.

simulation

6 shows the conditions used in the simulation.

Eight beacons are used and the location and frequency of each beacon are specified. In addition, the initial position and the moving speed of the underwater vehicle were set. Under simulated conditions, the underwater vehicle operated for a total of 500 seconds (8 minutes 20 seconds).

Fig. 7 shows the simulation result, Fig. 8 shows the position estimation result of the xy plane, and Fig. 9 shows the position estimation result of the xz plane.

The simulation results may vary depending on the beacon location and frequency, the initial location and movement speed of the underwater mobile, and the total travel time.

The estimated position and velocity accuracy of the underwater vehicle can be found through the difference between the actual position and velocity and the estimated position and velocity. The error of the position and velocity was obtained by the difference in the x, y and z directions.

The error averages of x, y and z directions are 0.2905m, 0.2321m and 0.2803m, respectively, which are within 0.3m in total.

The maximum error values in the x, y, and z directions are 2.0561m, 1.8437m, and 1.6715m, respectively, and within 2.1m overall.

The error averages of x, y, and z direction velocities are 0.0046 m / s, 0.0050 m / s, and 0.0051 m / s, respectively.

The maximum errors in the x, y, and z direction velocities are 0.0212m / s, 0.0284m / s, and 0.0207m / s, respectively, and are within 0.03m / s overall.

The mean square root mean square errors in the x, y, and z directions are 0.4049 m, 0.3328 m, and 0.3645 m, respectively, and are within 0.41 m overall.

The root mean square error of the velocity in the x, y and z directions is 0.0057m / s, 0.0063m / s and 0.0064m / s, respectively, and is 0.0065m / s or less overall.

The distance error considering the position error of all directions (three dimensional) is 0.5252m, 3.2732m, and 0.0638m, respectively.

The position and velocity errors are considered to be 3m / s, 3m / s and 2m / s respectively in the x, y and z directions and the maximum velocity in the directions of x, y and z respectively in the direction of the underwater mobile body is 60m, 150m and 50m , It can be seen that it is within an allowable range in actual use.

The foregoing description is merely illustrative of the present invention, and various modifications may be made by those skilled in the art without departing from the spirit of the present invention.

Accordingly, the embodiments disclosed in the specification of the present invention are not intended to limit the present invention. The scope of the present invention should be construed according to the following claims, and all the techniques within the scope of equivalents should be construed as being included in the scope of the present invention.

10: Acoustic signal receiving unit 20: Frequency measuring unit
30: control unit 40: memory
100: Beacon 200: Underwater vehicle

Claims (37)

A method for estimating a position and a velocity as a state of a moving object in a system comprising a plurality of beacons outputting acoustic signals having different frequencies and an underwater vehicle receiving acoustic signals from the plurality of beacons,
Calculating a predicted state by predicting a current state of the underwater mobile body from the state of the underwater mobile body at the previous time point;
Calculating a predicted state uncertainty at a current time point from a state uncertainty at a previous time point;
Estimating a measurement as a frequency of an acoustic signal received at a current time by the underwater vehicle using the predicted state,
Calculating a predicted measurement uncertainty at the current time point using the predicted state uncertainty;
Calculating a mutual error dependency of the predicted state and the predicted state;
Calculating a correction gain using the mutual error dependence and the estimated measurement uncertainty;
Calculating a state of a current time point using the predicted state and the correction gain;
And calculating a state uncertainty at the current time point using the predicted state uncertainty and the correction gain. The method according to claim 1,
Wherein the step of calculating the predicted state multiplies the state at the previous time by the state transformation matrix to obtain the predicted state. The method according to claim 1,
Wherein the step of calculating the predicted state uncertainty comprises obtaining the predicted state uncertainty using the state uncertainty of the previous time, the state transformation matrix, and the error covariance matrix of the state transformation model. The method according to claim 1,
Wherein the step of calculating the predicted measurement includes substituting the predicted state into a measurement model function to obtain the predicted measurement. The method according to claim 1,
Wherein the step of calculating the predicted measurement uncertainty comprises obtaining the predicted measurement uncertainty using the predicted state uncertainty, the measurement model matrix, and the error covariance matrix of the measurement model. The method according to claim 1,
Wherein the step of calculating the mutual error dependency comprises obtaining the mutual error dependence using the predicted state uncertainty and the measurement model matrix. The method according to claim 1,
Wherein the calculating the correction gain comprises dividing the mutual error dependence by the expected measurement uncertainty to obtain the correction gain. The method according to claim 1,
Wherein the calculating of the state at the current time is performed by obtaining a correction value by multiplying the correction gain by an actual measurement and a difference value of the predicted measurement and adding the prediction state to the correction value to obtain the state of the current time . The method according to claim 1,
Wherein the state uncertainty of the current time is calculated by obtaining a correction value by multiplying the correction gain by a measurement model matrix and a prediction state uncertainty and subtracting a correction value from a prediction state uncertainty to obtain a state uncertainty of the current time point. A method for estimating a position and a velocity as a state of a moving object in a system comprising a plurality of beacons outputting acoustic signals having different frequencies and an underwater vehicle receiving acoustic signals from the plurality of beacons,
Providing a plurality of beacon locations and an originating frequency of the acoustic signal;
Measuring a frequency of an acoustic signal received by the underwater vehicle from the plurality of beacons;
And estimating the state of the underwater vehicle using the plurality of beacon position information, the frequency information of the plurality of acoustic signals, and the reception frequency information of the underwater vehicle. 11. The method of claim 10,
Estimating the state of the underwater vehicle includes the steps of: predicting a state of the current time point from a state at a previous time point;
Estimating a frequency of an acoustic signal received by the underwater vehicle using the predictive state;
Calculating a correction gain using the uncertainty of the predicted state and the uncertainty of the expected frequency;
And calculating a state of the current point by correcting the predicted state using the actual measured frequency, the expected frequency, and the corrected gain. 12. The method of claim 11,
Wherein the step of predicting the state of the current time point is based on modeling based on the assumption that the state at the previous time point and the state at the current time point do not change abruptly. 12. The method of claim 11,
Wherein the step of predicting the frequency of the acoustic signal predicts a frequency corresponding to the predicted state based on mathematical modeling that can indicate how the frequency is determined by the state. 12. The method of claim 11,
Wherein the step of calculating the correction gain by using the uncertainty of the predicted state and the uncertainty of the expected frequency is characterized in that as the uncertainty of the predicted state is larger, the correction by the actual measured frequency becomes larger and as the uncertainty of the expected frequency becomes smaller, Wherein the correction gain is designed to be in proportion to the uncertainty of the predicted state and inversely proportional to the uncertainty of the expected frequency according to the principle that the correction due to the increase of the predicted state is large. 12. The method of claim 11,
Wherein the calculating of the state at the current time is performed by obtaining a correction value by multiplying the correction gain by the difference between the actual measurement frequency and the expected frequency and adding the predicted state to the correction value. A method for estimating a position and a velocity as a state of a moving object in a system comprising a plurality of beacons outputting acoustic signals having different frequencies and an underwater vehicle receiving acoustic signals from the plurality of beacons,
Measuring a frequency of an acoustic signal received by the underwater vehicle at a current time;
Calculating a predicted state by predicting a current state of the underwater mobile body from the state of the underwater mobile body at the previous time point;
Estimating a measurement as a frequency of an acoustic signal received at a current time by the underwater vehicle using the predicted state,
Calculating a correction gain for correcting the prediction state;
Calculating the state of the current point of time using the predicted state, the measured frequency and the predicted measurement and the corrected gain. 17. The method of claim 16,
The step of calculating the correction gain
Calculating a predicted state uncertainty at a current time point from a state uncertainty at a previous time point;
Calculating a predicted measurement uncertainty at the current time using the predicted state uncertainty;
Calculating a mutual error dependency of the predicted state and the predicted state;
And calculating the correction gain using the mutual error dependence and the expected measurement uncertainty. 18. The method of claim 17,
Further comprising calculating a state uncertainty at a current time point using the predicted state uncertainty and the correction gain. 1. An underwater vehicle for receiving an acoustic signal having a different frequency from a plurality of beacons and estimating a position and a velocity as a state of the underwater vehicle,
The underwater vehicle estimates the state of the underwater vehicle at the current time from the state of the underwater vehicle at the previous time point,
Calculating a predicted state uncertainty of the current point from the state uncertainty of the previous point of time,
Estimating a measurement as a frequency of an acoustic signal received at a current time by the moving object underwater using the predicted state,
Calculating a predicted measurement uncertainty at the current time point using the predicted state uncertainty,
Calculating a mutual error dependency of the predicted state and the predicted state,
Calculating a correction gain using the mutual error dependence and the estimated measurement uncertainty,
Calculates a state of the current point of time using the predicted state and the correction gain,
And calculates a state uncertainty at the current time point by using the predicted state uncertainty and the correction gain. 20. The method of claim 19,
Wherein the underwater vehicle obtains the predicted state by multiplying the state at the previous point by the state transformation matrix. 20. The method of claim 19,
Wherein the underwater vehicle obtains the predicted state uncertainty using the state uncertainty at the previous time point, the state transformation matrix, and the error covariance matrix of the state transformation model. 20. The method of claim 19,
Wherein the underwater vehicle obtains the predicted measurement by substituting the predicted state into a measurement model function. 20. The method of claim 19,
Wherein the underwater vehicle obtains the predicted measurement uncertainty using the error covariance matrix of the predicted state uncertainty, the measurement model matrix and the measurement model. 20. The method of claim 19,
Wherein the underwater vehicle obtains the mutual error dependence using the predicted state uncertainty and the measurement model matrix. 20. The method of claim 19,
Wherein the underwater vehicle obtains the correction gain by dividing the mutual error dependence by the estimated measurement uncertainty. 20. The method of claim 19,
Wherein the underwater vehicle obtains a correction value by multiplying the correction gain by an actual measurement and a difference value of the predicted measurement, and adds the predicted state to the correction value to obtain the state of the current time. 20. The method of claim 19,
Wherein the underwater vehicle obtains a correction value by multiplying the correction gain by a measurement model matrix and a prediction state uncertainty, and subtracts a correction value from the prediction state uncertainty to obtain a state uncertainty at the current time. A plurality of beacons for outputting acoustic signals having different frequencies,
Receiving a sound signal from the plurality of beacons; measuring a frequency of the received sound signal; determining a position and a speed as a state of the underwater mobile body by using the plurality of beacon positions, the frequency of the plurality of sound signals, And estimating the position and velocity of the moving object. 29. The method of claim 28,
The estimating device predicts the state of the current point of time from the state of the previous point of time,
Estimating a frequency of an acoustic signal received by the underwater vehicle using the predicted state,
Calculating a correction gain using the uncertainty of the predicted state and the uncertainty of the expected frequency,
And estimates the state of the moving object in the water at the current time by correcting the predicted state using the measured reception frequency, the expected frequency, and the correction gain. 30. The method of claim 29,
Wherein the estimating device predicts the state of the current point of time based on modeling based on the assumption that the state at the previous time and the state at the current time do not change abruptly. 30. The method of claim 29,
Wherein the estimating device predicts a frequency corresponding to the predicted state based on a mathematical modeling that can indicate how the frequency is determined by the state. 30. The method of claim 29,
Wherein the estimating device is configured to calculate a correlation between the uncertainty of the predicted state and the uncertainty of the predicted state according to the principle that the correction by the measured reception frequency increases as the uncertainty of the predicted state becomes larger and as the uncertainty of the expected frequency becomes smaller, And the correction gain is designed to be in inverse proportion to the uncertainty of the expected frequency. 30. The method of claim 29,
Wherein the estimating device obtains a correction value by multiplying the correction gain by a difference value between the measured reception frequency and the expected frequency, and adds the predicted value to the correction value to obtain the current state of the moving object in the water. Position and velocity estimation system. An acoustic signal receiver for receiving acoustic signals having different frequencies from a plurality of beacons;
A frequency measuring unit for measuring a frequency of the sound signal received by the sound receiver,
A predicted position and a velocity are calculated by predicting the position and velocity of the moving object in the water at the current time from the position and velocity of the moving object in the water at the previous point of time and the acoustic signal received by the acoustic signal receiving unit at the present time Calculating a predicted frequency by predicting a frequency of the predicted position and velocity, calculating a correction gain for correcting the predicted position and velocity, and calculating a correction gain for correcting the predicted position and velocity based on the predicted position and velocity, And a control unit for calculating a position and a velocity of the moving object. 35. The method of claim 34,
The control unit
Calculating a predicted position and velocity uncertainty of the current point from the position and velocity uncertainty of the previous point,
Calculating an expected frequency uncertainty at the current time point using the predicted position and velocity uncertainty,
Calculating a dependency of mutual error between the predicted position and speed and the expected frequency,
Wherein the correction gain is calculated using the mutual error dependency and the estimated frequency uncertainty. 36. The method of claim 35,
Wherein the controller calculates the position and velocity uncertainty of the current point of time using the predicted position and velocity uncertainty and the correction gain. A computer-readable recording medium storing a program for executing the method according to any one of claims 1 to 9.

Images