Discrete-time predictive trajectory tracking control for nonholonomic mobile robots with obstacle avoidance

Abstract

This article presents a tracking control approach with obstacle avoidance for a mobile robot. The control law is composed of two parts. The first is a discrete-time model predictive method-based trajectory tracking control law that is derived using an optimal quadratic algorithm. The second part is the obstacle avoidance strategies that switch according to two different designed obstacle avoidance regions. The controllability of the avoidance control law is analyzed. The simulation results validate the effectiveness of the proposed control law considering both trajectory tracking and obstacle avoidance.

Introduction

The motion control of robots has been an important research topic that has many practical applications in various industries. A kinematic control system with control laws for nonholonomic constraints of robots is considered under the assumptions in the Brockett’s theorem, ¹ with the nonholonomic mobile robot being controlled with fewer number of control inputs than the multiple state variables. The motion problem of the nonholonomic mobile robots can be analyzed with respect to in aspects as point stabilization, ² trajectory tracking, ^3,4 and path following. ^5,6

During the past several decades, many methods of motion tracking control of nonholonomic mobile robots have been proposed. Some motion tracking control schemes can be found in the literature, ^{7 –9} where the complex dynamics of the entire mechanical system are assumed to be exactly known. Moreover, various robust and adaptive control schemes have been implemented to address the control design for perturbative nonholonomic mechanical systems. ^4,10 Many methods, including linear differential equation methods, ¹¹ sliding mode control methods, ^12,13 backstepping methods, ^14,15 saturation feedback control methods, ¹⁶ and feedback linearization methods, ¹⁷ among others, have been proposed to address the problem of tracking control for nonholonomic mobile robots.

As an advanced process control method, the model predictive control (MPC) ^18,19 shows its wide application in the field of survey. The essential problem of MPC is to find optimal inputs to minimize a specific cost function with or without state and input constraints. The cost function is composed of the errors between the reference output and the predicted practical output. MPC, also known as receding horizon control, has also attracted many research interests in the study of nonholonomic mobile robot. Of particular interest here is on the application of MPC to address the trajectory tracking problem for mobile robots. ^{20 –23} The Lyapunov-based MPC was studied by Liu et al. ²⁰ for tracking control of nonholonomic mobile robots. An MPC scheme without stabilizing constraints and costs was studied by Worthmann et al. ²¹ for tracking problems. Asymptotic stability of controllers is only addressed for the set-point stabilization problem, ^20,21 and these controllers are not applied to trajectory tracking problems. Gu and Hu ²² proposed a classic MPC controller, which can guarantee the stability of control systems in the terminal state region. Škrjanc and Klan $\stackrel{⌣}{\text{c}}$ ar ²³ compared the discrete with continuous tracking-error model-based predictive control. The article shows that discrete MPC has similar performance to continuous MPC in ideal situations in the case of deterministically periodic sampling-time instants. Considering the practical implementation, the discrete MPC is adopted for trajectory tracking problems in this article, and the sampling-time instants are deterministically periodic.

Obstacle avoidance is a key research problem for the navigation of moving objects in a dynamic environment. The obstacle avoidance algorithms can be generally classified into global and local path planners. ²⁴ Kanjanawanishkul et al. ²⁵ proposed an obstacle avoidance approach, which is designed in path following control framework by using sensor information to generate new waypoints. However, the path planning problems are only considered in most obstacle avoidance approaches proposed in the literature by leaving out the control phase. ^24,26 The tracking control problem becomes complex when the obstacle environment is taken into account. The solution can be considered designing the controller with handling tracking and obstacle avoidance simultaneously, which means that the tracking of nonholonomic mobile robots is feedback controlled with designing the motion control of obstacle avoidance. The recent literature have contributed problems along these lines. An adaptive control system was proposed for tracking and obstacle avoidance of a class of mobile robots with limited detection ranges in the presence of unknown skidding and slipping. ²⁷ The adaptive control method was used with the potential function to propose a scheme to achieve the trajectory tracking control with collision avoiding for mobile robots. ²⁸ Different from setting potential functions, ^27,28 Rodrguez-Seda et al. ²⁹ represented the interaction of the vehicle and the obstacle as primary elements for avoiding collisions, and the safe distance was set to guarantee avoidance control performance without interfering with the trajectory tracking control. Akka and Khaber ³⁰ proposed a fuzzy controller to achieve trajectory tracking control and avoid collisions. The weighting matrices of linear quadratic regulator are adjusted dynamically to achieving the obstacle avoidance. However, the environment information is not considered in the article. A car-like mobile robot within an intelligent space with the mixed $H_2/H_{\infty }$ decentralized control was developed with the cameras used to solve the pose and obstacle position, ³¹ and the method was focused on vision-based approach. However, the implementation process of the method is more complex than the former ones interested in using detection region. Our method is to set the specific avoidance region for avoiding collisions in the light of considering the safe distance. ²⁹ When entering a specific obstacle avoidance region, the motion of the mobile robot can directly avoid obstacles without affecting trajectory tracking as far as possible.

