Fuzzy Vector Field Orientation Feedback Control-Based Slip Compensation for Trajectory Tracking Control of a Four Track Wheel Skid-steered Mobile Robot

Abstract

Skid-steered mobile robots have been widely used in exploring unknown environments and in military applications. In this paper, the tuning fuzzy Vector Field Orientation (FVFO) feedback control method is proposed for a four track wheel skid-steered mobile robot (4-TW SSMR) using flexible fuzzy logic control (FLC). The extended Kalman filter is utilized to estimate the positions, velocities and orientation angles, which are used for feedback control signals in the FVFO method, based on the AHRS kinematic motion model and velocity constraints. In addition, in light of the wheel slip and the braking ability of the robot, we propose a new method for estimating online wheel slip parameters based on a discrete Kalman filter to compensate for the velocity constraints. As demonstrated by our experimental results, the advantages of the combination of the proposed FVFO and wheel slip estimation methods overcome the limitations of the others in the trajectory tracking control problem for a 4-TW SSMR.

1. Introduction

In recent years, many mobile robotic system applications have been developed. Several types of system have been designed and manufactured based on the locomotion systems of robots. With a 4-TW SSMR, there is no steering mechanism and the direction of motion is changed by turning the left and the right side tracked wheels at different velocities. Because of complex wheel-ground interactions and kinematic constraints, the path tracking control of the mobile robot makes it difficult to obtain an accurate experimental result.

In the literature, there are several approaches to solving the path tracking problems of a mobile robot. For example, a combination of kinematic and torque control frameworks using a backstepping technique to join robot kinematics into dynamics allows us to apply approaches from a model-dependent computed torque method (CTM) to a robust sliding mode control (SMC) method [1]. In [2], the adaptive SMC is designed for an uncertain dynamic model with parametric uncertainties associated with the camera system. Another type of controller for an autonomous mobile robot is the stable tracking control method which is proposed based on an error model of the kinematic model [3]. Although there are many interesting features to all these control methods, they are difficult to tune, in contrast to the flexibility of the fuzzy logic control (FLC). The fuzzy logic controller for the path tracking of a wheeled mobile robot is presented in [4]. A fuzzy adaptive tracking control method for wheeled mobile robots is proposed, where unknown slippage occurs and violates the nonholonomic constraint in the form of state-dependent kinematic and dynamic disturbances [5]. In [6], a pseudo-static friction model is used to capture the interaction between the wheels and the ground, and an adaptive control algorithm is designed to simultaneously estimate the wheel/ground contact friction information and control the mobile robot's trajectory as desired.

In previous work, the novel Vector Field Orientation (VFO) feedback control method was developed for a Differentially Driven Vehicle (DDV), a Threecycle Mobile Robot and a 4-WD SSMR in [7–9]. Also in [10], the extended VFO control method for a mobile vehicle was proposed to compensate for skid-slip phenomena.

In a mobile robot's field of operation, wheel slip limits traction and braking ability, especially for a four track-wheel differentially skid-steered mobile robot (4-TWD SSMR). Accurate estimation of slip is essential to obtaining the precise position of a mobile robot operating in unstructured terrain. Several approaches to slip estimation have been developed. For example, using a sliding mode observer (SMO) as a means to estimate slip parameters based on the kinematic model of a skid-steering vehicle is proposed in [11]. In another study, a novel method combining an optical flow algorithm with a sliding mode observer to estimate the slip parameters of a SSMR is introduced [12]. However, in order to validate the proposed method, it needs to be tested on more complex trajectories and longer travelling distances. Reference [13] proposes an experimental slip model for exact kinematic modelling and the parameters of this model are determined based on experimental analysis.

The contribution of this paper is the combination of the proposed FVFO and novel wheel slip compensation methods for the trajectory tracking of a mobile robot. First, the tuning fuzzy Vector Field Orientation (FVFO) feedback control method is proposed for a 4-TW SSMR. While the stable tracking control in [3] and conventional VFO feedback control methods in [7–10] do not perform well over a wide range of operating conditions because of the fixed gains used, the FVFO used the FLC to tune parameters of the VFO method. That is the flexibility of the proposed method, thanks to which we are able to obtain more precise trajectories. Second, in the field of mobile robotics, wheel slip limits the traction and braking ability of the robot; the methods in [11–13] retain these limitations when the robot is travelling on different ground surfaces with different and longer trajectory shapes. In this paper, we propose a novel method for online wheel slip estimation based on a discrete Kalman filter to compensate for the velocity constraints. The experimental results show that the combination of the proposed FVFO and novel wheel slip compensation methods overcomes the limitations of the others in the trajectory tracking control problem. This system can be enhanced by using trajectory tracking control for 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 NT – Hazard escape – 1 mobile robot setup. Section 3 proposes a tuning fuzzy Vector Field Orientation (FVFO) feedback control method for a mobile robot. The implemented extended Kalman filter and slip estimation are designed in Section 4. The experimental results, including analysis and evaluation, are discussed in Section 5. Finally, Section 6 presents the conclusion.

2. The experimental NT-Hazard escape – 1 mobile robot setup

The NT – Hazard escape – 1 mobile robot, with four NT – track wheels, considered in this study, has been applied in a disaster area, has been used to run through and observe rough terrain including a construction site and has run up stairways. A robot with small actuators was created for the first time in Korea, providing the robots with various functions. Each of the eight motors operates and functions independently. The robot can easily lift 2 tracks even when fully loaded. The robot can move whilst maintaining its central balance like a tank. In addition, the RS232C is supported and enabled by an embedded board and computer which operate through wireless telecommunication. The dimensions of 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 is represented by the element component configuration shown in Figure 3, while a real photograph of the experimental system is displayed in Figure 4. Using these figures, the AHRS (AHRS – 03 – 300) was mounted on the top centre of the robot on a base platform. The AHRS specifications are shown in Table 1. Two incremental encoders (E40H-8-1024-3-N-5) were installed on the robot's front left and front right track wheels. A notebook model ASUS U36S was used to collect data from the sensors through an AVR 128-pro board and also performed all the algorithms in real-time. Here, the software control algorithm for the system was coded in the programming language C# (using the Microsoft Visual Studio 2008 version).

Table 1.

The AHRS – 03 – 300 specifications

	Range	Accuracy	Resolution
Attitude	±1800	0.10	0.0020
Angular Rate	±3000/S		0.1460/S
Acceleration	±2G		0.0025G
Heading	±1800	0.10	0.0020

Figure 3.

The system configuration

Figure 4.

The real photograph of the experimental system

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

3. The tuning Fuzzy VFO feedback control method for the path tracking of the mobile robot

A path tracking control method is used to control the trajectory of the experimental robot's motion as desired, based on closed–loop control systems and the sensor fusion technique for feedback control signals.

Figure 5 shows a diagram of the implemented control algorithm. As shown in this figure, the control systems include three closed-loop controls: two low-level closed-loop controls for motor speed on the left and right sides, and a high-level closed-loop control for the robot's position. For low-level closed-loop controls, discrete PID controllers are applied, and the tuning fuzzy VFO (FVFO) feedback control method is designed for high-level closed-loop control. Then, the extended Kalman filter (EKFx) is utilized to estimate the positions, velocities and orientation angles, which are used for feedback control signals in the high-level closed-loop control, based on the AHRS kinematic model and velocity constraints. Additionally, a discrete Kalman filter for the slip estimator (KFλ) is proposed to compensate for the velocity constraints for EKFx in order to improve the accuracy of estimation values. The EKFx and KFλ are implemented simultaneously using measurements from the AHRS sensor and encoders. They are introduced in the next sections.

Figure 5.

The diagram of the implemented control algorithm

3.1. The VFO feedback control method

The stable tracking control (STC) method is introduced in [3–4], based on an error model of the kinematic model. The linear velocity output v _c and the angular velocity output ω _c of the control law are:

$\begin{array}{l}{\mathrm{v}}_{\mathrm{c}}={\mathrm{v}}_{\mathrm{d}}\cos {\mathrm{e}}_{\mathrm{b}\mathrm{\Psi }}+{\mathrm{K}}_{\mathrm{x}}{\mathrm{e}}_{bx}\\ {\omega }_{\mathrm{c}}={\omega }_{\mathrm{d}}+{\mathrm{v}}_{\mathrm{d}}({\mathrm{K}}_{\mathrm{y}}{\mathrm{e}}_{by}+{\mathrm{K}}_{\mathrm{\Psi }}\sin {\mathrm{e}}_{\mathrm{b}\mathrm{\Psi }})\end{array}$

(1)

