Path Tracking Control of a Mobile Robot by Using Dual Estimation Algorithm

Abstract

Skid-steered mobile robots have been developed to explore unknown environments, especially in rough terrain situations, where the wheel slips always occur and vary when the robot is traveling with different trajectory shapes. The wheel slippage limits the traction and braking abilities of the robot. In this paper, we propose a new dual estimation algorithm to overcome the previous limitations. The estimated values such as the robot's positions and wheel slips are obtained based on the Kalman filtering technique. As demonstrated by our experimental results, the advantages of the new method are effective to overcome the slip limitations in the path tracking control problem of a four-track wheel skid-steered mobile robot (4-TW SSMR).

1. Introduction

Recently, several studies have proposed and developed various approaches for estimating the position of a mobile robot. For example, one study used an extended Kalman filter (EKF) with a low-cost GPS unit and inclinometer to develop a localization scheme for Ackerman steering vehicles in indoor autonomous navigation situations by estimating the positions of the vehicles and their sensor biases [1]. In another study, a relative localization method was used to determine the navigational route of a convoy of robotic units in an indoor environment using an EKF based on a low-cost laser range system and built-in odometric sensors [2]. In skid-steered vehicles, discrepancies remain between position measurements and the navigational estimations obtained from odometry, infrared, and ultrasonic measurements and Kalman filtering techniques [3]. However, this study only mentioned the advantages of the different extensions of the KF for nonlinear estimations. In [4], a laser range finder (LRF) was used to find the localization of a mobile robot based on the EKF design. That study focused on a statistically exact derivation of the input noise covariance matrix, according to the known noise variances of the angular velocity measurements of both robot wheels.

Additionally, the exploration of unknown planetary surfaces with the help of mobile robot is increasingly being developed. A localization and path selection of any vehicle like a mobile robot to achieve any planetary exploration missions were described in [5]. In this study, by using odometry and gyro measurements, these sensors information was integrated by EKF. In another research, a robot, named Hyperion, was designed for solar-powered operation in polar environments and developed sun-cognizant navigation method to enable rovers to dodge shadows, seek sun, and drive sun-synchronous routes [6].

In a mobile robot's field of operation, wheel slip limits traction and braking ability, especially for a 4-TW SSMR. Accurate estimation of slip is essential to obtain precise position for a mobile robot operating in unstructured terrain, especially in rough terrain situations. Several approaches to slip estimation have been developed. For example, a sliding mode observer (SMO) conducted to estimate slip parameters based on the kinematic model of a skid-steering vehicle is proposed in [7]. In another study, a novel method combining an optical flow algorithm with a sliding mode observer to estimate slip parameters of a SSMR was introduced [8]. Reference [9] proposed an experimental slip model for exact kinematic modeling, and the parameters of this model were determined based on experimental analysis. In [10], a rough-terrain control (RTC) methodology was presented in multi-wheeled mobile robot systems to improve ground traction and reduce power consumption. A key element of the method is able to estimate the wheel ground contact angles.

The methods in [7–9] remain limitations when the robot is traveling on the different ground surfaces with different and longer trajectory shapes. In this paper, we propose a new dual estimation algorithm for the robot's position and wheel slip estimations based on the Kalman filtering technique. First, the EKF is utilized to estimate the robot's positions, velocities and orientation angles, which are used for the feedback control signals. Second, a discrete KF is designed to estimate the slip parameters of the left and right track wheels, which are used to compensate for the velocity constraints and its variance values for the EKF in order to get more accurate estimated values. In the dual estimation algorithm, these two Kalman filters are performed simultaneously based on the measurements from AHRS sensor and two incremental encoders. The experimental results show that the advantages of the new method overcome the slip limitations in the path tracking control problem of a 4-TW SSMR. The experiment is performed on the NT-Hazard Escape-1, as shown in Figure 1.

Figure 1:

The NT-Hazard Escape-1 mobile robot.

This paper is organized as follows: Section 2 introduces the experimental setup and implemented control system. Dual estimation algorithm is presented in Section 3. The experimental results, including analysis and evaluation, are discussed in Section 4. Finally, Section 5 presents the conclusion.

2. The Experimental Setup and Implemented Control System

2.1. The Experimental Setup

The NT-Hazard Escape-1 mobile robot, with four NT-track wheels, considered in this study is applied in the investigation of disaster areas, running and watching rough areas including construction site and running up stairways. The robot with small actuators was made for the first time in Korea, providing various functions to robots. Each of the eight motors is faithful to its own move, and the robot can easily lift two tracks even when is fully loaded. The robot can also move while maintaining its central balance, like a tank. In addition, RS232C is supported to enable working with an embedded board and computer through wireless telecommunication. Therefore, it is easily applied in various applications. The dimensions for the mobile robot are shown in Figure 2.

Figure 2:

Dimensions of the NT-Hazard Escape-1.

For research purposes, an experimental mobile robot was designed. The system configuration was represented by a configuration for all of the elemental components as shown in Figure 3(a), while the real photograph of the experimental system is displayed in Figure 3(b). From these figures, the AHRS (AHRS-03-300) was mounted on the top and center of the robot based on a base platform. Two incremental encoders (E40H-8-1024-3-N-5) were installed on the robot's front-left and right tracked wheels. A notebook (model ASUS U36S) was used to collect the data from the AHRS sensor and two incremental encoders via an AVR 128-pro board. This notebook also performed all algorithms which were applied in real-time. Here, the control algorithm for the system was coded in C# programming language (using the Microsoft Visual Studio 2008 version).