The main contributions of this work are to use the discrete-time system model based on MPC to achieve the trajectory tracking control and to avoid obstacle collisions. Firstly, an MPC law is derived by minimizing the cost function composed of predictive discrete-time state and control variables. Secondly, we consider static obstacle ranges as the control excitation condition that divides avoidance region into two parts, named as conservative and regulated region, respectively. The former is the essential setup for avoiding obstacles control and the latter can ensure obstacle avoidance and tracking performance of mobile robots simultaneously. The constraints of feasible avoidance conditions are analyzed and then added into the MPC law for the purpose of achieving trajectory tracking control. Finally, the simulation is implemented and the result validates the effectiveness of the proposed control approach.

The rest of the article is organized in the following manner. The deduction of control law based on MPC in discrete-time system is given in the second section. The obstacle avoidance control strategy is described and analyzed in the third section. The performance of simulation experiment is presented in the fourth section, and conclusions are given at the end.

Problem formulations

Kinematic system for nonholonomic mobile robots

As shown in Figure 1, consider a differential drive robot on the ground for no sliding motion, the nonholonomic constraint for position coordinates x, y and the angle θ is as follows:

$\dot{x}\text{sin}\theta -\dot{y}\text{cos}\theta =0$

1

Figure 1.

The nonholonomic mobile robot.

The kinematic equations of robot are given as follows:

$\dot{\mathbf{x}}=\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{cc}\text{sin}\theta & 0\\ \text{cos}\theta & 0\\ 0& 1\end{array}\right]u$

2

where $\dot{\mathbf{x}}=[\begin{array}{ccc}\dot{x}& \dot{y}& \dot{\theta }\end{array}{]}^T\in {ℝ}^3$ is the differential form of state vector $\mathbf{x}=[\begin{array}{ccc}x& y& \theta \end{array}{]}^T$ , and $u=[\begin{array}{cc}v& w\end{array}{]}^T\in {ℝ}^2$ is the control vector, where v and w denote the robot’s forward linear velocity and angular velocity, respectively.

The orientation angle of the robot is a variable for three-dimensional space. However, the attitude angle of robotic posture cannot be obtained from Figure 1. The robot motion is only constrained on the plane while the limitation for speed of the wheel movement is not available, the motion constraint equations cannot be integrated, ³² such that the system is a nonholonomic constraint system. Furthermore, it holds the condition that there is no continuous or time-invariant feedback to drive the nonholonomic system to the stabilization point. ¹

Trajectory tracking

It is necessary to obtain a linear time-varying system for trajectory tracking. Since nonholonomic robot control systems are usually nonlinear, to simplify calculation, we linearize the system. The linearized feedback ¹¹ is proved to be asymptotically stable in the control process of the system.

To illustrate the control problem of tracking, the reference robot is defined with a reference state vector ${\mathbf{x}}_r=[\begin{array}{ccc}{x}_r& y_r& {\theta }_r\end{array}{]}^T\in {ℝ}^3$ and a reference control vector $u_r=[\begin{array}{cc}{v}_r& w_r\end{array}{]}^T\in {ℝ}^2$ , with the same constraints as in equation (1). Its kinematic equation is as follows:

${\dot{\mathbf{\text{x}}}}_r=\left[\begin{array}{c}{\dot{x}}_r\\ {\dot{y}}_r\\ {\dot{\theta }}_r\end{array}\right]=\left[\begin{array}{cc}\text{sin}{\theta }_r& 0\\ \text{cos}{\theta }_r& 0\\ 0& 1\end{array}\right]u_r$

3

For the point stabilization problem of tracking problem, it is assumed that $x_r$ has a constant value corresponding to the desired stable point with vector $u_r=[\begin{array}{cc}0& 0\end{array}{]}^T$ at any time constant. However, ${\mathbf{x}}_r$ and $u_r$ are time invariants of the trajectory tracking. The error state $x_e$ is introduced to solve two cases using two coordinate systems: one is the generalized coordinate system for the reference robot and the other is the coordinate system for the controlled robot. Let ${\mathbf{x}}_m=[\begin{array}{ccc}{x}_m& y_m& {\theta }_m\end{array}{]}^T$ be the motion state vector of the controlled robot. The error state ${\mathbf{x}}_e$ can be defined as follows:

${\mathbf{x}}_e=\left[\begin{array}{c}{x}_e\\ y_e\\ {\theta }_e\end{array}\right]=\left[\begin{array}{ccc}\text{cos}\theta & \text{sin}\theta & 0\\ -\text{sin}\theta & \text{cos}\theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_r-x_m\\ y_r-y_m\\ {\theta }_r-{\theta }_m\end{array}\right]$

4

It can be obtained the error dynamic model by differentiating the error state vector in equation (4):

$\begin{array}{l}{\dot{x}}_e=w{y}_e-v+v_r\text{cos}{\theta }_e\hfill \\ {\dot{y}}_e=-w{x}_e+v_r\text{sin}{\theta }_e\hfill \\ {\dot{\theta }}_e=w_r-w\hfill \end{array}$

5

