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

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.

OPEN IN VIEWER

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).

OPEN IN VIEWER

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.

OPEN IN VIEWER

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.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 ∊ ℝ 3 , V N ( t ) = [ V x ( t ) , V y ( t ) , V z ( t ) ] T ∊ ℝ 3 , and Θ ( t ) = [ ϕ ( t ) , θ ( t ) , ψ ( t ) ] T ∊ ℝ 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 B x , a B y , a B z ] T ∊ ℝ 3 , ω B = [ ω B x , ω B y , ω B z ] T ∊ ℝ 3 , b a B = [ b a B x , b a B y , b a B z ] T ∊ ℝ 3 , and b g B = [ b g B x , b g B y , b g B z ] T ∊ ℝ 3 , respectively.

OPEN IN VIEWER

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]

a B = x ‥ B + b a B + w accel , ω B = r B + b g B + w gyros ,

(1)

with

b ˙ a B = - 1 τ a b a B + 2 f s σ b a B 2 τ a w accbias , b ˙ g B = - 1 τ g b g B + 2 f s σ b g B 2 τ g w gyrobias ,

(2)

where the true acceleration vector, true angular rate vector, acceleration white noise, and angular rate white noise are x ‥ B = [ x ‥ B x , x ‥ B y , x ‥ B z ] T , r B = [ r B x , r B y , r B z ] 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 χ ( t ) = [ P N ( t ) , V N ( t ) , Θ ( t ) , b a B ( t ) , b g B ( t ) ] ∊ ℝ 15 of model process. The kinematic motion equation for AHRS can be simplified to:

χ ˙ = [ V I C B N ( a B - b a B + w accel ) q Θ ( ω B - b g B + w gyros ) - 1 τ a b a B - 1 τ g b g B ] + [ 0 0 0 2 f s σ b a B 2 τ a w accbias 2 f s σ b g B 2 τ g w gyrobias ] ,