Figure 3:

(a) The system configuration. (b) The real photograph of the experimental system.

During the experiment, the measurement sampling frequency for the AHRS and incremental encoders is set to 20 Hz. The robot is programmed to follow a desired trajectory. Based on the measurements from these two sensors, the robot position values are derived through the Kalman filtering technique, and then robot controllers are applied to drive the robot to the desired trajectories.

2.2. The Implemented Control System

In the experimental mobile robot, a path tracking control method for the robot motion is performed to follow the desired trajectory based on closed-loop control systems and the sensor fusion technique for feedback control signals. Figure 4 describes the diagram of the implemented control algorithm. As shown in this figure, the control system includes three closed-loop controls: two low-level closed-loop controls for motor speeds of the left and right sides and one high-level closed-loop control for robot position. The discrete PID controllers are applied for low-level closed-loop controls. For the path tracking control system, the stable tracking control (STC) method is introduced in [11, 12], based on an error model of the kinematical model. In another approach, vector field orientation (VFO) feedback control method is a motivating control technique to calculate such linear and angular velocities for DDVs. The reader can refer to [13] (an application for a Four wheel drive skid-steered mobile robot), [14] (VFO for a DDV) and [15] (an extension of VFO in the case of skid-slip phenomena). In this work, the VFO control feedback method is applied for the high-level closed-loop control of the robot position in a 2D plane.

Figure 4:

The diagram of the implemented control system.

Additionally, the dual estimation algorithm is proposed by integrating the EKF and the discrete KF. First, the EKF, denoted by EKF x, is utilized to estimate the positions, velocities and orientation angles which are used for the feedback control signals in the high-level closed-loop control. Second, a discrete KF, denoted by KFλ, is designed to estimate the slip parameters of the left and right track wheels, which are used to compensate the velocity constraints and its variance values for the EKF x in order to get more accurate estimation values. In the dual estimation algorithm, these EKF x and KFλ are performed simultaneously based on the measurements from AHRS sensor and two incremental encoders, and they are introduced in the next sections.

3. Dual Estimation Algorithm

3.1. AHRS Kinematics and Velocity Constraints

We define a navigational reference frame N(X, Y, Z) and robot body frame B(x, y, z) as shown in Figure 5. Let $P_N(t)={[X_N(t),Y_N(t),Z_N(t)]}^T∊{ℝ}^{\mathrm{3}}$ , $V_N(t)={[V_x(t),V_y(t),V_z(t)]}^T∊{ℝ}^{\mathrm{3}}$ , and $\mathrm{\Theta }(t)={[\varphi (t),\theta (t),\psi (t)]}^T∊{ℝ}^{\mathrm{3}}$ denote the position, velocity, and attitude angle vectors of AHRS in N frame, respectively. We also define the AHRS acceleration, angular rate, acceleration walking bias, and angular rate walking bias in the B frame as $a_B={[a_{Bx},a_{By},a_{Bz}]}^T∊{ℝ}^{\mathrm{3}}$ , ${\omega }_B={[{\omega }_{Bx},{\omega }_{By},{\omega }_{Bz}]}^T∊{ℝ}^{\mathrm{3}}$ , $b_{aB}={[b_{aBx},b_{aBy},b_{aBz}]}^T∊{ℝ}^{\mathrm{3}}$ , and $b_{gB}={[b_{gBx},b_{gBy},b_{gBz}]}^T∊{ℝ}^{\mathrm{3}}$ , respectively.

Figure 5:

An AHRS kinematics model of a 4-TW SSMR.

After subtracting the constant offset and local gravity vector, the acceleration and angular rate models of the AHRS can be described by [16]

$\begin{array}{c}{a}_B={\stackrel{‥}{x}}_B+b_{aB}+w_{\text{accel}},\\ {\omega }_B=r_B+b_{gB}+w_{\text{gyros}},\end{array}$

(1)

with

$\begin{array}{c}{\dot{b}}_{aB}=-\frac{\mathrm{1}}{{\tau }_a}{b}_{aB}+\sqrt{\frac{\mathrm{2}{f}_s{\sigma }_{baB}^{\mathrm{2}}}{{\tau }_a}}{w}_{\text{accbias}},\\ {\dot{b}}_{gB}=-\frac{\mathrm{1}}{{\tau }_g}{b}_{gB}+\sqrt{\frac{\mathrm{2}{f}_s{\sigma }_{bgB}^{\mathrm{2}}}{{\tau }_g}}{w}_{\text{gyrobias}},\end{array}$

(2)

where the true acceleration vector, true angular rate vector, acceleration white noise, and angular rate white noise are ${\stackrel{‥}{x}}_B={[{\stackrel{‥}{x}}_{Bx},{\stackrel{‥}{x}}_{By},{\stackrel{‥}{x}}_{Bz}]}^T$ , $r_B={[r_{Bx},r_{By},r_{Bz}]}^T$ , w_accel, and w_gyros, respectively. And τ _a , σ _baB ², w_accbias, τ _g , σ _bgB ² and w_gyrobias are time constants, noise variances, and zero-mean white noises with E[w²] = 1 of the acceleration walking bias and angular rate walking bias, and the f _s is the sampling frequency.