For the convenience of linear problems, we linearize the right-hand side of equations in equation (5). It yields

${\dot{\mathbf{x}}}_e=\left[\begin{array}{c}{\dot{x}}_e\\ {\dot{y}}_e\\ {\dot{\theta }}_e\end{array}\right]=\left[\begin{array}{ccc}0& w_r& 0\\ -w_r& 0& v_r\\ 0& 0& 0\end{array}\right]{\mathbf{x}}_e+\left[\begin{array}{cc}1& 0\\ 0& 0\\ 0& 1\end{array}\right]u_e$

6

where $u_e$ is defined as follows:

$u_e=\left[\begin{array}{c}-v+v_r\text{cos}{\theta }_m\\ w_r-w\end{array}\right]$

7

For the condition ${\text{lim}}_{t\to \infty }({\upsilon }_r{(t)}^2+{\omega }_r{(t)}^2=0)$ , the local linear controllability was studied, ²² and it is not discussed in this article. Moreover, to ensure the continuity of the trajectory for tracking and obstacle avoidance, the global control system is initially considered to obtain feedback control variables based on equation (6).

Predictive tracking control strategy

Now, we consider the characteristic of time-variant of reference state in the tracking control system. Assuming that the form of continuous-time system can be reformulated into discrete-time form from equation (6) as follows:

${\mathbf{x}}_e(k+1)={\mathbf{x}}_e(k)+{\dot{\mathbf{x}}}_e(k)\Delta t$

8

The discrete-time system (equation (8)) can be transformed into a discrete state-space system as follows:

$\begin{array}{ll}\hfill & {\mathbf{x}}_e(k+1)=A(k){\mathbf{x}}_e(k)+B(k)u_e(k)\hfill \\ \hfill & A=I+A_s\Delta t\hfill \\ \hfill & B=B_s\Delta t\hfill \\ \hfill & \hfill \end{array}$

9

The reference linear velocity ${\upsilon }_r(k)$ and angular velocity ${\omega }_r(k)$ are time-variants, the matrix $A\in {ℝ}^{3\times 3}$ is also a time-variant, $B\in {ℝ}^{3\times 2}$ is a constant matrix, 3 and 2 denote as the dimension for state and control variables, respectively, and $\Delta t$ is the sampling time interval.

Assuming the state and control variables of the system (equation (8)) are measurable and controllable at the sampling time $t_k$ , the cost function for the trajectory tracking based on MPC method can be rewritten as follows:

$\begin{array}{l}J({\mathbf{x}}_e,u_e,k)={\displaystyle \sum _{i=1}^p}{\mathbf{x}}_e{(i)}^{T}Q{\mathbf{x}}_e(i)+{\displaystyle \sum _{i=0}^{c-1}}{u}_e{(i)}^{T}R{u}_e(i)\hfill \end{array}$

10

where

$\begin{array}{l}\begin{array}{ll}\hfill & {\mathbf{x}}_e=[{\mathbf{x}}_e(1),{\mathbf{x}}_e(2),\dots ,{\mathbf{x}}_e(p)]\in {ℝ}^{3\times p}\hfill \\ \hfill & {\mathbf{x}}_e(i)={\mathbf{x}}_e(k+i|k),i=1,2,\dots ,p\hfill \\ \hfill & u_e=[u_e(0),u_e(1),\dots ,u_e(c-1)]\in {ℝ}^{2\times c}\hfill \\ \hfill & u_e(i)=u_e(k+i|k),i=0,1,\dots ,c-1\hfill \\ \hfill & \hfill \end{array}\hfill \end{array}$

11

where ${\mathbf{x}}_e$ and $u_e$ are error state and control matrices, respectively. p and c are predictive and control horizon with $p\ge c\in N^{+}$ . Without loss of generality, $p=c$ is assumed. $Q\in {ℝ}^{3\times 3}$ and $R\in {ℝ}^{2\times 2}$ are state and input weight matrices with $Q\ge 0$ and $R\ge 0$ , respectively.

Here is the transformed result of the cost function in quadratic form as follows:

$J({\stackrel{\sim }{\mathbf{x}}}_e,{\stackrel{\sim }{u}}_e,k)={\stackrel{\sim }{\mathbf{x}}}_e{(k+1)}^T\stackrel{\sim }{Q}{\stackrel{\sim }{\mathbf{x}}}_e(k+1)+{\stackrel{\sim }{u}}_e{(k)}^T\stackrel{\sim }{Q}{\stackrel{\sim }{u}}_e(k)$

12

where

$\begin{array}{ll}\hfill & {\stackrel{\sim }{\mathbf{x}}}_e(k+1)=[{\mathbf{x}}_e{(1)}^T,{\mathbf{x}}_e{(2)}^T,\dots ,{\mathbf{x}}_e{(p)}^T{]}^T\in {ℝ}^{3p}\hfill \\ \hfill & {\stackrel{\sim }{u}}_e(k)=[u_e{(0)}^T,u_e{(1)}^T,\dots ,u_e{(c-1)}^T{]}^T\in {ℝ}^{2c}\hfill \\ \hfill & \hfill \end{array}$

