Integrated Navigation and Local Mapping System for a ...

285

ISSN 2075-1087, Gyroscopy and Navigation, 2016, Vol. 7, No. 3, pp. 285–295. © Pleiades Publishing, Ltd., 2016.Original Russian Text © V.N. Maksimov, A.I. Chernomorskii, 2016, published in Giroskopiya i Navigatsiya, 2016, No. 1, pp. 116–132.

Integrated Navigation and Local Mapping Systemfor a Single-Axis Wheeled Module

V. N. Maksimova and A. I. Chernomorskiia, b

aMoscow Aviation Institute, Moscow, RussiabAcademy of Navigation and Motion Control, St. Petersburg, Russia

e-mail: [email protected] April 22, 2015

Abstract⎯The paper considers a design of integrated navigation system of a single-axis wheeled module usedto perform various functions in airdrome infrastructure. The core of the integrated navigation system is astrapdown inertial navigation system integrated with a video camera and dynamic model of a single-axiswheeled module with nonholonomic contact with underlying surface. The integrated navigation systemsimultaneously estimates the module navigation parameters and the coordinates of the landmarks observedby the video camera. The system is shown to determine the navigation parameters and to construct the struc-tural map of landmarks over rather long periods without external aiding.

DOI: 10.1134/S2075108716030111

INTRODUCTION

A single-axis wheeled module (SAWM) with a car-rying platform can be used as a carrier of special equip-ment for automatic optical landing and pseudolite-aided landing of aircrafts. To improve the automaticpseudolite-aided landing system, the pseudolites areinstalled on carriers such as SAWMs to move themduring the landing to reduce the geometrical factor inthe touchdown point. Moreover, SAWMs can beeffective in monitoring the geometric parameters ofairdrome coatings [1]. SAWM motion along the tra-jectories defined on the underlying surface is con-ducted using the data from the integrated navigationsystem (INS).

The purpose of this paper is to analyze an INS fora SAWM with nonholonomic contact with the under-lying surface. The core of the INS is a strapdown iner-tial navigation system (SINS) based on micromechan-ical sensors [2, 3]. SINS is aided by video camera dataon landmarks’ coordinates and the estimates ofSAWM navigation parameters obtained using itsdynamic model (DM). INS algorithm actually rep-resents SLAM algorithm (Simultaneous Localizationand Mapping) [4], i.e., simultaneous estimation ofSAWM navigation parameters and coordinates oflandmarks.

The airdrome infrastructure is nonstationary dueto periodical motions of objects (aircrafts, serviceequipment, etc.). The images of their elementsobserved by the video camera actually function aslandmarks.

In INS, video camera data are applied instead ofsatellite navigation system (SNS) data in SLAM,which allows local mapping of airdrome infrastructureand therefore online update SAWM trajectoryonboard during the motion, apart from merely SINSaiding. Also, using SLAM for SINS aiding signifi-cantly reduces the effect of SNS signal suppression (ifit is used) on the aiding process.

In [5–7], the carrier INS has a SINS aided bySLAM algorithm using the video camera as a core ele-ment. The novelties proposed in this paper are as fol-lows: INS information base is extended by including adynamic model of a specific carrier, namely, nonholo-nomic SAWM, SLAM algorithm is modernized byconstructing it in the form suitable for estimating thestate vector describing the INS errors, and modifiedmodels of errors in landmark coordinates parameter-ized using quaternions are used in integration.

SUBJECT OF RESEARCHThe single-axis wheeled module contains a carry-

ing platform with upper pendulosity placed on twocoaxial wheels with electric drives (Fig. 1).

INS with a SINS and digital video camera is placedon the platform. SAWM motion control system con-sists of locomotion and trajectory control loops andsolves the problem of SAWM motion along predefinedspace-time trajectories defined by program timedependencies of SAWM coordinates and heading onairdrome surface in navigation frame [8]. SAWM plat-form is kept near the horizon plane using the inertial

286

GYROSCOPY AND NAVIGATION Vol. 7 No. 3 2016

MAKSIMOV, CHERNOMORSKII

control principle: when the platform deviates from theplane, control inputs appear (force moments appliedto SAWM wheels by the control drives), the SAWMaccelerates, and inertia moments with respect to thewheelpair axis applied at the platform center of massreturn it to its horizontal position.

The following frames are introduced in Fig. 1: is a navigation frame (axes OX, OY are in hori-

zon plane); is a common frame for image sen-sor and SINS bound with them with an origin at theplatform center of mass С (the axes are aligned withthe platform main center lines of inertia, axis isperpendicular to its plane and passes through thewheelpair axis midpoint axis is aligned withthe video camera optical axis, axis is parallel to thewheelpair axis), axes are aligned with at the initial moment of SAWM motion; is amoving horizontal frame with the axes parallel to therelevant OXYZ axes. Additionally, the following deno-tations are used in the figure: is the deviation of axis

from the vertical is the distance betweenthe centers and С; is SAWM heading; is theplatform velocity in horizon plane; are the forcemoments on SAWM wheels.

Video camera parameters are denoted as follows: are the image sensor axes; are the video

camera focal distances; are the coordinates ofthe point where the video camera optical axis crossesthe image sensor; is a coordinate vector of the pro-