We also define the state vector $\chi (t)=[P_N(t),V_N(t),\mathrm{\Theta }(t),b_{aB}(t),b_{gB}(t)]∊{ℝ}^{\mathrm{15}}$ of model process. The kinematic motion equation for AHRS can be simplified to:

$\dot{\chi }=\left[\begin{array}{c}{V}_I\\ C_B^N\left(a_B-b_{aB}+w_{\text{accel}}\right)\\ q_{\mathrm{\Theta }}\left({\omega }_B-b_{gB}+w_{\text{gyros}}\right)\\ -\frac{\mathrm{1}}{{\tau }_a}{b}_{aB}\\ -\frac{\mathrm{1}}{{\tau }_g}{b}_{gB}\end{array}\right]+\left[\begin{array}{c}\mathrm{0}\\ \mathrm{0}\\ \mathrm{0}\\ \sqrt{\frac{\mathrm{2}{f}_s{\sigma }_{baB}^{\mathrm{2}}}{{\tau }_a}}{w}_{\text{accbias}}\\ \sqrt{\frac{\mathrm{2}{f}_s{\sigma }_{bgB}^{\mathrm{2}}}{{\tau }_g}}{w}_{\text{gyrobias}}\end{array}\right],$

(3)

where C _B ^N and q_Θ, the transformation matrix from the B frame to the N frame and transformation matrix of the Euler angles, as given by the following matrices:

$C_B^N=\left[\begin{array}{ccc}{c}_{\theta }{c}_{\psi }& -s_{\psi }{c}_{\varphi }+c_{\psi }{s}_{\varphi }{s}_{\theta }& s_{\varphi }{s}_{\psi }+c_{\psi }{s}_{\theta }{c}_{\varphi }\\ c_{\theta }{s}_{\psi }& c_{\varphi }{c}_{\psi }+s_{\theta }{s}_{\varphi }{s}_{\psi }& -s_{\varphi }{c}_{\psi }+s_{\theta }{c}_{\varphi }{s}_{\psi }\\ -s_{\theta }& c_{\theta }{s}_{\varphi }& c_{\varphi }{c}_{\theta }\end{array}\right],$

(4)

$q_{\mathrm{\Theta }}=\left[\begin{array}{ccc}1& s_{\varphi }{t}_{\theta }& c_{\varphi }{t}_{\theta }\\ 0& c_{\theta }& -s_{\theta }\\ 0& \frac{s_{\varphi }}{c_{\theta }}& \frac{c_{\varphi }}{c_{\theta }}\end{array}\right],$

(5)

where c_θ = cosθ, s_θ = sinθ, t_θ = tanθ, and the same notation convention is used for angles ϕ and ψ.

In the 4-TW SSMR, the motors that power the wheels at each side are geared internally to ensure that the velocities of two adjacent wheels at each side are synchronized (have the same angular velocity) and thus have the same velocity at ground contact. Let us define v _L and v _R asthe left and right linear velocities of the robot; we have

$\begin{array}{c}{v}_L=v_{\mathrm{1}x}=v_{\mathrm{2}x},\\ v_R=v_{\mathrm{3}x}=v_{\mathrm{4}x},\end{array}$

(6)

where v_1x, v_2x, v_3x, and v_4x are center linear velocities for front-left, rear-left, front-right, and rear-right wheels, respectively, shown in Figure 6. The longitudinal wheel slips of the left and right wheels λ _L and λ _R are defined as ratios of the wheel velocities and its center velocities [17], as follows:

$\begin{array}{c}{\lambda }_L=\frac{r{\omega }_L-v_L}{r{\omega }_L},\\ {\lambda }_R=\frac{r{\omega }_R-v_R}{r{\omega }_R},\end{array}$

(7)

where r is the wheel radius, ω _L and ω _R are the wheel angular speeds for the left and right sides of the mobile robot. From (7), it can be seen that the wheel slip λ ∊ [0, 1] if the wheel is under traction and λ ∊ (−∞, 0] if the wheel is under braking. Using the definition of the slip in (7), we have

$\begin{array}{c}{v}_L=\left(\mathrm{1}-{\lambda }_L\right)r{\omega }_L,\\ v_R=\left(\mathrm{1}-{\lambda }_R\right)r{\omega }_R.\end{array}$

(8)

Figure 6:

The 4-TW SSMR kinematics.

Because of the symmetric mechanical structure of the robot, it can be assumed that the center of mass (COM) of the robot is located at the center of geometry (COG) of the body frame. The AHRS coordinate is located at the COG in B frame, as shown in Figure 5. Using two wheel encoders, we obtain the AHRS velocity vector $v_B={[v_{Bx},v_{By},v_{Bz}]}^T$ in B frame. From (8), using two wheel encoder measurements and wheel slip parameters, we obtain the longitudinal velocity v _Bx as follows:

$v_{Bx}=\frac{r}{\mathrm{2}}\left[(\mathrm{1}-{\lambda }_L){\omega }_L+(\mathrm{1}-{\lambda }_R){\omega }_R\right].$

(9)