where ${\mathrm{v}}_{\mathrm{d}}$ , ${\omega }_{\mathrm{d}}$ are the desired linear and angular velocities of the mobile robot; K _x , K _y and ${\mathrm{K}}_{\mathrm{\Psi }}$ are all positive constants; and ${\mathrm{e}}_{bx}$ , ${\mathrm{e}}_{by}$ and ${\mathrm{e}}_{\mathrm{b}\mathrm{\Psi }}$ are three instantaneous error variables associated with the body frame. In this work, the VFO control feedback method is applied to high-level closed-loop control of the robot's position [7–10]. The kinematics modelling of the 4-TW SSMR can be expressed as

$\left[\begin{array}{c}{\stackrel{.}{X}}_{\mathrm{N}}\\ {\stackrel{.}{Y}}_{\mathrm{N}}\\ \stackrel{.}{\mathrm{\Psi }}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\Psi }& 0\\ \sin \mathrm{\Psi }& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{v}}_{\mathrm{B}}\\ \omega \end{array}\right]=\left[\begin{array}{c}\cos \mathrm{\Psi }\\ \sin \mathrm{\Psi }\\ 0\end{array}\right]{\mathrm{v}}_{\mathrm{B}}+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\omega$

(2)

or

$\stackrel{.}{q}={\mathrm{g}}_1{\mathrm{u}}_1+{\mathrm{g}}_2{\mathrm{u}}_2$

(3)