OXYZb b bCx y z

bCz

,cO bCxbCy

b b bCx y z OXYZc c c cO X Y Z

αbCz ;cOZ d

cO θ V,rτ τl

μ λ, μ λ,f f,C Cμ λ

,n

f ir

jected point (i-th landmark) in navigation frame; isthe unit directional vector of LOS connecting thepoint С and the projected point in b frame; is thelandmark projection on the image sensor.

SLAM PROBLEM IN SAWM NAVIGATION

SLAM problem is a problem of simultaneous esti-mation of carrier current navigation parameters and apriori unknown structural map of landmarks indicat-ing their coordinates in navigation frame. A majorSLAM algorithm is an estimation algorithm EKF-SLAM based on extended Kalman filter [9]. The esti-mated state vector is

(1)

where is the carrier state vector including its linearand angular coordinates in navigation frame; is thevector of coordinate parameters of the i-th landmark(specific vector form depends on the method of coor-dinate parameterization in navigation frame);

is a vector whose set of ele-ments forms the structural map; is a number of land-marks in the structural map of the local space, varyingduring the algorithm operation.

Covariance matrix of vector is given by

bie

,iμ λ i

1.. 1. ,... ,

i n

T T T Tf f f

m

T

x i nx x x xυ

⎡ ⎤⎢ ⎥= = …⎢ ⎥⎣ ⎦

��

υx

ifx

1... ...

i n

TT T Tf f fm x x x⎡ ⎤= ⎣ ⎦

n

P x

Fig. 1. Reference frames.

xb

yb

zb

Yc

Xc

Oc

Zc

Y

Z

μλ

α

Cd

Video camera

image sensor

Wheelpair

axis

V

τr

τl

θ

fμ, fλ

[μi, λ

i]

[Cμ, Cλ]

SINS

Platform

i-th landmark

LOS

Wheel

O

X

θ�

α�

bie

nf ,ir


INTEGRATED NAVIGATION AND LOCAL MAPPING SYSTEM 287

(2)

where lower indices of Р denote the relevant covari-

ance objects (for example, corresponds to the

covariance of vectors of coordinate parameters of the1st and the -th landmark).

Write the nonlinear discrete equations of systemdynamics and Kalman filter measurement equationsin EKF-SLAM in general form:

(3)

(4)

where is the carrier state vector at time is the

carrier control vector; and are the measure-ment, system white noise, and measurement whitenoise vectors, respectively.

For the carrier being a wheeled vehicle, we usuallyuse a monowheel kinematic model [10] including its

coordinates and heading as a model .

Landmarks are considered stationary during the

observation, and function in measurement

equation (4) is generated as a function of relative mea-surements of landmarks’ coordinates onboard the car-rier. The function is determined by the type of themeters used [11].

State vector (1) and covariance matrix (2) arerecurrently estimated using discrete extended Kalmanfilter equations:

(5)

(6)

(7)

(8)

(9)

where is the covariance matrix of

system noise vector are the

blocks of matrix defining covariances of vectors

and is the identity matrix of

relevant dimensionality; is the Kalman filter gain

matrix; is the covariance matrix of measurement

noise vector

1

1 1 1 1

1

,

n

n

n n n n

f f

T

f f f f f

T T

f f f f f

P P P

P P PP

P P P

υυ υ υ

υ

υ

⎡ ⎤⎢ ⎥⎢ ⎥=⎢ ⎥⎢ ⎥⎣ ⎦

�

�

� � � �

�

1 nf fP

n

( )1, ;

k k k kx f x u w−υ υ υ= +

( ), ,kk kz h x mυ= + v

υkx ;k ku

,kz kw kv

( )−υ υ 1,

k kf x u

( )υ ,k

h x m

� �( )1, ;

k k kx f x u−

−υ υυ=

1 1

1 1

;k k k k k

k k k

T

k m

k T T

m mm

F P F Q F PP

P F P

− −

− −

υ υυ υ υ υ−

υ υ

⎡ ⎤+= ⎢ ⎥⎢ ⎥⎣ ⎦

�

�

�

�

� �( )( )1

1

, ;k k

k kk k

k k

x xK z h x m

m m

+ −−υ υυ −

−

⎡ ⎤ ⎡ ⎤= + −⎢ ⎥ ⎢ ⎥

⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦

( ) ( ) ;T T

k k k k k k k k kP I K H P I K H K R K+ −= − − +

( ) ( ) 1

,T T

k k k k k k kK P H H P H R−− −= +

�

1;

kkF f x −υυ υ= ∂ ∂ kQ

;kw1,

kP

−υυ 1,

kmP−υ −1kmmP

−1kP

υx ,m ;k kH h x= ∂ ∂ I

kK

kR

.kv

SLAM algorithm has a specific feature, namely,dimensionality of vector during the filter operationchanges due to initialization of coordinates of newlandmarks and rejecting coordinates of defective land-marks in the course of SAWM motion, i.e., due tochanges in dimensionality of vector By defectivelandmark, we mean a landmark moving for some rea-son. New landmarks are identified and initialized asSAWM moves; the coordinate parameters of land-marks are unknown and should be estimated. To ini-