Based on [17], since the four tracked wheels of our robot are always in contact with the ground and since the AHRS is fixed on the robot platform, the velocity constraints v _By and v _Bz in y-axis and z-axis directions for the AHRS device can be simplified to equal zero.

The noises in the longitudinal velocity v _Bx can be expressed as the sum of the noises of the left and right wheels' angular speeds ω _L and ω _R as measured by encoders. It is assumed that the noises for ω _L and ω _R have a normal distribution with a zero-mean and corresponding variances, and there is no cross-correlation between the noise of ω _R and of ω _L [18]. The noise variance of the longitudinal velocity v _Bx can be expressed as

${\sigma }_{vBx}^{\mathrm{2}}=\frac{r^{\mathrm{2}}}{\mathrm{4}}\left({\left(\mathrm{1}-{\lambda }_R\right)}^{\mathrm{2}}\text{var}\left({\omega }_R\right)+{\left(\mathrm{1}-{\lambda }_L\right)}^{\mathrm{2}}\text{var}\left({\omega }_L\right)\right).$

(10)

The noise variances σ _vBy ² and σ _vBz ² of the lateral velocity v _By and ground surface topography v _Bz are also expressed by zeros. In order to obtain the longitudinal velocity v _Bx and its noise variance, we have to estimate slips λ _L and λ _R . The proposed dual estimation method, which can estimate these two slip values, is introduced in Section 3.2.

3.2. Dual Estimation Algorithm

Now, we define the state variable's vector $X(k)={[P_N(k),V_N(k),\mathrm{\Theta }(k),b_{aB}(k),b_{gB}(k)]}^T∊{ℝ}^{\mathrm{15}}$ . We rewrite the AHRS kinematics equation shown in (3) in discrete-time form, and we obtain the process model as

$X\left(k\right)=f\left(X\left(k-\mathrm{1}\right),u\left(k\right),w\left(k\right),\mathrm{\Delta }T\right)=X\left(k-\mathrm{1}\right)+\mathrm{\Delta }T*g\left(X\left(k-\mathrm{1}\right),u\left(k\right),w\left(k\right)\right),$

(11)

where the AHRS input signals at the kth sampling time is $u(k)={[a_B(k),{\omega }_B(k)]}^T∊{ℝ}^{\mathrm{6}}$ and the data sampling period is ΔT. Also, the nonlinear function g of the process model is

$g\left(X\left(k-\mathrm{1}\right),u\left(k\right),w\left(k\right)\right)\hspace{0.17em}=\left[\begin{array}{c}{g}_P\\ g_V\\ g_{\mathrm{\Theta }}\\ g_{aB}\\ g_{gB}\end{array}\right]\hspace{0.17em}=\left[\begin{array}{c}{V}_I\left(k-\mathrm{1}\right)\\ C_B^N\left(k-\mathrm{1}\right)\left(a_B\left(k\right)-b_{aB}\left(k-\mathrm{1}\right)+w_{\text{accel}}\left(k\right)\right)\\ q_{\mathrm{\Theta }}\left(k-\mathrm{1}\right)\left({\omega }_B\left(k\right)-b_{gB}\left(k-\mathrm{1}\right)+w_{\text{gyros}}\left(k\right)\right)\\ -\frac{\mathrm{1}}{{\tau }_a}{b}_{aB}\left(k-\mathrm{1}\right)\\ -\frac{\mathrm{1}}{{\tau }_g}{b}_{gB}\left(k-\mathrm{1}\right)\end{array}\right]\hspace{0.17em}+\left[\begin{array}{c}\mathrm{0}\\ \mathrm{0}\\ \mathrm{0}\\ \sqrt{\frac{\mathrm{2}{f}_s{\sigma }_{baB}^{\mathrm{2}}}{{\tau }_a}}{w}_{\text{accbias}}\left(k\right)\\ \sqrt{\frac{\mathrm{2}{f}_s{\sigma }_{bgB}^{\mathrm{2}}}{{\tau }_g}}{w}_{\text{gyrobias}}\left(k\right)\end{array}\right],$

(12)

and w(k) = [w_accle(k), w_gyros(k), w_accbias(k), w_gyrobias(k)] ^T ∊ ℝ¹² is the process noise vector.

With velocity constraints (9), the AHRS velocities in B frame are considered as the measurement vector $y(k)={[v_{Bx}(k),v_{By}(k),v_{Bz}(k)]}^T∊{ℝ}^{\mathrm{3}}$ . Including the wheel encoder measurement noise and ground topography, the measurement model is rewritten in discrete-time form as

$y\left(k\right)=h\left(X\left(k\right)+n\left(k\right)\right)={\left(C_B^N\right)}^T{V}_N+n\left(k\right),$

(13)

where n(k) is measurement noises. We assume that the measurement noises n(k) are independent, zero-mean, and Gaussian white noise; that is, n(k) ∼ N(0, R). The covariance matrix of the measurement noise is

$R\left(k\right)=\mathrm{diag}\left({\sigma }_{vBx}^{\mathrm{2}},{\sigma }_{vBy}^{\mathrm{2}},{\sigma }_{vBz}^{\mathrm{2}}\right)∊{ℝ}^{\mathrm{3}\times \mathrm{3}},$

(14)

