Optimal tracking control of a trajectory planned via fuzzy reactive approach for an autonomous mobile robot

Abstract

This article deals with the design of an optimal tracking controller for a wheeled mobile robot. The tracking control can be performed to track either a given or a planned trajectory. In our study, an improved linear quadratic tracker is adopted to track a path planned using an improved reactive approach that combines the dynamic window with the fuzzy logic to make the robot movement toward the target faster, smoother, and safer whatever the complexity of the environment. The fuzzy logic is used to dynamically adjust the weights of the terms included in the dynamic window objective function according to different environmental scenarios. Simulation results of the path planning and the tracking control prove that the proposed approaches are significantly superior to the conventional ones.

Introduction

In recent years, a lot of researches have been published about the path planning and the tracking control due to their importance in the field of mobile robotics. The majority of the authors have dealt with these two tasks separately; in this article, we try to achieve the continuity between them.

There have been many path planning techniques proposed in the literature; we have chosen the reactive method known as the dynamic window approach (DWA) ¹ because it takes the mobile robot dynamics into account.

The DWA was proposed by Fox in 1996 to be used in high-speed navigation by searching for control commands directly in the space of velocities. Brock and Khatib ² extended the original DW to a global DWA by using connectivity information about the free space to avoid the local minima. Arras et al. ³ reduced the DW to speed up the translational velocity selection. Philippsen and Siegwart ⁴ combined the DW, the elastic band, and NF1 to guarantee the convergence toward the target.

The control commands in the DWA are selected by maximizing an objective function, which is a weighted sum of three terms, and their main role is driving the robot fast and safe to the goal; however, this is not always guaranteed, due to the problem of choosing the appropriate weights. Several studies have been done to modify the objective function such as combining the DW with a particle filter to optimize the objective function, ⁵ employing fuzzy logic to change the velocity according to the obstacle density, ⁶ extending the classic DWA to a global DWA-based navigation scheme using model predictive control without an objective function, ⁷ and recently using a two-stage fuzzy-DWA: the first one to determine the heading angle and the second one to compute the target wheel velocities of the robot. ⁸ None of the mentioned extensions has solved the problem of finding suitable weights for the objective function.

The path tracking control is the process of determining speed and steering settings at each time step in order to make the robot follow a certain path. Various control strategies have been proposed over the years to handle this task, like the predictive control, ^9,10 the backstepping control, ^11,12 the adaptive control, ^13,14 and the fuzzy adaptive control. ¹⁵

In this article, the DWA is improved by using a fuzzy controller to dynamically adjust the weights of the objective function seeking for a fast and safe movement of the mobile robot. For the tracking control, a discrete linear quadratic tracker (LQT) ^16,17 with an additional control law is employed to ensure fast convergence of the mobile robot to the desired path and to maintain its stability along this path.

The article is organized as follows: The second section presents a brief description of the DWA with its disadvantages and our proposed approach. In the third section, a discrete LQT is designed. The fourth section discusses the simulation results, and the final section concludes the article.

Dynamic window approach

The DWA is a reactive path planning and obstacle avoidance technique designed to achieve high-speed navigation using the translational velocity (v) and the rotational velocity (w). These velocities must be optimal, safe, and selected from the velocity search space; therefore, the task of finding them is formulated as an optimization problem, which is feasible only for a discrete set of velocities bounded by v _max and w _max. The unsafe velocities are considered as nonadmissible and discarded from further calculations; in other words, we can say that the search space is reduced to three subspaces: the possible velocities V_d , the admissible velocities V_a , and the restricted initial velocities V_s . The intersection of the three subspaces gives the resulting search space V_r

$V_r=V_s\cap V_a\cap V_d$

1

From V_r , the optimal velocities will be selected by maximizing the following objective function

$G\left(v,w\right)=\alpha \cdot \text{head}\left(v,w\right)+\beta \cdot \text{dist}\left(v,w\right)+\gamma \cdot \text{vel}\left(v,w\right)$

2

