integrated navigation and local mapping system for a
TRANSCRIPT
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
GYROSCOPY AND NAVIGATION Vol. 7 No. 3 2016
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
GYROSCOPY AND NAVIGATION Vol. 7 No. 3 2016
MAKSIMOV, CHERNOMORSKII
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.
GYROSCOPY AND NAVIGATION Vol. 7 No. 3 2016
INTEGRATED NAVIGATION AND LOCAL MAPPING SYSTEM 289
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
GYROSCOPY AND NAVIGATION Vol. 7 No. 3 2016
MAKSIMOV, CHERNOMORSKII
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
GYROSCOPY AND NAVIGATION Vol. 7 No. 3 2016
INTEGRATED NAVIGATION AND LOCAL MAPPING SYSTEM 291
(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
GYROSCOPY AND NAVIGATION Vol. 7 No. 3 2016
MAKSIMOV, CHERNOMORSKII
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
GYROSCOPY AND NAVIGATION Vol. 7 No. 3 2016
INTEGRATED NAVIGATION AND LOCAL MAPPING SYSTEM 293
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
SAWM trajectory closing
Time, s
100 150
–0.573
–1.146
–1.719
Err
ors
in
att
itu
de a
ngle
s, d
eg
294
GYROSCOPY AND NAVIGATION Vol. 7 No. 3 2016
MAKSIMOV, CHERNOMORSKII
—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
SAWM trajectory closing
40 60 80 100 120 140
Time, s
Ra
dia
l p
osi
tio
n e
rro
rs
of
lan
dm
ark
s, m
GYROSCOPY AND NAVIGATION Vol. 7 No. 3 2016
INTEGRATED NAVIGATION AND LOCAL MAPPING SYSTEM 295
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
图书馆。
图书馆致力于便利、促进学习与科研,提供最强文献下载服务。
图书馆导航:
图书馆首页 文献云下载 图书馆入口 外文数据库大全 疑难文献辅助工具