where σ _vBx ², σ _vBy ², and σ _vBz ² are calculated in (10). The EKFx was implemented by using systems (11) and (13) in order to obtain the estimated positions, velocities, and attitudes of the robot.

Additionally, in order to determine wheel slip parameters to compensate the velocity constraints and its variance values, a discrete KF, denoted by KFλ, is designed. By giving v _Bx in body frame B and yaw rate $\dot{\psi }$ , the wheel center velocities along x-axis for the left and right sides wheels are calculated by

$\begin{array}{c}{v}_L=v_{Bx}-\frac{L}{\mathrm{2}}\dot{\psi },\\ v_R=v_{Bx}+\frac{L}{\mathrm{2}}\dot{\psi },\end{array}$

(15)

where L is lateral wheel bases. By substituting (15) into (7), we have

$\begin{array}{c}{\lambda }_L=\frac{r{\omega }_L-v_{Bx}+\left(L/\mathrm{2}\right)\dot{\psi }}{r{\omega }_L},\\ {\lambda }_R=\frac{r{\omega }_R-v_{Bx}-\left(L/\mathrm{2}\right)\dot{\psi }}{r{\omega }_R},\end{array}$

(16)

$⟹\begin{array}{c}{\omega }_L=\frac{\mathrm{1}}{\mathrm{1}-{\lambda }_L}\left[\frac{v_{Bx}}{r}-\frac{L\dot{\psi }}{\mathrm{2}r}\right],\\ {\omega }_R=\frac{\mathrm{1}}{\mathrm{1}-{\lambda }_R}\left[\frac{v_{Bx}}{r}+\frac{L\dot{\psi }}{\mathrm{2}r}\right].\end{array}$

(17)

We define unknown parameters as:

${\gamma }_L=\frac{\mathrm{1}}{\mathrm{1}-{\lambda }_L};\hspace{1em}\hspace{1em}{\gamma }_R=\frac{\mathrm{1}}{\mathrm{1}-{\lambda }_R},\lambda ∊\left(-\infty ,\mathrm{1}\right]⟹\gamma ∊\left[\mathrm{0},+\infty \right).$

(18)

Substituting (18) into (17), the relationship between the angular speed from encoders and unknown parameters is described as

$\left[\begin{array}{c}{\omega }_L\\ {\omega }_R\end{array}\right]=\left[\begin{array}{cc}\frac{v_{Bx}}{r}-\frac{L\dot{\psi }}{\mathrm{2}r}& \mathrm{0}\\ \mathrm{0}& \frac{v_{Bx}}{r}+\frac{L\dot{\psi }}{\mathrm{2}r}\end{array}\right]\left[\begin{array}{c}{\gamma }_L\\ {\gamma }_R\end{array}\right].$

(19)

The slip estimation based on the KFλ is now established. Assuming that the slip process is the random process, then the unknown parameter process can be modeled in discrete-time form as

$\gamma \left(k\right)={\mathrm{\Phi }}_k\gamma \left(k-\mathrm{1}\right)+w_{\gamma }\left(k-\mathrm{1}\right),$

(20)

where $\gamma (k)={[{\gamma }_L(k),{\gamma }_R(k)]}^T$ is the process state vector of the KFλ at time k step, $w_{\gamma }(k)={[w_{\gamma \mathrm{1}}(k),w_{\gamma \mathrm{2}}(k)]}^T$ is process noise vector, and Φ _k is the state transition matrix which is defined as:

${\mathrm{\Phi }}_k=\left[\begin{array}{cc}\mathrm{1}& \mathrm{0}\\ \mathrm{0}& \mathrm{1}\end{array}\right].$

(21)

From (19), the measurement model of the KFλ can be written relating to the encoder measurement noises as

$z\left(k\right)=H_{\gamma }\left(k\right)\gamma \left(k\right)+n_{\gamma }\left(k\right),$

(22)

where $z(k)={[{\omega }_L(k),{\omega }_R(k)]}^T$ is angular speed vector from two encoders at time k step, $n_{\gamma }(k)={[n_{\gamma \mathrm{1}}(k),n_{\gamma \mathrm{2}}(k)]}^T$ is measurement noise vector, and H_γ(k) is the measurement matrix which is calculated as follows:

$H_{\gamma }\left(k\right)=\left[\begin{array}{cc}\left(\frac{{\widehat{v}}_{Bx}\left(k\right)}{r}-\frac{L\widehat{\dot{\psi }}\left(k\right)}{\mathrm{2}r}\mathrm{\hspace{0.17em}\hspace{0.17em}}\right)& \mathrm{0}\\ \mathrm{0}& \left(\frac{{\widehat{v}}_{Bx}\left(k\right)}{r}+\frac{L\widehat{\dot{\psi }}\left(k\right)}{\mathrm{2}r}\right)\end{array}\right],$

(23)

where ${\widehat{v}}_{Bx}(k)$ and $\widehat{\dot{\psi }}(k)$ are the estimated values of the linear and angular velocities of mobile robot in B frame which are computed as

$\begin{array}{c}{\widehat{v}}_{Bx}\left(k\right)={\widehat{V}}_x\left(k\right)\text{cos}\left(\widehat{\psi }\left(k\right)\right)+{\widehat{V}}_y\left(k\right)\text{sin}\left(\widehat{\psi }\left(k\right)\right),\\ \widehat{\dot{\psi }}\left(k\right)=\frac{\widehat{\psi }\left(k\right)-\widehat{\psi }\left(k-\mathrm{1}\right)}{\mathrm{\Delta }T},\end{array}$

