# Terrain Adaptive Estimation of Instantaneous Centres of Rotation for Tracked Robots

Authors: Cuifeng Wang; Wenjun Lv; Xiaochuan Li; Mingliang Mei
Journal: Complexity
 (2018)

---

## Abstract

As a type of skid-steering mobile robot, the tracked robot suffers from inevitable slippage, which results in an imprecise kinematics model and a degradation of performance during navigation. Compared with the traditional robot, the kinematics model is able to reflect the influences of slippage through the introduction of instantaneous centres of rotation (ICRs). However, ICRs cannot be measured directly and are time-varying with terrain variation, and thus, here, we aim to develop an online estimation method to acquire the ICRs of a robot by means of data fusion technologies. First, an innovation-based extended Kalman filter (IEKF) is employed to fuse the readings from two incremental encoders and a GPS-compass integrated sensor, to provide a real-time ICR estimation. Second, a decision tree-based learning system is used to classify the terrains that the robot traverses, according to the vibration signals gathered by an accelerometer. The results of this terrain classification are improved via a Bayesian filter, by utilizing temporal correlation in the terrain time series. Third, the performances of the ICR estimation and terrain classification are mutually promoted. On one hand, terrain variation is detected with the aid of the terrain classification, and therefore, the process noise variance of IEKF can be automatically adjusted. Hence, the results of ICR estimation are smooth if the terrain does not change and converge rapidly upon terrain variation. On the other hand, the sudden changes in innovation are used to adjust the state transition probability during the recursive calculation of the Bayesian filter, thus increasing the accuracy of the terrain classification. A real-world experiment was undertaken on a tracked robot to validate the effectiveness of the proposed method. It is also demonstrated that the terrain adaptive odometry outperforms the traditional approach with the knowledge of ICRs.


---

## Body

## 1. Introduction

Tracked robots are able to govern their headings by adjusting the relative speed between the left and right driving wheels rather than resorting to independent steering mechanisms. Due to their robustness, the simplicity, and the ability of zero-radius rotation, tracked robots have become the preferred all-terrain robots in agriculture, industry, and military. However, due to the presence of the large contacting patches between the tracks and the ground, unpredictable slip is inevitable, which makes it difficult to build a precise kinematics model [1]. Because most strategies that concern navigation, motion control, obstacle avoidance, and route planning are designed based on the kinematics model, a robot’s performance can be significantly improved by the online identification of terrain-related slip parameters included in the kinematics model [2–8]. Therefore, this paper concentrates on designing a data fusion approach to acquire slip parameters in real time.Due to fact that the instantaneous centres of rotation (ICRs) of the chassis and tracks are almost constant when the robot is moving on the same terrain, the ICR kinematics model (as illustrated in Figure1) was proposed by introducing ICRs into the traditional kinematics model of a difference-driven mobile robot [9]. The ICR locations are able to reflect a robot’s lateral and longitude slippage, so they can be considered as a kind of slip parameters, and therefore, we are in the position to develop an ICR estimation method. In [9], two different methods were presented for the offline estimation of ICRs: the stationary response simulation of the dynamics model within the whole range of velocity, and the genetic algorithm based model parameters extracted from the gathered readings of sensors. In [10, 11], an empirical model combining ICRs and a robot’s kinematics state (i.e., the forward speed and radius of the path curvature) was established experimentally, which has been proven to improve the performance of dead reckoning significantly. Similar work in [12, 13] has been described. The primary issue of these off-line methods is that the robot-terrain mapping database should be established in advance, so that the ICR estimation cannot proceed when the robot enters the terrains which are not included in this database. Therefore, the current methods are converted to assist in the development of on-line ICR estimation methods.Figure 1
Schematic of the ICR locations of a tracked robot.Because the states of an ICR kinematics model are composed of the robot’s pose and ICRs, we can estimate the ICRs by means of pose measurements, which are implemented by fusing two incremental encoders and a GPS-compass integrated sensor. In [14], an extended Kalman filter (EKF) was employed to realize the ICR estimation, and its effectiveness was verified by a number of experiments in which the robot traverses terrains in different motion patterns. With a more precise kinematics model, the robot’s trajectory can be accurately calculated by incorporating the readings from on-board sensors into the model [15]. However, due to the uncertainties in the ICR kinematics model and the linearization-induced errors produced in EKF, the EKF-based ICR estimation method may lose accuracy or even become unreliable. The sudden terrain change and variation kinematics states may result in the degradation of system performance as well. In [16], a strong tracking filter (STF) and standard Kalman filter were applied to estimate the robot’s kinematics states and ICRs, respectively. By introducing suboptimal fading scaling factors, the filtering gain can be adjusted to guarantee the orthogonality of the innovation series. Hence, this method has a better state tracking ability and higher robustness against model uncertainties and state jump. The work was also accomplished by using a sliding mode observer and error-tolerant switched robust filter [17, 18].To the best of our knowledge, the aforementioned research represents state-of-the-art technologies concerning ICR estimation. The existing studies focus on the fusion approach, such as the Kalman filter and its extensions, which commonly suffer from the following two issues. The process noise covariance of ICRs should be set as a relatively large constant to enable the rapid convergence of ICR estimation, but the ICR estimation results will oscillate significantly around the true values. On the contrary, to guarantee smoothness, the process noise covariance should be set to a relatively small constant at the cost of slow convergence. Hence, the setup of the process noise covariance faces the inevitable trade-off between the convergence rate and the smoothness of the ICR estimation. Second, due to the model uncertainties rendered by linearization, parameter variation, and the measurement variance change, the filter-computed filtering gain may not maintain the optimality, so an adaptation mechanism should be introduced to automatically adjust the filtering gain.In this paper, we propose a solution to tackle these issues by using the terrain-adaptive innovation-based extended Kalman filter (TA-IEKF). First, an innovation-based Kalman filter (IKF) is employed to fuse the readings from two incremental encoders and a GPS-compass integrated sensor, to provide a real-time ICR estimation. Second, a decision tree-based learning system is utilized to classify the terrain over which the robot is traversing, according to vibration signals gathered by an accelerometer. The results of the terrain classification are improved via a Bayesian filter, by utilizing temporal correlation in the terrain time series. Third, the performances of the ICR estimation and terrain classification are mutually promoted. On one hand, any terrain variation is detected with the aid of terrain classification, and therefore, the process noise variance of the IEKF can be automatically adjusted. Hence, the results of the ICR estimation are smooth if the terrain does not change and converge rapidly upon terrain variation. On the other hand, the sudden changes in innovation are used to adjust the state transition probability during the recursive calculation of the Bayesian filter, thus increasing the accuracy of the terrain classification.The remainder of the paper is organized as follows. Section2 presents the ICR kinematics model. Section 3 describes the details of the proposed terrain adaptive innovation-based extended Kalman filter. Section 4 presents a real-world experiment and an analysis of its results. A conclusion is provided in Section 5.


