Terrain Adaptive Odometry for Mobile skid-steer Robots

Introduction

This paper proposes a novel approach improving precision and reliability of odometry of skid-steer mobile robots by means inspired by robotic terrain classification (RTC).

Novelty

In contrary to standard RTC approaches, this paper do not provide human labeled discrete terrain categories, but we classify the terrain directly by the values of coefficients correcting the robot’s odometry.

不是应该用terrain type判断correction coefficients吗，这里的意思好像是用correction coefficients来判断terrain type.

Rather than labeling the terrain categories by human means, it is more important to find representation that can be directly exploited by the robot, i.e. find terrain type representation by means essential for the robot. Sometimes, the same terrain type behaves differently under various circumstances (such as pebbles on flat terrain and pebbles on steep or uneven terrain) as well as different terrains can influence robot’s odometry in a very similar way.

Enhanced Odometry Model with Correction Coefficients

首先定义两个parameter：
$${\alpha }_l=\frac{v_l-v_l^{\prime }}{v_l}\text{\hspace{1em}(1)}$$ $${\alpha }_r=\frac{v_r-v_r^{\prime }}{v_r}\text{\hspace{1em}(2)}$$

where
$v_r,v_l$——the left and right track velocities measured by internal sensors
$v_r^{\prime },v_l^{\prime }$——the true velocities of the track gears moving through the coordinate frame

在线求解$\alpha$的两个公式：
$$\dot{\theta }=\frac{v_r(1-{\alpha }_r)-v_l(1-{\alpha }_l)}{2d}\text{\hspace{1em}(3)}$$ $$\frac{a_l}{a_r}=-\mathrm{s}\mathrm{g}\mathrm{n}(v_l\cdot v_r)\sqrt{\frac{v_r}{v_l}}\text{\hspace{1em}(4)}$$

结合这两个方程就可以直接求得$\alpha$, since $\dot{\theta }$ can be measured directly using the vertical axis angular rate sensor.

把上述$\alpha$代入下面两个方程，就可以求得当时的$\dot{x},\dot{y}$：
$$\dot{x}=\frac{v_r(1-{\alpha }_r)+v_l(1-{\alpha }_l)}{2}\mathrm{c}\mathrm{o}\mathrm{s}\theta$$ $$\dot{y}=\frac{v_r(1-{\alpha }_r)+v_l(1-{\alpha }_l)}{2}\mathrm{s}\mathrm{i}\mathrm{n}\theta$$

求得$\dot{x},\dot{y}$之后代入下面两个方程就可以求出position increments，重点是这里引入了一个重要参数$C$：

$$△x_{k-1}^k=\frac{{\dot{x}}_{k-1}+{\dot{x}}_k}{2}{T}_{s}C$$ $$△y_{k-1}^k=\frac{{\dot{y}}_{k-1}+{\dot{y}}_k}{2}{T}_{s}C$$

These corrected position increments are then used to update the current position.

Regression Function Training

To estimate the correction coefficients as (3) (4)，we have chosen a straightforward yet sufficient approach of training a linear regression function $C(x;\alpha )$， which processes features obtained from inertial data (three accelerations and three angular rates at 90 Hz) and outputs the correction coefficient directly.

$$C(x;\alpha )=\sum _{i=0}^n{\alpha }_i{x}_i$$

where
$n$——the number of features
$x_i$——the feature value
${\alpha }_i$——the corresponding parameter of the regression function

Training of the regression function was performed using normal equations corresponding to minimization of the following cost function in a least squares sense:
$$J(\alpha )=\frac{1}{2m}\sum _{k=1}^m(C(x^k;\alpha )-y^k{)}^2$$
where
$m$——the number of training examples
$y^k$——the correction coefficient obtained from the ground truth trajectory reference for the $k^{th}$ training example. They were computed by off-line local optimization in such a way to ensure that the trajectory obtained using the proposed model parameterized by these $y^k$ coefficients coincides with the reference.

Feature Extraction and Selection

This paper perform feature selection on the following features:

1. features from the time domain defined as various statistical moments and indicators such as min, max, mean, median, norm, skewness, kurtosis, and number of sign changes over a threshold given by 0.25, 0.5, 0.75 multiples of mean and median
2. features from the frequency domain computed using FFT and taking into account the magnitudes and frequencies of first three most significant harmonics
3. features defined as correlation coefficient between the signal samples and more than a hundred of Haar wavelets generated with the corresponding length.

The feature selection was performed off-line as part of the regression function training.

Formally, we are given a training set $\left\{x^1,y^1,...,x^K,y^K\right\}$ consisting of $K$ training samples, where $x^k$ are N-dimensional vectors containing values of features from the feature pool. We denote $x_j^k$ as the $j$-th feature of the $k$-th sample.

Robotic Platform Description

Plant: skid-steer robot (tracked vehicle)
Inertial Sensor: Xsens MTi-G unit providing calibrated and temperature compensated inertial data at 90 Hz in range of $\pm 300deg/s$ and $\pm 50m/s^2$ for angular rates and accelerations respectively.
Encoder: providing left and right track velocities at 10 Hz.

Data Collection: The robot was teleoperated during these experiments and tracked using overhead camera and video tracking system we have developed for this purpose.

Evaluation and Testing

1. First, we had to determine the optimal number of inertial data samples to be used for computing features.
   This way the optimal number of samples was determined to be 98.
   For the real-time on-line implementation this means: each time a new odometry measurement comes (at 10 Hz), from that time-step the last 98 samples of the 6 inertial samples signals (sampled at 90 Hz) are processed to obtained the selected features that form the input to the trained regression. This way one correction coefficient is obtained for each odometry.
2. Another series of experiments was carried out to determine the optimal number of features to be selected during the proposed feature selection procedure.
   This proved that using only the best 20 features is fully sufficient, most of which were from the group of statictical time domain features. These features proved to be the best for capturing information about changes in motion dynamics corresponding to disturbances to the proposed odometry model caused by the nature of terrain.
3. To field-test our algorithm and evaluated the desired improvement in precision of navigation, we compared the development of RMSE of position in time of our approach (with respect to the ground truth trajectory) ot the RMSE development of the trajectory obtained using the original uncorrected odometry model.