research article an adaptive ukf based slam method for ...mathematicalproblems in engineering ......
TRANSCRIPT
-
Hindawi Publishing CorporationMathematical Problems in EngineeringVolume 2013, Article ID 605981, 12 pageshttp://dx.doi.org/10.1155/2013/605981
Research ArticleAn Adaptive UKF Based SLAM Method forUnmanned Underwater Vehicle
Hongjian Wang, Guixia Fu, Juan Li, Zheping Yan, and Xinqian Bian
College of Automation, Harbin Engineering University, Harbin 150001, China
Correspondence should be addressed to Hongjian Wang; [email protected]
Received 24 June 2013; Revised 7 September 2013; Accepted 10 September 2013
Academic Editor: Yuri Vladimirovich Mikhlin
Copyright Β© 2013 Hongjian Wang et al.This is an open access article distributed under the Creative CommonsAttribution License,which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.
This work proposes an improved unscented Kalman filter (UKF)-based simultaneous localization and mapping (SLAM) algorithmbased on an adaptive unscented Kalman filter (AUKF) with a noise statistic estimator. The algorithm solves the issue thatconventional UKF-SLAM algorithms have declining accuracy, with divergence occurring when the prior noise statistic is unknownand time-varying. The new SLAM algorithm performs an online estimation of the statistical parameters of unknown systemnoise by introducing a modified Sage-Husa noise statistic estimator. The algorithm also judges whether the filter is divergent andrestrains potential filtering divergence using a covariancematchingmethod.This approach reduces state estimation error, effectivelyimproving navigation accuracy of the SLAM system. A line feature extraction is implemented through a Hough transform basedon the ranging sonar model. Test results based on unmanned underwater vehicle (UUV) sea trial data indicate that the proposedAUKF-SLAM algorithm is valid and feasible and provides better accuracy than the standard UKF-SLAM system.
1. Introduction
The simultaneous localization and mapping (SLAM) algo-rithm [1, 2] was first proposed by Smith, Self, and Cheesemanin 1988 to provide localization and map building for mobilerobots and is nowwidely used inmany different mobile robotsystems. The SLAM algorithm was first used for unmannedunderwater vehicle (UUV) navigation in September 1997 ina collaborative project between the Naval Undersea WarfareCenter (NUWC) and Groupe dβEtudes Sous-Marines delβAtlantique (GESMA). The objective of the trial using SLAMwas to get a UUV starting in an unknown location andwithout previous knowledge of the environment to build amap using its onboard sensors and then use the same map tocompute the robotβs location.
Given the recent wider use of UUV in the marine envi-ronment, it is notable that truly autonomous SLAM-basedUUV navigation is still lacking. However, it is challenging todevelop SLAM-based UUV navigation due to factors such assystem complexity, weak perception, nonstructure, increas-ing system noise, and unknown statistical characteristics.
SLAM solutions can be divided into two categories: anonprobabilistic probability estimationmethod andmethodsprimarily based on probability estimation; Table 1 gives the
summery of SLAM methods. The probability estimationmethod first developed [3] was the EKF based SLAMalgorithm, which suffers difficulty solving data associationproblems, high computational costs due to the calculationof Jacobian matrices, and inconsistency due to errors intro-duced during linearization. In trying to reduce storage andcomputational requirements, Thrun et al. [4] proposed aSLAMalgorithm based on sparse extended information filter.However, this method is only applicable for creating featuremaps and requires the existence of features that are easy toextract and distinguish in the environment, such as point,line, and face features. More recently, Montemerlo et al. [5]proposed a Rao-Blackwellized particle filter based SLAMmethod (FastSLAM), where each particle stores its map androbot positioning result. However, this algorithm results in acalculation and storage problem proportional to the numberof particles and is unable to avoid the disadvantages ofparticle degradation and sample dilution.
The unscented Kalman filter (UKF) [6, 7] is a nonlinearfilter based on unscented transform (UT). For nonlinear sys-tems,UKFs avoids linearization of the state andmeasurementequations. Additionally, the UKF principle is simple and easyto implement as it does not require the calculation of Jaco-bians at each time step, and the UKF is accurate up to second
-
2 Mathematical Problems in Engineering
Table 1: Summery of SLAMmethods.
Algorithm Drawbacks
EKF-SLAM
(1) Difficulty to solve data association problems(2) High computational costs(3) Inconsistency due to errors introduced duringlinearization
EIF-SLAM(1) Only applicable for creating feature maps(2) Requires the existence of features, such aspoint, line, and face features.
FastSLAM(1) Calculation and storage problem proportionalto the number of particles(2) Particle degradation and sample dilution
order moments in the probability distribution function prop-agation whereas the EKF is accurate up to first order moment[8]. However, when an UKF is used in underwater SLAM,it needs to predict mathematical model of the system anda priori knowledge of the noise statistics. In many practicalapplications, prior statistics of the noise is unknown or notaccurate. Even if this information is known, the statisticalcharacteristics easily change due to internal and externaluncertainties that reflect strong time-varying characteristics.Thus, a conventional UKF does not have the adaptive abilityto respond to changes in the noise statistics, which can lead tolarge estimation errors and even cause divergence in the caseof unknown and time-varying noise statistics [9β11].
In order to solve the above problem, we apply an adaptiveUKF (i.e., an adaptive unscented Kalman filter, or AUKF)filtering algorithm to underwater SLAM. By introducing amodified (i.e., suboptimal and unbiased) Sage-Husa maxi-mum a posterior (MAP) noise statistic estimator, the newalgorithm provides online estimation of the statistical param-eters of unknown system noises and restrains filtering diver-gence. In addition, the method uses a covariance matchingcriterion approach to determine the convergence of the filter.When the filter is divergent, the proposed method intro-duces an adaptive fading factor to correct prediction errorcovariance, adjust the filter gain matrix K, and suppress filterdivergence, thus enhancing the fast tracking capability of thefilter. Test results based on sea trial data of UUV indicate thatthe proposedAUKF-SLAMalgorithmprovides better naviga-tional accuracy than a conventional UKF-SLAM algorithm.
2. Adaptive UKF Algorithm
2.1. UKF Algorithm. Unscented Kalman filters were firstproposed by Julier and Uhlmann [12]. The algorithmβs mainprinciple is to select a number of sampling points in the statedistribution (sigma points), which can completely capture thetrue mean and covariance of state distribution. Those sigmapoints are then substituted into the nonlinear function toobtain the corresponding nonlinear function point set, andit can solve the mean and covariance after transformation bythese points.
The mean, estimate variance, and measurement varianceobtained from the unscented transform are introduced into agradually recursive process of Kalman filtering to obtain theUKF. The main steps of a UKF algorithm are as follows.
(1) Initialization
π₯0= πΈ [π₯
0] ,
P0= πΈ [(π₯
0β π₯0) (π₯0β π₯0)π] .
(1)
(2) For π β {1, . . . ,β}
(i) calculate sigma points
ππβ1
= [π₯πβ1
π₯πβ1
Β± β(ππ₯+ π)P
πβ1] . (2)
(ii) UKF prediction
ππ|πβ1
= f (ππβ1
, π’πβ1
) , (3)
π₯π|πβ1
=
2ππ₯
β
π=0
π(π)
πππ,π|πβ1
, (4)
Pπ|πβ1
=
2ππ₯
β
π=0
π(π)
π[ππ,π|πβ1
β π₯π|πβ1
] [ππ,π|πβ1
β π₯π|πβ1
]π+ π,
(5)
π§π|πβ1
= h (ππ|πβ1
, π’πβ1
) , (6)
π§π|πβ1
=
2ππ₯
β
π=0
π(π)
ππ§π,π|πβ1
. (7)
(iii) UKF update
PοΏ½ΜοΏ½ποΏ½ΜοΏ½π
=
2ππ₯
β
π=0
π(π)
π[π§π,π|πβ1
β π§π|πβ1
] [π§π,π|πβ1
β π§π|πβ1
]π+ π , (8)
Pπ₯ππ§π
=
2ππ₯
β
π=0
π(π)
π[ππ,π|πβ1
β π₯π|πβ1
] [π§π,π|πβ1
β π§π|πβ1
]π, (9)
Kπ= Pπ₯ππ§π
Pβ1οΏ½ΜοΏ½ποΏ½ΜοΏ½π
, (10)
π₯π= π₯π|πβ1
+ Kπ(π§πβ π§π|πβ1
) , (11)
Pπ= Pπ|πβ1
β KπPοΏ½ΜοΏ½ποΏ½ΜοΏ½π
Kππ, (12)
where π is the system noise covariance, π is the observationnoise covariance, K is the Kalman gain, andπ
πis the weight
of the mean and covariance.
2.2. Adaptive UKF Algorithm. Adaptive filtering technologyhas become a focus of research attempting to solve the filterdivergence problem caused by the inaccurate statistical prop-erties of the noise and the mathematical model itself. Sage-Husa suboptimal unbiased maximum a posterior (MAP)noise estimators have the advantage that their recursiveformula is simple, and the principle is clear and easy toimplement. Therefore we chose to use this type of system toestimate unknown system noise.
-
Mathematical Problems in Engineering 3
2.2.1. System Noise Estimation Method. Suppose a class ofnonlinear dynamic systems is described as
π₯π= f (π₯
πβ1, π’πβ1
) + ππβ1
,
π§π= h (π₯
π) + ππ,
(13)
where π₯πrepresents the state vector of the system at time
π, π’πβ1
represents the control, π§πrepresents the measurement
value of the state at time π, andππβ1
and ππare independent
white Gaussian noise with time-varying means of ππand ππ
and covariances of ππand π
π, respectively. Note that π
πis a
nonnegative definite symmetric matrix, while π πis positive
definite symmetric matrix.Emphasis should be placed on recent data when estimat-
ing time-varying noise statistics; that is, the algorithm shouldgradually forget data that is too old. In this paper, we adopt afading memory weighted exponent method to design a time-varying noise statistics estimator. According to the literature[13], we selected the weighting coefficient to satisfy
π½π= π½πβ1
π, 0 < π < 1,
π
β
π=1
π½π= 1 (14)
which can be written as
π½π= ππππβ1
, π = 1, . . . , π,
ππ=
(1 β π)
(1 β ππ)
.
(15)
The recursive formula of a fadingmemory time-varying noisestatistical estimator described by π
π, ππ, ππ, οΏ½ΜοΏ½πis
ππ= (1 β π
π) ππβ1
+ ππ[π₯πβ
2ππ₯
β
π=0
π(π)
πf (ππ,πβ1
)]
ππ= (1 β π
π) ππβ1
+ ππ[Kπππππ
πKππ+ Pπ
β
2ππ₯
β
π=0
π(π)
π(ππ,π|πβ1
β π₯π|πβ1
) (ππ,π|πβ1
β π₯π|πβ1
)π] ,
ππ= (1 β π
π) ππβ1
+ ππ[π§πβ
2ππ₯
β
π=0
π(π)
πh (ππ,π|πβ1
)] ,
οΏ½ΜοΏ½π= (1 β π
π) οΏ½ΜοΏ½πβ1
+ ππ[ππππ
πβ
2ππ₯
β
π=0
π(π)
πΓ (π§π,π|πβ1
β οΏ½ΜοΏ½π|πβ1
)
Γ (π§π,π|πβ1
β οΏ½ΜοΏ½π|πβ1
)π] ,
(16)
where ππ= π§πβ β(π₯
π|πβ1) is the output residual sequence of
the UKF.
2.2.2. Filter Divergence Suppression Method. Since subopti-mal Sage-Husa filters often diverge, in this paper we judgewhether filtering divergence is occurring according to con-vergence conditions derived from the covariance matchingcriterion. If the convergence conditions are satisfied, the Sage-Husa algorithm is applied. If filtering divergence occurs, theproposedmethod calculates an adaptiveweighting coefficientππthrough a computational fading factor formula and applies
this coefficient to correct Pπ|πβ1
; thus, the role of the observ-ables is strengthened and the filter divergence is suppressed.
The convergence conditions can be written as
VππVπβ€ π β tr [πΈ (V
πVππ)] , (17)
where π (π β₯ 1) is an adjustable coefficient presetting and Vπ
is the residual sequence, such that Vπ= π§πβ h(π₯
π|πβ1).
The correction method of covariance Pπ|πβ1
is
Pπ|πβ1
=ππβ
2ππ₯
β
π=0
π(π)
π[ππ,π|πβ1
β π₯π|πβ1
]
Γ [ππ,π|πβ1
β π₯π|πβ1
]π+ ππβ1
.
(18)
The adaptive weighting coefficient ππis calculated based
on the fading factor formula [14, 15]
ππ= {
π0
π0β₯ 1
1 π0< 1,
π0= tr
[ππ]
tr [ππ]
,
ππ= tr (πΆ
0,πβ π )π,
ππ= tr(
2ππ₯
β
π=0
π(π)
π[π§π,π|πβ1
β π§π|πβ1
] [π§π,π|πβ1
β π§π|πβ1
]π) ,
πΆ0,π
=
{{
{{
{
VπVππ
π = 1
ππΆ0,π
+ VπVππ
1 + π
π > 1,
(19)
where tr(β ) accounts for matrix trace. Here, 0 < π β€ 1is a forgetful factor (typically about 0.95) used to increasethe filterβs tracking ability, with greater values of the factorcreating a smaller proportion of information before timeπ and causing a more prominent residual vector effect.This method has a strong tracking ability for sudden statuschanges but still keeps tracking for slowly varying state andmutation status changes when the filter reaches a steady state.
2.2.3. Realization of Adaptive UKF Algorithm
(1) Initialization. State initialization is done according to (1)and (20)
π0= π0, π
0= π0. (20)
(2) Time Update.Given values for the variables π₯πβ1
and Pπβ1
,we solve π₯
π|πβ1and π§π|πβ1
based on the unscented transform,
-
4 Mathematical Problems in Engineering
that is, using (4) and (7), and covariance Pπ|πβ1
is predictedby (5).
(3) Convergence Judgment. At this stage, the method uses(17) to judge whether the filter is converging. If the filter isconverging, move to the next step; otherwise correct P
π|πβ1
using (18)βΌ(19).(4)Measurement Update.Obtain observation covariances
PοΏ½ΜοΏ½ποΏ½ΜοΏ½π
and Pπ₯ππ§π
and filter gain Kπaccording to (8)βΌ(10), and
then do a measurement update using (11)βΌ(12).(5) Recursively Estimate System Statistical Noise Char-
acteristics. Recursively estimate the systemβs statistical noisecharacteristics according to (16).
3. SLAM Algorithm Based on Adaptive UKF
3.1. SLAM System Model
3.1.1. AUV Nonlinear Dynamic Model. As seen from Figure 1,πΏ is the global coordinate system established with the initiallocation and bow of the UUV, where π₯, π¦, and π describe theposition and heading of theUUVwithin this system.Whileπis the UUV vessel frame system and π is the sonar coordinatesystem. The North-East coordinates are given by πΈ, with itsNorth direction based on magnetic North. Obviously, π§
π=
π + ππΏ0
, where π§πis the heading of the UUV as measured by
its OCTANS.Note that there is a distance offset (1.85m in theπ direc-
tion, 0.65m in the π direction, and a very small deviation inthe π direction that can be ignored) between the mountingpositions of three ranging sonar and the UUVβs center ofgravity. The coordinate systems π and π are shown in moredetail in Figure 2. We assume that the positions of the originof S and π in πΈ are (π
π₯, ππ¦) and (π
π₯, ππ¦), respectively, and
that three ranging sonar are mounted together (i.e., theirmounting positions are the same). This gives a mountingdeviation of three ranging sonar in π of π = 1.85m andπ = 0.65m, where π is the deviation in the π direction andπ is the deviation in the π direction. The mounting angle ofthe middle sonar is π, with mounting angles of πΒ±7.5 for theleft and right sonar. According to Figure 2, using the rangingsonar as a reference we get
ππ₯= ππ₯+ βπ2+ π2β cos (π β π§
π) ,
ππ¦= ππ¦+ βπ2+ π2β sin (π β π§
π) .
(21)
The method uses a simple 4 DOF (degree of freedom)constant velocity kinematics to predict how the state willevolve from time π β 1 to time π:
[
[
[
[
[
[
[
[
[
[
[
π₯
π¦
π§
π
π’
Vπ€
π
]
]
]
]
]
]
]
]
]
]
]π
=
[
[
[
[
[
[
[
[
[
[
[
[
π₯ + π’π cos (π) β Vπ sin (π)π¦ + π’π sin (π) + Vπ cos (π)
π§ + π€π
π + ππ
π’
Vπ€
π
]
]
]
]
]
]
]
]
]
]
]
]πβ1
+ nπ, (22)
Z
ZE
Down East
North
S
X
X
UUV
Line f
eature
L
π
Y
Y
L
Vy
x
π
π
zπ
πL0
πL0
π
Figure 1: Global coordinate system, the UUV vessel frame systemand the sonar coordinate system.
πbZ a
S
X
UUV
π
Y
π
π
zπ
Figure 2: Installation position and angle deviation of the rangingsonar.
where [π₯, π¦, π§, π] is the position and heading of the UUV inπΏ; [π’, V, π€, π] gives the line velocity and angle velocity of theUUV in π, and π is the sample time. In this equation, n(π) isthe portion of the system noise with a time-varyingmean andcovariance, with the covariance of vector π given by systemnoise π
πΈ [ππ] = ππ,
πΈ [(ππβ ππ) (ππβ ππ)
π
] = πΏππππ,
(23)
where πΏππis a delta function.
3.1.2. Feature Model. The feature data used in this paper isderived from measurements of the structured port environ-ment, so the algorithm itself is what chooses for line featureswithwhich to build a featuremap.The line featuremodel usedin the proposed method is
Xππ,π= Xππ,πβ1
π = 1, . . . , π. (24)
-
Mathematical Problems in Engineering 5
3.1.3. Observation Model. The UUV uses a Doppler velocitylog (DVL), compass, and pressure sensor to provide directmeasurements of the vehicleβs velocity, heading, and depth,respectively.Thus, the observation model is linear, and so thecommon model becomes
zπ= Hπ₯π|πβ1
+mπ, (25)
where z is the observation vector,m is a white Gaussian withzero mean, andH varies with changes in the measurement:
π» =
[
[
[
[
[
[
0 0 1 0 0 0 0 0
0 0 0 1 0 0 0 0
0 0 0 0 1 0 0 0
0 0 0 0 0 1 0 0
0 0 0 0 0 0 1 0
]
]
]
]
]
]
. (26)
3.1.4. Ranging Sonar Model. The transmission beam of aranging sonar creates a conical surface, which is a fan in atwo-dimensional plane. David Ribas et al. [16] proposed theunderwatermechanical scanning imaging sonarmodel basedon the terrestrial single beam ranging sonar model [17]. Inthis paper, we determine the location of line features in theenvironment using the measurement data from the rangingsonar.
In Figure 3, πΌ represents the horizontal beam width, π½represents the incidence angle, and ππ is the range at whichthe bin was measured from the sensor. Reference frame πdefines the position of the transducer head at the momentthe sensor obtains a particular bin, where [π₯π΅
π, π¦π΅
π, ππ΅
π] is the
transformation defining the position of S with respect to π΅to the chosen base reference. Both [π₯π΅
π, π¦π΅
π, ππ΅
π] and ππ are
obtained from information in the data buffer.To emulate the effect of the horizontal beam width, the
model uses a set of π values at a given resolution within anaperture of Β±πΌ/2 around the direction in which the transduc-er is oriented.This set of values is also referred to as ππ΅
π, where
ππ΅
πβ
πΌ
2
β€ ππ΅
πβ€ ππ΅
π+
πΌ
2
. (27)
Each value ππ΅πrepresents a bearing parameter for a line
tangent of the arc that models the horizontal beam width. Asstated earlier, not only are all lines tangent to the arc candi-dates for line features, but the ones inside the maximum inci-dence angle limits of Β±π½ are also considered candidates. Forthis reason, the algorithm takes each π value at a given resolu-tion for each value of ππ΅
πand within an aperture of Β±π½; that is,
ππ΅
πβ π½ β€ π
π΅
π,πβ€ ππ΅
π+ π½. (28)
The results of this operation are πΓπ different values of ππ΅π
for the given aperture.These are the bearings for a set of linesrepresenting all the possible candidates compatible with thebin. Given the geometry of the problem, the range parameterππ΅
π,πcorresponding to each one of the ππ΅
π,πbearings is
ππ΅
π,π= π₯π΅
πcos (ππ΅
π,π) + π¦π΅
πsin (ππ΅
π,π) + ππ cos (π
π,π) . (29)
π½
πΌ
πS πi,k
πBi,k
πBi
πBi,k
πBS
πBi,k
S
B
yBS
xBS
Line featureL(πBi,k, π
Bi,k)
Figure 3: Ranging sonar model to distinguish line features.
3.2. Feature Extraction. In the sea trial, the applicationenvironment of the SLAM algorithm is a cross-section ofthe ports, dams, and other environment structures. Note thatthe scanning surface of the sonar and the vertical wall orother vertical extensions of the surfacewill create line featuresin the resulting acoustic image. However, the parameters ofsuch static line features will not change as the sonar positionchanges.
Themost popular line feature extractionmethods includethe split-and-merge method [18] proposed by Pavlidis, theRANSAC method [19] proposed by Fischler and Bolles, andthe Hough transform (HT) [20] proposed by Illingworthand Kittler. Between these choices, HT is the most popularline feature extraction method, and a number of othershave developed many methods to improve HT line featureextraction [21β23]. In this paper, we use the HT method toextract line features from the ranged sonar data. The HT is avoting schemewhere the distance values of each ranged sonarimage accumulates evidence for the presence of a line feature.Units that get the most votes in the HT space correspond toline features in the actual environment.
3.2.1. Data Processing of Ranging Sonar Data. A data bufferhelps to separate and manage the stream of measurementsproduced by the continuous arrival of the sonar beams. Thebuffer stores variables such as the range and bearing for eachbin used in the voting, the position and heading in theNorth-East coordinate systemπΈ, and the transmit angle of the beamsso that π-π parameter pairs used to present line features areextracted based on HT.
The steps to process the data set from each ranged sonarscan [24, 25] are given below, using the left sonar as anexample. First, the data buffer is set, the data is loadedwith therange values, and a 0-1 matrix is built where the units withoutrange values are set to 0 and units with range values are setto 1. In the second step, the transmit angle of the sonar in π,the time, the position and heading of the UUV in πΈ, and theposition and transmit angle of the sonar in πΈ are stored intothe data buffer.The third step defines the base frame π΅, whereπ΅ is the current position of the sonar head when the votingis performed. Finally, the position and heading of the sonar
-
6 Mathematical Problems in Engineering
instrument in π΅ at every moment is acquired and stored inthe data buffer.
3.2.2. Hough Transform. There are three steps to extract linefeatures from the sonar image data. First, the data from allthree sonar instruments are loaded, and distance resolution,angle resolution, and threshold value are defined. Secondly,the accumulator is defined, and the index values of thenonzero elements of the accumulator are found. Finally, weuse (27)βΌ(29) to vote, and the π-π parameter pairs that getthe most votes are used to represent the detected line featuresin π΅.
3.3. Data Association. Once the model has extracted linefeatures in the environment based on the HT algorithm, itneeds to create an environment map and improve the stateestimation of the UUV by fusing the detected line features.The next step is then to perform data association [26] todetermine if a measured line ππ
πcorresponds to any of the
features πΉπ, π = 1, . . . , π already existing in the map and so
used to update the system or if it is a new line and has tobe incorporated into the map. To make this distinction, themost popular individual compatibility nearest neighbor dataassociation algorithm is used to select the best candidate.
Given the transformation of π΅ and π, the position of theπth line measurement in π can be represented by
ππ
π= [ππ
πππ
π]
π
. (30)
If we assume the position of the line feature π alreadyexists in the map in πΏ; that is,
ππΏ
π= [
ππ
ππ
] . (31)
Then the position of line feature π in V is
ππ
π= [
ππβ π₯ cos π
πβ π¦ sin π
π
ππβ π
] . (32)
And line feature π corresponds to the πth line measure-ment, where
π§π
π,π= βπ(π₯π, sπ) ,
βπ(π₯π, sπ) = [
ππβ π₯ cos π
πβ π¦ sin π
π
ππβ π
] + sπ.
(33)
Here, πππand ππ
πare the parameters of the line features in
the π frame of reference, ππand ππare the parameters of the
line features presented in the πΏ frame of reference, and sπis
the noise with a zero-mean white noise with covariance Raffecting the line feature observation.
The proposed method uses an innovation term ^ππto
calculate the discrepancy between the measurement and itsprediction, with its associate covariance matrix S
ππobtained
by
Vππ,π
= π§π
π,πβ π§π,π|πβ1
,
π§π,π|πβ1
= βπ(π₯π|πβ1
, sπ) ,
πππ,π
= PοΏ½ΜοΏ½ποΏ½ΜοΏ½π
+ Rπ.
(34)
To determine if the correspondence is valid, an individualcompatibility (IC) test [27] using theMahalanobis distance iscarried out
D2ππ= ]Tππ,π
πβ1
ππ,ππ]ππ,π
< π2
π,πΌ, (35)
where π = dim(βπ) and πΌ is the desired confidence level.
Data association is only performed if and when a linefeature is detected based on HT. If the data associationis successful, that is, the line feature exists in the map,then the model updates the state estimate. Otherwise, stateaugmentation is carried out where the new measurement isadded to the current state vector as a new feature. However,the algorithm cannot do this augmentation directly becausethe new feature is represented inπ. Thus, the algorithmmustfirst perform a change of reference using the following:
π (π) =
[
[
[
[
[
[
ππ(π)
ππ1
(π)
...πππ
(π)
]
]
]
]
]
]
β π(π)+=
[
[
[
[
[
[
[
[
ππ(π)
ππ1
(π)
...πππ
(π)
ππ(π) β π§
π
π(π)
]
]
]
]
]
]
]
]
. (36)
4. AUKF-SLAM Algorithm VerificationBased on Sea Trial Data
4.1. Test Conditions. The data used in the test conductedin this work comes from a sea trial completed in October2010 near Dalian Xiaoping Island using a UUV developedby the authors. The navigation route of the UUV was frompoint π΄ to point π΅ in Figure 4. As configured for the trails,the UUV possessed a number of different sensors includingDVL, OCTANS, depth sensor, and three ranging sonar whichmounted in the horizontal frame as a whole set to observe theenvironment.
The initial position of the UUV (point π΄) was longitude121.5231β, north latitude 38.8271β, and the trial ended atlongitude 121.5083β, North latitude 38.8328β (position B).Theinitial heading of the UUV was β70.70β. During the trail, theUUV stayed near the surface of the water so that GPS wasavailable throughout the trial. The total navigation time was17 minutes and 6 seconds.
DVL canmeasure aUUVβs current velocity, bottom track-ing speed, and so on. However, in the sea trial DVL wasonly used to measure the bottom track speed while OCTANSwas used to measure the UUVβs heading in real-time, thatis, the angle between the front of the UUV and magneticNorth.Thepressure sensor provided depth data bymeasuringthe water pressure, and three ranging sonar provided onlineenvironment perception and measurement. Three rangingsonar mounted in the horizontal frame is shown as Figure 5.
A general description of the AUKF-SLAM algorithm isgiven in Figure 6.
4.2. Acquisition of Embankment Measurement Points UsingRanging Sonar. Given a true trajectory as provided by GPS,we can obtain embankment measurement features by fusingthis GPS information with the ranging sonar data and then
-
Mathematical Problems in Engineering 7
Figure 4: Satellite image of the sea trial test area.
Figure 5: The ranging sonar mounted in the horizontal frame.
performing a coordinate transformation for the measure-ment data in a two-dimensional plane. Assume the positionof pointπ in π isππ
π= [π₯π
π¦π]
π.Then for each ranging sonar,we have
ππ
π= [
π₯π
π¦π
] =
[
[
[
[
ππΓ sin(
ππΓ π
180
)
ππΓ cos(
ππΓ π
180
)
]
]
]
]
, (37)
where ππand π
πrepresent distance values and mounting
angles of the three sonar instruments. For the sea trial, themounting angles were π
1= 7.5
β, π2= 0β, and π
3= β7.5
β,corresponding to the left (π = 1), middle (π = 2), and right(π = 3) sonar.
The relationship between π and π is shown in Figure 2,with the position of point π in π given by
ππ
π= [
π₯π
π¦π
] = [
π₯π+ π
π¦π+ π
] . (38)
Threerangingsonars
Measurementsand positions
UUV position
Newfeatures
Velocity
Line
WinnersDVL OCTANS
featuresObservation
Prediction
VotingNew hough space
Update
Heading
Reference votersto B
Databuffer
Systemstate
Assign votes
Data association(ICNN)
AUKF
Figure 6: Basic principle of the AUKF-SLAM algorithm.
β1600 β1400 β1200 β1000 β800 β600 β400 β200 2000β100
0
100
200
300
400
500
600
700
East (m)
Nor
th (m
)
GPSLeft ranging sonar
Middle ranging sonarRight ranging sonar
Figure 7: Feature measurement of the embankment.
The position of π relative to πΏ is ππΏπ= [π₯πΏ
π¦πΏ
ππΏ]
π, sowe can obtain the position of point π in πΏ using the followingsynthesis operator
ππΏ
π= ππΏ
πβ ππ
π= [
π₯πΏ+ π₯πcosππΏβ π¦πsinππΏ
π¦πΏ+ π₯πsinππΏ+ π¦πcosππΏ
] . (39)
Figure 7 gives the embankment measurement, where thegreen, red, and blue points were acquired by the left, middle,and right sonar, respectively.
4.3. Line Feature Extraction of the Embankment Based onRanging Sonar Model. The proposed model can extract linefeatures using an HT form ranging sonar model based on thefollowing assumptions:
-
8 Mathematical Problems in Engineering
(a) angle resolution of the HT space is 5β and the distanceresolution is 1m;
(b) the largest number of votes is selected as the thresh-old;
(c) a 10β arc is used to model the horizontal beam widthof the ranging sonar; that is, πΌ= 10β;
(d) the model does not consider uncertainty in thevertical beam width;
(e) assume that there is only an echo signalwhen transmitbeam of the ranging sonar is parallel to the verticalextension surface; that is, π½ = 0β.
In the sea trail the algorithm extracted six line featuresL1βΌL6. The π-π parameter, time, and position of the UUVwhen the feature was detected are given in Table 2 for eachline feature.
4.4. Test Validation Based on Sea Trial Data. To verify theperformance of the AUKF-SLAM algorithm, we comparedthe results of tests using AUKF-SLAM, UKF-SLAM, andEKF-SLAM algorithms against trial data with the UUV andassuming that the statistical properties of the system noisewas unknown in all cases.
4.4.1. Test Conditions. The actual statistical characteristics ofthe system noise during the field trial was unknown, so forthis work we assumed that actual system noise behavior wasbased on two laws, one time-varying and one constant. Usingthese laws, we conducted both a time-varying noise test anda constant noise test. Table 3 gives the results of the systemnoise tests and the resulting laws, where π is the step number.The observation noise is
π = diag ([0.042 0.042 0.042 0.042 0.042]) . (40)
4.4.2. Test Results. Figures 8βΌ15 are the test results given forboth time-varying and constant noise conditions. Figures 8and 12 compare the trajectory estimations from the differentalgorithms against the UUVβs GPS trajectory with time-varying noise and constant noise, respectively. Figures 9 and13 compare position errors in the East direction between thedifferent algorithms for the different types of noise, whileFigures 10 and 14 provide a comparison of the position errorsin the North direction. Figures 11 and 15 show the adaptiveweighting coefficient of AUKF-SLAM algorithm in time-varying and constant noise test, respectively.
4.4.3. Analysis of Test Results. The heading π§π
measuredby the OCTANS is relative to the magnetic north of πΈcoordination system shown in Figure 1, and it should betransformed to the heading π in the πΏ coordinate system forstate updating. The heading π is shown as Figure 16.
(1) Performance Analysis of the AUKF-SLAM Algorithm. Wecalculate the estimation error of the algorithm by
πΏπ= [
πΏπ,π
πΏπ,π
] = [
π₯πβ π₯π
π¦πβ π¦π
] , (41)
β1600 β1400 β1200 β1000 β800 β600 β400 β200 0β100
0
100
200
300
400
500
600
700
East (m)
Nor
th (m
)
GPSEKF-SLAM
UKF-SLAMAUKF-SLAM
Figure 8: Comparison of the trajectory estimations with time-varying noise.
0 100 200 300 400 500 600 700 800 900β50
β40
β30
β20
β10
0
10
Erro
r in
the E
ast d
irect
ion
(m)
EKF-SLAMUKF-SLAMAUKF-SLAM
k (step)
Figure 9: Comparison of position error in the East direction withtime-varying noise.
where πΏπ,π
and πΏπ,π
are the estimation error in the East andNorth directions at time k, respectively, while (π₯
π, π¦π) and
(π₯π, π¦π) are the true position and estimated position of the
UUV at time π. Note that |πΏπ,π| and |πΏ
π,π| represent absolute
values of the error in the East and North directions at time k,respectively. Obviously, small values of |πΏ
π| and |πΏ
π| indicate
higher accuracy in the filtering algorithm. Figures 8βΌ10 andFigures 11βΌ14 show that |πΏ
π| and |πΏ
π| for the proposed AUKF-
SLAM algorithm are lower than the estimation errors givenby the other algorithms.
From Figures 11 and 15, it can be seen that the adaptiveweighting coefficient is greater than one at some certain
-
Mathematical Problems in Engineering 9
Table 2: Extracted line feature parameters.
Line feature π π Time (step) Position of UUV in E(m)L1 13 β0.2618 260βΌ310 (β786.3968, β40.6424)L2 33 0 320βΌ370 (β949.0549, β47.3198)L3 39 0 430βΌ500 (β1279.0335, 47.42)L4 15 β0.0873 570βΌ610 (β1504.7369, 206.5273)L5 31 β0.0873 640βΌ720 (1449.9067, 483.5776)L6 35 β0.2618 730βΌ820 (β1278.9245, 649.0867)
Table 3: Change law of system noise.
Test Value of π
Time-varying noise test ππ=
diag ([9π β 6 1π β 6 8π β 5 3π β 5 9π β 6 1π β 6 8π β 6 3π β 6]) 1 < π < 320diag ([9π β 5 1π β 5 8π β 3 3π β 3 9π β 4 1π β 4 8π β 5 3π β 5]) 320 β€ π < 640diag ([9π β 5 1π β 5 8π β 4 3π β 4 9π β 4 1π β 3 8π β 4 3π β 4]) 640 β€ π < 820
Constant noise test ππ= diag ([5π β 6 5π β 6 5π β 4 9π β 3 2π β 4 2π β 4 4π β 4 4π β 4])
0 100 200 300 400 500 600 700 800 900
0
5
10
15
20
25
30
Erro
r in
the N
orth
dire
ctio
n (m
)
β5
EKF-SLAMUKF-SLAMAUKF-SLAM
k (step)
Figure 10: Comparison of position error in theNorth directionwithtime-varying noise.
time. The results indicate that once the Sage-Husa UKF-SLAM tends to diverge according to convergence conditions,the adaptive weighting coefficient is introduced to correctcovariance and ensure the tracking ability.(2) Root-Mean Square Error. We use the root-mean squareerror (RMSE) of the position to compare the performance ofthe various nonlinear filters
RMSEpos = β1
π
π
β
π=1
((π₯πβ π₯π)2+ (π¦πβ π¦π)2), (42)
where π is the total running step and (π₯π, π¦π) and (π₯
π, π¦π) are
the true position and estimated position, respectively, of the
0 100 200 300 400 500 600 700 800 9000
5
10
15Ad
aptiv
e wei
ghtin
g co
effici
ent
k (step)
Figure 11: Adaptive weighting coefficient of AUKF-SLAM algo-rithm in time-varying noise test.
Table 4: Comparison of RMSE.
Algorithm Time-varyingnoise test (m)Constant noise
test (m)AUKF-SLAM 26.1487 28.5021UKF-SLAM 28.0639 29.4876EKF-SLAM 38.5579 39.8530
UUV at time π. Table 4 gives the RMSE for each of the testedalgorithms. From the table, we can see that the RMSE of theAUKF-SLAM algorithm is the smallest for both time-varyingsystem noise and constant system noise. The RMSE in thecase of time-varying noise for the AUKF-SLAM algorithm issmaller than the RMSE found in the constant noise scenarioby 2.3534m. In the time-varying noise test, the RMSE of
-
10 Mathematical Problems in Engineering
β1600 β1400 β1200 β1000 β800 β600 β400 β200 0β100
0
100
200
300
400
500
600
700
East (m)
Nor
th (m
)
GPSEKF-SLAM
UKF-SLAMAUKF-SLAM
Figure 12: Comparison of the trajectory estimations with constantnoise.
0 100 200 300 400 500 600 700 800 900β50
β40
β30
β20
β10
0
10
Erro
r in
the E
ast d
irect
ion
(m)
EKF-SLAMUKF-SLAMAUKF-SLAM
k (step)
Figure 13: Comparison of position error in the East direction withconstant noise.
the AUKF-SLAM algorithm is smaller than the RMSE foundusing the UKF-SLAM algorithm by 1.9152m; in the constantnoise test the AUKF-SLAM algorithm RMSE is smaller by0.9855m.
From the above analysis, we can see that theAUKF-SLAMalgorithm has good tracking ability and produces a RMSEthat is smaller than what the other algorithms are capable ofachieving for both time-varying and constant system noise.Also, the AUKF-SLAM algorithm produces a smaller RMSEin the presence of time-varying system noise as compared towhen operating in a system with constant noise.
Figure 17 shows a comparison of the line features extract-ed during the environment and feature measurement of the
0 100 200 300 400 500 600 700 800 900β5
0
5
10
15
20
25
30
Erro
r in
the N
orth
dire
ctio
n (m
)
EKF-SLAMUKF-SLAMAUKF-SLAM
k (step)
Figure 14: Comparison of position error in theNorth directionwithconstant noise.
0 100 200 300 400 500 600 700 800 9000
2
4
6
8
10
12
14
16
18
Adap
tive w
eigh
ting
coeffi
cien
t
k (step)
Figure 15: Adaptive weighting coefficient of AUKF-SLAM algo-rithm in constant noise test.
embankment, while Figure 18 is a simultaneous localizationand mapping based on AUKF. During UUV navigation,the algorithm uses the three ranging sonar to perceive theenvironment, continuously extracting π-π parameters basedon theHough transform.Themodel accomplishes navigationbased on the AUKF-SLAM algorithm by repeatedly observ-ing available line features to continuously correct the UUVβsposition and update its existing line featuresmap.TheAUKF-SLAM algorithm avoids the error acceleration caused bydead reckoning and ensures that a UUV no longer has toperiodically float to the surface for GPS correction; one of themost important conditions for UUVβs tasked with long-termmissions where they must remain hidden.
-
Mathematical Problems in Engineering 11
0 100 200 300 400 500 600 700 800 900β150
β100
β50
0
50
100
k (step)
Hea
ding
of t
he U
UV
(deg
)
Figure 16: Heading of the UUV during the field trial.
β1600 β1400 β1200 β1000 β800 β600 β400 β200 2000β100
0
100
200
300
400
500
600
700
East (m)
Nor
th (m
)
GPSLine featureLeft ranging sonar
Middle ranging sonar
Right ranging sonar
Figure 17: Comparison of line features extracted from the environ-ment and the feature measurement of the embankment.
5. Conclusion
The proposed AUKF-SLAM algorithm adopts an improvedSage-Hausa suboptimal unbiased maximum posterior noisestatistical estimator to estimate unknown system noise. Thealgorithm estimates and corrects the statistical characterof noise in real time and decreases estimated error. Atthe same time, the algorithm judges whether the filter isconverging and introduces an adaptive forgetting factor tocorrect the predicted covariance adjust the Kalman gain, andrestrain the divergence of the filter when it diverges, thereforeincreasing the algorithmβs fast tracking ability. The AUKF-SLAM algorithm provides a new method for simultaneousunderwater localization and mapping in an unknown envi-ronment. Assistant navigation based on the proposed AUKF-SLAM algorithm can help UUVβs fulfil missions requiring
β1600 β1400 β1200 β1000 β800 β600 β400 β200 0β100
0
100
200
300
400
500
600
700
East (m)
Nor
th (m
)
GPSLine featureAUKFβSLAM
Figure 18: SLAM diagram based on the AUKF.
marine environment monitoring, marine terrain inspection,and long-term underwater tasks.
Acknowledgments
This research work is supported by the National NaturalScience Foundation of China (Grant no. E091002/50979017),the Ph.D. Programs Foundation of Ministry of Education ofChina andBasic Technology (Grant no. 20092304110008), theResearch Operation Item Foundation of Central University(Grant no. HEUCFZ 1026), the Harbin Science and Technol-ogy Innovation Talents of Special Fund Project (OutstandingSubject Leaders) (no. 2012RFXXG083), and the NewCenturyExcellent Talents Foundation of Education of China (no.NCET-10-0053).
References
[1] H. Durrant-Whyte and T. Bailey, βSimultaneous localizationand mapping: part I,β IEEE Robotics and Automation Magazine,vol. 13, no. 2, pp. 99β108, 2006.
[2] T. Bailey and H. Durrant-Whyte, βSimultaneous localizationand mapping (SLAM): part II,β IEEE Robotics and AutomationMagazine, vol. 13, no. 3, pp. 108β117, 2006.
[3] R. C. Smith and P. Cheeseman, βOn the representation and esti-mation of spatial uncertainty,β International Journal of RoboticsResearch, vol. 5, no. 4, pp. 56β68, 1986.
[4] S. Thrun, Y. Liu, D. Koller, A. Y. Ng, Z. Ghahramani, and H.Durrant-Whyte, βSimultaneous localization and mapping withsparse extended information filters,β International Journal ofRobotics Research, vol. 23, no. 7-8, pp. 693β716, 2004.
[5] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, βFast-SLAM: a factored solution to the simultaneous localization andmapping problem,β in Proceedings of the 18th National Con-ference on Artificial Intelligence (AAAI-02), 14th InnovativeApplications of Artificial Intelligence Conference (IAAI β02), pp.593β598, August 2002.
-
12 Mathematical Problems in Engineering
[6] S. Julier, J. Uhlmann, and H. F. Durrant-Whyte, βA newmethodfor the nonlinear transformation of means and covariances infilters and estimators,β IEEE Transactions on Automatic Control,vol. 45, no. 3, pp. 477β482, 2000.
[7] S. J. Julier and J. K. Uhlmann, βUnscented filtering and nonlin-ear estimation,β Proceedings of the IEEE, vol. 92, no. 3, pp. 401β422, 2004.
[8] R. Van der Merwe, Sigma-Point Kalman Filters for probabiliticinference in dynamic state-space models [Ph.D. thesis], OregonHealth and Science University, 2004.
[9] K. Xiong, H. Y. Zhang, and C. W. Chen, βPerformance evalua-tion of UKF-based nonlinear filtering,β Automatica, vol. 42, no.2, pp. 261β270, 2006.
[10] Y. Wu, D. Hu, and X. Hu, βComments on βPerformance evalua-tion of UKF-based nonlinear filteringβ,βAutomatica, vol. 43, no.3, pp. 567β568, 2007.
[11] K.Xiong,H.Y. Zhang, andC.W.Chan, βAuthorβs reply to βCom-ments on βPerformance evaluation of UKF-based nonlinearfilteringβ,β Automatica, vol. 43, no. 3, pp. 569β570, 2007.
[12] S. J. Julier and J. K. Uhlmann, βNew extension of the Kalmanfilter to nonlinear systems,β in Proceedings of the Signal Process-ing, Sensor Fusion, and Target Recognition VI, pp. 182β193, April1997.
[13] L. Zhao, X.-X. Wang, M. Sun, J.-C. Ding, and C. Yan, βAdaptiveUKF filtering algorithm based onmaximum a posterior estima-tion and exponential weighting,β Acta Automatica Sinica, vol.36, no. 7, pp. 1007β1019, 2010.
[14] D. H. Zhou, Y. G. Xi, and Z. J. Zhang, βSuboptimal fadingextended kalman filter for nonlinear systems,β Control andDecision, no. 5, pp. 1β6, 1990.
[15] D. H. Zhou, Y. G. Xi, and Z. J. Zhang, βA suboptimal multiplefading extended kalman filter,β Acta Automatica Sinica, vol. 17,no. 6, pp. 689β695, 1991.
[16] D. Ribas, P. Ridao, J. D. TardoΜs, and J. Neira, βUnderwaterSLAM in man-made structured environments,β Journal of FieldRobotics, vol. 25, no. 11-12, pp. 898β921, 2008.
[17] J. D. TardoΜs, J. Neira, P. M. Newman, and J. J. Leonard, βRobustmapping and localization in indoor environments using sonardata,β International Journal of Robotics Research, vol. 21, no. 4,pp. 311β330, 2002.
[18] T. Pavlidis, Algorithms for Graphics and Image Processing, vol.877 of Lecture Notes in Mathematics, Computer Science Press,Rockville, Md, USA, 1982.
[19] M. A. Fischler and R. C. Bolles, βRandom sample consensus: aparadigm for model fitting with applications to image analysisand automated cartography,β Communications of the ACM, vol.24, no. 6, pp. 381β395, 1981.
[20] J. Illingworth and J. Kittler, βA survey of the hough transform,βComputer Vision, Graphics and Image Processing, vol. 44, no. 1,pp. 87β116, 1988.
[21] J. Ji, G. Chen, and L. Sun, βA novel Hough transform methodfor line detection by enhancing accumulator array,β PatternRecognition Letters, vol. 32, no. 11, pp. 1503β1510, 2011.
[22] H. J. WANG, J. WANG, Q. U. L P, and L. I. U. Z Y, βSeaenvironment feature extraction based on fuzzy adaptive Houghtransform,β Chinese Journal of Scientific Instrument, vol. 34, no.1, pp. 32β37, 2013.
[23] J. Cha, R. H. Cofer, and S. P. Kozaitis, βExtended Hough trans-form for linear feature detection,β Pattern Recognition, vol. 39,no. 6, pp. 1034β1043, 2006.
[24] X. Zhang, H. J. Wang, J. J. Zhou, X. Q. Bian, and L. Xiong,βA method study on SFEKF-SLAM for a UUVβs structuralenvironment,β Journal of Harbin Engineering University, vol. 33,no. 8, pp. 1016β1021, 2011.
[25] L. Xiong, Research and design on the underwater SLAM in man-made structures environments based onmulti single-beam sonars[M.S. thesis], Harbin Engineering University, 2011.
[26] T. Bailey, Mobile robot localisation and mapping in extensiveoutdoor environments [Ph.D. thesis], Sydney University, Sydney,Australia, 2002.
[27] J. Neira and J. Tardos, βRobust and feasible data association forsimultaneous localization and map building,β in Proceedings ofthe IEEE conference on Robotic and Automation Workshop W4,San Francisco, Calif, USA, 2000.
-
Submit your manuscripts athttp://www.hindawi.com
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
MathematicsJournal of
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Mathematical Problems in Engineering
Hindawi Publishing Corporationhttp://www.hindawi.com
Differential EquationsInternational Journal of
Volume 2014
Applied MathematicsJournal of
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Probability and StatisticsHindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Journal of
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Mathematical PhysicsAdvances in
Complex AnalysisJournal of
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
OptimizationJournal of
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
CombinatoricsHindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
International Journal of
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Operations ResearchAdvances in
Journal of
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Function Spaces
Abstract and Applied AnalysisHindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
International Journal of Mathematics and Mathematical Sciences
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
The Scientific World JournalHindawi Publishing Corporation http://www.hindawi.com Volume 2014
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Algebra
Discrete Dynamics in Nature and Society
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Decision SciencesAdvances in
Discrete MathematicsJournal of
Hindawi Publishing Corporationhttp://www.hindawi.com
Volume 2014 Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014
Stochastic AnalysisInternational Journal of