## 2. ICR Kinematics Model

The section presents the ICR kinematics model for a tracked robot. As shown in Figure1, the body coordinate system (BCS) is assumed to have its origin on the geometric centre of robot body. The X-axis and Y-axis are aligned with the longitudinal forward direction and lateral rightward direction, respectively. The ICR location of the robot body on the Y-axis, yc, is
(1)yc=vxω,where vx denotes the robot velocity along the X-axis and ω denotes the turning rate. The ICR locations of the left and right tracks on the Y-axis, yℓ and yr, are
(2a)yℓ=vx−vℓxω,(2b)yr=vx−vrxω,where vℓx and vrx denote the left- and right-track velocities along the X-axis, respectively. The ICR location of the robot chassis on the X-axis, xc, is
(3)xc=−vyω,where vy denotes the robot velocity along the Y-axis. The ICR locations of the left and right tracks on the X-axis, xℓ and xr, are
(4)xℓ=−vy−vℓyω,xr=−vy−vryω,where vℓy and vry denote the left- and right-track velocities along the Y-axis, respectively.It is assumed that the rotating tracks do not produce the track motion along theY-axis, so vℓy=vry=0, and therefore, we have
(5)xc=xℓ=xr=−vyω.Hence, in the ICR locations of the chassis, the left and right tracks are on the same line parallel to theY-axis.As seen in (1), when the robot moves in a straight line, i.e., ω=0, yr ranges within −∞,+∞. However, the numerators and denominators of (2a), (2b), and (3) are infinitesimals of the same order, which result in bounded values of yℓ, yr, and xc [9].Solving the velocities with respect to BCS, i.e.,vx, vy, and ω, from (2a), (2b), and (3), we have
(6)vx=vryℓ−vℓyryℓ−yr,vy=vℓ−vrxcyℓ−yr,ω=−vℓ−vryℓ−yr,the denominators of which are nonzero values. Here, since vℓy=vry=0, the superscripts of vℓx and vrx are discarded, i.e., vℓ=vℓx and vr=vrx. The ICRs lie on or outside the tracks’ central lines when slippage does or does not occur [10]. Hence, yℓ−yr≥−W where W denotes the robot width.Now, we are in the position to transform the kinestates with respect to BCS to those with respect to the world coordinate system (WCS). As illustrated in Figure2, the E-axis and N-axis of WCS are eastward and northward, respectively. To establish the ICR kinematics model, the following assumptions are made:
(i)
All tracks are in contact with the ground, so the robot does not separate from the ground
(ii)
The moving plane is planar, so the height, pitch, and roll are ignored
(iii)
All the driving wheels are the same in size, and they do not suffer from deformationFigure 2
Coordinate systems on a tracked robot. The blue axes represent the body coordinate system, and the green ones represent the world coordinate system.The kinestates with respect to BCS and WCS can be related by(7)ė=vxsinθ+vycosθ,ṅ=vxcosθ−vysinθ,θ̇=ω,where e and n denote the eastward and northward location coordinate with respect to WCS and θ denotes the heading angle which is defined by the angle from the N-axis and the X-axis.


## 3. Terrain Adaptive IKF

The schematic of the proposed ICR estimation based on TA-IEKF is illustrated in Figure3. For the ICR estimation, we assign a relatively small value to the process noise variance of ICR, thus to guarantee the estimation smoothness. At the moment that the terrain changes, a relatively large value is assigned to the process noise variance of the ICR, thus accelerating the estimation of the convergence. After obtaining several sampling points, the previously assigned value is reassigned to the process noise variance of ICR. Therefore, the smoothness and rapid convergence of the ICR estimation can both be guaranteed.Figure 3
Schematic of the proposed terrain adaptive innovation-based extended Kalman filter. TC stands for terrain classification, BF stands for Bayesian filter, and KF stands for Kalman filter.In order to identify the event at which the terrain changes, we resort to vibration-based terrain classification. First, the vibration signals for different terrains are gathered by using an accelerometer. After the data segmentation, feature extraction, and model training, the classifier is obtained, and therefore, the terrain on which the robot is moving can be recognized in real time. Among various classification methods, the efficiency is the primary factor to be considered, because a tracked robot prefers low energy consumption and low computational complexity. Second, the lost accuracy rendered by the employment of simple features and a weak classifier is compensated by using a Bayesian filter, based on the temporal correlation in the terrain time series. To guarantee a higher filtering accuracy, the diagonal elements of the state transition probability matrix should be set at a value approximating to 1. However, massive errors occur at the moment that the terrain changes. Fortunately, a sudden increase in innovation is read as a sign indicating terrain variation, and consequently, the state transition probability matrix can be adjusted to reduce the error rate. It is noted that a sudden change in innovation cannot be used to activate the adjustment in the process noise variation of ICR, because it is a rough indicator of the terrain variation. A wrong indicator does not give rise to a large error in terrain classification, but a significant degradation in ICR estimation.(8)et+1nt+1θt+1yℓ,t+1yr,t+1xc,t+1=et+Tvr,tyℓ,t−vℓ,tyr,tyℓ,t−yr,tsinθt+xc,tvℓ,t−vr,tyℓ,t−yr,tcosθtnt+Tvr,tyℓ,t−vℓ,tyr,tyℓ,t−yr,tcosθt−xc,tvℓ,t−vr,tyℓ,t−yr,tsinθtθt+Tvr,t−vℓ,tyℓ,t−yr,tyℓ,tyr,txc,t+T⋅we,twn,twθ,twℓ,twr,twc,t,(9a)FK+t=10Tvr,ty^ℓ,t−vℓ,ty^r,ty^ℓ,t−y^r,tcosθ^t+x^c,tvr,t−vℓ,ty^ℓ,t−y^r,tsinθ^t01Tvℓ,ty^r,t−vr,ty^ℓ,ty^ℓ,t−y^r,tsinθ^t+x^c,tvr,t−vℓ,ty^ℓ,t−y^r,tcosθ^t001,(9b)FS,t=Tvℓ,t−vr,ty^r,tsinθ^ty^ℓ,t−y^r,t2−x^c,tcosθ^ty^ℓ,t−y^r,t2−y^ℓ,tsinθ^ty^ℓ,t−y^r,t2+x^c,tcosθ^ty^ℓ,t−y^r,t2cosθ^ty^ℓ,t−y^r,ty^r,tcosθ^ty^ℓ,t−y^r,t2−x^c,tsinθ^ty^ℓ,t−y^r,t2−y^ℓ,tcosθ^ty^ℓ,t−y^r,t2−x^c,tsinθ^ty^ℓ,t−y^r,t2−sinθ^ty^ℓ,t−y^r,t−1y^ℓ,t−y^r,t21y^ℓ,t−y^r,t20.