(24)

where ${\widehat{V}}_x(k)$ , ${\widehat{V}}_y(k)$ , and $\widehat{\psi }(k)$ are given from the EKFx. The Systems (20) and (22) are used to perform the KFλ in order to estimate the unknown parameters of the left and right sides. From (18), the estimated slip parameters ${\widehat{\lambda }}_L(k)$ and ${\widehat{\lambda }}_R(k)$ at k step are calculated through the estimated unknown parameters ${\widehat{\gamma }}_L(k)$ and ${\widehat{\gamma }}_R(k)$ as follows:

$\begin{array}{c}{\widehat{\lambda }}_L\left(k\right)=\mathrm{1}-\frac{\mathrm{1}}{{\widehat{\gamma }}_L\left(k\right)},\\ {\widehat{\lambda }}_R\left(k\right)=\mathrm{1}-\frac{\mathrm{1}}{{\widehat{\gamma }}_R\left(k\right)}.\end{array}$

(25)

From previous descriptions, the estimated slip values ${\widehat{\lambda }}_L$ and ${\widehat{\lambda }}_R$ are obtained indirectly through the unknown parameters ${\widehat{\gamma }}_L$ and ${\widehat{\gamma }}_R$ . In this method, the current values ${\widehat{v}}_{Bx}(k)$ and $\widehat{\dot{\psi }}(k)$ (in (24)) are calculated by using the current state $\widehat{X}(k)$ and $\widehat{X}(k-\mathrm{1})$ of the EKFx. Similarly, the current estimated slip values in (25) are used to compute the longitudinal velocity v _Bx (k) in (9) and its noise variance value in (10) in order to perform the EKFx. From the previous analyses, in this work, there are two Kalman filters running concurrently; it is called the dual estimation algorithm [19, 20]. As illustrated in Figure 7, the algorithm consists of two kinds of Kalman filters that run concurrently. The top EKFx generates state estimatesand requires $\widehat{\lambda }(k-\mathrm{1})$ for the measurement update. The bottom KFλ generates slip state estimates and requires $\widehat{X}(k-\mathrm{1})$ for the measurement update. The following describes the dual estimation process through mathematical equations [21, 22].

Figure 7:

The dual estimation.

Initialize with

$\begin{array}{c}\widehat{\gamma }\left(\mathrm{0}\right)=E\left[\gamma \left(\mathrm{0}\right)\right],\\ P_{\gamma }\left(\mathrm{0}\right)=E\left[\left(\gamma \left(\mathrm{0}\right)-\widehat{\gamma }\left(\mathrm{0}\right)\right){\left(\gamma \left(\mathrm{0}\right)-\widehat{\gamma }\left(\mathrm{0}\right)\right)}^T\right],\\ \widehat{X}\left(\mathrm{0}\right)=E\left[X\left(\mathrm{0}\right)\right],\\ P\left(\mathrm{0}\right)=E\left[\left(X\left(\mathrm{0}\right)-\widehat{X}\left(\mathrm{0}\right)\right){\left(X\left(\mathrm{0}\right)-\widehat{X}\left(\mathrm{0}\right)\right)}^T\right].\end{array}$

(26)

For k ∊ {1, …, ∞}, the prediction step for KFλ is

$\begin{array}{c}\widehat{\gamma }\left(k/k-\mathrm{1}\right)={\mathrm{\Phi }}_{k-\mathrm{1}}\widehat{\gamma }(k-\mathrm{1})\\ P_{\gamma }\left(k/k-\mathrm{1}\right)={\mathrm{\Phi }}_{k-\mathrm{1}}{P}_{\gamma }\left(k-\mathrm{1}\right){\mathrm{\Phi }}_{k-\mathrm{1}}^T+Q_{\gamma },\end{array}$

(27)

and update step equations for KFλ are

$\begin{array}{c}{H}_{\gamma }\left(k\right)=\left[\begin{array}{cc}\left(\frac{{\widehat{v}}_{Bx}(k-\mathrm{1})}{r}-\frac{L\widehat{\dot{\psi }}(k-\mathrm{1})}{\mathrm{2}r}\mathrm{\hspace{0.17em}\hspace{0.17em}}\right)& \mathrm{0}\\ \mathrm{0}& \left(\frac{{\widehat{v}}_{Bx}(k-\mathrm{1})}{r}+\frac{L\widehat{\dot{\psi }}(k-\mathrm{1})}{\mathrm{2}r}\right)\end{array}\right],\\ K_{\gamma }\left(k\right)=P_{\gamma }\left(k/k-\mathrm{1}\right)H_{\gamma }^T\left(k\right)\phantom{K_{\gamma }\left(k\right)=}\times {\left[H_{\gamma }\left(k\right)P_{\gamma }(k/k-\mathrm{1})H_{\gamma }^T\left(k\right)+R_{\gamma }\left(k\right)\right]}^{-\mathrm{1}}\\ \widehat{\gamma }\left(k\right)=\widehat{\gamma }\left(k/k-\mathrm{1}\right)\phantom{\widehat{\gamma }\left(k\right)=}+K_{\gamma }\left(k\right)\left[z\left(k\right)-H_{\gamma }\left(k\right)\widehat{\gamma }\left(k/k-\mathrm{1}\right)\right],\\ P_{\gamma }\left(k\right)=\left[I-K_{\gamma }\left(k\right)H_{\gamma }\left(k\right)\right]P_{\gamma }\left(k/k-\mathrm{1}\right)\\ {\widehat{\lambda }}_L\left(k\right)=\mathrm{1}-\frac{\mathrm{1}}{{\widehat{\gamma }}_L\left(k\right)}\\ {\widehat{\lambda }}_R\left(k\right)=\mathrm{1}-\frac{\mathrm{1}}{{\widehat{\gamma }}_R\left(k\right)},\end{array}$