13

$\stackrel{\sim }{Q}=\left[\begin{array}{cccc}Q& 0& \cdots & 0\\ 0& Q& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & Q\end{array}\right],\stackrel{\sim }{R}=\left[\begin{array}{cccc}R& 0& \cdots & 0\\ 0& R& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & R\end{array}\right]$

14

where $\stackrel{\sim }{Q}\in {ℝ}^{3p}\times {ℝ}^{3p}$ , and $\stackrel{\sim }{R}\in {ℝ}^{2c}\times {ℝ}^{2c}$ .

${\stackrel{\sim }{\mathbf{x}}}_e(k+1)$ can be written specifically from equation (9) as follows:

$\begin{array}{ll}\hfill & {\stackrel{\sim }{\mathbf{x}}}_e(k+1)=\left[\begin{array}{c}{\Lambda }_k\\ {\Lambda }_{k+1}\\ ⋮\\ {\Lambda }_{k+p-1}\end{array}\right]\hfill \\ \hfill & {\Lambda }_k=A(k|k){\mathbf{x}}_e(k|k)+B(k|k)u_e(k|k)\hfill \\ \hfill & {\Lambda }_{k+1}=A(k+1)A(k){\mathbf{x}}_e(k|k)+A(k+1)B(k)u_e(k|k)\hfill \\ \hfill & +A(k+1)B(k+1)u_e(k+1|k)\hfill \\ \hfill & {\Lambda }_{k+p-1}={\displaystyle \prod _{i=0}^{p-1}}A(k+i|k){\mathbf{x}}_e(k|k)\hfill \\ \hfill & +({\displaystyle \sum _{j=0}^p}{\displaystyle \prod _{i=j}^{p-1}}A(k+i+1|k))\times B(k+j|k)u_e(k+j|k)\hfill \\ \hfill & +B(k+p-1|k)u_e(k+p-1|k)\hfill \\ \hfill & \hfill \end{array}$

15

Collating equation (13), it yields

${\stackrel{\sim }{\mathbf{x}}}_e(k+1)=\stackrel{\sim }{A}(k){\mathbf{x}}_e(k)+\stackrel{\sim }{B}(k){\stackrel{\sim }{u}}_e(k)$

16

where

$\begin{array}{ll}\hfill & \begin{array}{l}\stackrel{\sim }{A}(k)=\left[\begin{array}{c}A(k|k)\\ A(k+1|k)A(k|k)\\ ⋮\\ {\displaystyle \prod _{i=0}^{p-1}}A(k+i|k)\end{array}\right]\hfill \\ \stackrel{\sim }{B}(k)=\left[\begin{array}{cccc}{\Xi }_k& 0& \dots & 0\\ {\Upsilon }_{k+1}{\Xi }_k& {\Xi }_{k+1}& \dots & ⋮\\ ⋮& ⋮& \ddots & ⋮\\ {\Upsilon }_{k+p-1}{\Xi }_k& {\Upsilon }_{k+p-2}{\Xi }_{k+1}& \dots & {\Xi }_{b+k-1}\end{array}\right]\hfill \end{array}\hfill \\ \hfill & {\Xi }_i=B(i|k)\hfill \\ \hfill & {\Upsilon }_i={\displaystyle \prod _{l=k+p-i}^{i-1-k}}A(k+l|k)\hfill \\ \hfill & \hfill \end{array}$

17

where $\stackrel{\sim }{A}(k)\in {ℝ}^{3p\times 3}$ and $\stackrel{\sim }{B}(k)\in {ℝ}^{3c\times 2c}$ . ${\stackrel{\sim }{\mathbf{x}}}_e\in {ℝ}^{3p\times 1}$ is the error state vector at time $t_k$ , and ${\stackrel{\sim }{u}}_e\in {ℝ}^{2c\times 1}$ is the input vector, which contains control variables from time $t_k$ to $t_{k+c}$ .

It can be acknowledged that equation (16) derived from cost function (equation (12)) is considered as a quadratic system, which is composed of an input vector $u_e(k)$ and a state vector ${\mathbf{x}}_e(k)$ . Through minimizing the cost function (equation (12)), the trajectory tracking control variables υ and ω of next sampling time can indirectly be derived from equation (16) as follows:

$\begin{array}{ll}\frac{\partial J}{\partial {\stackrel{\sim }{u}}_e}=\hfill & 2\stackrel{\sim }{B}{(k)}^T\stackrel{\sim }{Q}\stackrel{\sim }{A}(k){\mathbf{x}}_e(k)+2(\stackrel{\sim }{B}{(k)}^T\stackrel{\sim }{Q}\stackrel{\sim }{B}(k)+\stackrel{\sim }{R}){\stackrel{\sim }{u}}_e(k)\hfill \\ =\hfill & 0\hfill \\ \hfill & \hfill \end{array}$

18

The control vector is developed into the form as follows:

${\stackrel{\sim }{u}}_e(k)=(\stackrel{\sim }{B}{(k)}^T\stackrel{\sim }{Q}\stackrel{\sim }{B}(k)+\stackrel{\sim }{R}{)}^{-1}\stackrel{\sim }{B}{(k)}^T\stackrel{\sim }{Q}(-\stackrel{\sim }{A}(k)){\mathbf{x}}_e(k)$

19

where ${\stackrel{\sim }{u}}_e(k)$ contains discrete optimal control variables from time $t_k$ to $t_{k+p}$ , $u_e(k|k)\in {ℝ}^{2\times 1}$ is the vector of optimal solution for control variables at time $t_k$ , and it can be represented as follows:

$u(k)=\left[\begin{array}{c}{u}_1(k)\\ u_2(k)\end{array}\right]=\left[\begin{array}{c}v(k)\\ w(k)\end{array}\right]=\left[\begin{array}{c}{v}_r(k)\text{cos}{\theta }_m\\ w_r(k)\end{array}\right]-{\stackrel{\sim }{u}}_e(k)$

20

Obstacle avoidance control strategy

Obstacle avoidance zone

As shown in Figure 2, the dark region represents the obstacle area. In this article, all obstacle regions are modeled as circle-shaped objects. The collision area of the ith obstacle can be defined as follows:

${\Gamma }_i=\{p\in {ℝ}^2,l_i\le r_o\}$

21

where $p=[\begin{array}{cc}x& y\end{array}{]}^T$ and $l_i=\parallel p_m-p_{oi}\parallel$ , $p_m\in {ℝ}^2$ is coordinates of the mobile robot, and $p_{oi}\in {ℝ}^2$ is the coordinates of center $O_i$ for the ith obstacle.

Figure 2.

The illustration of trajectory tracking and obstacle avoidance.

We proposed a new collision avoidance strategy for predictive trajectory tracking control. The strategy consists of two parts, namely conservative and regulated obstacle avoidance control strategies. The conservative region ${\Lambda }_i$ and regulated region ${\Omega }_i$ are represented as follows:

${\Lambda }_i=\{p\in {ℝ}^2,r_o\le l_i\le r\}\hspace{1em}{\Omega }_i=\{p\in {ℝ}^2,r\le l_i\le R\}$

22

and $D_i={\Lambda }_i{\displaystyle \cup }{\Omega }_i$ is the set for the avoidance region.

Conservative control strategy

When the mobile robot moves to the region ${\Lambda }_i$ , the conservative control strategy is a necessary control strategy for avoiding collision. Because obstacle avoidance conditions are considered in trajectory tracking, the linear and angular velocities are renamed as $\widehat{v}$ and $\widehat{w}$ , respectively. In the conservative control strategy, the linear velocity and angular velocity are controlled independently, so the advantage of the control strategy is that the position and orientation of the robot can be controlled more effectively.

The obstacle avoidance condition essentially constrains the input variables in discrete control systems. Based on the required orientation of the robot under obstacle avoidance conditions, the control variables presented in formula (equation (7)) are readjusted. Based on the consideration of obstacle avoidance control conditions, ²⁸ we make the following assumptions for obstacle avoidance of robots in discrete control systems.

Assumption 1

The reference robot is not in contact with obstacle regions (excluding obstacle avoidance area) during the whole motion process. When it moves close to obstacle regions, the reference robot can move away from obstacle regions for a period of time. The reference trajectory forms a smooth, continuous, and derivable curve during the control time.

In the course of trajectory tracking, the robot may have a special linear velocity and an angular velocity before it enters the conservative region at time $t_k$ . The control variables may lead to the robot not being controlled after it enters the conservative obstacle avoidance region, thus directly contacting the obstacle region. To satisfy the feasibility of the conservative control in region ${\Lambda }_i$ , the control variables for avoiding collisions are discussed in the avoidance region $D_i$ as follows.

As shown in Figure 2, in the geometric relationship, straight lines $A{P}_m$ and $B{P}_m$ are tangent lines from the robot $P_m$ to the edge of the obstacle, while $l_{O_{i}A}$ and $l_{O_{i}B}$ are vertical distances from the centre $O_i$ of the obstacle region to the tangent line, respectively, and naturally, $r_o=l_{O_{i}A}=l_{O_{i}B}$ . At present, the extension line of the orientation ${\overrightarrow{j}}_m$ is line $C{P}_m$ , and $l_{O_{i}C}$ is the vertical distance from the center $O_i$ of obstacle region to linear $C{P}_m$ .

Assumption 2

When the orientation of robot is not in the angle between the directions of $\overrightarrow{P_{m}A}$ and $\overrightarrow{P_{m}B}$ at time $t_k$ , apparently, there is $l_{O_{i}C}\ge l_{O_{i}A}$ . The mobile robot does not touch the obstacle region at time $t_{k+1}$ .

According to geometric principle from Figure 2, the robot will not contact the obstacle region when it enters the region ${\Lambda }_i$ with the orientation direction at time $t_k$ . Then, we only need to control the robot’s linear velocity and angular velocity at time $t_{k+1}$ .

Assumption 3