### 3.1. ICR Estimation Using Terrain Adaptive IKF

First, the process model is established as(10)ṡ=gs,vℓ,vr+w,where s=e,n,θ,yℓ,yr,xv′. The specific form of (10) is
(11)ė=vxsinθ+vycosθ+we,ṅ=vxcosθ−vysinθ+wn,θ̇=ω+wθ,ẏℓ=wℓ,ẏr=wr,ẋv=wv,where w=we,wn,wθ,wℓ,wr,wv′ represents an additive Gaussian white noise vector. Each noise in w is unrelated to another. Previous research has demonstrated that the ICR locations are almost constant when moving on the same terrain, regardless of the robot manoeuvres [14]. Hence, the ICR locations can be modelled as a Markov-Gaussian stochastic process, that is, the ICR location at the current sampling point equals the sum of those at the last sampling point and an additive Gaussian white.Furthermore, to facilitate the calculation in the digital devices, we derive the discrete-time form of (10) as
(12)st+1=fst,vℓ,t,vr,t+Twt,where st=et,nt,θt,yℓ,t,yr,t,xv,t′ denotes the state vector and wt=we,t,wn,t,wθ,t,wℓ,t,wr,t,wv,t′~ℕ0,Q denotes the process noise vector. The process noise variance Q=diagq1,q2,q3,q4,q5,q6=blockdiagQk,Qs where Qk=diagq1,q2,q3 and Qs=diagq4,q5,q6. The specific form of (12) is shown in (8), where T denotes the sampling period.Second, to estimatest, the observation model is established as
(13)zt=Hst+vt,where zt=ze,t,zn,t,zθ,t′ denotes the observation vector, vt=ve,t,vn,t,vθ,t′~ℕ0,R denotes the observation noise vector which is assumed to be Gaussian white noises, and H=I3×3,O3×3 denotes the observation matrix where O3×3 and I3×3 denote a 3×3 null matrix and identity matrix, respectively. The observation can be realized by using a GPS-compass integrated sensor, which provides real-time measurements of robot’s location and heading. Now st is estimated recursively by using the following estimation method.


#### 3.1.1. Process Noise Variance Adjustment

Checkλ consecutive recognized terrains provided by the Bayesian filter. If more than half of them are another terrain different from the current one, Qs is set as diagqmax,qmax,qmax where qmax is a relatively large value, say 100. Otherwise, Qs is set as diagqmin,qmin,qmin where qmin is a relatively small value, say 0.01.The setup ofλ is according to the classification accuracy. First, λ is an odd. Second, defining the minimum classification accuracy by p, the probability that more than λ/2 of the λ recognized terrains are correct is
(14)ρ=∑i=λ2λλi·pi·1−pλ−i.Letp=0.8, then setting λ=3 can guarantee ρ=0.9. If ρ=0.98 is desired, then λ must be larger than 9.


#### 3.1.2. Priori Estimation

Calculate the a priori estimations~t by
(15)s^t+1=fs˜t,vℓ,t,vr,t,and the corresponding error variance by
(16)P~t+1=FtP^tFt′+LtQLt′.In (16), Ft and Lt denote the Jacobian matrices of f·, that is,
(17)Ft=∂f∂stst=s^t=FK,tFS,tO3×3I3×3,Lt=∂f∂wtst=s^t=T·I6×6,where On×n and In×n denotes n×n null matrix and identity matrix, respectively. The specific form of FK,t and FS,t is shown in (9a) and (9b), respectively.


#### 3.1.3. Posterior Estimation

Calculate the innovationδt+1 by
(18)δt+1=zt+1−Hs~t+1,and its covariance is estimated by
(19)Vt+1=1N∑i=t−N+2t+1δiδi′=Vt+1Nδt+1δt+1′−δt−N+1δt−N+1′,where N denotes windowing size.Furthermore, calculate the filtering gainKt by
(20)Kt+1=P~t+1H′Vt+1−1.Finally, the a posteriori estimations^t+1 can be calculated by
(21)s^t+1=s~t+1+Kt+1δt+1,coupled with its error variance
(22)P^t+1=I6×6−Kt+1HP~t+1.


### 3.2. Vibration-Based Terrain Classification

The vibration is measured by an accelerometer mounted on the robot perpendicularly. The vertical acceleration time series describes the vibration caused by irregular terrain surfaces. First, experiments on different terrains should be conducted to constitute training samples. Second, we use a decision tree (DT) to learn the relationship between the vibration and terrain classes. Third, the classifier outputs are imported into an adaptive Bayesian filter, thus increasing the classification accuracy. The following sections will cover the feature extraction, DT classification, and Bayesian filter.


#### 3.2.1. Feature Extraction

