Terrain Adaptive Estimation of Instantaneous Centres of Rotation for Tracked Robots

. As a type of skid-steering mobile robot, the tracked robot su ﬀ ers 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 re ﬂ ect the in ﬂ uences 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 ﬁ lter (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 classi ﬁ cation are improved via a Bayesian ﬁ lter, by utilizing temporal correlation in the terrain time series. Third, the performances of the ICR estimation and terrain classi ﬁ cation are mutually promoted. On one hand, terrain variation is detected with the aid of the terrain classi ﬁ cation, 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 ﬁ lter, thus increasing the accuracy of the terrain classi ﬁ cation. A real-world experiment was undertaken on a tracked robot to validate the e ﬀ ectiveness of the proposed method.ItisalsodemonstratedthattheterrainadaptiveodometryoutperformsthetraditionalapproachwiththeknowledgeofICRs.


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][3][4][5][6][7][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 Figure 1) was proposed by introducing ICRs into the traditional kinematics model of a differencedriven 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.
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 EKFbased 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 treebased 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.Section 2 presents the ICR kinematics model.Section 3 2 Complexity 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.

ICR Kinematics Model
The section presents the ICR kinematics model for a tracked robot.As shown in Figure 1, 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, y c , is where v x 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 y r , are where v x ℓ and v x r denote the left-and right-track velocities along the X-axis, respectively.The ICR location of the robot chassis on the X-axis, x c , is where v y denotes the robot velocity along the Y-axis.The ICR locations of the left and right tracks on the X-axis, x ℓ and x r , are where v y ℓ and v y r 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 the Y-axis, so v y ℓ = v y r = 0, and therefore, we have Hence, in the ICR locations of the chassis, the left and right tracks are on the same line parallel to the Y-axis.
Solving the velocities with respect to BCS, i.e., v x , v y , and ω, from (2a), (2b), and (3), we have the denominators of which are nonzero values.Here, since v y ℓ = v y r = 0, the superscripts of v x ℓ and v x r are discarded, i.e., v ℓ = v x ℓ and v r = v x r .The ICRs lie on or outside the tracks' central lines when slippage does or does not occur [10].Hence, y ℓ − y r ≥ −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 Figure 2, 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 deformation The kinestates with respect to BCS and WCS can be related by 3 Complexity 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.

Terrain Adaptive IKF
The schematic of the proposed ICR estimation based on TA-IEKF is illustrated in Figure 3.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.
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.Complexity where s = e, n, θ, y ℓ , y r , x v ′.The specific form of ( 10) is where w = w e , w n , w θ , w ℓ , w r , w v ′ 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 where s t = e t , n t , θ t , y ℓ,t , y r,t , x v,t ′ denotes the state vector and w t = w e,t , w n,t , w θ,t , w ℓ,t , w r,t , w v,t ′ ~ℕ 0, Q denotes the process noise vector.The process noise variance Q = diag q 1 , q 2 , q 3 , q 4 , q 5 , q 6 = block diag Q k , Q s where Q k = diag q 1 , q 2 , q 3 and Q s = diag q 4 , q 5 , q 6 .The specific form of ( 12) is shown in (8), where T denotes the sampling period.
Second, to estimate s t , the observation model is established as where z t = z e,t , z n,t , z θ,t ′ denotes the observation vector, v t = v e,t , v n,t , v θ,t ′ ~ℕ 0, R denotes the observation noise vector which is assumed to be Gaussian white noises, and H = I 3×3 , O 3×3 denotes the observation matrix where O 3×3 and I 3×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 s t 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, Q s is set as diag q max , q max , q max where q max is a relatively large value, say 100.Otherwise, Q s is set as diag q min , q min , q min where q min 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 Let p = 0 8, then setting λ = 3 can guarantee ρ = 0 9.If ρ = 0 98 is desired, then λ must be larger than 9.

Priori Estimation. Calculate the a priori estimation
and the corresponding error variance by In ( 16), F t and L t denote the Jacobian matrices of f • , that is, where O n×n and I n×n denotes n × n null matrix and identity matrix, respectively.The specific form of F K,t and F S,t is shown in (9a) and (9b), respectively.and its covariance is estimated by where N denotes windowing size.Furthermore, calculate the filtering gain K t by Finally, the a posteriori estimation ŝt+1 can be calculated by coupled with its error variance where The case â i t = 0 is ignored since it is singular.This feature is an approximation of the frequency of ât .
(ii) ϕ 2 t : the mean of ât Although the DC component of the raw data has been eliminated by ( 23), the mean of ât may considerably diverge from zero for some course terrain.
(iii) ϕ 3 t : the variance of ât Intuitively, the variance is higher when the terrain becomes coarser.

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 6 Complexity 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.

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, c t ∈ 1, 2, ⋯, k as the classifier output, and C t = c 1 , c 2 ⋯ , c t as the measurement set.The purpose is to acquire P χ t | C t , the a posteriori possibility distribution function (pdf) of χ t in the condition of C t .Given P χ t−1 | C t−1 , the a priori pdf of χ t is

28
and the a posteriori pdf is calculated by where P χ t = i | χ t−1 = j denotes the probability that the robot moves from terrain j to i at time t and P c t = 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 should be defined.The diagonal elements m ii where i = 1, 2, … , k should be assigned a relatively large value not greater than 1.By assigning m ii = μ, a large value can enable most misclassified results to be corrected, but a succession of mistakes may appear after terrain variation.This is because m ii should be reassigned a small value and m ij 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     7 Complexity study, if terrain does not change, then V t tends to be steady state; if the terrain changes, then a sudden change occurs in V t .After detecting this event, we decrease m ii and increase the related nondiagonal elements, and therefore, the accuracy of the Bayesian filter is increased.

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 was 260 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 1 Hz 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 200 Hz, and the other sensors were read by the data collector at 1 Hz.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.Complexity and 1 3 m/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.We use the trained DT classifier to recognize terrains when the robot is in actual operation.The classifier outputs are shown in Figure 5.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 V t is introduced to adjust μ, the misclassification rate can be further reduced to a level under 0 6%.

ICR Estimation.
The results of the ICR estimation test are shown in Figure 7.In Test 1 where q min = 0 1 2 , 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 q max = 20 2 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.

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.

Figure 1 :
Figure 1: Schematic of the ICR locations of a tracked robot.

Figure 2 :
Figure 2: Coordinate systems on a tracked robot.The blue axes represent the body coordinate system, and the green ones represent the world coordinate system.

Figure 3 :
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.

Figure 4 :
Figure 4: Photos of the traversed terrains.

Figure 5 :
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 :
Figure 6: Misclassification rate after using Bayesian filter under different μ.

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 between 0 4 m/s

8
t ŷr,t ŷℓ,t − ŷr,t cos θ t + xc,t v r,t − v ℓ,t ŷℓ,t − ŷr,t sin θ t 0 1 T v ℓ,t ŷr,t − v r,t ŷℓ,t ŷℓ,t − ŷr,t sin θ t + xc,t v r,t − v ℓ,t ŷℓ,t − ŷr,t cos θ 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, a t is preprocessed by n t as the raw acceleration vector gathered from sampling point t − 1 to t.

Table 1 :
Confusion matrix of 10-fold cross-validation with all terrains.