(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 = [ c θ c ψ - s ψ c ϕ + c ψ s ϕ s θ s ϕ s ψ + c ψ s θ c ϕ c θ s ψ c ϕ c ψ + s θ s ϕ s ψ - s ϕ c ψ + s θ c ϕ s ψ - s θ c θ s ϕ c ϕ c θ ] ,

(4)

q Θ = [ 1 s ϕ t θ c ϕ t θ 0 c θ - s θ 0 s ϕ c θ c ϕ c θ ] ,

(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

v L = v 1 x = v 2 x , v R = v 3 x = v 4 x ,

(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:

λ L = r ω L - v L r ω L , λ R = r ω R - v R r ω R ,

(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

v L = ( 1 - λ L ) r ω L , v R = ( 1 - λ R ) r ω R .

(8)

OPEN IN VIEWER

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 B x , v B y , v B z ] 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 B x = r 2 [ ( 1 - λ L ) ω L + ( 1 - λ R ) ω R ] .

(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

σ v B x 2 = r 2 4 ( ( 1 - λ R ) 2 var ( ω R ) + ( 1 - λ L ) 2 var ( ω L ) ) .

(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 ) , Θ ( k ) , b a B ( k ) , b g B ( k ) ] T ∊ ℝ 15 . We rewrite the AHRS kinematics equation shown in (3) in discrete-time form, and we obtain the process model as

X ( k ) = f ( X ( k - 1 ) , u ( k ) , w ( k ) , Δ T ) = X ( k - 1 ) + Δ T * g ( X ( k - 1 ) , u ( k ) , w ( k ) ) ,

(11)

where the AHRS input signals at the kth sampling time is u ( k ) = [ a B ( k ) , ω B ( k ) ] T ∊ ℝ 6 and the data sampling period is ΔT. Also, the nonlinear function g of the process model is

g ( X ( k - 1 ) , u ( k ) , w ( k ) ) = [ g P g V g Θ g a B g g B ] = [ V I ( k - 1 ) C B N ( k - 1 ) ( a B ( k ) - b a B ( k - 1 ) + w accel ( k ) ) q Θ ( k - 1 ) ( ω B ( k ) - b g B ( k - 1 ) + w gyros ( k ) ) - 1 τ a b a B ( k - 1 ) - 1 τ g b g B ( k - 1 ) ] + [ 0 0 0 2 f s σ b a B 2 τ a w accbias ( k ) 2 f s σ b g B 2 τ g w gyrobias ( k ) ] ,

(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 B x ( k ) , v B y ( k ) , v B z ( k ) ] T ∊ ℝ 3 . Including the wheel encoder measurement noise and ground topography, the measurement model is rewritten in discrete-time form as

y ( k ) = h ( X ( k ) + n ( k ) ) = ( C B N ) T V N + n ( k ) ,

(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 ( k ) = diag ( σ v B x 2 , σ v B y 2 , σ v B z 2 ) ∊ ℝ 3 × 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 ψ ˙ , the wheel center velocities along x-axis for the left and right sides wheels are calculated by

v L = v B x - L 2 ψ ˙ , v R = v B x + L 2 ψ ˙ ,

(15)

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

λ L = r ω L - v B x + ( L / 2 ) ψ ˙ r ω L , λ R = r ω R - v B x - ( L / 2 ) ψ ˙ r ω R ,

(16)

⟹ ω L = 1 1 - λ L [ v B x r - L ψ ˙ 2 r ] , ω R = 1 1 - λ R [ v B x r + L ψ ˙ 2 r ] .

(17)

We define unknown parameters as:

γ L = 1 1 - λ L ; γ R = 1 1 - λ R , λ ∊ ( - ∞ , 1 ] ⟹ γ ∊ [ 0 , + ∞ ) .

(18)

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

[ ω L ω R ] = [ v B x r - L ψ ˙ 2 r 0 0 v B x r + L ψ ˙ 2 r ] [ γ L γ R ] .

(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

γ ( k ) = Φ k γ ( k - 1 ) + w γ ( k - 1 ) ,

(20)

where γ ( k ) = [ γ L ( k ) , γ R ( k ) ] T is the process state vector of the KFλ at time k step, w γ ( k ) = [ w γ 1 ( k ) , w γ 2 ( k ) ] T is process noise vector, and Φ _k is the state transition matrix which is defined as:

Φ k = [ 1 0 0 1 ] .

(21)

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

z ( k ) = H γ ( k ) γ ( k ) + n γ ( k ) ,

(22)

where z ( k ) = [ ω L ( k ) , ω R ( k ) ] T is angular speed vector from two encoders at time k step, n γ ( k ) = [ n γ 1 ( k ) , n γ 2 ( k ) ] T is measurement noise vector, and H_γ(k) is the measurement matrix which is calculated as follows:

H γ ( k ) = [ ( v ^ B x ( k ) r - L ψ ˙ ^ ( k ) 2 r ) 0 0 ( v ^ B x ( k ) r + L ψ ˙ ^ ( k ) 2 r ) ] ,

(23)

where v ^ B x ( k ) and ψ ˙ ^ ( k ) are the estimated values of the linear and angular velocities of mobile robot in B frame which are computed as

v ^ B x ( k ) = V ^ x ( k ) cos ( ψ ^ ( k ) ) + V ^ y ( k ) sin ( ψ ^ ( k ) ) , ψ ˙ ^ ( k ) = ψ ^ ( k ) - ψ ^ ( k - 1 ) Δ T ,

(24)

where V ^ x ( k ) , V ^ y ( k ) , and ψ ^ ( 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 λ ^ L ( k ) and λ ^ R ( k ) at k step are calculated through the estimated unknown parameters γ ^ L ( k ) and γ ^ R ( k ) as follows:

λ ^ L ( k ) = 1 - 1 γ ^ L ( k ) , λ ^ R ( k ) = 1 - 1 γ ^ R ( k ) .

(25)

From previous descriptions, the estimated slip values λ ^ L and λ ^ R are obtained indirectly through the unknown parameters γ ^ L and γ ^ R . In this method, the current values v ^ B x ( k ) and ψ ˙ ^ ( k ) (in (24)) are calculated by using the current state X ^ ( k ) and X ^ ( k - 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 λ ^ ( k - 1 ) for the measurement update. The bottom KFλ generates slip state estimates and requires X ^ ( k - 1 ) for the measurement update. The following describes the dual estimation process through mathematical equations [21, 22].

OPEN IN VIEWER

Figure 7:

The dual estimation.

Initialize with

γ ^ ( 0 ) = E [ γ ( 0 ) ] , P γ ( 0 ) = E [ ( γ ( 0 ) - γ ^ ( 0 ) ) ( γ ( 0 ) - γ ^ ( 0 ) ) T ] , X ^ ( 0 ) = E [ X ( 0 ) ] , P ( 0 ) = E [ ( X ( 0 ) - X ^ ( 0 ) ) ( X ( 0 ) - X ^ ( 0 ) ) T ] .

(26)

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

γ ^ ( k / k - 1 ) = Φ k - 1 γ ^ ( k - 1 ) P γ ( k / k - 1 ) = Φ k - 1 P γ ( k - 1 ) Φ k - 1 T + Q γ ,

(27)

and update step equations for KFλ are

H γ ( k ) = [ ( v ^ B x ( k - 1 ) r - L ψ ˙ ^ ( k - 1 ) 2 r ) 0 0 ( v ^ B x ( k - 1 ) r + L ψ ˙ ^ ( k - 1 ) 2 r ) ] , K γ ( k ) = P γ ( k / k - 1 ) H γ T ( k ) K γ ( k ) = × [ H γ ( k ) P γ ( k / k - 1 ) H γ T ( k ) + R γ ( k ) ] - 1 γ ^ ( k ) = γ ^ ( k / k - 1 ) γ ^ ( k ) = + K γ ( k ) [ z ( k ) - H γ ( k ) γ ^ ( k / k - 1 ) ] , P γ ( k ) = [ I - K γ ( k ) H γ ( k ) ] P γ ( k / k - 1 ) λ ^ L ( k ) = 1 - 1 γ ^ L ( k ) λ ^ R ( k ) = 1 - 1 γ ^ R ( k ) ,

(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

X ^ ( k / k - 1 ) = X ^ ( k - 1 ) + Δ T g ( X ^ ( k - 1 ) , u ( k ) , 0 ) , P ( k / k - 1 ) = F X ( k ) P ( k - 1 ) F X T ( k ) + W ( k ) Q W T ( k ) ,

(29)

and update step for EKFx is

v B x ( k ) = r 2 [ ( 1 - λ ^ L ( k - 1 ) ) ω L ( k ) v B x ( k ) = r 2 + ( 1 - λ ^ R ( k - 1 ) ) ω R ( k ) ] σ v B x 2 = r 2 4 ( ( 1 - λ ^ R ( k - 1 ) ) 2 var ( ω R ) + ( 1 - λ ^ L ( k - 1 ) ) 2 var ( ω L ) ) , S ( k ) = H X ( k ) P ( k / k - 1 ) H X T ( k ) + R ( k ) , K ( k ) = P ( k / k - 1 ) H X T ( k ) S - 1 ( k ) , X ^ ( k ) = X ^ ( k / k - 1 ) + K ( k ) [ y ( k ) - h ( X ^ ( k / k - 1 ) , 0 ) ] , P ( k ) = P ( k / k - 1 ) - K ( k ) H X ( k ) P ( k / k - 1 ) ,

(30)

where

F X ( k ) = ∂ f ( X , u ( k ) , 0 ) ∂ X | X ^ ( k - 1 ) , W ( k ) = ∂ f ∂ w | ( X ^ ( k - 1 ) , u ( k ) ) , H X ( k ) = ∂ h ( X , 0 ) ∂ X | X ^ ( k / k - 1 ) ,

(31)

and Q is the process covariance matrix of w.

Update estimation values v ^ B x ( k ) and ψ ˙ ^ ( k ) for KFλ as

v ^ B x ( k ) = V ^ x ( k ) cos ( ψ ^ ( k ) ) + V ^ y ( k ) sin ( ψ ^ ( k ) ) , ψ ˙ ^ ( k ) = ψ ^ ( k ) - ψ ^ ( k - 1 ) Δ T .

(32)