In [19–21], frequency domain-based features and deep features are employed in terrain classification. Using these features may improve the classification accuracy to some extent. However, their extraction algorithms are time-consuming and resource-intensive, which affects the real-time terrain classification implemented at low-performance digital computer. For example, in order to extract frequency domain-based features, the Fourier transform, which is relatively complex in computation, is an essential step. Hence, we extract features in time domain directly.Between two successive sampling points, the robot vibration is measured at an accelerometer everyT/n time, so n samples are obtained. Define at=at1,at2,⋯,atn as the raw acceleration vector gathered from sampling point t−1 to t. Due to the presence of gravity, the accelerometer does not detect a pure vibration, but the vertical acceleration mixed with gravitational acceleration. Owing to gravity G being relatively stable, which can be read as a DC component, at is preprocessed by
(23)a^ti=ati−G,where a^t=a^t1,a^t2,⋯,a^tn is the pure vibration vector. The feature vector ϕt=ϕt1,ϕt2,ϕt3 is extracted from a^t, and its entries are shown as follows:
(i)
ϕt1: the number of sign changes
(24)ϕt1=∑i=2nζi,where
(25)ζi=1,ifsigna^ti·a^ti−1<0,0,ifsigna^ti·a^ti−1>0.The casea^ti=0 is ignored since it is singular. This feature is an approximation of the frequency of a^t.
(ii)
ϕt2: the mean of a^t
(26)ϕt2=1n∑i=1na^ti.Although the DC component of the raw data has been eliminated by (23), the mean of a^t may considerably diverge from zero for some course terrain.
(iii)
ϕt3: the variance of a^t
(27)ϕt3=1n∑i=1na^ti−ϕt22.Intuitively, the variance is higher when the terrain becomes coarser.


#### 3.2.2. DT Classification

The decision tree (DT) is a frequently used machine learning method, the purpose of which is to establish a tree with high generalization ability. It is characterized by high classification efficiency and interpretability. A DT can be generated by using a ID3 algorithm. The core of a ID3 algorithm is to select features on each node of a DT by applying information gain, and then structuring the DT recursively. First, starting from the root node, calculate the information gain of all possible features on each node. Assign the feature with the highest information gain to the node, and then structure the child node based on different values of this feature. Second, apply the method in step 1 to the child nodes recursively, until the information gains of all the features are small or there is no feature that could be selected, thus structuring a DT. The ID3 algorithm is similar to the probability model selection using the method of maximum likelihood.The generated DT usually has high classification accuracy on the training data, but low accuracy on the test data, namely, overfitting occurs. This is due to an overcomplicated DT being trained during the learning process to fit the training data. Hence, it is necessary to simplify the DT to reduce its complexity, which is usually called pruning. Specifically, pruning means cutting some subtrees or leaf nodes from the generated DT, and treating their root or father nodes as new leaf nodes. The pruning is realized via minimizing the cost function. First, calculate the empirical entropy of each node. Second, retract upwards from each leaf node. Third, if the cost function value of the retracted DT is smaller than that of the unretracted DT, then conduct the pruning. Fourth, return to step 2 until it cannot be continued, thus obtaining a simplified DT.


#### 3.2.3. Bayesian Filter

The recursive form of Bayesian filter can be seen in [22]. Define χt∈1,2,⋯,k as the label of real terrain at sampling point t, ct∈1,2,⋯,k as the classifier output, and Ct=c1,c2⋯,ct as the measurement set. The purpose is to acquire Pχt∣Ct, the a posteriori possibility distribution function (pdf) of χt in the condition of Ct. Given Pχt−1∣Ct−1, the a priori pdf of χt is
(28)Pχt=i∣Ct−1=∑j=1kPχt=i∣χt−1=jPχt−1=j∣Ct−1,and the a posteriori pdf is calculated by
(29)Pχt=i∣Ct=Pct=j∣χt=iPχt=i∣Ct−1∑i=1kPct=j∣χt=iPχt=i∣Ct−1,where Pχt=i∣χt−1=j denotes the probability that the robot moves from terrain j to i at time t and Pct=j∣χt=i denotes the probability that the classifier outputs terrain j while the robot locates at terrain i. Pχt∣χt−1, which is needed during the time-update procedure, describes the intrinsic rules of terrain variation. Given k terrains, a k×k square matrix M with elements mij=Pχt=i∣χt−1=j should be defined. The diagonal elements mii where i=1,2,…,k should be assigned a relatively large value not greater than 1. By assigning mii=μ, a large value can enable most misclassified results to be corrected, but a succession of mistakes may appear after terrain variation. This is because mii should be reassigned a small value and mij should be relatively large at the moment that terrain changes from i to j. Hence, the terrain transition probability matrix should be dynamically adjusted when the terrain changes. In this study, if terrain does not change, then Vt tends to be steady state; if the terrain changes, then a sudden change occurs in Vt. After detecting this event, we decrease mii and increase the related nondiagonal elements, and therefore, the accuracy of the Bayesian filter is increased.


## 3.1. ICR Estimation Using Terrain Adaptive IKF

First, the process model is established as(10)ṡ=gs,vℓ,vr+w,where s=e,n,θ,yℓ,yr,xv′. The specific form of (10) is
(11)ė=vxsinθ+vycosθ+we,ṅ=vxcosθ−vysinθ+wn,θ̇=ω+wθ,ẏℓ=wℓ,ẏr=wr,ẋv=wv,where w=we,wn,wθ,wℓ,wr,wv′ represents an additive Gaussian white noise vector. Each noise in w is unrelated to another. Previous research has demonstrated that the ICR locations are almost constant when moving on the same terrain, regardless of the robot manoeuvres [14]. Hence, the ICR locations can be modelled as a Markov-Gaussian stochastic process, that is, the ICR location at the current sampling point equals the sum of those at the last sampling point and an additive Gaussian white.Furthermore, to facilitate the calculation in the digital devices, we derive the discrete-time form of (10) as
(12)st+1=fst,vℓ,t,vr,t+Twt,where st=et,nt,θt,yℓ,t,yr,t,xv,t′ denotes the state vector and wt=we,t,wn,t,wθ,t,wℓ,t,wr,t,wv,t′~ℕ0,Q denotes the process noise vector. The process noise variance Q=diagq1,q2,q3,q4,q5,q6=blockdiagQk,Qs where Qk=diagq1,q2,q3 and Qs=diagq4,q5,q6. The specific form of (12) is shown in (8), where T denotes the sampling period.Second, to estimatest, the observation model is established as
(13)zt=Hst+vt,where zt=ze,t,zn,t,zθ,t′ denotes the observation vector, vt=ve,t,vn,t,vθ,t′~ℕ0,R denotes the observation noise vector which is assumed to be Gaussian white noises, and H=I3×3,O3×3 denotes the observation matrix where O3×3 and I3×3 denote a 3×3 null matrix and identity matrix, respectively. The observation can be realized by using a GPS-compass integrated sensor, which provides real-time measurements of robot’s location and heading. Now st is estimated recursively by using the following estimation method.