where head is the function that keeps the robot going toward the goal location, dist is the distance to the closest obstacle; it saves the robot from collision, and vel is the forward velocity that makes the robot move fast. α, β, and γ are weighting factors used to control the influence of the terms included in the objective function on the behavior of the robot.

Drawbacks of the dynamic window

As in other reactive approaches, ¹⁸ during the process of path planning using the DWA, we have found that the robot can reach the goal position easily and safely in several environments with less number of obstacles; unfortunately, this is not the case if the environment is crowded.

We have done a lot of simulations by changing the weights α, β, and γ in order to get a universal set or a set that suits for many complex environments, which was impossible because the robot performed this kind of behaviors multiple times:

Unable to move around or pass the first obstacle

Figure 1 shows that if we choose a high value for α, a low value for β, and whatever the value of γ, the robot will always be stopped by the first obstacle in its path.

• Incapable of going across the narrow space between obstacles ¹⁸

Figure 1.

The robot can’t avoid the first obstacle.

Figure 2 shows that if we choose a low value for α and a high value for β, the robot will perform a long path to the goal.

• Going to a free space or passing the target

Figure 2.

The robot can’t cross the narrow spaces.

Figure 3 shows that if we choose a high value for β and γ, the robot will cross the goal.

• Reaching the target without performing an optimal path ¹⁸

Figure 3.

The robot passes the target position.

Figure 4 shows that if we choose equal or close values for α, β, and γ, then after several simulations using different sets, the robot can make a short path to the goal compared to Figure 2; however, it is not the shortest.

Figure 4.

The robot performs a short path.

In this section, like the authors in the original DW and its extensions, we have used random sets of weights. In the next section, we are going to use a fuzzy controller to adjust those weights in real time.

Real-time weights adjustment

The objective function is responsible for the DWA’s good or bad performance and the weights α, β, and γ are the key for that; therefore, to change the performance, we have to change the weights. In our work, these weights are adjusted dynamically according to the current environmental scenario using fuzzy logic, which is highly recommended in this kind of issues.

The fuzzy controller has three inputs and three outputs. The inputs are the current distance (dist) between the robot and each of the detected obstacles on the path, the forward velocity (vel), and the target heading angle (head). The outputs are the weights α, β, and γ. The distance range is [0, 15 m], divided into three subranges: close, medium, and far; the velocity domain contains three subdomains: slow, normal, and fast; the heading angle range is [0, 180°], divided into three fields: little, more and lots; the weights range is [0, 1] divided into three subdomains: low, medium and high. The definitions of the membership functions are shown in Figure 5(a) and (b).

Figure 5.

(a) Membership functions of the fuzzy inputs, (b) Membership functions of the fuzzy outputs, (c) Flowchart of the fuzzy-DWA. DWA: dynamic window approach.

Figure 5(c) shows the flowchart of the proposed fuzzy-DWA. Before starting the path planning process, we must have map information like target and obstacles coordinates, and we have to initialize the weights to be used in the first iteration, then the fuzzy controller will adjust them dynamically until the robot reaches the target.

Table 1 summarizes the fuzzy rules used in the controller design based on logic arguments like

• – If the robot is far away from an obstacle, α and γ should be high and β should be low.

• – If the robot is approaching an obstacle, we should increase β and reduce α and γ.

• – If the robot is passing an obstacle, we must reduce β and increase α and γ.

• – If the robot is approaching the target, we have to decrease β and γ and increase α.

Table 1.

The sets of rules used in the fuzzy controller.

Rule number	Linguistic inputs			Linguistic outputs
	Dist	Vel	Head	α	β	γ
1	Close	Slow	Little	Low	High	Low
2	Close	Normal	More	Low	High	Low
3	Close	Fast	Lots	Low	High	Low
4	Medium	Slow	Little	Medium	Low	Medium
5	Medium	Normal	More	Medium	Low	Medium
6	Medium	Fast	Lots	Low	Medium	Medium
7	Far	Slow	Little	High	Low	High
8	Far	Normal	More	High	Low	High
9	Far	Fast	Lots	High	Low	High