tialize a new landmark, function is used deter-mined by the measuring instrument type (e.g., a video

camera), which calculates the coordinate vector

by SAWM current navigation parameters and

measurement vector

(10)

The state vector is augmented due to addition of

a new ( )-th landmark, and covariance matrix augmented due to adding of new columns and rows isgiven by

(11)

(12)

(13)

(14)

where is the

measurement noise covariance matrix. If a defectivelandmark is deleted, relevant columns and rows of

matrix are also deleted.

INS algorithm based on traditional SLAMapproach has a number of drawbacks. It includesSINS equations in system dynamics model (3), whichmakes it necessary to use large computationalresources. Moreover, EKF-SLAM algorithm dictatesthat attitude angles of the system components beselected as orientation parameters, which, particu-larly, leads to a certain inefficiency of the computa-tional algorithm.

Consider a possibility of constructing a modifiedEKF-SLAM algorithm, whose state vector describesthe errors of vector in (1). Also, to reduce the errorsin navigation parameters in INS measurements usethe additional data from SAWM dynamic model. The

error vector is written as

(15)

x

.m

+1nF

+1nfx

υkx

kz

( )1 1 , .

n knf kzx F x+ + υ=

x

+ 1n P

1 1

1 1 1 1 1

1 1 1 1 1

;

n

n

n n n n

f f

T

f f f f f

T T

f f f f f

P P P

P P PP

P P P

+

+

+ + + +

υυ υ υ

υ

υ

⎡ ⎤⎢ ⎥⎢ ⎥=⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

�

�

� � � �

�

1 1;

n n

T T

f f z z zP H P H H R H+ + υ υυ υ= +

1( ) ;

n

T

fP H P+ υ υυυ =

+ υ υ=1 1 1

( ,)n

T

f f fP H P

1 ;nH F xυ + υ= ∂ ∂ 1 ;nzH F z+= ∂ ∂ zR

P

x

δx

υδ δ

⎡ ⎤⎢δ ⎥δ =⎢ ⎥⎣ ⎦

δ δ δ δ� ��

1,

i n

T T T T T

INS DM f f

T

f

x m

x x x x xx

288



where is SAWM state error vector; is SINS

error vector; is the DM error vector; is the

-th landmark coordinates error vector; is the error

vector of structural map components.

As compared with (1) in (15), component of

state vector is substituted with error vectors

and and the monocycle kinematic equation (3)

in modified algorithm should be replaced with error

dynamics equations of SINS and DM. Landmark ini-

tialization equations (10)–(14) remain mostly

unchanged with the only difference that at landmark

initialization the vector error (15) is augmented by

adding an initially zero vector of its coordinates’ errors

with further assigning significant components to it as

the coordinates of the landmark are being measured.

In this paper, landmarks are identified as typical

image areas provided by the video camera image sen-

sor. Landmarks possess certain properties facilitating

their further detection in different frames with chang-

ing perspective and image noise. Here we use the

FAST landmark detection algorithm [12]. FAST algo-

rithm processes the current frame of digital camera

and generates the array of coordinates of landmarks’

central points. For further landmark identification, a

square image area with its central point and side of 41

pixel is saved. Figure 2 presents the digital camera

frame with the landmarks detected by FAST algo-

rithms (shown by markers).

Defective landmarks are detected using an empiri-

cal threshold, namely, ratio between the quantities of

failed and successful identifications. Landmark iden-

tification is performed by two-dimensional cross-cor-

relation of the sample landmark image saved at initial-

ization and the current sample landmark image from

the video camera. If the ratio exceeds the predefined

empirical threshold, the landmark is considered

defective and is deleted.

υδx δ INSx

δ DMx δif

x

i δm

υx

x δ INSx

,DMxδ

SINS AND DM ERROR DYNAMIC EQUATIONS

SINS error dynamic equations in matrix form aregiven by [13, 14]:

(16)

where is the matrix of orientation with

respect to are the vectorsof errors in SAWM platform coordinates, velocity,

angular rate, and linear acceleration; is the error

vector of orientation of the calculated frame

with respect to ideal frame; are the skew-sym-

metric matrices corresponding to the vector multipli-

cation operation; and are the vectors of gyro and

accelerometer white noises; is the vector of abso-lute linear velocity of SAWM platform in the bound

frame; is the gravity vector in navigation frame.

Approach to using DM data in INS is based onpseudomeasurements—estimates of some kinematicparameters of carrier motion calculated by numericalintegration of carrier dynamic equations onboardduring displacement along a predefined space-timetrajectory [15, 16]. Inputs for SAWM dynamic model

are the reference velocities and received from tra-jectory control system, which generate the forcemoments on the wheels as a result of locomotion con-trol loop operation [8].

In [17], a dynamic model of nonholonomic SAWMis received

(17)

where is a vector nonlinear function;

is the vector of

moments on SAWM wheels.

As pseudomeasurements, we use the model valuesof vector of platform center of mass С linear velocity

and of vector of its angular rate in the bound frame

0 0 0

0

0 0 0 0

0 0 0 0 0

0 0 0 0 0

0 0

0 0

0 0

0

0

INSINS

INS

n n n b

b b

b b n n b

b

n

bb

b

Ax

n

b

b

b

x

r C C V

V C g V I

C

a