### 3.1.1. Process Noise Variance Adjustment

Checkλ consecutive recognized terrains provided by the Bayesian filter. If more than half of them are another terrain different from the current one, Qs is set as diagqmax,qmax,qmax where qmax is a relatively large value, say 100. Otherwise, Qs is set as diagqmin,qmin,qmin where qmin is a relatively small value, say 0.01.The setup ofλ is according to the classification accuracy. First, λ is an odd. Second, defining the minimum classification accuracy by p, the probability that more than λ/2 of the λ recognized terrains are correct is
(14)ρ=∑i=λ2λλi·pi·1−pλ−i.Letp=0.8, then setting λ=3 can guarantee ρ=0.9. If ρ=0.98 is desired, then λ must be larger than 9.


### 3.1.2. Priori Estimation

Calculate the a priori estimations~t by
(15)s^t+1=fs˜t,vℓ,t,vr,t,and the corresponding error variance by
(16)P~t+1=FtP^tFt′+LtQLt′.In (16), Ft and Lt denote the Jacobian matrices of f·, that is,
(17)Ft=∂f∂stst=s^t=FK,tFS,tO3×3I3×3,Lt=∂f∂wtst=s^t=T·I6×6,where On×n and In×n denotes n×n null matrix and identity matrix, respectively. The specific form of FK,t and FS,t is shown in (9a) and (9b), respectively.


### 3.1.3. Posterior Estimation

Calculate the innovationδt+1 by
(18)δt+1=zt+1−Hs~t+1,and its covariance is estimated by
(19)Vt+1=1N∑i=t−N+2t+1δiδi′=Vt+1Nδt+1δt+1′−δt−N+1δt−N+1′,where N denotes windowing size.Furthermore, calculate the filtering gainKt by
(20)Kt+1=P~t+1H′Vt+1−1.Finally, the a posteriori estimations^t+1 can be calculated by
(21)s^t+1=s~t+1+Kt+1δt+1,coupled with its error variance
(22)P^t+1=I6×6−Kt+1HP~t+1.


## 3.1.1. Process Noise Variance Adjustment

Checkλ consecutive recognized terrains provided by the Bayesian filter. If more than half of them are another terrain different from the current one, Qs is set as diagqmax,qmax,qmax where qmax is a relatively large value, say 100. Otherwise, Qs is set as diagqmin,qmin,qmin where qmin is a relatively small value, say 0.01.The setup ofλ is according to the classification accuracy. First, λ is an odd. Second, defining the minimum classification accuracy by p, the probability that more than λ/2 of the λ recognized terrains are correct is
(14)ρ=∑i=λ2λλi·pi·1−pλ−i.Letp=0.8, then setting λ=3 can guarantee ρ=0.9. If ρ=0.98 is desired, then λ must be larger than 9.


## 3.1.2. Priori Estimation

Calculate the a priori estimations~t by
(15)s^t+1=fs˜t,vℓ,t,vr,t,and the corresponding error variance by
(16)P~t+1=FtP^tFt′+LtQLt′.In (16), Ft and Lt denote the Jacobian matrices of f·, that is,
(17)Ft=∂f∂stst=s^t=FK,tFS,tO3×3I3×3,Lt=∂f∂wtst=s^t=T·I6×6,where On×n and In×n denotes n×n null matrix and identity matrix, respectively. The specific form of FK,t and FS,t is shown in (9a) and (9b), respectively.


## 3.1.3. Posterior Estimation

Calculate the innovationδt+1 by
(18)δt+1=zt+1−Hs~t+1,and its covariance is estimated by
(19)Vt+1=1N∑i=t−N+2t+1δiδi′=Vt+1Nδt+1δt+1′−δt−N+1δt−N+1′,where N denotes windowing size.Furthermore, calculate the filtering gainKt by
(20)Kt+1=P~t+1H′Vt+1−1.Finally, the a posteriori estimations^t+1 can be calculated by
(21)s^t+1=s~t+1+Kt+1δt+1,coupled with its error variance
(22)P^t+1=I6×6−Kt+1HP~t+1.


## 3.2. Vibration-Based Terrain Classification

The vibration is measured by an accelerometer mounted on the robot perpendicularly. The vertical acceleration time series describes the vibration caused by irregular terrain surfaces. First, experiments on different terrains should be conducted to constitute training samples. Second, we use a decision tree (DT) to learn the relationship between the vibration and terrain classes. Third, the classifier outputs are imported into an adaptive Bayesian filter, thus increasing the classification accuracy. The following sections will cover the feature extraction, DT classification, and Bayesian filter.


### 3.2.1. Feature Extraction

In [19–21], frequency domain-based features and deep features are employed in terrain classification. Using these features may improve the classification accuracy to some extent. However, their extraction algorithms are time-consuming and resource-intensive, which affects the real-time terrain classification implemented at low-performance digital computer. For example, in order to extract frequency domain-based features, the Fourier transform, which is relatively complex in computation, is an essential step. Hence, we extract features in time domain directly.Between two successive sampling points, the robot vibration is measured at an accelerometer everyT/n time, so n samples are obtained. Define at=at1,at2,⋯,atn as the raw acceleration vector gathered from sampling point t−1 to t. Due to the presence of gravity, the accelerometer does not detect a pure vibration, but the vertical acceleration mixed with gravitational acceleration. Owing to gravity G being relatively stable, which can be read as a DC component, at is preprocessed by
(23)a^ti=ati−G,where a^t=a^t1,a^t2,⋯,a^tn is the pure vibration vector. The feature vector ϕt=ϕt1,ϕt2,ϕt3 is extracted from a^t, and its entries are shown as follows:
(i)
ϕt1: the number of sign changes
(24)ϕt1=∑i=2nζi,where
(25)ζi=1,ifsigna^ti·a^ti−1<0,0,ifsigna^ti·a^ti−1>0.The casea^ti=0 is ignored since it is singular. This feature is an approximation of the frequency of a^t.
(ii)
ϕt2: the mean of a^t
(26)ϕt2=1n∑i=1na^ti.Although the DC component of the raw data has been eliminated by (23), the mean of a^t may considerably diverge from zero for some course terrain.
(iii)
ϕt3: the variance of a^t
(27)ϕt3=1n∑i=1na^ti−ϕt22.Intuitively, the variance is higher when the terrain becomes coarser.