Dist: distance; Vel: velocity; Head: heading angle.

Tracking control

The main purpose of the tracking control is making the robot follow a desired path, in our case this path is planned using fuzzy-DWA.

Kinematic model of the mobile robot

The state space model of the mobile robot in Figure 6 is given by

$\{\begin{array}{l}\dot{x}=v\text{\hspace{0.17em}cos}\theta \\ \dot{y}=v\text{\hspace{0.17em}sin}\theta \\ \dot{\theta }=w\end{array}$

3

Figure 6.

The model of the mobile robot.

And can be written in a compact form as

$\dot{x}=f\left(x,u\right)$

4

where ${\left[xy\theta \right]}^T$ describes the position and the orientation of the robot and ${\left[u=vw\right]}^T$ represents the control input.

The discrete time state space model at sample rate T_s is given as follows

$\{\begin{array}{l}x\left(k+1\right)=x\left(k\right)+T_s{u}_1\left(k\right)\text{cos}\theta \\ y\left(k+1\right)=y\left(k\right)+T_s{u}_1\left(k\right)\text{sin}\theta \\ \theta \left(k+1\right)=\theta \left(k\right)+T_s{u}_2\left(k\right)\end{array}$

5

From equation (5), it can be seen that the equations representation can be formulated in linear state space form

$x\left(k+1\right)=Ax\left(k\right)+Bu\left(k\right)$

6

where $A=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\text{ and }B=\left[\begin{array}{cc}\begin{array}{c}{T}_s\text{cos}\theta \\ T_s\text{sin}\theta \\ 0\end{array}& \begin{array}{c}0\\ 0\\ T_s\end{array}\end{array}\right]$

Linear quadratic tracker

The LQT generates optimal control inputs that make the robot track the desired path with the minimum tracking error. To obtain these optimal control inputs, a performance index should be minimized.

Equation (7) represents the discrete time performance index to be minimized ^16,17

$\begin{array}{l}J=\frac{1}{2}{\left[Cx\left(k_f\right)-r\left(k_f\right)\right]}^{T}F\left[Cx\left(k_f\right)-r\left(k_f\right)\right]\\ +\frac{1}{2}{\displaystyle \sum _{k=k_0}^{k_f-1}\left\{{\left[Cx\left(k\right)-r\left(k\right)\right]}^{T}Q\left[Cx\left(k\right)-r\left(k\right)\right]+u^T\left(k\right)Ru\left(k\right)\right\}}\end{array}$

7

where x(k) and u(k) are the state and the control vectors at time step k, respectively. C is the discrete time output matrix. F ≥ 0, Q ≥ 0, and R > 0 are diagonal weighting matrices that penalize the final state, the state, and the control inputs, respectively. r(k) represents the desired state vector which contains the coordinates of the reference trajectory at each time step.

To solve the minimization problem in equation (7), the Hamiltonian is defined as

$\begin{array}{l}H\left(x\left(k\right),u\left(k\right),\lambda \left(k+1\right)\right)=\frac{1}{2}{\displaystyle \sum _{k=k_0}^{k_f-1}\{{\left[Cx\left(k\right)-r\left(k\right)\right]}^{T}Q\left[Cx\left(k\right)-r\left(k\right)\right]}\\ +u^T\left(k\right)Ru\left(k\right)\}+{\lambda }^T\left(k+1\right)\left[Ax\left(k\right)+Bu\left(k\right)\right]\end{array}$

8

where λ is the Lagrange multiplier that is derived from the Hamiltonian as shown in equation (9)

$\lambda \left(k\right)=\frac{\partial H}{\partial x\left(k\right)}$

$\lambda \left(k\right)=A^T\lambda \left(k+1\right)+Vx\left(k\right)-Wr\left(k\right)$

9

with $V=C^{T}QC \text{ and }W=C^{T}Q$ .

Equation (9) is also called the costate, and its final state is given by

$\lambda \left(k_f\right)=C^{T}FCx\left(k_f\right)-C^{T}Fr\left(k_f\right)$

10

By deriving the Hamiltonian with respect to the control input, we get the open loop optimal control law as

$\frac{\partial H}{\partial u\left(k\right)}=0$

$u\left(k\right)=-R^{-1}{B}^T\lambda \left(k+1\right)$

11

By substituting equation (11) in equation (6), we obtain the state space in the following form

$x\left(k+1\right)=Ax\left(k\right)-B{R}^{-1}{B}^T\lambda \left(k+1\right)$

12

The combination of equations (12) and (9) yields the so-called homogeneous Hamiltonian system

$\left[\begin{array}{l}x\left(k+1\right)\\ \lambda \left(k\right)\end{array}\right]=\left[\begin{array}{cc}\begin{array}{l}A\\ V\end{array}& \begin{array}{l}-E\\ A^T\end{array}\end{array}\right]\left[\begin{array}{l}x\left(k\right)\\ \lambda \left(k+1\right)\end{array}\right]+\left[\begin{array}{l}0\\ -W\end{array}\right]r\left(k\right)$

13

where $E=B{R}^{-1}{B}^T$ .

Due to the forcing term $C^{T}Qr\left(k\right)$ in equations (9) and (10), the optimal control is no longer a linear state feedback; however, it can be expressed as a combination of a linear state feedback plus a term depending on r(k). ¹⁶

According to equation (10), we assume that for all k < N, we can write

$\lambda \left(k\right)=P\left(k\right)x\left(k\right)-g\left(k\right)$

14

To validate the assumption in equation (14), consistent equations for P(k) and g(k) must be found. To do so, we use equation (14) in the state equation portion of equation (13) and after a few calculations, ¹⁶ we get the following equations

$P(k)=A^{T}P\left(k+1\right){\left[I+EP\left(k+1\right)\right]}^{-1}A+V$

15

$g(k)=\left\{A^T-A^{T}P\left(k+1\right){\left[I+EP\left(k+1\right)\right]}^{-1}E\right\}g\left(k+1\right)+Wr\left(k\right)$

16

By comparing equations (14) and (10), the boundary conditions for P(k) and g(k) are defined by

$P\left(k_f\right)=C^{T}FC$

17

$g\left(k_f\right)=C^{T}Fr\left(k\right)$

18

Solving backward in time both of equation (15) and equation (16) using the boundary conditions in equation (17) and equation (18), respectively, leads to the time varying gains L(k) in equation (19) and L_g (k) in equation (20), which are used to formulate the closed loop optimal control law in equation (21)

$L\left(k\right)={\left[R+B^{T}P\left(k+1\right)B\right]}^{-1}{B}^{T}P\left(k+1\right)A$

19

$L_g\left(k\right)={\left[R+B^{T}P\left(k+1\right)B\right]}^{-1}{B}^T$

20

$u\left(k\right)=-L\left(k\right)x\left(k\right)+L_g\left(k\right)g\left(k+1\right)$

21

Consequently, the resulting optimal state is expressed as follows

$x\left(k+1\right)=\left[A-BL\left(k\right)\right]x\left(k\right)+B{L}_g\left(k\right)g\left(k+1\right)$

22

In this article, to ensure a fast transition of the robot from any initial position toward the reference trajectory, the performance index is extended to the following form

$\begin{array}{l}J=\frac{1}{2}{\left[Cx\left(k_f\right)-r\left(k_f\right)\right]}^{T}F\left[Cx\left(k_f\right)-r\left(k_f\right)\right]\\ +\frac{1}{2}{\displaystyle \sum _{k=k_0}^{k_f-1}\{{\left[Cx\left(k\right)-r\left(k\right)\right]}^{T}Q\left[Cx\left(k\right)-r\left(k\right)\right]}\\ +{\left[u\left(k\right)-u_r\left(k\right)\right]}^{T}R\left[u\left(k\right)-u_r\left(k\right)\right]\}\end{array}$

23

where u_r is the control input that makes the robot reaches the desired path quickly and maintains its stability along this path, which can be calculated as follows

$Ar\left(k\right)+B{u}_r\left(k\right)=0$

$u_r\left(k\right)=-B^{-1}Ar\left(k\right)$

24

The optimal control law in this case is expressed as

$u\left(k\right)-u_r\left(k\right)=-L\left(k\right)\left[x\left(k\right)-r\left(k\right)\right]+L_g\left(k\right)g\left(k+1\right)$

$u\left(k\right)=-L\left(k\right)x\left(k\right)+L_g\left(k\right)g\left(k+1\right)+\left[L\left(k\right)-B^{-1}A\right]r\left(k\right)$

25

The proposed control strategy can be seen in Figure 7.

Figure 7.

Block diagram of the path planner and the optimal tracking controller.

Simulation results

The DWA and the fuzzy-DWA, the LQT and the LQT + u_r algorithms are implemented and compared in Matlab. The robot has a circular shape with a radius of 0.15 m and moves in two different maps of the size 15 m × 15 m and 25 m × 25 m. The maximum translational and rotational velocities are 1 m/s and 90°/s, respectively. At each iteration, the weights of the objective function must satisfy this condition: α + β + γ ≤ 1. The sample rate T_s is 0.1 s. The performance index parameters are assumed as follows: F = I ₃, Q = 10 × I ₃, and R = I ₃. The lemniscate shape trajectory is given by these equations: x(t) = 1.1 + 0.7 sin(2t/30) and y(t) = 0.9 + 0.7 sin(4t/30).

Path planning results

By comparing Figure 8(a) with Figures 1 to 4, it is obvious that the proposed fuzzy-DWA has improved the performance of the mobile robot by making it reach the goal safely with the shortest path possible. In Figure 8(b) to (d), we can see how the weights change according to the environment situation.

Figure 8.

(a) Optimal path generated by the fuzzy-DWA, (b) corresponding α values, (c) corresponding β values, and (d) corresponding γ values. DWA: dynamic window approach.

Figure 9 as well shows the efficiency of the proposed approach in driving the robot toward the target through a complex environment, and it displays the weights adjustment during the path planning process.

Figure 9.

(a) Optimal path generated by the fuzzy-DWA, (b) corresponding α values, (c) corresponding β values, and (d) corresponding γ values.

In Figure 10(a), the robot can reach the goal from several starting positions, and in Figure 10(b), it is able to travel from the same position to different targets, which confirms the robustness of our approach.

Figure 10.

Fuzzy-DWA path planning (a) from several positions and (b) to different goals. DWA: dynamic window approach.

Tracking control results

Figure 11(a) and (b) shows a comparison between the trajectories obtained by applying the conventional LQT controller and our LQT + u_r controller. The tracking errors are shown in Figure 12(a) and (b). According to Figure 11, both controllers perform a good tracking of the desired trajectories; however, the LQT + u_r shows better accuracy in the tracking performance which can be seen in Figure 12, as a fast convergence of the tracking errors and more stability along the trajectories.

Figure 11.

(a) Tracking a planned trajectory and (b) tracking a given trajectory.

Figure 12.

(a) Tracking errors of the planned trajectory and (b) tracking errors of the given trajectory.

Conclusion

In this article, a fuzzy controller was used to dynamically adjust the weights of the DW objective function, and a control law was added to the LQT to improve the tracking performances. Simulation results of the path planning prove that the proposed fuzzy-DWA is significantly superior to the original DW, especially in cramped environments with numerous obstacles. The optimal tracking control results indicate that the proposed LQT + u_r controller achieves better tracking performances.

It is clear that the results achieved in this article prove the potential of the fuzzy-DWA in solving the path planning problem, and the effectiveness of the LQT in tracking different trajectories. Future goals are to perform the path planning in dynamic environment and to focus on the experimental validation of the proposed algorithm.