r

V

I

Ia

δ

δ

⎡ ⎤ ⎡ ⎤⎡ ⎤δ ×⎣ ⎦⎢ ⎥ ⎢ ⎥δ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥ − ω × × ×⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎣ ⎦⎢ ⎥ ⎢ ⎥=δΨ −⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥δω⎢ ⎥ ⎢ ⎥δ ⎢ ⎥⎢ ⎥ ⎣ ⎦⎣ ⎦

⎡ ⎤δ ⎡⎢ ⎥ ⎢δ⎢ ⎥ ⎢⎢ ⎥δΨ× + ⎢⎢ ⎥δω⎢ ⎥⎢ ⎥ ⎣δ⎣ ⎦

�

�

�

�

�

��

��

,

INS

a

G

n

n

ω

⎤⎥⎥ ⎡ ⎤⎥ ⎢ ⎥⎣ ⎦⎢ ⎥

⎢ ⎥⎦

n

bC b b bCx y z

;OXYZ ,n

rδ ,b

Vδ ,bδω δ b

a

δΨb b bCx y z

[ ]… ×

ωn anb

V

ng

rV θ� r

( ), ,DM DM DM DMx xf u=�

( ),DM DM DMf x u

;T

DMx V= α θ⎡ ⎤⎣ ⎦�

� [ ]= τ τ T

DM r lu

b

mV

Fig. 2. Digital camera frame with the landmarks detectedby FAST algorithm.



expressed via the velocities received bynumerical integration of model (17) with account fornonholonomic contact between SAWM wheels andthe underlying surface:

(18)

(19)

In constructing the DM error vector as acomponent of vector (15) we limit ourselves to theerrors arising due to errors in setting model controlmoments on SAWM wheels. Assuming that the model(17) and its parameters are adequate, represent the

error vector as follows

(20)

Write the error equations of SAWM dynamic modelbased on linearization (17) as follows:

(21)

where is the vector of errors

in setting moments on SAWM wheels.

As a linearization point of model (17) it is expedientto select a point from SAWM state space correspond-ing to its uniform displacement with a leveled plat-form.

ERRORS IN LANDMARKS’ COORDINATES

Parameterize the landmark coordinates via inverse

distance [18]. Coordinate vector of the -th landmark

is written as

(22)

ωb

m α θ�

�, ,V

cos sinsin ;Tb

m d dV V V= α − α −θ α α⎡ ⎤⎣ ⎦�

�

sin c s .oTb

m = −θ α α θ α⎡ ⎤⎣ω ⎦� �

�

δ DMx

:DMxδ

.T

DMx Vδ = δα δ δθ⎡ ⎤⎣ ⎦�

�

( )

( )

,

,,

DM DMDM DM

DM

DM DM DMDM

DM

f ux x

x

f uu

u

x

x

∂δ = δ∂

∂+ δ∂

�

[ ]δ = δτ δτ T

DM r lu ,rδτδτl

i

fix

⎡ ⎤= ρ⎣ ⎦0, ,T Tn

fi i i ix r q

where is the vector of coordi-

nates of initial landmark sighting point in navigation

frame; is the orientation quaternion of initial land-

mark line of sight (LOS) in navigation frame; is theinverse distance from the initial sighting point to thelandmark along LOS (Fig. 3).

Write the vector coordinates parameterized usingthe inverse distance in navigation frame

(23)

where is the unit vector of

initial LOS from the initial point; is the orientation

matrix corresponding to

Directional vector of the current LOS in thebound frame is given by

(24)

where is the vector of SAWM coordinates in navi-gation frame.

Error vector of the -th landmark coordinates iswritten as

(25)

where is the error vector of coordinates of land-

mark initial sighting point;

is the error vector of

orientation of landmark initial LOS in navigation

frame; is the error in inverse distance determina-tion.

The accepted method of landmark parameteriza-tion defines the following relations, which realize the

function from (10) (here, ).

⎡ ⎤= ⎣ ⎦0, 0, 0, 0,

Tn n n n

i i i ixr y z

iq

ρi

( )= + ρ0, 0,1 ,n n

fi i i i ir er q

( ) ( )[ ]=0, 1 0 0Tn

i fi iqe Cq

n

fC

.iq

b

ie

( ) ( )( )0, 0, ,b b n n

i n i i i irr qe C e= ρ − +

nr

i

⎡ ⎤δ = δ δΨ δρ⎣ ⎦0, , ,Tn T

fi i f i

T

ix r

δ 0,

n

ir

[ ]δΨ = δ δ δ, , ,Ψ Ψ ΨT

fi f i x f i y f i z

δρi

( )υ+1 ,k knF x z [ ]μ λ= T

k i iz

Fig. 3. Parameterization of landmark coordinates via inverse distance.

0

n,ir 0,i i

e (q )

Initial

sighting

point

SAWM platform

yb

xb

zb

Current LOS

i-th landmark

Initial LOS

ebi

0 0,i i

1e (q )

n,i

i

r +ρ

nr

nbC

1

iρ

290



Write the equation for the directional vector of -thlandmark initial LOS in navigation frame

(26)

Represent attitude angles and of initial LOS:

(27)

(28)

where are the relevant projections of on

navigation frame.