### 3.2.2. DT Classification

The decision tree (DT) is a frequently used machine learning method, the purpose of which is to establish a tree with high generalization ability. It is characterized by high classification efficiency and interpretability. A DT can be generated by using a ID3 algorithm. The core of a ID3 algorithm is to select features on each node of a DT by applying information gain, and then structuring the DT recursively. First, starting from the root node, calculate the information gain of all possible features on each node. Assign the feature with the highest information gain to the node, and then structure the child node based on different values of this feature. Second, apply the method in step 1 to the child nodes recursively, until the information gains of all the features are small or there is no feature that could be selected, thus structuring a DT. The ID3 algorithm is similar to the probability model selection using the method of maximum likelihood.The generated DT usually has high classification accuracy on the training data, but low accuracy on the test data, namely, overfitting occurs. This is due to an overcomplicated DT being trained during the learning process to fit the training data. Hence, it is necessary to simplify the DT to reduce its complexity, which is usually called pruning. Specifically, pruning means cutting some subtrees or leaf nodes from the generated DT, and treating their root or father nodes as new leaf nodes. The pruning is realized via minimizing the cost function. First, calculate the empirical entropy of each node. Second, retract upwards from each leaf node. Third, if the cost function value of the retracted DT is smaller than that of the unretracted DT, then conduct the pruning. Fourth, return to step 2 until it cannot be continued, thus obtaining a simplified DT.


### 3.2.3. Bayesian Filter

The recursive form of Bayesian filter can be seen in [22]. Define χt∈1,2,⋯,k as the label of real terrain at sampling point t, ct∈1,2,⋯,k as the classifier output, and Ct=c1,c2⋯,ct as the measurement set. The purpose is to acquire Pχt∣Ct, the a posteriori possibility distribution function (pdf) of χt in the condition of Ct. Given Pχt−1∣Ct−1, the a priori pdf of χt is
(28)Pχt=i∣Ct−1=∑j=1kPχt=i∣χt−1=jPχt−1=j∣Ct−1,and the a posteriori pdf is calculated by
(29)Pχt=i∣Ct=Pct=j∣χt=iPχt=i∣Ct−1∑i=1kPct=j∣χt=iPχt=i∣Ct−1,where Pχt=i∣χt−1=j denotes the probability that the robot moves from terrain j to i at time t and Pct=j∣χt=i denotes the probability that the classifier outputs terrain j while the robot locates at terrain i. Pχt∣χt−1, which is needed during the time-update procedure, describes the intrinsic rules of terrain variation. Given k terrains, a k×k square matrix M with elements mij=Pχt=i∣χt−1=j should be defined. The diagonal elements mii where i=1,2,…,k should be assigned a relatively large value not greater than 1. By assigning mii=μ, a large value can enable most misclassified results to be corrected, but a succession of mistakes may appear after terrain variation. This is because mii should be reassigned a small value and mij should be relatively large at the moment that terrain changes from i to j. Hence, the terrain transition probability matrix should be dynamically adjusted when the terrain changes. In this study, if terrain does not change, then Vt tends to be steady state; if the terrain changes, then a sudden change occurs in Vt. After detecting this event, we decrease mii and increase the related nondiagonal elements, and therefore, the accuracy of the Bayesian filter is increased.


## 3.2.1. Feature Extraction

In [19–21], frequency domain-based features and deep features are employed in terrain classification. Using these features may improve the classification accuracy to some extent. However, their extraction algorithms are time-consuming and resource-intensive, which affects the real-time terrain classification implemented at low-performance digital computer. For example, in order to extract frequency domain-based features, the Fourier transform, which is relatively complex in computation, is an essential step. Hence, we extract features in time domain directly.Between two successive sampling points, the robot vibration is measured at an accelerometer everyT/n time, so n samples are obtained. Define at=at1,at2,⋯,atn as the raw acceleration vector gathered from sampling point t−1 to t. Due to the presence of gravity, the accelerometer does not detect a pure vibration, but the vertical acceleration mixed with gravitational acceleration. Owing to gravity G being relatively stable, which can be read as a DC component, at is preprocessed by
(23)a^ti=ati−G,where a^t=a^t1,a^t2,⋯,a^tn is the pure vibration vector. The feature vector ϕt=ϕt1,ϕt2,ϕt3 is extracted from a^t, and its entries are shown as follows:
(i)
ϕt1: the number of sign changes
(24)ϕt1=∑i=2nζi,where
(25)ζi=1,ifsigna^ti·a^ti−1<0,0,ifsigna^ti·a^ti−1>0.The casea^ti=0 is ignored since it is singular. This feature is an approximation of the frequency of a^t.
(ii)
ϕt2: the mean of a^t
(26)ϕt2=1n∑i=1na^ti.Although the DC component of the raw data has been eliminated by (23), the mean of a^t may considerably diverge from zero for some course terrain.
(iii)
ϕt3: the variance of a^t
(27)ϕt3=1n∑i=1na^ti−ϕt22.Intuitively, the variance is higher when the terrain becomes coarser.


## 3.2.2. DT Classification

The decision tree (DT) is a frequently used machine learning method, the purpose of which is to establish a tree with high generalization ability. It is characterized by high classification efficiency and interpretability. A DT can be generated by using a ID3 algorithm. The core of a ID3 algorithm is to select features on each node of a DT by applying information gain, and then structuring the DT recursively. First, starting from the root node, calculate the information gain of all possible features on each node. Assign the feature with the highest information gain to the node, and then structure the child node based on different values of this feature. Second, apply the method in step 1 to the child nodes recursively, until the information gains of all the features are small or there is no feature that could be selected, thus structuring a DT. The ID3 algorithm is similar to the probability model selection using the method of maximum likelihood.The generated DT usually has high classification accuracy on the training data, but low accuracy on the test data, namely, overfitting occurs. This is due to an overcomplicated DT being trained during the learning process to fit the training data. Hence, it is necessary to simplify the DT to reduce its complexity, which is usually called pruning. Specifically, pruning means cutting some subtrees or leaf nodes from the generated DT, and treating their root or father nodes as new leaf nodes. The pruning is realized via minimizing the cost function. First, calculate the empirical entropy of each node. Second, retract upwards from each leaf node. Third, if the cost function value of the retracted DT is smaller than that of the unretracted DT, then conduct the pruning. Fourth, return to step 2 until it cannot be continued, thus obtaining a simplified DT.