(28)

where Q_γ and R_γ(k) are the process and measurement covariance matrices of w_γ(k) and n_γ(k), respectively.

And the prediction step for EKFx is

$\begin{array}{c}\widehat{X}\left(k/k-\mathrm{1}\right)=\widehat{X}\left(k-\mathrm{1}\right)+\mathrm{\Delta }Tg\left(\widehat{X}\left(k-\mathrm{1}\right),u\left(k\right),\mathrm{0}\right),\\ P\left(k/k-\mathrm{1}\right)=F_X\left(k\right)P\left(k-\mathrm{1}\right)F_X^T\left(k\right)+W\left(k\right)Q{W}^T\left(k\right),\end{array}$

(29)

and update step for EKFx is

$\begin{array}{c}{v}_{Bx}\left(k\right)=\frac{r}{\mathrm{2}}[\left(\mathrm{1}-{\widehat{\lambda }}_L\left(k-\mathrm{1}\right)\right){\omega }_L\left(k\right)\phantom{v_{Bx}\left(k\right)=\frac{r}{\mathrm{2}}}\hspace{0.5em}+\left(\mathrm{1}-{\widehat{\lambda }}_R\left(k-\mathrm{1}\right)\right){\omega }_R\left(k\right)]\\ {\sigma }_{vBx}^{\mathrm{2}}=\frac{r^{\mathrm{2}}}{\mathrm{4}}({\left(\mathrm{1}-{\widehat{\lambda }}_R\left(k-\mathrm{1}\right)\right)}^{\mathrm{2}}\text{var}\left({\omega }_R\right)\hspace{0.17em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}+{\left(\mathrm{1}-{\widehat{\lambda }}_L\left(k-\mathrm{1}\right)\right)}^{\mathrm{2}}\text{var}\left({\omega }_L\right)),\\ S\left(k\right)=H_X\left(k\right)P\left(k/k-\mathrm{1}\right)H_X^T\left(k\right)+R\left(k\right),\\ K\left(k\right)=P\left(k/k-\mathrm{1}\right)H_X^T\left(k\right)S^{-\mathrm{1}}\left(k\right),\\ \widehat{X}\left(k\right)=\widehat{X}\left(k/k-\mathrm{1}\right)+K\left(k\right)\left[y\left(k\right)-h\left(\widehat{X}\left(k/k-\mathrm{1}\right),\mathrm{0}\right)\right],\\ P\left(k\right)=P\left(k/k-\mathrm{1}\right)-K\left(k\right)H_X\left(k\right)P\left(k/k-\mathrm{1}\right),\end{array}$

(30)

where

$\begin{array}{c}{F}_X\left(k\right)={\frac{\partial f\left(X,u\left(k\right),\mathrm{0}\right)}{\partial X}|}_{\widehat{X}\left(k-\mathrm{1}\right)},\\ W\left(k\right)={\frac{\partial f}{\partial w}|}_{\left(\widehat{X}(k-\mathrm{1}),u\left(k\right)\right)},\\ H_X\left(k\right)={\frac{\partial h(X,\mathrm{0})}{\partial X}|}_{\widehat{X}(k/k-\mathrm{1})},\end{array}$

(31)

and Q is the process covariance matrix of w.

Update estimation values ${\widehat{v}}_{Bx}(k)$ and $\widehat{\dot{\psi }}(k)$ for KFλ as

$\begin{array}{c}{\widehat{v}}_{Bx}\left(k\right)={\widehat{V}}_x\left(k\right)\text{cos}\left(\widehat{\psi }\left(k\right)\right)+{\widehat{V}}_y\left(k\right)\text{sin}\left(\widehat{\psi }\left(k\right)\right),\\ \widehat{\dot{\psi }}\left(k\right)=\frac{\widehat{\psi }\left(k\right)-\widehat{\psi }\left(k-\mathrm{1}\right)}{\mathrm{\Delta }T}.\end{array}$

(32)

4. Experimental Results

In this section, the experiments are performed to prove the effectiveness of the proposed dual estimation algorithm (DEA) for the robot's path tracking problem in real-time application. The VFO feedback control method is applied for the trajectory tracking control. In fact, the wheel slips, which always occur and vary while the robot is traveling on the different ground surfaces with different trajectory shapes, limit the traction and braking abilities of the robot, especially in 4-TW SSMR. Therefore, a proposed DEA is performed to overcome the previous limitations in this research. The estimated values such as robot's positions, velocities, attitudes and wheel slip parameters are obtained more accurately, which are used for the feedback control signals in the high-level closed-loop control. In this work, the experiments have been carried out on concrete terrain in an outdoor environment, involving two scenarios.