Equations (26)–(28) make it possible to find qua-

ternion by a known method [19]. Coordinate vector

equals SAWM vector at the time of landmark

initialization. At the same time, inverse distance isselected arbitrarily based on a priori data on typicalranges to the landmarks in SAWM functioning area.

Note that components and of error vector

(25) of i-th landmark coordinates with account forcalibration and constancy of video camera model fac-tors are defined by SINS errors at the time of land-

mark initialization. Component of vector (25) is

defined by the error in arbitrary assigning to initial-ization moment. The landmarks are considered sta-tionary during the observation, so components of vec-

n

ie i

( ) ( )[ ]1 .i

n

i

n

b iC C Ce f fμ μ λλμ − −λ=

ξ γ

( )arctan , ;n n

y xe eξ = − π < ξ ≤ π

( )2 2arctan ( ,

2 2,

( ) )n n n

z x yee eγ = +

− π < γ < π

,n

xe ,n

yen

zen

ie

iq

0,

n

irn

r

ρi

δ 0,

n

ir δΨ fi

δρi

ρi

tor (25) are modeled hereinafter as random constants

( ).

ALGORITHM OF SAWM INTEGRATED NAVIGATION SYSTEM

Block diagram of SAWM INS is presented inFig. 4.

The inputs for INS are the measurements of iner-

tial sensors (gyros) and (accelerometers), video

camera measurements of landmark projections

on image sensor, and control actions from tra-jectory control system coming to SAWM dynamicmodel. The estimates of parameters received by Kal-man filter are denoted by (ˆ) in the diagram.

Components of measurement vector of the fil-ter are generated as differences of linear velocity vec-tors and of angular rate vectors received from SINSand SAWM DM:

(29)

On the other hand, we have an equation

(30)

where is the measurement noise vector; isthe matrix of measurements in SINS-DM channel

δ =� 0fix

ωb ba

μ λ,i i

,rV θ� r