## 3.2.3. Bayesian Filter

The recursive form of Bayesian filter can be seen in [22]. Define χt∈1,2,⋯,k as the label of real terrain at sampling point t, ct∈1,2,⋯,k as the classifier output, and Ct=c1,c2⋯,ct as the measurement set. The purpose is to acquire Pχt∣Ct, the a posteriori possibility distribution function (pdf) of χt in the condition of Ct. Given Pχt−1∣Ct−1, the a priori pdf of χt is
(28)Pχt=i∣Ct−1=∑j=1kPχt=i∣χt−1=jPχt−1=j∣Ct−1,and the a posteriori pdf is calculated by
(29)Pχt=i∣Ct=Pct=j∣χt=iPχt=i∣Ct−1∑i=1kPct=j∣χt=iPχt=i∣Ct−1,where Pχt=i∣χt−1=j denotes the probability that the robot moves from terrain j to i at time t and Pct=j∣χt=i denotes the probability that the classifier outputs terrain j while the robot locates at terrain i. Pχt∣χt−1, which is needed during the time-update procedure, describes the intrinsic rules of terrain variation. Given k terrains, a k×k square matrix M with elements mij=Pχt=i∣χt−1=j should be defined. The diagonal elements mii where i=1,2,…,k should be assigned a relatively large value not greater than 1. By assigning mii=μ, a large value can enable most misclassified results to be corrected, but a succession of mistakes may appear after terrain variation. This is because mii should be reassigned a small value and mij should be relatively large at the moment that terrain changes from i to j. Hence, the terrain transition probability matrix should be dynamically adjusted when the terrain changes. In this study, if terrain does not change, then Vt tends to be steady state; if the terrain changes, then a sudden change occurs in Vt. After detecting this event, we decrease mii and increase the related nondiagonal elements, and therefore, the accuracy of the Bayesian filter is increased.


## 4. Experiment and Analysis

In this section, a real-world experiment based on a tracked robot, equipped with two incremental encoders, a GPS compass integrated sensor and an accelerometer, is presented to verify the effectiveness of the proposed terrain adaptive IEKF. We mainly focus on the convergence rate and smoothness of the ICR estimation. Additionally, it is demonstrated that the accuracy of dead reckoning could be improved with knowledge of the ICRs.The experimental robot was260 millimeters in length, 210 millimeters in width, and 200 millimeters in height. Each driving wheel was equipped with an incremental encoder of 540 resolutions. The GPS-compass integrated sensor was able to provide the robot’s pose (including the position and heading) at 1Hz with accuracies of 2 meters and 1 degree. As shown in Figure 4, during the experiment, the robot traversed soil, grass, asphalt, and paving successively. The accelerometer worked at the rate of 200Hz, and the other sensors were read by the data collector at 1Hz. After the data gathering, these data were sent to a computer (3.2 GHz with 8 GB RAM) and processed using MATLAB. It was noted that if the robot was equipped with a suspension system, or soft wheels, the vibration would be dampened or even absorbed. Hence, the accelerometer needed to be directly mounted on the robot chassis.Figure 4
Photos of the traversed terrains.


### 4.1. Terrain Classification

In order to collect the data for the classifier training, the tracked robot traversed the aforementioned four terrains at a speed of between0.4m/s and 1.3m/s. The test on each terrain lasted for about 1000 seconds, that is, 1000 samples were obtained for each terrain. We randomly divided these data into 10 parts on average and used each part to test the trained classifier once while used the remainder for training. The confusion matrix is shown in Table 1. Observing the diagonal elements, it can be seen that the classifier performs best at recognizing asphalt. This may be caused by the fact that most asphalt roads are flatter compared with soil, grass, and paving. When soil is exposed to the air, it turns dry and hard. After covering grass and then soil, the terrain becomes flatter and softer. However, the two terrains still share similar roughness, and therefore, the classifier performs worse at distinguishing soil and grass. It can also be observed that about one-tenth of the paving samples were misclassified as soil and vice versa. This is because soil and paving have the same hardness and roughness.Table 1
Confusion matrix of 10-fold cross-validation with all terrains.
Soil
Grass
Asphalt
Paving
Soil
71
18
3
8
Grass
13
75
7
5
Asphalt
8
1
89
2
Paving
11
2
4
83We use the trained DT classifier to recognize terrains when the robot is in actual operation. The classifier outputs are shown in Figure5. Its misclassification rate is 20.39%, which cannot be acceptable. After using Bayesian filter, the misclassification rate can be decreased to a great extent. The filtering accuracy under assigning μ with different values is summarized in Figure 6. Observe that the misclassification rate can be reduced to a level under 2%. If Vt is introduced to adjust μ, the misclassification rate can be further reduced to a level under 0.6%.Figure 5
Filtering results. “GT” stands for the real terrain, “classifier” stands for the classifier output, and “filter” stands for the filter output.Figure 6
Misclassification rate after using Bayesian filter under differentμ.


### 4.2. ICR Estimation

