An Algorithm for Determining the Navigation ... - ScienceDirect

8 downloads 0 Views 455KB Size Report
turntable with 3 degrees of freedom Aerosmith and of the firm test on underwaterr rescue robots for big fire have proved the correctness of the algorithm.
Available online at www.sciencedirect.com

ScienceDirect Procedia Computer Science 103 (2017) 331 – 338

XIIth International Symposium «Intelligent Systems», INTELS’16, 5-7 October 2016, Moscow, Russia

An algorithm for determining the navigation parameters of AUVs based on the combination of measuring devices Nguyen Quang Vinh* Academy of Military Science and Technology, Ha noi, Viet Nam

Abstract An extended nonlinear Kalman filter (EKF) for a real-time estimation of the navigation parameters of autonomous underwater vehicles (AUVs) based on the combination of angular rate sensors, magnetometers, accelerometers and speedometers, pressure sensors or GPS is recently developed. Due to the combination of the measuring devices using the EKF, the accuracy of the navigation parameters is improved because the drifts of angular rate sensors, accelerometers and noise of the measuring devices are ignored. Moreover, this combination helps us to reduce the capacity of the computation in comparison with the inertial navigation methods. We combine the inertial navigation equipments with the speed measurement devices based on an extended Kalman filter in order to improve the speed accuracy of an underwater equipment. The obtained results of the test on the turntable with 3 degrees of freedom Aerosmith and of the firm test on underwaterr rescue robots for big fire have proved the correctness of the algorithm. © 2017 2017The TheAuthors. Authors.Published Published Elsevier © by by Elsevier B.V.B.V. This is an open access article under the CC BY-NC-ND license (http://creativecommons.org/licenses/by-nc-nd/4.0/). Peer-review under responsibility of the scientific committee of the XIIth International Symposium «Intelligent Systems». Peer-review under responsibility of the scientific committee of the XIIth International Symposium “Intelligent Systems” Keywords: - extended nonlinear Kalman filter; angular rate sensors, magnetometers; accelerometer, speedometers, pressure sensors; GPS.

1. Introduction The inertial navigation algorithm generates the navigation parameters with errors. These errors increase over time because the drifts are integral twice. The additional information from other navigation systems (e.g. GPS or devices measuring the velocity and position of AUVs) is often used to reduce the mentioned errors 1,2. The use of a linear

* Corresponding author. E-mail address: [email protected]

1877-0509 © 2017 The Authors. Published by Elsevier B.V. This is an open access article under the CC BY-NC-ND license (http://creativecommons.org/licenses/by-nc-nd/4.0/). Peer-review under responsibility of the scientific committee of the XIIth International Symposium “Intelligent Systems” doi:10.1016/j.procs.2017.01.116

332

Nguyen Quang Vinh / Procedia Computer Science 103 (2017) 331 – 338

Kalman filter for solving the problem above has been studied by many authors3,4. However, the basic drawback of this method is making two successive stages: the inertial navigation algorithm and the Kalman filter algorithm, resulting in the increment of computational time and the number of calculations which affect the response time of the system. In this paper, the author proposes an application of the extended nonlinear Kalman filter in the combination of the measuring devices to remove the drifts and measuring errors. AUVs have a short working distance and move slowly. As a result, the local frame OX 0Y0 Z 0 can be considered as a navigation frame. The motion of AUVs is described in the body frame Gb X bYb Z b that has the origin coinciding with the center of gravity (see Fig. 1). 2. An algorithm for determining the navigation parameters of AUVs The direction cosine matrix between the navigation frame and the body frame is denoted here by the symbol A and can be determined through four-parameters Rodring-Hamilton q0 , q1 , q2 , q3 5:

A

aij

C

T

cij

2q02 2q12 -1 2q1q2 2q0 q3 2q1q3 2q0 q2

T

2q1q2 2 q0 q3 2q02 2q22 -1 2q2 q3 2q0 q1

2q1q3 2q0 q2 2q2 q3 2q0 q1 2q02 2q32 -1

(1)

Four-parameters q0 , q1 , q2 , q3 and the navigation parameters (the components of velocity Vx , V y , Vz and the position x, y, z of AUVs) can be determined by solving the following equations5

Fig. 1. Reference frames for AUVs

2 q0 2 q1 2 q2 2 q3

x q1 q x 0 y q0 z q0 T

[Vx , V y , Vz ]

x

where

Vx , y x,

y,

z

y q2 q z 2 z q1 y q1

z q3 q y 3 x q3 x q2

C [ Ax , Ay , Az ]T

Vy , z

(2)

T

[ 0, 0, g ]

Vz

(3) (4)

are the components of the angle velocity vector of the body frame relative to the navigation

frame, A x , A y , A z are the values of the ideal accelerometers.

333

Nguyen Quang Vinh / Procedia Computer Science 103 (2017) 331 – 338

However, the angular rate sensors and accelerometers measure the included changing slowly parameters and noise n x n y n z

y

b1 b2

wx wy

z

b3

wz

Axn Ayn

Ax Ay

c1 c2

wax way

Azn

Az

c3

waz

x

(5)

(6)

where b1 , b2 , b3 , c1 , c2 , c3 are the changing slowly parameters which express the gyro drift and wx , w y , wz , wax , way , waz are the white noise.

After solving the equations (2), (3), (4) one gets the navigation parameters with errors. In this paper the author applies an EKF to estimate the navigation parameters based on the information provided by the angular rate sensors, accelerometers, magnetometers and speedmeters, pressure sensors or GPS. Assume that the motion of AUVs is described by the discrete dynamic equations as follows. Xk

Fk

1(X k 1)

Zk

h( X k )

G( X k

1)

( k ),

(7)

v ( k ).

where X k , X k 1 are the state vector X at step k and k-1, G is the noise matrix, is the white noise vector, Fk 1 is the function vector F at step k-1, Z k is the observed output vector h at step k that is measured by the measuring devices, v is a white noise vector with zero mathematical expectation. ~ N (0, Qk ), M v ~ N (0, Rk ), M

(k )

(k )

Q ( k ),

0, T

M v(k )

Pk

T

( j ),

0, M v ( j ), v ( k )

R ( k ),

Xˆ k ][ X k

T Xˆ k ] where H is the mathematical expectation. The procedures of estimating the state vector X on the basis of the observed state vector (measured) by Kalman

([ X k

are as follows6. ( ) Xˆ k

Pk(

( ) Xˆ k 1 ,

1

)

P(

Fk k 1

) T

k 1 k 1

Zˆ k

k 1

G (k

1) Q ( k

( )

( )

Pk

Xˆ k( ) )

T Hk

(I

X

( ) Xˆ k 1

T

,

1)G ( k

Hk

hk X

X

( ) Xˆ k 1

,

1),

(8) ( )

( H k Pk

Xˆ k( )

1

X

hk ( Xˆ k ),

Kk

Pk(

Fk

T Hk

Rk )

( )

,

Zˆ k ),

K k (Zk

K k H k ) Pk

1

,

where I is the unit matrix To process EKF, the functions Fk 1 ( X k 1 ) , h ( X k ) in the expression (7) are established, from which a state

334

Nguyen Quang Vinh / Procedia Computer Science 103 (2017) 331 – 338

Fk k 1

X

1

X

Hk

( ) Xˆ

hk

ˆ( X X X

)

k k 1 and a measuring matrix transition matrix can be built. We define: x1=q0; x2=q1; x3=q2; x4=q3; x5=b1; x6=b2; x7=b3; x8=c1; x9=c2; x10=c3.

x11 X

Vx ; x12

V y ; x13

Vz ; x14

z;

(9)

( x1, x2 ,..., x14 )T

From the discretization of equation (2), (3), (4) we have functions ( f1 , f 2 , f 3 , f 4 , f11 , f12 , f13 , f14 ) of Fk 1 ( X k 1 ) . In this case, b1 , b2 , b3 , c1 , c2 , c3 change slowly, we have x j ( k ) f j ( X k 1 ) x j ( k 1), j 5 10 . To determine the state transition matrix . have fi ij

xj

,i

1, 2,...,14; j

k 1

. by taking partial derivatives fi / x j , (i

1, 2,...,14

1,...,14; j

1,...,14) , we

(10)

If the measuring noise of the angular rate sensors and accelerometers are uncorrelated with the white noise, [ w x , w y , w z , wax , way , waz ]T is the noise impact on the right side of dynamical equation (7). The noise matrix G

(11)

[ g ij ] i=1,14,j=1,6

Fig. 2. An extension of the Kalman filter for nonlinear systems.

AUVs have magnetometers that measure z1 , z 2 , z3 in the body frame of the Earth's magnetic field vector where B ,B ,B

AUVs are operating. Suppose that the values of the Earth's magnetic field in the navigation frame are x y z . It is easily to see that z1 , z 2 , z3 and Bx , B y , B z are closely related to each other only via the direction cosine matrix, that means [ z1 , z 2 , z3 [

T

T

A[ B x , B y , B z ] .

(12)

335

Nguyen Quang Vinh / Procedia Computer Science 103 (2017) 331 – 338

The speedometers measure three velocity components in the body frame (using the speedometers mounted directly on the AUVs as DVL Dopler) then

Fig. 3. The simulation results T

[ z 4 , z5 , z 6 ]

A[V x , V y , V z ]T .

(13)

The pressure sensors measure the depth z 7 of AUVs. Assuming that the measuring noise of the magnetometers, speedometers and pressure sensors are white noise, we have z1

h1 ( X k )

vT 1 , z 2

h2 ( X k )

vT 2 , z3

h3 ( X k )

vT 3 , z 4

z5

h5 ( X k )

vVy , z6

h6 ( X k )

vVz , z7

h7 ( X k )

vz ,

h4 ( X k )

vVx ,

where vT 1 , vT 2 , vT 3 are noise of the magnetometers, vVx , vVy , vVz are noise of the speedometers and v z is noise of pressure sensors. To determine measured matrix i

1, 2, ..., 7, j

Hk

by taking partial derivative hi / x j . we have H

hij ,

1, 2,...,14.

In case where AUVs work on the water surface, we can use GPS instead of speedometers and pressure sensors. In this case xi (i 1 13) are defined as (9), x14 x, x15 y , x16 z , X ( x1, x2 ,..., x16 )T . The functions ( f14 , f15 , f16 ) are established from (4). Observed output vector z1 , z 2 , z3 are the same with (12). From GPS receiver we have the observed output vector [ z 4 , z5 , z 6 , z 7 , z8 , z9 ]T

T

[V x , V y , V z , x , y , z ] . Thus, in this case we have 9

observed functions and the EKF is applied to estimate 16 states. If the angular rate sensors and accelerometers are not correlated with each other, the Q matrix is a diagonal matrix, the diagonal values are the variance of the angular rate sensors and accelerometers. If the magnetometers and speedometers, pressure sensors or GPS received are not correlated with each other then R is a diagonal matrix, the diagonal values are the variance of the magnetometers, speedometers, pressure sensors and GPS. Thus all the conditions are fully qualified to perform the Kalman filter algorithm (8). 3. The simulation results The simulation results are shown in Fig. 3. They show that the filter parameters are close to the ideal parameters. From the parameters the Haminton Rodring-state angle and the direction cosine matrix are determined. After that we determine the position of the AUV in the coordinate system fixed at the center of the earth through the IMU velocity value after a correction. The equation for calculating the position of the AUV can be written as discrete equations6:

336

Nguyen Quang Vinh / Procedia Computer Science 103 (2017) 331 – 338

Fig. 4. In the laboratory we use the turntable with three degrees of freedom 1573 Ideal Aerosmith.

ˆ (k

1)

(k )

T vˆ N ( k

1) / R ,

ˆ (k

1)

ˆ (k )

T vˆE ( k

1) / ( R cos ˆ ( k

z

KF

(k

1)

z

KF

(k )

T vˆD ( k

1).

1)),

.

(14)

In this initial position (0), (0) and z (0) are known in advance of the underwater facilities. The original location , is received with a high precision from the device before diving. 3.1. The simulation, testing the algorithm on the turn table with 3 degrees of freedom In the laboratory we use the turntable with three degrees of freedom 1573 Ideal Aerosmith. IMU modules are mounted on the turning table in Figure 46. The product mounts onto submarine rescue robots and robots for big fire protection to tests devices for robots and moves by the preset orbit in the pool. Received results (Figures 5-7) show that the velocity errors of the AUV navigation system are significantly smaller than the errors of IMU and the navigation devices while operating DVL individual. The data is processed through the Kalman filter significantly effectively for reducing the velocity errors and the disturbance rejection of IMU and DVL. The velocity of the position error corrected by the IMU and the Kalman filter increases but does not exceed the lower rate of the increment of the velocity of the position error in the IMU case when it is not calibrated. Suppose that, simulation time is up to 400S, the velocity of the IMU position errors are hundreds of meters while the velocity of the position error adjusted by IMU / DVL is only from a few meters to a few tens of meters.

Nguyen Quang Vinh / Procedia Computer Science 103 (2017) 331 – 338

Fig. 5. Comparison of the error velocity in the X-axis of the DVL, and after adjustment by the IMU Kalman filter through IMU / DVL

Fig. 6. Comparison of the speed error of DVL Z-axis, and after adjustment by the IMU Kalman filter through IMU / DVL

337

338

Nguyen Quang Vinh / Procedia Computer Science 103 (2017) 331 – 338

Fig. 7. Comparison of the speed error of DVL E-axis, and after adjustment by the IMU Kalman filter through IMU / DVL.

4. Conclusions The information of magnetometers, accelerometers, angular rate sensors and speedometers, pressure sensors or q ,q ,q ,q GPS allow us to use the EKF algorithm and receive four Rodring Hamilton parameters 0 1 2 3 . The components of velocity, the coordinates of AUVs are estimated in the navigation frame if GPS is applied. In case when speedometers, pressure sensors are applied the depth of AUVs is estimated, the position of the AUVs in the horizontal plane is determined by solving equation (4) with the estimated components of velocity. From RodringHamilton parameters, the Euler angles and the direction cosin matrix are determined. This method reduces the computation time. In addition, the combination of the measuring devices by the Kalman filter makes the precision of navigation parameters improved because it ignores the drifts of angular rate sensors, accelerometers and measuring noise of measuring devices. This paper presents a method of combining an inertial navigation equipment with speed measurement devices based on an extended Kalman filter applied to an underwater vehicle in order to improve the speed precision of the underwater equipment. Results obtained in a simulation on the turntable with 3 degrees of freedom, on submarine rescue robots Aerosmith have proved the correctness of the algorithm. However, for reducing the error of the sensor the system will be evolved by the author and tested by a number of other algorithms. References 1. Titterton DH, Weston JL. Strapdown Inertial Navigation Technology, 2nd Edition. The Institution of Electrical Engineers; 2004 2. Jalving B, Gade K, Hagen OK. A toolbox of aiding techniques for the HUGIN AUV integrated inertial navigation system. Oceans 2003 MTS/IEEE, San Diego, 2003; p. 22-6. 3. Hegrenæs O, Berglund E, Hallingstad O. Model-aided inertial navigation for underwater vehicles. The IEEE International Conference On Robotics and Automation; 2008. 4. Kaniewski P, Kaliski GS, Integrated positioning system for AUV, Molecular and Quantum Acoustics 2005;26. 5. Nguyen Quang Vinh, Truong Duy Trung Guidance, navigation and control of autonomous underwater vehicle. International Symposium on Electrical-Electronics Engineering Viet Nam (ISEE2013), p. 44-49. 6. Nguyen Quang Vinh, Tran Duc Thuan. Design, fabrication inertial navigation module with adjustment based sensors integrated microelectromechanical. Journal of Scientific Research and military technology 2014; 4: 86-92.