When the orientation of robot is in the angle between the directions of $\overrightarrow{P_{m}A}$ and $\overrightarrow{P_{m}B}$ at time $t_k$ , apparently, there is $l_{O_{i}C}\le l_{O_{i}A}$ . The linear velocity of the mobile robot in region ${\Omega }_i$ can be constrained, and the mobile robot does not touch the obstacle region at time $t_{k+1}$ .

To study the orientation of the robot toward the point $O_i$ , it is assumed that the position of the robot is exactly at the point E at time $t_k$ , therefore, the minimum collision distance is $l_{DE}$ . The minimum obstacle avoidance condition can be assumed as: when angular velocity $w(k)\to 0$ , the setting value of $l_{DE}$ for the conservative region is enough to enable the mobile robot to control the obstacle avoidance at time $t_{k+1}$ and not to contact the obstacle region.

The value of $l_{DE}$ depends on the linear velocity of the mobile robot before entering the region ${\Lambda }_i$ at time $t_k$ . As depicted in Figure 3, $P_m(k)$ is the position of the robot at time $t_k$ and the orientation is $\overrightarrow{i}$ . Assume that the robot moves to the estimated position ${\widehat{P}}_{m_1}(k+1)$ at time $t_{k+1}$ , the position exactly satisfies $l_{P_m(k)E}=l_{P_m(k){\widehat{P}}_{m1}(k+1)}$ . The linear control velocity is named as lower bound velocity $v_d(k)$ at time $t_k$ . Assuming the robot moves to the estimated position ${\widehat{P}}_{m_2}(k+1)$ at time $t_{k+1}$ , the position exactly satisfies $l_{P_m(k)D}=l_{P_m(k)P_{m_2}(k+1)}$ . The linear control velocity is named as upper bound velocity $v_u(k)$ at time $t_k$ . The linear velocity of the mobile robot is constrained by the lower and upper bound velocities in region ${\Omega }_i$ and satisfies $v(k)\in [v_d(k),v_u(k)]$ . The lower and upper bound velocities satisfy $l_i-r_o-v_d(k)\Delta t<l_{DE}<v_u(t)\Delta t$ .

Figure 3.

Illustration of obstacle avoidance movement before entering the region ${\Lambda }_i$ .

Assume that the robot is subject to obstacle avoidance control at the time constant k, then the conservative control strategy is defined as follows:

Step 1: Let the linear velocity satisfy $\widehat{v}(k+1)=0$ at time $t_{k+1}$ , and only angular velocity control is considered. The orientation of the pose is parallel to the tangent of point nearest to the edge of the obstacle region. In Figure 2, the orientation of the robot is $\overrightarrow{m}$ , and ${\theta }_1$ is the angle error for orientation of the mobile robot between time $t_{k+1}$ and $t_{k+2}$ .

Step 2: Let the angular velocity satisfy $\widehat{w}(k+2)=0$ at time $t_{k+2}$ , and the linear velocity control is considered. The linear velocity is set the same as the velocity of MPC at time $t_{k+2}$ .

Steps 1 and 2 are applied alternately and the robot can be controlled as follows:

$\begin{array}{ll}\hfill & \widehat{v}(k+i+1)\begin{array}{ll}=0,i\in \{a|a=2a+1,a\in Z\}\hfill & \hfill \\ =u_1(k+i),i\in \{a|a=2a,a\in N^{*}\}\hfill & \hfill \end{array}\hfill \\ \hfill & \widehat{w}(k+j)\{\begin{array}{ll}=\frac{{\theta }_1}{\Delta t},j\in \{a|a=2a+1,a\in Z\}\hfill & \hfill \\ =u_1(k+i),j\in \{a|a=2a,a\in N^{*}\}\hfill & \hfill \end{array}\hfill \end{array}$

23

where ${\theta }_1={\widehat{\theta }}_m(k+j)-{\theta }_m(k+j)$ and ${\widehat{\theta }}_m(k+j)=0.5\pi +\text{atan2}(y_o-y,x_o-x)$ at $t_{k+1}$ time.

Theorem 1

When the Assumptions 1, 2, and 3 are held in the motion control system, assuming that the mobile robot enters to region ${\Lambda }_i$ at time $t_{k+1}$ , as depicted in Figure 2, and the orientation of the mobile robot is ${\overrightarrow{j}}_m$ , and the robot is controlled by steps 1 and 2 from the conservative strategy, then the robot can move away from the conservative region without contacting with the obstacle region after a period of time.

Proof

Since avoidance control time is discrete, the conservative control stability of the mobile robot is demonstrated by mathematical geometry. As shown in Figure 4, since the robot is controlled by steps 1 and 2 in region ${\Lambda }_i$ , it is assumed that the rough dashed line is denoted as the motion control trajectory of the robot, and the position of $P_m(k+n)$ is the same as $P_m(k+n+1)$ , $n\in N^{*}$ . We add a new auxiliary circle with radius $\widehat{r}(k+1)$ , $O_i$ is the center of the circle, so that the $P_m(k+1)$ and $P_m(k+2)$ is just on the edge of the circle, and $P_m(k+1)P_m(k+3)$ is the tangent of the auxiliary circle. The point G on the auxiliary circle satisfies $l_i=\widehat{r}(k+1)=l_{O_{i}G}$ . When the robot moves at $P_m(k+3)$ and $P_m(k+n)$ , the corresponding radius can be drawn as an auxiliary circle. According to the control law in Theorem 1, there are the following geometric relations as follows:

$O_1{P}_m(k+1)\perp P_m(k+1)P_m(k+3)$

24

Figure 4.

Illustration of moving away from the region ${\Lambda }_i$ .

Apparently, it will deduce $\widehat{r}(k)<\widehat{r}(k+1)$ . In the extension of the same case, it can be obtained that

$O_1{P}_m(k+n)\perp P_m(k+n)P_m(k+n+2),n\in N^{*}$

25

It will be obtained the inequality relation as $\widehat{r}(k+n)<\widehat{r}(k+n+1),n\in N^{*}$ . As long as the robot keeps conservative control strategy in region ${\Lambda }_i$ , the radius $\widehat{r}(k+n)$ of the auxiliary circle made by the position $P_m$ of the robot can be approximately equal to r when it is large enough, and it can be expressed as follows:

$\underset{n\to \infty }{{\displaystyle \text{lim}}}\widehat{r}(k+n)\approx r$

26

The relationship $l_i\approx r$ is represented in Figure 4. Finally, the robot moves away from the conservative region ${\Lambda }_i$ under the effect of linear velocity at a certain time instant, which represents $l_i>r$ .

Regulated control strategy

In view of the simplicity of the obstacle avoidance strategy of conservative control, we consider the trajectory tracking performance and propose a method that takes both the tracking performance and the obstacle avoidance control performance into account. The advantage of this strategy is that in the process of trajectory tracking, the robot can move away from the obstacle avoidance area directly under regulated control strategy without adopting conservative control strategy, so that the trajectory of the robot retains the tracking characteristics and the whole tracking trajectory is optimized.

Assume that the robot moves to the conservative region ${\Omega }_i$ at time $t_k$ . Based on the trajectory tracking control law, the orientation of the robot is reset at time $t_{k+1}$ in Figure 2. The control law of MPC is redefined as follows:

$\begin{array}{ll}\hfill & \widehat{v}(k+1)=u_1(k+1)\hfill \\ \hfill & \widehat{w}(k+1)=u_2(k+1)=\frac{{\theta }_2}{\Delta t}=\frac{{\stackrel{\sim }{\theta }}_m(k)-{\theta }_m(k)}{\Delta t}\hfill \end{array}$

27

The direction of robot orientation is $\overrightarrow{n}$ at time $t_{k+1}$ , ${\theta }_m(k)$ is redefined as follows:

$\begin{array}{ll}\hfill & {\stackrel{\sim }{\theta }}_m(k)={\sigma }_k{\theta }_e+{\eta }_k(0.5\pi +\text{atan2}(y_o-y_m,x_o-x_m))\hfill \\ \hfill & {\sigma }_k=\frac{l_i-r+r_o}{R-r}\in [0,1],{\eta }_k=1-{\sigma }_k\in [0,1],k\in N^{*}\hfill \end{array}$

28

where ${\sigma }_k$ and ${\eta }_k$ are tracking weight and regulating weight, respectively. Under the assumption of Assumptions 1, 2, and 3, the control law in equation (25) for the regulated region ${\Omega }_i$ is called the regulated control strategy.

Figure 5 shows the block diagram of the control algorithm of the conservative obstacle avoidance control and the regulated obstacle avoidance control. The linear velocity selection in the process of obstacle avoidance is obtained by the algorithm of MPC trajectory tracking and calculated in real time according to the location information in the whole motion control process. In general, obstacle avoidance control is based on trajectory tracking control for the steering control of mobile robots in the article.

Figure 5.

Both conservative and regulated obstacle avoidance control strategies.

Simulation results

To verify the proposed control strategies for trajectory tracking, the experiments are performed by using differential drive robot kinematic model. The one is to show the performance of the conservative obstacle avoidance control strategy, and the other is to show the performance of the conservative and regulated obstacle avoidance control strategy. To better show the control performance, we add other tracking and obstacle avoidance control strategies for comparisons.

Conservative strategy for tracking control

Assuming that sizes of all obstacles are same and randomly distributed around the reference trajectory, the number of obstacles is M = 3, the radius $r_o$ and r are 0.1 and 0.2 m, respectively, which satisfy the conditions of Assumptions 2 and 3. Obstacles are represented by the red circle area of Figures 6 and 12. Setting the initial pose of the mobile robot is ${[\begin{array}{ccc}2.5& 0.5& \pi /3\end{array}]}^T$ (m, m, rad), the reference trajectory in Figure 6 is chosen as follows:

$\begin{array}{l}{x}_r=1.6+1.5\text{cos}(0.15t)\hfill \\ y_r=0.5+1.5\text{sin}(0.15t)\hfill \end{array}$

29

Figure 6.

Circle-shape trajectory tracking with avoiding obstacles.

The time shift is set as $\Delta t$ = 0.05 s, predictive horizon p = 5, the weight matrices Q and R of the cost function (equation (12)) are chosen as diagonal matrices with elements defined as $[\begin{array}{ccc}2& 60& 2\end{array}]$ for Q and as $[\begin{array}{cc}2& 20\end{array}]$ for R.

A simple normal algorithm is considered for comparing the performance of conservative obstacle avoidance control. Assume that the trajectory tracking controller of simple algorithm is the same as the one in this article. When the distance from the obstacle is the same as r = 0.2 m, the controller makes the mobile robot turn to the $\overrightarrow{m}$ direction, as shown in Figure 2, and then moves straight forward until accomplishing the robot’s function of obstacle avoidance. Finally, the mobile robot restores to the state of trajectory tracking control. The time of moving forward is properly set to 2 s.

Figure 6 shows the motion effect of mobile robots for the model predictive tracking control with conservative and normal control strategies. Figures 7 to 11 show the results, which are expressed, respectively, for the tracking trajectory, three error states, linear velocities, and angular velocities for using both obstacle avoidance methods. Figure 6 shows that controlled robots can effectively avoid obstacles while tracking the reference trajectory. Figures 7 to 9 show the variation of error state components. Figures 10 and 11 show the variation of the linear speed and angular velocity control of both methods. Figure 10 shows the change time of the normal and conservative obstacle avoidance control. It can be derived that control time of the normal method is larger than the proposed method for obstacle avoidance. In addition, the control time of avoiding collisions should be given priority to the normal control strategy, which is not considered in the proposed method.

Figure 7.

The error state for x position.

Figure 8.

The error state for y position.

Figure 9.

The error state for orientation.

Figure 10.

The comparison of linear velocity and reference.

Figure 11.

The comparison of angular velocity and reference.

Conservative and regulated avoidance strategies for tracking control

Assuming that the sizes of all obstacles are same and randomly distributed around reference trajectory, the number of obstacles is M = 3, and the radii $r_o$ , r, and R are 0.1, 0.3, and 0.58 m, respectively, which satisfy the conditions of Assumptions 2 and 3. Setting the initial pose of the mobile robot is ${[\begin{array}{ccc}1& -0.5& 0\end{array}]}^T$ (m, m, rad), the reference trajectory (equation (13)) is chosen as follows:

$\begin{array}{l}{x}_r=1.1+1.5\text{sin}(0.2t)\hfill \\ y_r=-0.9+1.5\text{sin}(0.1t)\hfill \end{array}$

30

The time interval is set as $\Delta t$ = 0.05 s, predictive horizon p = 5, and the weight matrices Q and R of the cost function (equation (12)) are chosen as diagonal matrices with elements defined as $[\begin{array}{ccc}3& 30& 0.5\end{array}]$ for Q and as $[\begin{array}{cc}3& 20\end{array}]$ for R.

The adaptive tracking control strategy with obstacle avoidance is represented to compare the performance of both control strategy methods in Figures 12 to 17. The specific control algorithm can be seen in the literature. ²⁸ Figures 12 to 17 show the results which are expressed, respectively, for the tracking trajectory with obstacle avoidance, three error states, linear velocities, and angular velocities.

Figure 12.

Eight-shape trajectory tracking with avoiding obstacles.

Figure 13.

The error state for x position.

Figure 14.

The error state for y position.

Figure 15.

The error state for orientation.

Figure 16.

The comparison of linear velocity and reference.

Figure 17.

The comparison of angular velocity and reference.

Figure 12 shows the motion effect of mobile robots for the model predictive tracking control with both control (conservative and regulated) and adaptive control strategies. It can be seen that the trajectory tracking motion of the robot is adjusted obstacle avoidance control, and the multiobstacle synchronous trajectory tracking and obstacle avoidance motion are realized. For the trajectory tracking, both adaptive control and MPC can meet the tracking requirements. MPC is more radical than adaptive control in avoiding obstacles for the motion control of mobile robots. In general, the performance of the proposed method is the same effectiveness as an adaptive control method for obstacle avoidance. Figures 12 to 14 show the variation of error state components of both control strategies. Figures 16 and 17 show the variation of the linear speed and angular velocity control of the robot. Figure 10 shows the change time of the two control strategies for obstacle avoidance.

Conclusions

In this article, we have presented an approach based on discrete-time MPC for tracking and obstacle avoidance control of nonholonomic mobile robots. The simulation validates the effectiveness of the proposed approaches by using the essential obstacle avoidance strategy (conservative control strategy) and both obstacle avoidance control (conservative and regulated control strategies), respectively. It is critical that the control variables of setting avoidance region are constrained by the discrete-time condition. The discrete-time linear systems are used to solve the problem of trajectory tracking and obstacle avoidance. The stability of motion control depends entirely on the structure and parameters of the system. Because the robot motion control environment is complex and changeable in practical applications, we will further study the robot motion control in complex ground environment in the follow-up work.