Scenario 1. The mobile robot is controlled to follow a desired trajectory with two curves.

Scenario 2. The mobile robot is controlled to follow a desired circular trajectory with a 2 m radius.

In the first scenario, the output trajectories are shown in Figure 9(a). As illustrated in Figure 9(a), the output response without DEA is quite far to thedesired response while that with DEA is close to the desired response. The linear and angular velocities of two methods (without and with DEA), shown in Figures 8(a) and 8(b), are also close to the desired velocities despite the mechanical vibrations and sensor noises. In fact, although the working environment contains large numbers of nonlinear and uncertain factors, which impact with the system performances, the results point out that the proposed DEA has improved the performance of the path tracking problem. The previous evaluation is proved by considering the position errors as shown in Figure 9(b). In Figure 9(b), the position error with DEA at any instant of time, which is the distance from the estimated position of the robot at the defined instant to the nearest point of the desired trajectory, is the closest to zero compared to that without DEA. Consequently, the root mean square error (RMSE) of robot position with DEA is the smallest (0.0239 [m]), while that without DEA is 0.0845 [m].

Figure 8:

(a) The desired and estimated linear velocities without and with DEA in scenario 1. (b) The desired and estimated angular velocities without and with DEA in scenario 1.

Figure 9:

(a) The desired trajectory, estimated trajectories without and with DEA in scenario 1. (b) The position errors without and with DEA in scenario 1.

In order to evaluate the proposed method, the system is also tested with a circular trajectory, in the second scenario. The output responses without and with DEA are shown in Figure 11(a). It can see that the response with DEA is still the closest to the desired response. The linear and angular velocities are shown in Figures 10(a) and 10(b). Figure 11(b) clearly depicts that the result with DEA is the best while comparing to that without DEA; the position error graph with DEA is still the closest to zero. Hence, the RMSE of robot position with DEA is the smallest (0.0236 [m]), while that without DEA is 0.0882 [m].

Figure 10:

(a) The desired and estimated linear velocities in scenario 2. (b) The desired and estimated angular velocities in scenario 2.

Figure 11:

(a) The desired trajectory and estimated trajectories without and with DEA in scenario 2. (b) The position errors without and with DEA in scenario 2.

In both scenarios, the results show that the trajectories with DEA are the closest to the desired responses. It results from the estimated slip values which are used to compensate the velocity constraints and its variances. The estimated values in EKFx such as velocities and trajectories with DEA are more precise than those without DEA. It means that the feedback control signals with DEA are more accurate, and then the high-level closed-loop control with DEA performs better than the previous testbed without DEA.

From the previous analyses, the robot positioning results with DEA are better than the previous results without DEA. It is clear that the DEA, with wheel slip compensation, is very essential in the mobile robot field. For the first scenario, the robot follows the desired trajectory, which has two curves. At the left rotation in the first curve, the angular speed of the right wheels is greater than that of the left wheels. Then, the traction phenomenon occurs on the right side, while the braking phenomenon appears on the left. Consequently, the slip value of the right wheels is positive, and that of the left wheels is negative. Similarly, at the right rotation in the second curve, the slip value of the right wheels is negative while that of the left side is positive. Furthermore, when the robot travels along the straight segment in the first trajectory, less force is needed to drive the robot and the left and right wheel slips are close to zero. Figure 12(a) illustrates the estimated slip parameters of the left and right wheels in the first scenario.

Figure 12:

The estimated slip parameters of the left and right wheels in scenarios 1 (a) and 2 (b).

For the second scenario, the robot follows the desired circular trajectory with counterclockwise direction. The angular speed of the right wheels is always larger than that of the left wheels. Consequently, the slip value of the right wheels is always positive and that of the left wheels is always negative, as shown in Figure 12 (b). All types of the estimated slip values in both scenarios are consistent. On the whole, the robot positioning results are improved significantly by considering the new dual estimation method. Table 1 shows RMSEs of all experiments in this work; the RMSE values with DEA are reduced more significantly.

Table 1:

RMSEs of all experiments.

Method	RMSE [m]
	Scenario 1	Scenario 2
Without DEA	0.0845	0.0882
With DEA	0.0239	0.0236

5. Conclusion

This paper presents experiments to control the 4-TW SSMR based on the estimated feedback control signals. In the mobile robot field, the wheel slip limits the traction and braking ability of the robot, especially the 4-TW SSMR. The wheel slip always occurs while the robot is traveling on the different ground surfaces with different trajectory shapes. In this paper, we propose the new dual estimation algorithm for robot's positions, velocities, attitudes, and wheel slip parameters based on the Kalman filtering technique in order to obtain more accurate estimation values, which are used for the feedback control signals. In the dual estimation algorithm, two Kalman filters performed simultaneously based on the measurements from AHRS sensor and two incremental encoders. Experimental results show the advantages of the novel method which is effective in the path tracking control problem for a 4-TW SSMR, for example, the commercialized Hazard Escape I mobile robot.

Conflict of Interests

This company does not have any relationship with the name of Hazard Escape 1. So there is no competing interest such as financial gain between two sides.