DMz

) ( )( .Tb b b b

DM m

T T

mz V V ω⎡ ⎤= − − ω⎣ ⎦

⎡ ⎤= δ δ + ν⎣ ⎦ ,TT T

DM DM INS DM DMH x xz

νDM DMH

Fig. 4. SAWM INS block diagram.

Extrapolator

of directional

vector

Kalman

filter

Dynamic model

of SAWM with locomotion

control system

Video

camera

SINS

Structural map

Landmark initialization

From trajectory control system



(31)

Matrices and are given below:

(32)

(33)

The filter measurement vector is generated as

a difference between the estimate vector receivedby the extrapolator using (24), and directional vector

of the landmark received as a measurement fromthe video camera:

(34)

On the other side,

(35)

where is the measurement matrix in SINS-

video camera channel; is the measurement noisevector.

Measurement matrix relating withSINS errors and the landmark coordinate errors canbe obtained, with account for constant video cameramodel coefficients, by varying (24) for the directional

vector of the current LOS:

(36)

(37)

Note that generally the measurement noises

and are not white. According to the results of theauthors’ experimental research we assume for defi-

niteness that the main contributors to the noise in

(30) are the pseudomeasurement noises of vectors

and due to errors in generating the moments which in first approximation can be considered to bediscrete white noises with experimentally defined

RMS. Then RMS of pseudomeasurement noise can be determined using the linearized error model ofSAWM DM (21) and linearized equations for model

values (18) and (19) of vectors and As a linear-ization point for (18) and (19) it is expedient, as earlier,to use a point from SAWM state space correspondingto its uniform displacement with a leveled platform.

Then the estimate of covariance matrix of

noise vector can be taken in the form [20]:

(38)

where are RMS of discrete

noises of measuring the components of SAWM linearand angular rates.

Similarly, according to the results of experimentalresearch assume that the main contributors to noise

in (35) are the noises in measuring coordinates

and of the landmark projection on image sensorgenerated by the white noises of this matrix deter-

mined experimentally. Then RMS noises of discrete

measurements of directional vector can be deter-

mined using (26) if matrix is a unity matrix. Cova-

riance matrix of noise vector then has theform

(39)

where are RMS discrete noises of measur-

ing the components

The set of equations (16), (17), (21) (after discreti-zation [21]), and equations (29)–(39) is sufficient forgenerating the discrete extended Kalman filter [22].The results of functioning of SAWM INS algorithm

are the estimates of SAWM navigation parameters

and estimates of coordinates of landmarks inthe structural map of the local space.

SIMULATION OF SAWM INTEGRATED NAVIGATION SYSTEM PERFORMANCE

For simulation, we used the parameters of typicalSAWM [1]. SAWM dynamics under control momentson its wheels was simulated using numerical integra-tion techniques.

Parameters of error models of MEMS inertial sen-sors correspond to the parameters of commercial sen-sors. Digital video camera has the following parame-ters: resolution—320 × 240 pixels; viewing angle—90°. For the trajectory control system to generate thecircular trajectory, the following speeds were pre-

0 0 0 0.

0 0 0 0

V

DM

I HH

I H ω

⎡ ⎤= ⎢ ⎥⎣ ⎦

VH ωH

cos 0

0 0 sin ;

0 sin 0

V

d

H d

− α⎡ ⎤⎢ ⎥= α⎢ ⎥

− α⎢ ⎥⎣ ⎦

0 0 sin

1 0 0 .

0 0 cos

H ω

α⎡ ⎤⎢ ⎥= −⎢ ⎥

α⎢ ⎥⎣ ⎦

CAMz

� ,b

ie

b

ie

� .b biC iAMz e e= −

⎡ ⎤= δ δ + ν⎣ ⎦ ,TT T

CAM CAM INS fi CAMH xz x

CAMH

νCAM

CAMH CAMz

b

ie

( )0, 0,0 0 ( ;)0i

b b b n b n n

CAM n i n i n f i n ieH C H C C C C r rψ⎡ ⎤⎡ ⎤= − ρ ρ × −⎣ ⎦⎣ ⎦

( )( ) ( )

0,

0, .i

b n

n i i

b n b

n f i

n

i n

r

r

H C

C C C e

ψ ⎡ ⎤= ρ ×⎣ ⎦

⎡ ⎤ ⎡ ⎤− ρ × + ×⎣ ⎦ ⎣ ⎦

νDM

νCAM

νDM

b

mV

ωb

m τ ,τr l

νDM

b

mV .b

mω

DMR

νDM

2 2 2 2 2 2diag ,

x y z x y zVM V VDR ω ω ω⎡ ⎤= σ σ σ σ σ σ⎣ ⎦

, ,,x y zV V Vσ σ σ ω ω ωσ σ σ, ,

x y z

νCAM

μ i λ i

b

ien

bC

CAMR νCAM

2 2 2diag ,b b b

x y zeCA eM eR ⎡ ⎤= σ σ σ

⎣ ⎦

σ σ σ, ,b b bx y ze e e

.b

ie

� ,n

r

� ,b

V �

n

bC n

292



defined: m/s, rad/s. The totalduration of model experiment was 150 s; for the first30 s SAWM was in the start position observing 10 land-marks with the apriori known coordinates. Then it fol-lowed a circular trajectory for 120 s; as one landmarkfrom the set of 10 simultaneously observed landmarksdisappeared, a new simulated landmark appeared, andINS continuously determined and updated the land-marks’ coordinates and SAWM navigation parame-ters.

Figure 5 presents the landmarks’ structural mapand SAWM trajectory within an experiment. Refer-ence positions of the landmarks are denoted by roundmarkers, and INS positions, by square markers. Theleft part of the figure presents the estimate of the struc-tural map of landmarks and SAWM coordinates beforethe trajectory closing. As is seen, errors accumulatedin INS lead to biased estimates. The right part of thefigure presents the structural map after trajectory clos-ing, i.e., after SAWM returns to the suburb of the start-ing point. By repeated observing the landmarks withthe known coordinates, SLAM algorithm updatescoordinates of SAWM and all the landmarks.

Figures 6 and 7 present the errors in SAWM coor-

dinates velocities and attitude angles Two sets of plots present INS algorithm performanceusing only the channel SINS-video camera (index

) and using additionally the channel SINS-DM

(index ).

Analysis of the plots suggests that using DM datasignificantly reduces the errors in SAWM navigationparameters.

Figure 8 presents the radial position errors of 10landmarks initialized at the 40th second of simulationexperiment during SAWM displacement. As is seen,

= 1.0rV θ =� 0.05r

Δ ,n

r Δb

V ΔΨ.

(.)CAM

(.)DM

even one circle of closed trajectory makes it possible toconstruct a structural map of landmarks with subme-ter accuracy.

SLAM method uses relative rather than absolutemeasurements of landmarks’ coordinates, which is aserious disadvantage: the measurements are con-ducted onboard the carrier, and after a rather longperiod the estimates of carrier navigation parametersand landmarks’ coordinates become inadequate [23].This inadequacy is due to accumulation of SINSerrors, errors in landmarks’ coordinates, DM errors,INS model errors even with closed trajectories.

To make the estimates adequate, INS should beperiodically updated for example by SNS data. Simu-lation of SAWM INS performance has shown thatupdate interval for SAWM operation in airdromestructure is about 2 minutes.

CONCLUSIONS

The paper proposes a design of integrated naviga-tion and local mapping system for a single-axiswheeled module with nonholonomic contact with theunderlying surface. INS algorithm is based on modi-fied EKF-SLAM algorithm using data from SINS,video camera and SAWM dynamic model. The map-ping problem is shown to be solved with submeteraccuracy, and additional DM updates critically reducethe errors in SAWM navigation parameters. On thewhole the considered INS design provides determina-tion of SAWM navigation parameters and construc-tion of the landmark structural map in airdrome infra-structure on rather long intervals for INS with SINSon low-accuracy inertial sensors in unaided mode.

This work has been supported by the Ministry ofEducation and Science of the Russian Federation(task №8.1573.2014/К).

Fig. 5. Simulation of SAWM INS algorithm.

–40

–40

–60

–80

–20

–20

0

0

20

20

–40

–60

–80

–20

0

20

40 60

Distance, m

–40 –20 0 20 40 60

Distance, m

Dis

tan

ce,

m

ybzb

xbX

Y

Before trajectory closing After trajectory closing

Landmark

SAWM

SAWMtrajectory

Landmark

estimate

zb X

Y

yb xb



APPENDIX

Denotations

—platform center of mass

—midpoint of SAWM wheelpair axis

—platform deviation from the horizon

—SAWM heading angle

C

cO

αθ

—distance from to

—SAWM motion velocity in horizon plane

—torques at the wheels

—image sensor frame

—coordinates of landmark projection on theimage sensor

d cO C

V

,rτ τl

μ λ,

,iμ λ i

Fig. 6. Errors in SAWM coordinates and velocities.

1

1 1

2

3

4

5

6

1

2

3

4

5

6

2

2

3

3

4

4

5

5

6

100

20

20 40 60 80 100 120 140

40 60 80 100 120 140

0.02

0

–0.02

–0.04

110 120 130 140

6

100 110 120 130 140

0.4

0.2

0

–0.2

–0.4

Co

ord

ina

te e

rro

rs,

m/s

Velo

cit

y e

rro

rs,

m/s

Time, s

SAWM trajectory closing

bxΔυ

bxΔυ

byΔυ

byΔυ

,bzΔυ

,bz DM

,DM

,DM

CAM

, CAM

, CAM

Δυ

nx , CAMrΔny, CAMrΔ

ny, DMrΔ

nx, DMrΔ

nz, CAMrΔ

nz, DMrΔ

Fig. 7. Errors in SAWM attitude.

1.719

1.146

0.573

0

0 50

1

12

2

33

4

4

5566

ΔΨz, CAM

ΔΨy, CAM

ΔΨx, CAM

ΔΨz, DM

ΔΨy, DM

ΔΨx, DM


Time, s

100 150

–0.573

–1.146

–1.719

Err

ors

in

att

itu

de a

ngle

s, d

eg

294



—focal distances and coordinates of

videocamera principal point

—ector of landmark coordinates

—unit directing vectors of LOS

—filter state vector

—vehicle state vector

—vector of landmark coordinate parameters

—vector of landmark coordinates on structuralmap

—number of landmarks

—vector covariance matrix

—measurement and control vectors

and —system and measurement noise vectors

—covariance matrixes of vectors and

—unity matrix

—Kalman filter gain matrix

—measurement noise covariance matrix at land-mark initialization

—INS, SINS and torquer error vec-tors

—error vector of landmark coordinate parame-

ters

—attitude platform of SAWM platform

—coordinate, velocity, angular rate,and linear acceleration vectors of SAWM platform

—coordinate, velocity,angular rate, and linear acceleration error vectors ofSAWM platform

and — gyro and accelerometer noise vectors

—gravity vector

—SAWM state vector and dynamicmodel controls

, ;f fμ λ μ λ,C C

,

n

f ir

,b

ien

ie

x

υx

ifx

m

n

P x

,kz ku

kw kv

,kQ kR kv kw

I

kK

zR

,xδ ,INSxδ δ Tx

δif

x

n

bC

,n

r ,b

V ,bω b

a

,n

rδ ,b

Vδ ,δΨ ,bδω δ b

a

ωn an

ng

,DMx DMu

и —vectors of linear and angular velocity ofthe platform center of mass

—vectors of coordinates of initial sighting

point and its error

—attitude quaternion of initial LOS,

relevant attitude matrix and their error vector

ξ, —attitude angles of initial LOS

—unity vector of initial LOS

—inverse distance and its error

—vector, matrix and measurement noisevector in SINS-torquer channel

—vector, matrix and measure-ment noise vector in SINS-video camera channel

—covariance matrices of vectors and

REFERENCES

1. Aleshin, B.S., Chernomorskii, A.I., Feshchenko, S.V.,Sachkov, G.P., Plekhanov, V.E., Maksimov, V.N., andVeremeenko, K.K., Orientatsiya, navigatsiya i stabili-zatsiya odnoosnykh kolesnykh modulei (Orientation,Navigation, and Stabilization of Single-Axis WheeledModules), Moscow: MAI, 2012.

2. Peshekhonov, V.G., Nesenyuk, L.P., Gryazin, D.G.,Nekrasov, Ya.A., Yevstifeyev, M.I., Blazhnov, B.A.,and Aksenenko, V.D., Inertial units on micromechani-cal sensors. Development and tests results, 15th JubileeSt. Petersburg International Conference on IntegratedNavigation Systems, St. Petersburg: Elektropribor,2008, pp. 9–15.

3. Peshekhonov, V.G., Nesenyuk, L.P., Gryazin, D.G.,Nekrasov, Y.A., Yevstifeev, M.I., Blazhnov, B.A., andAksenenko, V.D., Inertial measurement unitson micromechanical sensors, IEEE Aerospace andElectronics Systems Magazine, 2008, vol. 23, no. 10,pp. 26–31.

b

mV ωb

m

0, ,n

ir δ 0,

n

ir

,iq ,n

fC δΨ fi

γ

0,ie

,iρ δρi

,Tz ,TH νT

,CAMz ,CAMH νCAM

,TR CAMR νT

νCAM

Fig. 8. Radial position errors of landmarks 1–10.

1.2

1.0

0.8

0.6

0.4

0.2 6–104

32

1


40 60 80 100 120 140

Time, s

Ra

dia

l p

osi

tio

n e

rro

rs

of

lan

dm

ark

s, m



4. Dissanayeke, M., Newman, P., Clark, S., Durrant-Whyte, H., and Csorba, M., A solution to the simulta-neous localization and map building (SLAM) problem,IEEE Transactions on Robotics and Automation, 2001,vol. 17, pp. 229–241.

5. Jones, E. and Soatto, S., Visual-inertial navigation,mapping and localization, The International Journal ofRobotics Research, 2011, vol. 30, p. 407.

6. Kleinert, M. and Stilla, U., A Sensor-centric EKF forinertial-aided visual odometry, Proceedings of IPIN,2013, pp. 771–778.

7. George, M. and Sukkareih, S., Inertial navigation aidedby monocular camera observations of unknown land-marks, IEEE International Conference on Robotics andAutomation, ICRA, 2007.

8. Maksimov, V.N. and Chernomorskii, A.I., Control sys-tem of nonholonomic single-axis wheeled module formonitoring the geometrical parameters of airdromecoatings, Izvestiya RAN. TiSU, 2015, no. 3, pp. 200–211.

9. Durrant-Whyte, H., Rye, D., and Nebot, E., Localisa-tion of automatic guided vehicles, Robotics Research:The 7th International Symposium, 1996, pp. 613–625.

10. Thrun, S., Burgard, W., and Fox, D., ProbabilisticRobotics, Cambridge, MA: The MIT Press, 2005.

11. Durrant-Whyte, H. and Bailey, T., SimultaneousLocalization and Mapping: Part 1, Robotics & Automa-tion Magazine, 2006, pp. 99–110.

12. Rosten, E. and Drummond, T., Fusing points and linesfor high performance tracking, Proceedings of the IEEEInternational Conference on Computer Vision, 2005,vol. 2, pp. 1508–1511

13. Gelb, A., Applied Optimal Estimation, Analytic SciencesCorporation, 1974.

14. Titterton, D. and Weston, J., Strapdown Inertial Navi-gation Technology, Institution of Engineering and Tech-nology, 2005.

15. Koifman, M. and Bar-Itzhack, I.Y., Inertial navigationsystem aided by aircraft dynamics, IEEE Transactionson Control Systems Technology, 1999, vol. 7.

16. Crocoll, P., Seibold, J., Scholz, G., andTrommer, G.F., Model-aided navigation for a quadro-tor helicopter: A novel navigation system and firstexperimental results, Navigation, Journal of the Instituteof Navigation, 2014, vol. 61, no. 4, pp. 253–217.

17. Chernomorskii, A.I., Maksimov, V.N., andPlekhanov, V.E., Mathematical model of measuringsingle-axis wheeled module in matrix form, VestnikMAI, 2013, vol. 20, no. 2.

18. Civera, J., Davison, A., and Montiel, J., Inverse depthparametrization for monocular SLAM, TRO, 2008,vol. 24, pp. 932–945.

19. Chelnokov, Yu.N., Kvaternionnye i bikvaternionnyemodeli i metody mekhaniki tverdogo tela i ik prilozheniya(Quaternion and Biquaternion Models and Methods ofSolid Body Mechanics and Their Applications), Mos-cow: FIZMATLIT, 2006.

20. Stepanov, O.A., Osnovy teorii otsenivaniya s prilozheni-yami k zadacham obrabotki navigatsionnoi informatsii.Part 2. Vvedenie v teoriyu fil’tratsii (Fundamentals of theEstimation Theory with Applications to the Problemsof Navigation Information Processing. Part 2. Intro-duction to the Filtering Theory), St. Petersburg: CSRIElektropribor, 2012.

21. Stepanov, O. A., Osnovy teorii otsenivaniya s prilozheni-yami k zadacham obrabotki navigatsionnoi informatsii.Part 1. Vvedenie v teoriyu otsenivaniya (Fundamentals ofthe Estimation Theory with Applications to the Prob-lems of Navigation Information Processing. Part 1.Introduction to the Estimation Theory), St. Petersburg:CSRI Elektropribor, 2010.

22. Stepanov, O.A., Integrated inertial-satellite navigationsystems, Giroskopiya i Navigatsiya, 2002, no. 1, pp. 23–45.

23. Julier, S. and Uhlmann, J.K., A counter example to theTheory of Simultaneous Localization and Map Build-ing, Proceedings of IEEE ICRA, 2001, vol. 4.

本文献由“学霸图书馆-文献云下载”收集自网络，仅供学习交流使用。

学霸图书馆（www.xuebalib.com）是一个“整合众多图书馆数据库资源，

提供一站式文献检索和下载服务”的24 小时在线不限IP

图书馆。

图书馆致力于便利、促进学习与科研，提供最强文献下载服务。

图书馆导航：

图书馆首页文献云下载图书馆入口外文数据库大全疑难文献辅助工具

http://www.xuebalib.com/cloud/

http://www.xuebalib.com/

http://www.xuebalib.com/cloud/


http://www.xuebalib.com/vip.html

http://www.xuebalib.com/db.php

http://www.xuebalib.com/zixun/2014-08-15/44.html


Integrated Navigation and Local Mapping System for a ...

Documents

Transcript of Integrated Navigation and Local Mapping System for a ...