where $\mathrm{q}={[{\mathrm{X}}_{\mathrm{N}},{\mathrm{Y}}_{\mathrm{N}},\mathrm{\Psi }]}^{\mathrm{T}}$ , ${\mathrm{g}}_1\triangleq {\left[\begin{array}{ccc}\cos \mathrm{\Psi }& \sin \mathrm{\Psi }& 0\end{array}\right]}^{\mathrm{T}}$ , ${\mathrm{g}}_2={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^{\mathrm{T}}$ , ${\mathrm{u}}_1={\mathrm{v}}_{\mathrm{B}}$ and ${\mathrm{u}}_2=\omega$ . The VFO approach comes from a simple geometrical interpretation of a possible time evolution of Equation (2) or (3).

In the VFO strategy the trajectory tracking control problem is divided into two subtasks: convergence of position and orientation to the desired values. The vehicle is driven by the pushing controlv _B with a careful pushing strategy, while the orienting controlw is responsible for matching the vehicle's heading vector with the position convergence vector. Such tasks are accomplished with the help of the choice of a proper convergence vector field, which defines the instantaneous convergence direction and orientation of the vehicle. The control position errors ${\mathrm{e}}_{\mathrm{p}}={[{\mathrm{e}}_{\mathrm{x}},{\mathrm{e}}_{\mathrm{y}}]}^{\mathrm{T}}$ are defined as:

${\mathrm{e}}_{\mathrm{p}}=\left[\begin{array}{c}{\mathrm{e}}_{\mathrm{x}}\\ {\mathrm{e}}_{\mathrm{y}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{X}}_{\mathrm{d}}-{\mathrm{X}}_{\mathrm{N}}\\ {\mathrm{Y}}_{\mathrm{d}}-{\mathrm{Y}}_{\mathrm{N}}\end{array}\right]={\mathrm{q}}_{\mathrm{d}}^{*}-{\mathrm{q}}^{*}$

(4)

where ${\mathrm{q}}_{\mathrm{d}}^{*}={[{\mathrm{X}}_{\mathrm{d}},{\mathrm{Y}}_{\mathrm{d}}]}^{\mathrm{T}}$ and ${\mathrm{q}}^{*}\triangleq {[{\mathrm{X}}_{\mathrm{N}},{\mathrm{Y}}_{\mathrm{N}}]}^{\mathrm{T}}$ are the desired and actual positions of the mobile robot. The convergence vector field is defined as $\mathrm{h}\triangleq {[{\mathrm{h}}_1,{\mathrm{h}}_2,{\mathrm{h}}_3]}^{\mathrm{T}}={[{\mathrm{h}}_{\mathrm{p}}^{\mathrm{T}},{\mathrm{h}}_{\mathrm{o}}]}^{\mathrm{T}}\in {ℝ}^3$ where ${\mathrm{h}}_{\mathrm{p}}\in {ℝ}^2$ defines the convergence direction and orientation of the ${\mathrm{q}}^{*}$ sub-state, and ${\mathrm{h}}_{\mathrm{o}}$ defines the convergence orientation of the Ψ variable. For VFO tracking control, the positional component of the convergence vector field is defined as a linear combination of the control position errors and feedforward terms as follows:

${\mathrm{h}}_{\mathrm{p}}=\left[\begin{array}{c}{\mathrm{h}}_1\\ {\mathrm{h}}_2\end{array}\right]={\mathrm{K}}_{\mathrm{p}}{\mathrm{e}}_{\mathrm{p}}+{\stackrel{.}{q}}_{\mathrm{d}}^{*},\begin{array}{cc}& {\mathrm{K}}_{\mathrm{p}}>0\end{array}$

(5)

where ${\mathrm{K}}_{\mathrm{p}}$ is a design parameter, ${\stackrel{.}{q}}_{\mathrm{d}}^{*}$ is the desired velocity vector.

Using kinematics modelling (2), the simultaneous attenuation of both the position and orientation errors during a transient stage requires highly oscillatory vehicle movement, which usually does not occur in practice [10]. To avoid transient oscillations the third component of h can be designed to introduce the so-called auxiliary orienting variable ${\psi }_{\mathrm{a}}=\arg ({\mathrm{h}}_{\mathrm{p}})$ rather than the desired orientation angle ${\mathrm{\Psi }}_{\mathrm{d}}$ . The auxiliary orienting variable ${\psi }_{\mathrm{a}}$ allows the robot to approach a position trajectory ${\mathrm{q}}_{\mathrm{d}}^{*}={[{\mathrm{X}}_{\mathrm{d}},{\mathrm{Y}}_{\mathrm{d}}]}^{\mathrm{T}}$ smoothly, and is described by Equation (6) as:

${\psi }_{\mathrm{a}}=\mathrm{A}\tan 2\mathrm{c}({\mathrm{h}}_2\mathrm{sgn}({\mathrm{v}}_{\mathrm{d}}),{\mathrm{h}}_1\mathrm{sgn}({\mathrm{v}}_{\mathrm{d}}))$

(6)

where $\mathrm{A}\tan 2\mathrm{c}(.,.):ℝ\times ℝ\to ℝ$ is a continuous version of the four-quadrant function $\mathrm{A}\tan 2(.,.):ℝ\times ℝ\to [-\pi ,\pi )$ (refer to the appendix in [7]). The auxiliary orienting variable is now introduced as:

${\mathrm{e}}_{\mathrm{a}}={\psi }_{\mathrm{a}}-\mathrm{\Psi }$

(7)

Since the third component, which is the convergence orientation, is:

${\mathrm{h}}_{\mathrm{o}}={\mathrm{h}}_3={\mathrm{K}}_{\mathrm{a}}{\mathrm{e}}_{\mathrm{a}}+{\stackrel{.}{\psi }}_{\mathrm{a}},\begin{array}{cc}& {\mathrm{K}}_{\mathrm{a}}>0\end{array}$

(8)

where ${\mathrm{K}}_{\mathrm{a}}$ is a design parameter, and

${\stackrel{.}{\psi }}_{\mathrm{a}}=\frac{{\stackrel{.}{h}}_2{\mathrm{h}}_1-{\mathrm{h}}_2{\stackrel{.}{h}}_1}{{\mathrm{h}}_2^2+{\mathrm{h}}_1^2}$

(9)

Using the principles of the VFO method, the VFO feedback control law is:

${\mathrm{u}}_1={\mathrm{g}}_1^{*\mathrm{T}}{\mathrm{h}}_{\mathrm{p}}\Rightarrow {\mathrm{v}}_{\mathrm{c}}={\mathrm{u}}_1={\mathrm{h}}_1\cos \mathrm{\Psi }+{\mathrm{h}}_2\sin \mathrm{\Psi }$

(10)

${\mathrm{u}}_2={\mathrm{h}}_{\mathrm{o}}\Rightarrow {\omega }_{\mathrm{c}}={\mathrm{u}}_2={\mathrm{K}}_{\mathrm{a}}{\mathrm{e}}_{\mathrm{a}}+{\stackrel{.}{\psi }}_{\mathrm{a}}$

(11)

where v _c and ω _c are the linear and angular velocity outputs from the VFO controller. The left and right angular speed inputs $[{\omega }_{\mathrm{L}},{\omega }_{\mathrm{R}}]$ are computed using v _c and ω _c by using the Jacobian matrix as follows:

$\left[\begin{array}{c}{\omega }_{\mathrm{L}}\\ {\omega }_{\mathrm{R}}\end{array}\right]=\Im \left[\begin{array}{c}{\mathrm{v}}_{\mathrm{c}}\\ {\omega }_{\mathrm{c}}\end{array}\right]=\left[\begin{array}{cc}\frac{1}{\mathrm{r}}& \frac{-\mathrm{L}}{2\mathrm{r}}\\ \frac{1}{\mathrm{r}}& \frac{\mathrm{L}}{2\mathrm{r}}\end{array}\right]\left[\begin{array}{c}{\mathrm{v}}_{\mathrm{c}}\\ {\omega }_{\mathrm{c}}\end{array}\right]$

(12)

where r is the radius of the tracked wheel and L is the distance between the left and right sides of the robot.

3.2. The tuning fuzzy VFO feedback control method

From Equations (5) and (8), it can be seen that the position and orienting components of the convergence vector field are similar to a PD control, but the derivative parameter is fixed at one. The ${\mathrm{K}}_{\mathrm{p}}$ and ${\mathrm{K}}_{\mathrm{a}}$ are considered to be proportional parameters. However, the conventional VFO feedback control does not offer reasonable performance over a wide range of operating conditions because of the fixed gains used. That is why fuzzy logic has to be chosen to tune the proportional parameters ${\mathrm{K}}_{\mathrm{p}}$ and ${\mathrm{K}}_{\mathrm{a}}$ of the VFO feedback control automatically. This is called the fuzzy VFO feedback control method (FVFO). Based on the definition of the control position errors ${\mathrm{e}}_{\mathrm{p}}={[{\mathrm{e}}_{\mathrm{x}},{\mathrm{e}}_{\mathrm{y}}]}^{\mathrm{T}}$ , Equation (5) can be rewritten as

${\mathrm{h}}_{\mathrm{p}}=\left[\begin{array}{c}{\mathrm{h}}_1\\ {\mathrm{h}}_2\end{array}\right]=\left[\begin{array}{cc}{\mathrm{K}}_{px}& 0\\ 0& {\mathrm{K}}_{py}\end{array}\right]\left[\begin{array}{c}{\mathrm{e}}_{\mathrm{x}}\\ {\mathrm{e}}_{\mathrm{y}}\end{array}\right]+{\stackrel{.}{q}}_{\mathrm{d}}^{*},\begin{array}{cc}& {\mathrm{K}}_{px},{\mathrm{K}}_{py}>0\end{array}$

(13)

where the ${\mathrm{K}}_{\mathrm{p}}$ in Equation (5) is separated into two components ${\mathrm{K}}_{px},{\mathrm{K}}_{py}$ corresponding to e _x and e _y , respectively. Then the three parameters ${\mathrm{K}}_{px}$ , ${\mathrm{K}}_{py}$ and ${\mathrm{K}}_{\mathrm{a}}$ are tuned by using three fuzzy tuners. The detailed fuzzy VFO scheme is shown in Figure 6.

Figure 6.

The tuning fuzzy VFO scheme of the high-level closed-loop control

From Figure 6 it can be seen that there are three fuzzy tuners for the three output parameters ${\mathrm{K}}_{px}$ , ${\mathrm{K}}_{py}$ and ${\mathrm{K}}_{\mathrm{a}}$ . Two input signals are needed for each fuzzy tuner [14]; the absolute error $\left|\mathrm{e}\right|$ and derivative error $\left|de\right|$ , such as $\left|{\mathrm{e}}_{\mathrm{x}}\right|$ and $\left|\mathrm{d}{\mathrm{e}}_{\mathrm{x}}\right|$ for the ${\mathrm{K}}_{px}$ tuner, $\left|{\mathrm{e}}_{\mathrm{y}}\right|$ and $\left|\mathrm{d}{\mathrm{e}}_{\mathrm{y}}\right|$ for the ${\mathrm{K}}_{py}$ tuner, and $\left|{\mathrm{e}}_{\mathrm{a}}\right|$ and $\left|\mathrm{d}{\mathrm{e}}_{\mathrm{a}}\right|$ for the ${\mathrm{K}}_{\mathrm{a}}$ tuner. The ranges of these inputs are from 0 to 5, which are obtained from the absolute values of the system error and its derivative through the scale factors chosen empirically from experiments. Triangle and trapeze membership functions are then utilized to create the fuzzy input partitions. Here, five membership functions (VS, S, M, B, VB) representing the five input states (very small, small, medium, big, very big), respectively, are used for the controller. Details of the fuzzy inputs' membership functions are shown in Figures 7 and 8 (a = b = 5).

Figure 7.

Membership functions of inputs |e _x |, |e _y |and |e _a |

Figure 8.

Membership functions of inputs |de _x |, |de _y |and |de _a |

There are three outputs from the three fuzzy tuners: k _px , k _py and k _a with the outputs having ranges from 0 to 1. Singleton membership functions are then used for the fuzzy output partitions. Figure 9 shows five membership functions (VS, S, M, B, VB) corresponding with the five output states (very small, small, medium, big, very big), respectively. Because the same properties of the three output parameters ${\mathrm{K}}_{px}$ , ${\mathrm{K}}_{py}$ and ${\mathrm{K}}_{\mathrm{a}}$ are used as proportional parameters, fuzzy rules are composed generally as follows, using the above fuzzy sets of input and output variables:

Figure 9.

Membership functions of the outputs kp _x , kp _y and kp _a

Rule i: If |e _x | (|e _y |or |e _a |) is A _i and |de _x | (|de _y |or |de _a |) is B _i then k _px (k _py or k _a ) is C _i , i = 1, 2,…, n

where n is the number of fuzzy rules; A _i , B _i and C _i are the i ^th fuzzy sets of the input and output variables used in the fuzzy rules. A _i , B _i and C _i are also the linguistic variable values of the input and output signals in the fuzzy tuners.

The rules of the fuzzy tuners are designed as shown in Table 2. The MAX-PROD formula is chosen as the main strategy for the implication process:

${\mu }_{out}^{\mathrm{i}}=\max \left(\mu (\mathrm{e})\cdot \mu (de)\right)\hspace{0.17em}\hspace{0.17em}$

(14)

where $\mu (\mathrm{e})$ , $\mu (de)$ are membership values with respect to input variables; ${\mu }_{out}^{\mathrm{i}}$ is the membership value with respect to the output variable at the i ^th rule. The centroid defuzzification method is used to convert the aggregated fuzzy set to a crisp output value. In this case, because the membership functions for the fuzzy output partitions are in Singleton form, the outputs of fuzzy tuners are calculated as:

${\mathrm{y}}_{out}\hspace{0.17em}=\frac{{\displaystyle \sum _{\mathrm{i}=1}^{25}{\mu }_{out}^{\mathrm{i}}\cdot {\mathrm{y}}_{out}^{\mathrm{i}}}}{{\displaystyle \sum _{\mathrm{i}=1}^{25}{\mu }_{out}^{\mathrm{i}}}}$

(15)

where ${\mathrm{y}}_{out}^{\mathrm{i}}$ is the output value of the i ^th rules which can be determined in Figure 9; the output of the fuzzy tuner ${\mathrm{y}}_{out}$ is k _px , k _py or k _a . Then, these output values of the fuzzy tuners are substituted into Equation (16) to compute the proportional parameters ${\mathrm{K}}_{px}$ , ${\mathrm{K}}_{py}$ and ${\mathrm{K}}_{\mathrm{a}}$ in Equations (8) and (13).

$\begin{array}{l}{\mathrm{K}}_{px}={\mathrm{K}}_{px\min }+{\mathrm{k}}_{px}({\mathrm{K}}_{px\max }-{\mathrm{K}}_{px\min })\\ {\mathrm{K}}_{py}={\mathrm{K}}_{py\min }+{\mathrm{k}}_{py}({\mathrm{K}}_{py\max }-{\mathrm{K}}_{py\min })\\ {\mathrm{K}}_{\mathrm{a}}={\mathrm{K}}_{\mathrm{a}\min }+{\mathrm{k}}_{\mathrm{a}}({\mathrm{K}}_{\mathrm{a}\max }-{\mathrm{K}}_{\mathrm{a}\min })\end{array}$

(16)

where $[{\mathrm{K}}_{px\max },{\mathrm{K}}_{px\min }]$ , $[{\mathrm{K}}_{py\max },{\mathrm{K}}_{py\min }]$ and $[{\mathrm{K}}_{\mathrm{a}\max },{\mathrm{K}}_{\mathrm{a}\min }]$ are the ranges of ${\mathrm{K}}_{px}$ , ${\mathrm{K}}_{py}$ and ${\mathrm{K}}_{\mathrm{a}}$ , respectively. In this paper, these ranges are set from 2 to 4.5and they are determined from experiments.

Table 2.

Rule table of the fuzzy tuners

kpx,kpy,ka		|de|
		VS	S	M	B	VB
|e|	VS	VS	VS	VS	VS	VS
	S	M	M	S	S	S
	M	B	B	M	M	M
	B	VB	VB	M	M	M
	VB	VB	VB	VB	VB	VB

4. Extended Kalman Filter (EKFx) and Slip estimation (KFλ) design

4.1. AHRS kinematic model and velocity constraints

4.1.1. AHRS kinematics model

We define a navigational reference frame N(X,Y,Z) and robot body frame B(x,y,z) as shown in Figure 10. Let P_N(t)=[X_N(t),Y_N(t),Z_N(t)]^T $\in {ℝ}^3$ , V_N(t)=[V_x(t), V_y(t), V_z(t)]^T $\in {ℝ}^3$ and $\mathrm{\Theta }={[\varphi ,\theta ,\psi ]}^{\mathrm{T}}$ $\in {ℝ}^3$ denote the position, the velocity and the attitude angle vectors of AHRS in the N frame, respectively. We also define AHRS acceleration, angular rate measurement, acceleration walking bias and angular rate walking bias in the B frame as a_B = [a_Bx, a_By, a_Bz]^T $\in {ℝ}^3$ , ω_B= [ω_Bx, ω_By, ω_Bz]^T $\in {ℝ}^3$ , b_aB = [b_aBx, b_aBy, b_aBz]^T $\in {ℝ}^3$ and b_gB = [b_gBx, b_gBy, b_gBz]^T $\in {ℝ}^3$ , respectively.

Figure 10.

An AHRS kinematics model of a 4TW SSMR

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

${\mathrm{a}}_{\mathrm{B}}={\stackrel{..}{x}}_{\mathrm{B}}+{\mathrm{b}}_{aB}+{\mathrm{w}}_{accel}$

(17)

${\omega }_{\mathrm{B}}={\mathrm{r}}_{\mathrm{B}}+{\mathrm{b}}_{gB}+{\mathrm{w}}_{gyros}$

(18)

with

${\stackrel{.}{b}}_{aB}=-\frac{1}{{\tau }_{\mathrm{a}}}{\mathrm{b}}_{aB}+\sqrt{\frac{2{\mathrm{f}}_{\mathrm{s}}{\sigma }_{baB}^2}{{\tau }_{\mathrm{a}}}}{\mathrm{w}}_{\text{accbias}}$

(19)

${\stackrel{.}{b}}_{gB}=-\frac{1}{{\tau }_{\mathrm{g}}}{\mathrm{b}}_{gB}+\sqrt{\frac{2{\mathrm{f}}_{\mathrm{s}}{\sigma }_{bgB}^2}{{\tau }_{\mathrm{g}}}}{\mathrm{w}}_{\text{gyrobias}}$

(20)

where the true acceleration vector, the true angular rate vector of the vehicle in the B frame, the acceleration white noise and the angular rate white noise are ${\stackrel{..}{x}}_{\mathrm{B}}={[{\stackrel{..}{x}}_{Bx},{\stackrel{..}{x}}_{By},{\stackrel{..}{x}}_{Bz}]}^{\mathrm{T}}$ , ${\mathrm{r}}_{\mathrm{B}}={\text{[}{\mathrm{r}}_{Bx},{\mathrm{r}}_{By},{\mathrm{r}}_{Bz}\text{]}}^{\mathrm{T}}$ , ${\mathrm{w}}_{accel}$ and ${\mathrm{w}}_{gyros}$ , respectively. And ${\tau }_{\mathrm{a}}$ , ${\sigma }_{baB}^2$ , ${\mathrm{w}}_{\text{accbias}}$ , ${\tau }_{\mathrm{g}}$ , ${\sigma }_{bgB}^2$ and ${\mathrm{w}}_{\text{gyrobias}}$ are time constants, noise variances and zero-mean white noises with $\mathrm{E}[{\mathrm{w}}^2]=1$ of acceleration walking bias and angular rate walking bias and ${\mathrm{f}}_{\mathrm{s}}$ is the sampling frequency.

We define the state vector $\chi \left(\mathrm{t}\right)=\left[{\mathrm{P}}_{\mathrm{N}}\left(\mathrm{t}\right),{\mathrm{V}}_{\mathrm{N}}\left(\mathrm{t}\right),\mathrm{\Theta }\left(\mathrm{t}\right),{\mathrm{b}}_{aB}\left(\mathrm{t}\right),{\mathrm{b}}_{gB}\left(\mathrm{t}\right)\right]\in {ℝ}^{15}$ of the model process. The kinematic motion equation for AHRS can be simplified as:

$\stackrel{.}{\chi }=\left[\begin{array}{c}{\mathrm{V}}_{\mathrm{I}}\\ {\mathrm{C}}_{\mathrm{B}}^{\mathrm{N}}\left({\mathrm{a}}_{\mathrm{B}}-{\mathrm{b}}_{aB}{\text{+w}}_{accel}\right)\\ {\mathrm{q}}_{\mathrm{\Theta }}\left({\omega }_{\mathrm{B}}-{\mathrm{b}}_{gB}{\text{+w}}_{gyros}\right)\\ -\frac{1}{{\tau }_{\mathrm{a}}}{\mathrm{b}}_{aB}\\ -\frac{1}{{\tau }_{\mathrm{g}}}{\mathrm{b}}_{gB}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ \sqrt{\frac{2{\mathrm{f}}_{\mathrm{s}}{\sigma }_{baB}^2}{{\tau }_{\mathrm{a}}}}{\mathrm{w}}_{\text{accbias}}\\ \sqrt{\frac{2{\mathrm{f}}_{\mathrm{s}}{\sigma }_{bgB}^2}{{\tau }_{\mathrm{g}}}}{\mathrm{w}}_{\text{gyrobias}}\end{array}\right]$

(21)

where ${\mathrm{C}}_{\mathrm{B}}^{\mathrm{N}}$ and ${\mathrm{q}}_{\mathrm{\Theta }}$ are the transformation matrix from the B frame to the N frame and transformation matrix of the Euler angles, as given by the following matrices:

${\mathrm{C}}_{\mathrm{B}}^{\mathrm{N}}=\left[\begin{array}{ccc}{\mathrm{c}}_{\theta }{\mathrm{c}}_{\psi }& -{\mathrm{s}}_{\psi }{\mathrm{c}}_{\varphi }+{\mathrm{c}}_{\psi }{\mathrm{s}}_{\varphi }{\mathrm{s}}_{\theta }& {\mathrm{s}}_{\varphi }{\mathrm{s}}_{\psi }+{\mathrm{c}}_{\psi }{\mathrm{s}}_{\theta }{\mathrm{c}}_{\varphi }\\ {\mathrm{c}}_{\theta }{\mathrm{s}}_{\psi }& {\mathrm{c}}_{\varphi }{\mathrm{c}}_{\psi }+{\mathrm{s}}_{\theta }{\mathrm{s}}_{\varphi }{\mathrm{s}}_{\psi }& -{\mathrm{s}}_{\varphi }{\mathrm{c}}_{\psi }+{\mathrm{s}}_{\theta }{\mathrm{c}}_{\varphi }{\mathrm{s}}_{\psi }\\ -{\mathrm{s}}_{\theta }& {\mathrm{c}}_{\theta }{\mathrm{s}}_{\varphi }& {\mathrm{c}}_{\varphi }{\mathrm{c}}_{\theta }\end{array}\right]$

(22)

${\mathrm{q}}_{\mathrm{\Theta }}=\left[\begin{array}{ccc}1& {\mathrm{s}}_{\varphi }{\mathrm{t}}_{\theta }& {\mathrm{c}}_{\varphi }{\mathrm{t}}_{\theta }\\ 0& {\mathrm{c}}_{\theta }& -{\mathrm{s}}_{\theta }\\ 0& {\mathrm{s}}_{\varphi }/{\mathrm{c}}_{\theta }& {\mathrm{c}}_{\varphi }/{\mathrm{c}}_{\theta }\end{array}\right]$

(23)

where ${\mathrm{c}}_{\theta }=\cos \theta ,{\mathrm{s}}_{\theta }=\sin \theta ,{\mathrm{t}}_{\theta }=\tan \theta$ and the same notation convention is used for angles ϕ and ψ.

4.1.2. Velocity constraints

In the 4-TW SSMR, the motors that power the wheels on each side are geared internally to ensure that the velocity of the two adjacent wheels on each side are synchronized and thus have the same velocity at ground contact. We define ${\mathrm{v}}_{\mathrm{L}},{\mathrm{v}}_{\mathrm{R}}$ as the left and right linear velocities of the robot, thus, we have:

$\begin{array}{l}{\mathrm{v}}_{\mathrm{L}}={\mathrm{v}}_{1\mathrm{x}}={\mathrm{v}}_{2\mathrm{x}}\\ {\mathrm{v}}_{\mathrm{R}}={\mathrm{v}}_{3\mathrm{x}}={\mathrm{v}}_{4\mathrm{x}}\end{array}$

(24)

where ${\mathrm{v}}_{1\mathrm{x}}$ , ${\mathrm{v}}_{2\mathrm{x}}$ , ${\mathrm{v}}_{3\mathrm{x}}$ and ${\mathrm{v}}_{4\mathrm{x}}$ are the centre linear velocities for the front-left, rear-left, front-right and rear-right wheels, respectively, as shown in Figure 11. The longitudinal wheel slips of the left and right wheels ${\lambda }_{\mathrm{L}},{\lambda }_{\mathrm{R}}$ are defined as ratios of the wheel velocities and centre velocities [16], as follows

Figure 11.

The 4-TW SSMR kinematics

$\begin{array}{l}{\lambda }_{\mathrm{L}}=\frac{\mathrm{r}{\omega }_{\mathrm{L}}-{\mathrm{v}}_{\mathrm{L}}}{\mathrm{r}{\omega }_{\mathrm{L}}}\\ {\lambda }_{\mathrm{R}}=\frac{\mathrm{r}{\omega }_{\mathrm{R}}-{\mathrm{v}}_{\mathrm{R}}}{\mathrm{r}{\omega }_{\mathrm{R}}}\end{array}$

(25)

where ${\omega }_{\mathrm{L}},{\omega }_{\mathrm{R}}$ are the wheel angular speeds for the left and right sides of the mobile robot. From Equation (25), it can be seen that the wheel slip is $\lambda \in [0,1]$ if the wheel is under traction and $\lambda \in (-\infty ,0]$ when the wheel is braking. Using the definition of the slip in (25), we have

$\begin{array}{l}{\mathrm{v}}_{\mathrm{L}}=(1-{\lambda }_{\mathrm{L}})\mathrm{r}{\omega }_{\mathrm{L}}\\ {\mathrm{v}}_{\mathrm{R}}=(1-{\lambda }_{\mathrm{R}})\mathrm{r}{\omega }_{\mathrm{R}}\end{array}$

(26)

Because of the symmetric mechanical structure of the robot, it can be assumed that the centre of mass (COM) of the robot is located at the centre of the geometry (COG) of the body frame. The AHRS coordinate is located at the COG of the body frame in the B frame, as shown in Figure 10. Using two wheel encoders (one for each side of the vehicle), we obtain the AHRS velocity vector ${\mathrm{v}}_{\mathrm{B}}={[{\mathrm{v}}_{Bx},{\mathrm{v}}_{By},{\mathrm{v}}_{Bz}]}^{\mathrm{T}}$ in the B frame. From (26), using two wheel encoder measurements and estimated slip parameters, we obtain the longitudinal velocity ${\mathrm{v}}_{Bx}$ as:

${\mathrm{v}}_{Bx}=\frac{{\mathrm{v}}_{\mathrm{L}}+{\mathrm{v}}_{\mathrm{R}}}{2}=\frac{\mathrm{r}}{2}\left[(1-{\lambda }_{\mathrm{L}}){\omega }_{\mathrm{L}}+(1-{\lambda }_{\mathrm{R}}){\omega }_{\mathrm{R}}\right]$

(27)

Based on [16], 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 ${\mathrm{v}}_{By}$ , ${\mathrm{v}}_{Bz}$ in the 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 noise of the left and right wheels' angular speeds ω _L and ω _R as measured by the encoders. It is assumed that the noises for ω _L and ω _R have a normal distribution with a zero mean Gaussian and corresponding variances, and there is no cross-correlation between the noise of ω _R and of ω _L [17]. The noise variance of the longitudinal velocity v _Bx can be expressed as:

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

(28)

The variances ${\sigma }_{vBy}^2$ , ${\sigma }_{vBz}^2$ of the lateral velocity ${\mathrm{v}}_{By}$ and the ground surface topography ${\mathrm{v}}_{Bz}$ , respectively, are also expressed by zero. In order to obtain the longitudinal velocity v _Bx and its noise variance, we have to estimate slips ${\lambda }_{\mathrm{L}}$ and ${\lambda }_{\mathrm{R}}$ . A new method to estimate these two slip values is introduced in Section 4.3

4.2. Extended Kalman Filter (EKFx) design

Now, we define the state variable's vector $\mathrm{X}(\mathrm{k})={[{\mathrm{P}}_{\mathrm{N}}(\mathrm{k}),{\mathrm{V}}_{\mathrm{N}}(\mathrm{k}),\mathrm{\Theta }(\mathrm{k}),{\mathrm{b}}_{aB}(\mathrm{k}),{\mathrm{b}}_{gB}(\mathrm{k})]}^{\mathrm{T}}\in {ℝ}^{15}$ . We rewrite the AHRS kinematics equation shown in (21) in discrete-time form, then we obtain the process model as:

$\begin{array}{l}\mathrm{X}(\mathrm{k})=\mathrm{f}(\mathrm{X}(\mathrm{k}-1),\mathrm{u}(\mathrm{k}),\mathrm{w}(\mathrm{k}),\mathrm{\Delta }\mathrm{T})\\ \begin{array}{cc}& \end{array}=\mathrm{X}(\mathrm{k}-1)+\mathrm{\Delta }\mathrm{T}*\mathrm{g}(\mathrm{X}(\mathrm{k}-1),\mathrm{u}(\mathrm{k}),\mathrm{w}(\mathrm{k}))\end{array}$

(29)

where the AHRS input signals at the k ^th sampling time are $\mathrm{u}(\mathrm{k})={[{\mathrm{a}}_{\mathrm{B}}(\mathrm{k}),{\omega }_{\mathrm{B}}(\mathrm{k})]}^{\mathrm{T}}\in {ℝ}^6$ , and the data sampling period is ∆T. Also, the nonlinear function g of the process model is:

$\begin{array}{l}\mathrm{g}(\mathrm{X}(\mathrm{k}-1),\mathrm{u}(\mathrm{k}),\mathrm{w}(\mathrm{k}))=\left[\begin{array}{c}{\mathrm{g}}_{\mathrm{P}}\\ {\mathrm{g}}_{\mathrm{V}}\\ {\mathrm{g}}_{\mathrm{\Theta }}\\ {\mathrm{g}}_{aB}\\ {\mathrm{g}}_{gB}\end{array}\right]\\ \begin{array}{cc}& \end{array}=\left[\begin{array}{c}{\mathrm{V}}_{\mathrm{I}}(\mathrm{k}-1)\\ {\mathrm{C}}_{\mathrm{B}}^{\mathrm{N}}(\mathrm{k}-1)\left({\mathrm{a}}_{\mathrm{B}}(\mathrm{k})-{\mathrm{b}}_{aB}(\mathrm{k}-1)+{\mathrm{w}}_{accel}(\mathrm{k})\right)\\ {\mathrm{q}}_{\mathrm{\Theta }}(\mathrm{k}-1)\left({\omega }_{\mathrm{B}}(\mathrm{k})-{\mathrm{b}}_{gB}(\mathrm{k}-1)+{\mathrm{w}}_{gyros}(\mathrm{k})\right)\\ -\frac{1}{{\tau }_{\mathrm{a}}}{\mathrm{b}}_{aB}(\mathrm{k}-1)\\ -\frac{1}{{\tau }_{\mathrm{g}}}{\mathrm{b}}_{gB}(\mathrm{k}-1)\end{array}\right]\\ \begin{array}{cc}& \end{array}+\left[\begin{array}{c}0\\ 0\\ 0\\ \sqrt{\frac{2{\mathrm{f}}_{\mathrm{s}}{\sigma }_{baB}^2}{{\tau }_{\mathrm{a}}}}{\mathrm{w}}_{\text{accbias}}(\mathrm{k})\\ \sqrt{\frac{2{\mathrm{f}}_{\mathrm{s}}{\sigma }_{bgB}^2}{{\tau }_{\mathrm{g}}}}{\mathrm{w}}_{\text{gyrobias}}(\mathrm{k})\end{array}\right]\end{array}$

(30)

and

$\mathrm{w}(\mathrm{k})={[{\mathrm{w}}_{accle}(\mathrm{k}),{\mathrm{w}}_{gyros}(\mathrm{k}),{\mathrm{w}}_{\text{accbias}}(\mathrm{k}),{\mathrm{w}}_{\text{gyrobias}}(\mathrm{k})]}^{\mathrm{T}}\in {ℝ}^{12}$ is the process noise vector.

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

$\mathrm{y}(\mathrm{k})=\mathrm{h}(\mathrm{X}(\mathrm{k})+\mathrm{n}(\mathrm{k}))={\left({\mathrm{C}}_{\mathrm{B}}^{\mathrm{N}}\right)}^{\mathrm{T}}{\mathrm{V}}_{\mathrm{N}}+\mathrm{n}(\mathrm{k})$

(31)

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

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

(32)

where ${\sigma }_{vBx}^2$ , ${\sigma }_{vBy}^2$ , ${\sigma }_{vBz}^2$ are calculated in Equation (28). The EKFx was implemented by using the systems (29) and (31) in order to obtain the estimated positions, velocities and attitudes of the robot.

4.3. Slip estimation (KFλ) method

In order to determine wheel slip parameters to compensate for the velocity constraints and their variance values in Section 4.1.2, a discrete Kalman filter, denoted by KFλ, is designed. By giving v _Bx in the body frame B and yaw rate $\stackrel{.}{\psi }$ , the wheel centre velocities along the x-axis for the left and right side's wheels are calculated by

$\begin{array}{l}{\mathrm{v}}_{\mathrm{L}}={\mathrm{v}}_{Bx}-\frac{\mathrm{L}}{2}\stackrel{.}{\psi }\\ {\mathrm{v}}_{\mathrm{R}}={\mathrm{v}}_{Bx}+\frac{\mathrm{L}}{2}\stackrel{.}{\psi }\end{array}$

(33)

By substituting (33) into (25), we have:

$\begin{array}{l}{\lambda }_{\mathrm{L}}=\frac{\mathrm{r}{\omega }_{\mathrm{L}}-{\mathrm{v}}_{Bx}+\frac{\mathrm{L}}{2}\stackrel{.}{\psi }}{\mathrm{r}{\omega }_{\mathrm{L}}}\\ {\lambda }_{\mathrm{R}}=\frac{\mathrm{r}{\omega }_{\mathrm{R}}-{\mathrm{v}}_{Bx}-\frac{\mathrm{L}}{2}\stackrel{.}{\psi }}{\mathrm{r}{\omega }_{\mathrm{R}}}\end{array}$

(34)

$\Rightarrow \begin{array}{l}{\omega }_{\mathrm{L}}=\frac{1}{1-{\lambda }_{\mathrm{L}}}\left[\frac{{\mathrm{v}}_{Bx}}{\mathrm{r}}-\frac{\mathrm{L}\stackrel{.}{\psi }}{2\mathrm{r}}\right]\\ {\omega }_{\mathrm{R}}=\frac{1}{1-{\lambda }_{\mathrm{R}}}\left[\frac{{\mathrm{v}}_{Bx}}{\mathrm{r}}+\frac{\mathrm{L}\stackrel{.}{\psi }}{2\mathrm{r}}\right]\end{array}$

(35)

We define the unknown parameters:

${\gamma }_{\mathrm{L}}=\frac{1}{1-{\lambda }_{\mathrm{L}}};{\gamma }_{\mathrm{R}}=\frac{1}{1-{\lambda }_{\mathrm{R}}},\begin{array}{c}\end{array}\lambda \in \left(-\infty ,1\right]\Rightarrow \gamma \in \left[0,+\infty \right)$

(36)

Substituting (36) into (35), the relationship between the angular speed from the encoders and unknown parameters is described as:

$\left[\begin{array}{c}{\omega }_{\mathrm{L}}\\ {\omega }_{\mathrm{R}}\end{array}\right]=\left[\begin{array}{cc}\frac{{\mathrm{v}}_{Bx}}{\mathrm{r}}-\frac{\mathrm{L}\stackrel{.}{\psi }}{2\mathrm{r}}& 0\\ 0& \frac{{\mathrm{v}}_{Bx}}{\mathrm{r}}+\frac{\mathrm{L}\stackrel{.}{\psi }}{2\mathrm{r}}\end{array}\right]\left[\begin{array}{c}{\gamma }_{\mathrm{L}}\\ {\gamma }_{\mathrm{R}}\end{array}\right]$

(37)

The slip estimation based on the discrete Kalman filer (KFλ) is now established. We assume that the slip process is a random process and then the unknown parameter process can be modelled in discrete-time form

$\gamma (\mathrm{k})={\mathrm{\Phi }}_{\mathrm{k}}\gamma (\mathrm{k}-1)+{\mathrm{w}}_{\gamma }(\mathrm{k}-1)$

(38)

where $\gamma (\mathrm{k})={[{\gamma }_{\mathrm{L}}(\mathrm{k}),{\gamma }_{\mathrm{R}}(\mathrm{k})]}^{\mathrm{T}}$ is the process state vector at time k step, ${\mathrm{w}}_{\gamma }(\mathrm{k})={[{\mathrm{w}}_{\gamma 1}(\mathrm{k}),{\mathrm{w}}_{\gamma 2}(\mathrm{k})]}^{\mathrm{T}}$ is a process noise vector, and ${\mathrm{\Phi }}_{\mathrm{k}}$ is the state transition matrix which is defined as:

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

(39)

From Equation (37), a measurement model can be written which relates to the encoder measurement noises as:

$\mathrm{z}(\mathrm{k})={\mathrm{H}}_{\gamma }(\mathrm{k})\gamma (\mathrm{k})+{\mathrm{n}}_{\gamma }(\mathrm{k})$

(40)

where $\mathrm{z}(\mathrm{k})={[{\omega }_{\mathrm{L}}(\mathrm{k}),{\omega }_{\mathrm{R}}(\mathrm{k})]}^{\mathrm{T}}$ is the angular speed vector from two encoders at time step k, ${\mathrm{n}}_{\gamma }(\mathrm{k})={[{\mathrm{n}}_{\gamma 1}(\mathrm{k}),{\mathrm{n}}_{\gamma 2}(\mathrm{k})]}^{\mathrm{T}}$ is the measurement noise vector and ${\mathrm{H}}_{\gamma }(\mathrm{k})$ is the measurement matrix, which is calculated as:

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

(41)

where ${\widehat{v}}_{Bx}(\mathrm{k}),\widehat{\stackrel{.}{\psi }}(\mathrm{k})$ are the estimation values of the linear and angular velocities of mobile robot in the body frame, which are computed as:

${\widehat{v}}_{Bx}(\mathrm{k})={\widehat{V}}_{\mathrm{x}}(\mathrm{k})\cos (\widehat{\psi }(\mathrm{k}))+{\widehat{V}}_{\mathrm{y}}(\mathrm{k})\sin (\widehat{\psi }(\mathrm{k}))$

(42)

$\widehat{\stackrel{.}{\psi }}(\mathrm{k})=\frac{\widehat{\psi }(\mathrm{k})-\widehat{\psi }(\mathrm{k}-1)}{\mathrm{\Delta }\mathrm{T}}$

(43)

where ${\widehat{V}}_{\mathrm{x}}(\mathrm{k})$ , ${\widehat{V}}_{\mathrm{y}}(\mathrm{k})$ and $\widehat{\psi }(\mathrm{k})$ are given by the EKFx in Section 4.2. Systems (38) and (40) are used to perform the KFλ in order to estimate the wheel slip parameters of the left and right sides. From Equation (36), the estimated slip parameters at the k step are calculated using the estimated unknown parameters ${\widehat{\gamma }}_{\mathrm{L}}(\mathrm{k})$ and ${\widehat{\gamma }}_{\mathrm{R}}(\mathrm{k})$ as follows:

$\begin{array}{l}{\widehat{\lambda }}_{\mathrm{L}}(\mathrm{k})=1-\frac{1}{{\widehat{\gamma }}_{\mathrm{L}}(\mathrm{k})}\\ {\widehat{\lambda }}_{\mathrm{R}}(\mathrm{k})=1-\frac{1}{{\widehat{\gamma }}_{\mathrm{R}}(\mathrm{k})}\end{array}$

(44)

Based on previous descriptions, the estimated slip values ${\widehat{\lambda }}_{\mathrm{L}}$ and ${\widehat{\lambda }}_{\mathrm{R}}$ are obtained indirectly using the unknown parameters ${\widehat{\gamma }}_{\mathrm{L}}$ and ${\widehat{\gamma }}_{\mathrm{R}}$ . With this method, the current values ${\widehat{v}}_{Bx}(\mathrm{k})$ $,\widehat{\stackrel{.}{\psi }}(\mathrm{k})$ (in Equations (42) and (43)) are calculated by using the current state $\widehat{X}(\mathrm{k})$ and $\widehat{X}(\mathrm{k}-1)$ of the EKFx in Section 4.2. Similarly, the current estimated slip values in Equation (44) are used to compute the longitudinal velocity ${\mathrm{v}}_{Bx}(\mathrm{k})$ in (27) and its noise variance value in (28) in order to perform the EKFx. After these analyses, there are two Kalman filters running concurrently; this is called a dual estimation algorithm [18–19]. As illustrated in Figure 12, the top EKFx generates state estimates and requires $\widehat{\lambda }(\mathrm{k}-1)$ for the measurement update, the bottom KFλ generates slip state estimates and requires $\widehat{X}(\mathrm{k}-1)$ for the measurement update. Table 3 describes the dual estimation process using mathematical equations [20–21].

Figure 12.

The dual estimation algorithm

Table 3.

The dual estimation equations

○Initialize with: $\begin{array}{l}\widehat{\gamma }(0)=\mathrm{E}[\gamma (0)],{\mathrm{P}}_{\gamma }(0)=\mathrm{E}[(\gamma (0)-\widehat{\gamma }(0)){(\gamma (0)-\widehat{\gamma }(0))}^{\mathrm{T}}]\hfill \\ \widehat{\mathrm{X}}(0)=\mathrm{E}[\mathrm{X}(0)],\mathrm{P}(0)=\mathrm{E}[(\mathrm{X}(0)-\widehat{\mathrm{X}}(0)){(\mathrm{X}(0)-\widehat{\mathrm{X}}(0))}^{\mathrm{T}}]\hfill \end{array}$

○For k ϵ {1,…,∞}, the prediction step for KFλ is $\begin{array}{c}\widehat{\gamma }(\mathrm{k}/\mathrm{k}-1)={\mathrm{\Phi }}_{\mathrm{k}-1}\widehat{\gamma }(\mathrm{k}-1)\\ {\mathrm{P}}_{\gamma }(\mathrm{k}/\mathrm{k}-1)={\mathrm{\Phi }}_{\mathrm{k}-1}{\mathrm{P}}_{\gamma }(\mathrm{k}-1){\mathrm{\Phi }}_{\mathrm{k}-1}^{\mathrm{T}}+{\mathrm{Q}}_{\gamma }\end{array}$ and the update step for KFλ is $\begin{array}{c}{\mathrm{H}}_{\gamma }(\mathrm{k})=\left[\begin{array}{cc}\left(\frac{{\widehat{\mathrm{v}}}_{\text{Bx}}(\mathrm{k}-1)}{\mathrm{r}}-\frac{\mathrm{L}\widehat{\psi }(\mathrm{k}-1)}{2\mathrm{r}}\right)& 0\\ 0& \left(\frac{{\widehat{\mathrm{v}}}_{\text{Bx}}(\mathrm{k}-1)}{\mathrm{r}}+\frac{\mathrm{L}\widehat{\psi }(\mathrm{k}-1)}{2\mathrm{r}}\right)\end{array}\right]\\ {\mathrm{K}}_{\gamma }(\mathrm{k})={\mathrm{P}}_{\gamma }(\mathrm{k}/\mathrm{k}-1){\mathrm{H}}_{\gamma }^{\mathrm{T}}(\mathrm{k}){\left[{\mathrm{H}}_{\gamma }(\mathrm{k}){\mathrm{P}}_{\gamma }(\mathrm{k}/\mathrm{k}-1){\mathrm{H}}_{\gamma }^{\mathrm{T}}(\mathrm{k})+{\mathrm{R}}_{\gamma }(\mathrm{k})\right]}^{-1}\\ \widehat{\gamma }(\mathrm{k})=\widehat{\gamma }{(\mathrm{k}/\mathrm{k}-1)+\mathrm{K}}_{\gamma }(\mathrm{k})[\mathrm{z}(\mathrm{k})-{\mathrm{H}}_{\gamma }(\mathrm{k})\widehat{\gamma }(\mathrm{k}/\mathrm{k}-1)]\\ P_{\gamma }(k)=\left[\mathrm{I}-{\mathrm{K}}_{\gamma }(\mathrm{k}){\mathrm{H}}_{\gamma }(\mathrm{k})\right]{\mathrm{P}}_{\gamma }(\mathrm{k}/\mathrm{k}-1)\\ {\widehat{\lambda }}_{\mathrm{L}}(\mathrm{k})=1-\frac{1}{{\widehat{\gamma }}_{\mathrm{L}}(\mathrm{k})},{\widehat{\lambda }}_{\mathrm{R}}(\mathrm{k})=1-\frac{1}{{\widehat{\gamma }}_{\mathrm{R}}(\mathrm{k})}\end{array}$ where Q y and R y (k) are the process and measurement covariance matrices of w y (k) and n y (k), respectively.

○The prediction step for EKFx is $\begin{array}{c}\widehat{\mathrm{X}}(\mathrm{k}/\mathrm{k}-1)=\widehat{\mathrm{X}}(\mathrm{k}-1)+\Delta \text{Tg}(\widehat{\mathrm{X}}(\mathrm{k}-1),\mathrm{u}(\mathrm{k}),0)\\ \mathrm{P}(\mathrm{k}/\mathrm{k}-1)={\mathrm{F}}_{\mathrm{X}}(\mathrm{k})\mathrm{P}(\mathrm{k}-1){\mathrm{F}}_{\mathrm{X}}^{\mathrm{T}}(\mathrm{k})+\mathrm{W}(\mathrm{k}){\text{QW}}^{\mathrm{T}}(\mathrm{k})\end{array}$ and the update step for EKFx is $\begin{array}{c}{\mathrm{v}}_{\text{Bx}}(\mathrm{k})=\frac{\mathrm{r}}{2}\left[(1-{\widehat{\mathit{\lambda }}}_{\mathrm{L}}){\omega }_{\mathrm{L}}(\mathrm{k})+(1-{\widehat{\mathit{\lambda }}}_{\mathrm{R}}){\omega }_{\mathrm{R}}(\mathrm{k})\right]\\ {\sigma }_{\text{vBx}}^2=\frac{{\mathrm{r}}^2}{4}\left({(1-{\widehat{\mathit{\lambda }}}_{\mathrm{R}}(\mathrm{k}))}^{2}var({\omega }_{\mathrm{R}})+{(1-{\widehat{\mathit{\lambda }}}_{\mathrm{L}}(\mathrm{k}))}^{2}var({\omega }_{\mathrm{L}})\right)\\ {\mathrm{y}(\mathrm{k})=[\mathrm{v}}_{\text{Bx}}{(\mathrm{k}),\mathrm{v}}_{\text{By}}{(\mathrm{k}),\mathrm{v}}_{\text{Bz}}{(\mathrm{k})]}^{\mathrm{T}}\in {ℝ}^3\\ \mathrm{R}(\mathrm{k})=\text{diag}({\sigma }_{\text{vBx}}^2,{\sigma }_{\text{vBy}}^2,{\sigma }_{\text{vBz}}^2)\in {ℝ}^{3\times 3}\\ {\mathrm{S}(\mathrm{k})=\mathrm{H}}_{\mathrm{X}}(\mathrm{k})\mathrm{P}(\mathrm{k}/\mathrm{k}-1){\mathrm{H}}_{\mathrm{X}}^{\mathrm{T}}(\mathrm{k})+\mathrm{R}(\mathrm{k})\\ {\mathrm{K}(\mathrm{k})=\mathrm{P}(\mathrm{k}/\mathrm{k}-1)\mathrm{H}}_{\mathrm{X}}^{\mathrm{T}}(\mathrm{k}){\mathrm{S}}^{-1}(\mathrm{k})\\ \widehat{\mathrm{X}}(\mathrm{k})=\widehat{\mathrm{X}}(\mathrm{k}/\mathrm{k}-1)+\mathrm{K}(\mathrm{k})[\mathrm{y}(\mathrm{k})-\mathrm{h}(\widehat{\mathrm{X}}(\mathrm{k}/\mathrm{k}-1),0)]\\ {\mathrm{P}(\mathrm{k})=\mathrm{P}(\mathrm{k}/\mathrm{k}-1)-\mathrm{K}(\mathrm{k})\mathrm{H}}_{\mathrm{X}}(\mathrm{k})\mathrm{P}(\mathrm{k}/\mathrm{k}-1)\end{array}$ where $\begin{array}{c}{\mathrm{F}}_{\mathrm{X}}(\mathrm{k})=\frac{\partial \mathrm{f}(\mathrm{X},\mathrm{u}(\mathrm{k}),0)}{\partial \mathrm{X}}{|}_{\widehat{\mathrm{X}}(\mathrm{k}-1)},\mathrm{W}(\mathrm{k})=\frac{\partial \mathrm{f}}{\partial \mathrm{w}}{|}_{(\widehat{\mathrm{X}}(\mathrm{k}-1),\mathrm{u}(\mathrm{k}))}\prime \\ {\mathrm{H}}_{\mathrm{X}}(\mathrm{k})=\frac{\partial \mathrm{h}(\mathrm{X},0)}{\partial \mathrm{X}}{|}_{\widehat{\mathrm{X}}(\mathrm{k}/\mathrm{k}-1)},\end{array}$

Q is the process covariance and the detailed calculations of FX(k), W(k) and HX(k) are described in the Appendix.

○Updating estimation values ${\widehat{\mathrm{v}}}_{\text{Bx}}(\mathrm{k}),\widehat{\psi }(\mathrm{k})$ (k) for KFλ as $\begin{array}{c}{\widehat{\mathrm{v}}}_{\text{Bx}}(\mathrm{k})={\widehat{\mathrm{V}}}_{\mathrm{X}}(\mathrm{k})cos(\widehat{\psi }(\mathrm{k}))+{\widehat{\mathrm{V}}}_{\mathrm{y}}(\mathrm{k})sin(\widehat{\psi }(\mathrm{k}))\\ \widehat{\psi }(\mathrm{k})=\frac{\widehat{\psi }(\mathrm{k})-\widehat{\psi }(\mathrm{k}-1)}{\Delta \mathrm{T}}\end{array}$

5. Experimental results

In this section, experiments were performed to prove the effectiveness of the combination of the proposed FVFO and slip compensation methods for the robot's position in a real-time application. The system was performed on concrete terrain in an outdoor environment. The experiments involve two scenarios:

• Scenario 1: The mobile robot is controlled along a desired trajectory with two curves in two cases: without and with slip compensation.

• Scenario 2: The mobile robot is controlled along a desired circular trajectory with a 2-m radius in two cases: without and with slip compensation.

5.1. The use of the FVFO method for mobile robot without slip compensation

In this section, the proposed FVFO method is performed in two scenarios without slip compensation. Additionally, the stable tracking control method (STC) and the conventional VFO feedback control were applied in order to compare them with the FVFO results.

In the first scenario, the output trajectories of the three methods are shown in Figure 13 are and compared to the desired trajectory. The response of STC is quite different from the desired response, while those of VFO and FVFO are close to the desired response. The linear and angular velocities of the three methods, shown in Figures 14 and 15, are also close to the desired velocities despite the mechanical vibrations and sensor noises.

Figure 13.

The desired, STC, VFO and FVFO trajectories, respectively, without slip compensation in scenario 1

Figure 14.

The desired, STC, VFO and FVFO linear velocities, respectively, without slip compensation in scenario 1

Figure 15.

The desired, STC, VFO, FVFO angular velocities, respectively, without slip compensation in scenario 1

In Figure 16, the position error of FVFO, which is the distance from the estimated position of the robot at the defined instant to the nearest point of the desired trajectory, at any instance in time is the closest to zero compared to those of the STC and conventional VFO. The results suggest that the proposed FVFO has improved the performance of the solving of the path tracking problem. Consequently, the Root Mean Square Error (RMSE) of the robot's position of FVFO is the smallest (0.0494[m]), while those of the STC and VFO are 0.0994[m] and 0.0845[m], respectively.

Figure 16.

The position errors of STC, VFO and FVFO, respectively, without slip compensation in scenario 1

The trajectory responses of the three methods in the second scenario are shown in Figure 17. The linear angular velocities and position errors of the three methods are shown in Figures 18, 19 and 20. It is clear that the FVFO result is the best (with the smallest RMSE (0.0688 [m])).

Figure 17.

The desired, STC, VFO and FVFO trajectories, respectively, without slip compensation in scenario 2

Figure 18.

The desired, STC, VFO and FVFO linear velocities, respectively, without slip compensation in scenario 2

Figure 19.

The desired, STC, VFO, FVFO angular velocities, respectively, without slip compensation in scenario 1

Figure 20.

The position errors of STC, VFO and FVFO, respectively, without slip compensation in scenario 2

5.2. The use of the FVFO method for a mobile robot with slip compensation

In Section 5.1, although the FVFO method reduced more position errors than the STC and VFO method, the system error is still significant. In the field of mobile robotics, wheel slippage limits the traction and braking ability of the robot, and causes large position errors in path tracking control. In this section, the system is performed again with slip compensation.

The STC, VFO and FVFO trajectories with slip compensation in both scenarios are shown in Figures 21 and 24. The FVFO responses with slip compensation are closest to the desired responses. Based on the slip estimation, the estimated values from EKFx are more precise than the previous values without slip compensation. By considering position errors, it is clear that the performance results with slip compensation are more accurate than previous results without slip, as shown in Figures 22 and 25. Consequently, the smallest RMSEs of the proposed method are 0.0124[m] and 0.0121[m] corresponding to the first and second scenarios.

Figure 21.

The desired, STC, VFO and FVFO trajectories, respectively, with slip compensation in scenario 1

Figure 22.

The position errors of STC, VFO and FVFO, respectively, without and with slip compensation in scenario 1

Figure 23.

The estimated slip parameters of the left and right wheels of the STC, VFO and FVFO methods, respectively, in scenario 1

Figure 24.

The desired, STC, VFO and FVFO trajectories, respectively, with slip compensation in scenario 2

Figure 25.

The position errors of STC, VFO and FVFO, respectively, without and with slip compensation in scenario 2

It is clear from the above results that slip compensation is very effective in the field of mobile robotics. For the first scenario, the robot follows the desired trajectory, which has two curves. For 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 right wheel slip value is positive and the left wheel slip value is negative. Similarly, for the right rotation in the second curve, the right wheel slip value is negative and the left wheel slip value is positive. Furthermore, when the robot travels along the straight segment, less force is needed to drive the robot and the left and right wheel slips are close to zero. Figure 23 illustrates the estimated slip parameters of the left and right wheels for the three methods.

For the second scenario, the robot follows the desired circular trajectory counterclockwise. The angular speed of the right wheels is always larger than that of the left wheels. Consequently, the right wheel slip value is always positive and the left wheel slip value is always negative, as shown in Figure 26. All types of the estimated slip parameters in both scenarios are consistent. Tables 4 and 5 show RMSEs of all the experiments in this work.

Figure 26.

The estimated slip parameters of the left and right wheels of the STC, VFO and FVFO methods, respectively, in scenario 2

Table 4.

The Root Mean Square Errors (RMSEs) of the robot position in scenario 1

RMSE [m]	STC	VFO	FVFO
Without slip	0.0994	0.0845	0.0494
With slip	0.0688	0.0239	0.0124

Table 5.

The Root Mean Square Errors (RMSEs) of the robot position in scenario 2

RMSE [m]	STC	VFO	FVFO
Without slip	0.1116	0.0882	0.0688
With slip	0.0818	0.0236	0.0121

6. Conclusions

This paper presents experiments into a method of controlling the 4-TW SSMR along a desired trajectory based on estimated feedback control signals from an extended Kalman Filter, which uses measurements from an AHRS and two incremental encoders. In this paper, we proposed a combination of the proposed FVFO and novel wheel slip compensation methods for the trajectory tracking of a mobile robot. The extended Kalman filter is utilized to estimate the positions, velocities and orientation angles, which are used for feedback control signals in the FVFO method, based on an AHRS kinematic motion model and velocity constraints. Experimental results show the advantages of the combination of these two methods, which are effective at overcoming the limitations of the others in the trajectory tracking control problem for a 4-TW SSMR, e.g. the commercialized Hazard Escape I^® mobile robot.

Footnotes

7. Acknowledgments

This work was supported by the 2011 Research Fund of University of Ulsan.

Appendix