The results of the ICR estimation test are shown in Figure7. In Test 1 where qmin=0.12, as shown in Figure 7(a), the convergent tendency is obvious, but it took too much time to track the reality of the ICRs. If the terrain changes frequently, the ICR estimation may not track the truths. The results of Test 2 where qmax=202 are shown in Figure 7(b). The ICR estimation is able to converge rapidly, but not in a smooth manner. In Tests 3 and 4, by introducing terrain classification, the ICR estimation can track the truths rapidly when the terrain changes. After the estimation becomes stable, it does not oscillate around the truths significantly. In contrast to Test 3, IEKF is adopted in Test 4 to reduce the effect brought by uncertainties in the model and the measurements. Comparing Figures 7(c) and 7(d), TA-IEKF outperforms TA-EKF. A common method to verify the correctness of the ICR estimation is by terrain adaptive odometry (also known as terrain adaptive dead reckoning). As shown in Figure 8, we incorporate the ICRs obtained in Tests 1 to 4. Obviously, the TA-IEKF is able to provide the robot’s ICRs with the highest accuracy, so the trajectory obtained from the terrain adaptive odometry is consistent with the outputs of the GPS, which has no cumulative errors.Figure 7
Results of ICR estimation test.
(a)
Results for Test 1
(b)
Results for Test 2
(c)
Results for Test 3
(d)
Results for Test 4Figure 8
Results of dead reckoning with knowledge of ICRs. “GPS” stands for the outputs of GPS, “IEKF1” stands for the terrain adaptive odometry whose ICRs are obtained using IEKF with smallQs, “IEKF2” stands for the terrain adaptive odometry whose ICRs are obtained using IEKF with large Qs, “TA-EKF” stands for the terrain adaptive odometry whose ICRs are obtained using TA-EKF, and “TA-IEKF” stands for the terrain adaptive odometry whose ICRs are obtained using TA-IEKF.


## 4.1. Terrain Classification

In order to collect the data for the classifier training, the tracked robot traversed the aforementioned four terrains at a speed of between0.4m/s and 1.3m/s. The test on each terrain lasted for about 1000 seconds, that is, 1000 samples were obtained for each terrain. We randomly divided these data into 10 parts on average and used each part to test the trained classifier once while used the remainder for training. The confusion matrix is shown in Table 1. Observing the diagonal elements, it can be seen that the classifier performs best at recognizing asphalt. This may be caused by the fact that most asphalt roads are flatter compared with soil, grass, and paving. When soil is exposed to the air, it turns dry and hard. After covering grass and then soil, the terrain becomes flatter and softer. However, the two terrains still share similar roughness, and therefore, the classifier performs worse at distinguishing soil and grass. It can also be observed that about one-tenth of the paving samples were misclassified as soil and vice versa. This is because soil and paving have the same hardness and roughness.Table 1
Confusion matrix of 10-fold cross-validation with all terrains.
Soil
Grass
Asphalt
Paving
Soil
71
18
3
8
Grass
13
75
7
5
Asphalt
8
1
89
2
Paving
11
2
4
83We use the trained DT classifier to recognize terrains when the robot is in actual operation. The classifier outputs are shown in Figure5. Its misclassification rate is 20.39%, which cannot be acceptable. After using Bayesian filter, the misclassification rate can be decreased to a great extent. The filtering accuracy under assigning μ with different values is summarized in Figure 6. Observe that the misclassification rate can be reduced to a level under 2%. If Vt is introduced to adjust μ, the misclassification rate can be further reduced to a level under 0.6%.Figure 5
Filtering results. “GT” stands for the real terrain, “classifier” stands for the classifier output, and “filter” stands for the filter output.Figure 6
Misclassification rate after using Bayesian filter under differentμ.


## 4.2. ICR Estimation

The results of the ICR estimation test are shown in Figure7. In Test 1 where qmin=0.12, as shown in Figure 7(a), the convergent tendency is obvious, but it took too much time to track the reality of the ICRs. If the terrain changes frequently, the ICR estimation may not track the truths. The results of Test 2 where qmax=202 are shown in Figure 7(b). The ICR estimation is able to converge rapidly, but not in a smooth manner. In Tests 3 and 4, by introducing terrain classification, the ICR estimation can track the truths rapidly when the terrain changes. After the estimation becomes stable, it does not oscillate around the truths significantly. In contrast to Test 3, IEKF is adopted in Test 4 to reduce the effect brought by uncertainties in the model and the measurements. Comparing Figures 7(c) and 7(d), TA-IEKF outperforms TA-EKF. A common method to verify the correctness of the ICR estimation is by terrain adaptive odometry (also known as terrain adaptive dead reckoning). As shown in Figure 8, we incorporate the ICRs obtained in Tests 1 to 4. Obviously, the TA-IEKF is able to provide the robot’s ICRs with the highest accuracy, so the trajectory obtained from the terrain adaptive odometry is consistent with the outputs of the GPS, which has no cumulative errors.Figure 7
Results of ICR estimation test.
(a)
Results for Test 1
(b)
Results for Test 2
(c)
Results for Test 3
(d)
Results for Test 4Figure 8
Results of dead reckoning with knowledge of ICRs. “GPS” stands for the outputs of GPS, “IEKF1” stands for the terrain adaptive odometry whose ICRs are obtained using IEKF with smallQs, “IEKF2” stands for the terrain adaptive odometry whose ICRs are obtained using IEKF with large Qs, “TA-EKF” stands for the terrain adaptive odometry whose ICRs are obtained using TA-EKF, and “TA-IEKF” stands for the terrain adaptive odometry whose ICRs are obtained using TA-IEKF.


## 5. Conclusion

In this paper, we propose an online estimation method to acquire robot’s ICRs by means of data fusion technologies. First, an innovation-based extended Kalman filter (IEKF) is employed to fuse the readings from two incremental encoders and a GPS-compass integrated sensor, to provide a real-time ICR estimation. Second, a decision tree-based learning system is used to classify the terrains over which the robot is traversing, according to the vibration signals gathered by an accelerometer. The results of the terrain classification are improved via a Bayesian filter, by utilizing temporal correlation in the terrain time series. Third, the performances of the ICR estimation and terrain classification are mutually promoted. On the one hand, terrain variation is detected with the aid of terrain classification, and therefore, the process of the noise variance of the IEKF can be automatically adjusted. Hence, the results of the ICR estimation are smooth on the same terrains and converge rapidly on terrain variation. On the other hand, sudden changes in the innovation are used to adjust the state transition probability during the recursive calculation of the Bayesian filter, thus increasing the accuracy of the terrain classification. The experiment indicates that the TA-IEKF-based ICR estimation has relatively higher accuracy, because it is able to converge rapidly at the moment the terrain changes and maintain the estimation smoothness when the robot is on the same terrain. Using the obtained ICRs to correct the odometry can retard its accumulation of localization errors, which is of great significance to the localization in GPS-denied areas.



---

*Source: 4816712-2018-11-01.xml*