Minimum control effort–based path planning and nonlinear guidance for autonomous mobile robots

Abstract

This article puts forth a framework using model-based techniques for path planning and guidance for an autonomous mobile robot in a constrained environment. The path plan is synthesized using a numerical navigation function algorithm that will form its potential contour levels based on the “minimum control effort.” Then, an improved nonlinear model predictive control approach is employed to generate high-level guidance commands for the mobile robot to track a trajectory fitted along the planned path leading to the goal. A backstepping-like nonlinear guidance law is also implemented for comparison with the NMPC formulation. Finally, the performance of the resulting framework using both nonlinear guidance techniques is verified in simulation where the environment is constrained by the presence of static obstacles.

Introduction

The focus of this article is to present a framework for path planning and guidance for autonomous mobile robots. The framework is designed as a step toward increased autonomy for differential drive mobile robots. The quantifiable measures of autonomy the authors recognize in this article are the vehicle’s ability to observe, orient, make decisions, and act. ¹ Of those autonomy measures, this article will focus primarily on expanding a mobile robot’s ability to make decisions and to control its actions within a given environment.

The first component of the framework discussed in this article is the navigation function (NF) path planner. NF planners are designed as a special class of artificial potential functions (APFs), wherein the local minimum problem is eliminated. In fact, within the potential field of a NF, there exists a unique global minimum located at an objective destination in the given environment. ^{2 –4} And there are two main approaches found in the literature for constructing a suitable NF.

The first method is to define an analytic function which possesses an attractive component associated with the objective and repulsive components attached to the obstacles. These approaches are motivated by the research introduced by Khatib ⁵ for online collision avoidance using APFs. Examples of the analytical approach with an NF are presented in previous studies, ^{6 –8} whose results originate from the NF definition introduced by Rimon and Koditschek. ⁴ With this approach, the NF is defined as a composition of several functions each designed to satisfy specific properties established by Rimon and Koditschek. ⁴ And while effective, this method requires proper tuning of several parameters within the function before the local minima can be removed and for the NF to be properly defined.

The second approach is to define the NF numerically in a discrete environment. Examples of this approach come in two flavors, either through wavefront expansion as described in previous works ^2,3,9,10 or through numerical solutions to harmonic partial differential equations (PDE) as put forth in the literature. ^{11 –13} With the harmonic PDE methods, the boundary conditions of the numerical solvers are set for the obstacles and the objective to form a potential field with its unique and global minimum at the goal. In contrast, the wavefront expansion method identifies the goal and expands its potential through the free space and is computed based on the distance from the goal. ^2,3 Also, with the numerical approaches, the path plans are typically determined through a graph search method such as the best first graph search method.

The path plan algorithm presented in this article expands upon earlier results by the authors. ¹⁴ Our approach is based on the the wavefront expansion algorithm. However, in the original wavefront expansion, as described by Latombe, ² a distance-based metric from the goal is used to form the potential levels of the NF. With the new method, the potential contours are formed with a minimum control effort metric. Ultimately, this approach is dependent upon the kinematic model of the system being studied (model-based wavefront expansion method).

Other path planning techniques that make use of the control effort and the kinematic model of the system are discussed in terms of rapid exploring randomized trees (rrt and rrt*), ^15,16 kinodynamic rrt, ¹⁷ and probabilistic roadmap approach. ¹⁸ These methods use a randomized approach with state information of the system to determine the path plans, and the paths are designed with information of the environment and the initial location. While these methods can be computationally intensive, depending on the model, they also generate plans without needing to know where the objective is located.

In contrast, the path plan algorithm introduced in this article can generate reference paths from anywhere in the free environment. However, the algorithm will require knowledge of both the objective location as well as a final desired state and would need to be regenerated if a new objective is given. The result, however, is a path plan that will guide the robot safely to its objective, while also considering the control effort to get there as well as how to form its approach to a goal state. Additionally, the path plan algorithm enables the inclusion of the kinematics of the mobile robot in its formation without increasing the dimensions of the configuration space of the system. The planner in this article considers the model of the mobile robot to consist of four state variables, but it generates the path plan within a two-dimensional environment making it computationally cheap.

The second component of the framework put forth in this article is the generation of high-level guidance commands based on a nonlinear model predictive control (NMPC) design. This approach is used to generate high-level guidance commands for the mobile robot to track a given trajectory. The derived guidance commands will be unique since it will make use of the state-dependent coefficient (SDC) form of the nonlinear kinematic equations, and the system’s inputs will be found by quadratic programming. The SDC formulation is used because it can preserve the nonlinear nature of the system being studied.

The use of model predictive control, or receding horizon control (RHC), is a widely researched topic in controls engineering for a variety of applications. Model predictive control is a process control method where the current inputs to the system are determined by forecasting the behavior of the system model over a finite horizon. The control is designed to minimize a cost function over the finite horizon and can be used for regulation or tracking of a reference trajectory. ¹⁹ Traditionally, NMPC involves linearizing the system’s dynamics about a nominal trajectory. ^{19 –21} Kwon and Han ¹⁹ and Rawlings ²⁰ give an overview of the traditional formulation for NMPC.

The SDC form for nonlinear systems as it pertains to controller, observer, or filter design is discussed previous research. ^{22 –25} The focus of using the SDC form is to transform a nonlinear system into a psuedo-linear form and then implement optimal control or estimation techniques through solving the state dependent Riccati equation (SDRE). As it relates to control design, the SDC form allows for the synthesis of nonlinear feedback controllers that are similar to the linear quadratic regulator (LQR) structure.

The SDC form and control designs using the SDRE discussed by researchers ^{22 –25} center on solutions over an infinite time horizon for LQR types of problems. In contrast, the results described by Heydari and Balakrishnan ²⁶ and Khamis and Naidu ²⁷ look at solutions for control problems with a finite horizon. Both papers include a change of variables to solve for the control over a finite horizon as well as fixed terminal constraints, so that they can achieve their respective objectives. ^26,27

In general, studies ^{22 –27} highlight the use of the SDC form and the nonlinear feedback control laws that can be found through solving the SDRE. These results, however, do not consider constraints on the system’s inputs or outputs (other than some terminal constraints, shown in previous studies ^26,27 ). The novelty of the contribution of this article, using the SDC formulation and NMPC, is that it will enforce both input and output constraints while performing reference trajectory tracking.

An alternate approach for NMPC using an SDC approach employs a Control Lyapunov Function (CLF) to help with achieving stability and applying an approximation of the terminal cost to the tail of the infinite horizon problem. ^{28 –33} The CLF approach for RHC along with an SDC-factorized system is shown by Sznaier and colleagues, ^30,31 without considering constraints. Primbs ²⁸ presents the CLF technique in detail with RHC and focuses on time varying and input constrained systems. Jadbabaie ²⁹ also covers the CLF approach with RHC, where the developments pertain to the stabilization of an unconstrained nonlinear systems and use the CLF as a terminal cost function.

The contribution of this component of the framework covered in this article is motivated by the developments of Ru and Subbarao. ³⁴ However, the research presented distinguishes itself in that the NMPC guidance design is applied to a wheeled mobile robot which is verified in simulations. Overall, the contribution from this aspect of the work comes from the application of a combined planning and guidance scheme to a mobile robot vehicle and its comparison with another nonlinear guidance design based on backstepping.

Problem description

The proposed framework in this article will consist of two main capabilities meant to enhance the autonomous capabilities of a mobile robot. Figure 1 illustrates the main flow of information in a typical guidance, navigation, and control framework. The main contributions in this article will be in the areas of a NF path planner and two different nonlinear guidance designs, as highlighted in Figure 1. For the results given, it is assumed that information concerning the environment such as obstacle positions and objectives are known to the vehicle and that the maneuvers occur within a flat environment.

Figure 1.

Block diagram depicting general guidance, navigation and control (GNC) framework.

The first capability of the framework evident with the path planning algorithm is that it has the ability to guide the vehicle safely through a hazardous, obstacle laden environment to a reachable state. This aspect of the work will be addressed through a special class of APFs called NFs. The objective with this component of the framework is to introduce the technique with the vehicle’s kinematic model and to generate a path plan that directs the approach of the vehicle to a final desired reachable state.

The second capability is the guidance law which generates high-level commands to have the vehicle track a desired path through the environment. This aspect within the framework is handled through two different nonlinear guidance techniques. The first technique is based on a NMPC derivation and the second is a backstepping-based design. The NMPC technique applied in this framework is given in “SDC-based NMPC” section of this article and is based on the developments found by Ru and Subbarao. ³⁴ For this methodology, the SDC form is used to preserve the nonlinear nature of the vehicle’s kinematic equations and the weight on the terminal state of the system is found by solving the discrete time SDRE. The guidance commands are then found by quadratic programming with input and state constraints enforced on the system.

Then, the backstepping guidance law in this framework is presented in the “Nonlinear guidance law” section of this article. This approach is chosen for comparison with the NMPC design as it is an extensively studied method used for controlling differential drive mobile robots. This guidance law is designed to provide stable, high-level guidance commands and will be verified through simulation.

The simulation results involve the combination of the NF path planner with the nonlinear guidance designs to close the loop of the framework. The overall contributions of this article are to first present the path planning algorithm considering the kinematic model of a two-wheeled mobile robot. Second, this article will provide insight into the capabilities of two different nonlinear guidance techniques while tracking a reference trajectory fitted along a path plan through an obstacle laden environment.

Kinematic model of the mobile robot vehicle

The kinematic model of the vehicle is illustrated in Figure 2. For the results discussed, an East-North-Up convention is used. The heading angle, ψ, is defined positively going in a counterclockwise direction about the z-axis (up) going from the inertial x-axis (x_I) to the body x-axis (x_b). The state of the vehicle is taken as the inertial x and y position, its heading angle (ψ), and its forward velocity (v), and it can be represented by the vector $x={\left[x,y,\psi ,v\right]}^{\text{T}}$ . Without loss of generality, the subscript I for the inertial x and y positions of the robot is dropped.

Figure 2.

Kinematics of the robot vehicle.

The set of nonlinear kinematic equations used to represent the mobile robot are given by

$\begin{array}{l}\dot{x}=v\text{cos}\left(\psi \right)\hfill \\ \dot{y}=v\text{sin}\left(\psi \right)\hfill \\ \dot{\psi }=u_1\hfill \\ \dot{v}=u_2\hfill \end{array}$

1

where u₁ and u₂ are the guidance commands. These equations can be used as a representative model with control over the heading angle turn rate and the vehicle’s forward acceleration. The set of equations defined in equation (1) are used for the path planning algorithm and the nonlinear guidance derivations.

The equations of motion given in equation (1) also need to be placed in linear state-space form for use in the path planning algorithm. The linear equations of motion are placed in the form

$\delta \dot{x}=A\delta x+B\delta u$

2

where the $A\in {\Re }^{4\times 4}$ and $B\in {\Re }^{4\times 2}$ matrices are evaluated through equations (3) and (4), respectively, and $\delta x\in {\Re }^4$ and $\delta u\in {\Re }^2$ are perturbations of the state and control vectors from the nominal trajectory, that is, $\delta \mathbf{x}=\mathbf{x}-{\mathbf{x}}_n$ and $\delta u=u-u_n$ . The linear system A and B matrices for the model are defined as

$A=\left[\begin{array}{cccc}0& 0& -v_n\text{sin}\left({\psi }_n\right)& \text{cos}\left({\psi }_n\right)\\ 0& 0& v_n\text{cos}\left({\psi }_n\right)& \text{sin}\left({\psi }_n\right)\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]$

3

and

$B=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 1& 0\\ 0& 1\end{array}\right]$

4

For the path planning algorithm, the nominal trajectory for the robot is considered as a straight line, non-accelerating trajectory between two points in a grid over a constant sample time, $\Delta t$ . The nominal values for ${\psi }_n$ and v_n are evaluated as

${\psi }_n={\text{tan}}^{-1}\left(\frac{\Delta y}{\Delta x}\right)$

5

$v_n=\sqrt{{\left(\frac{\Delta x}{\Delta t}\right)}^2+{\left(\frac{\Delta y}{\Delta t}\right)}^2}$

6

Within the path planning algorithm, the values for $\Delta x$ and $\Delta y$ are evaluated as the cell difference of two neighboring points in a discrete grid.

Path plan algorithm

The main contribution of this research is the construction of the NF, which is motivated by the wavefront expansion described in previous studies. ^2,3 However, the novel design discussed here distinguishes itself from the traditional methods, since it will use a metric based on the control effort of the system to form the contour levels in the potential field as opposed to a distance-based metric. The work presented in this article expands on the previous results ¹⁴ by studying the linear kinematic model of a mobile robot and observing how it influences the potential contour levels with the algorithm. The end result is a path plan algorithm that is model-dependent.

The full algorithm is outlined in Figure 3 with the contributions to the algorithm highlighted. These modifications were added to the original wavefront expansion algorithm to enable the inclusion of a metric defined by the control effort of the system and allow for some constraints to be considered.

Figure 3.

Control effort-based NF algorithm. NF: navigation function.

The method introduced in this article will make use of the solution to the minimum control effort problem given a fixed initial and final state for a linear system. This method is constructed to plan a path to an objective reachable state, expressed as $x_{\text{goal}}$ , and will form a minimum control effort path plan. This section will first explain how the model is defined and used in the context of the planner. Then, a general overview of the algorithm will be given with descriptions of the main blocks from the flowchart depicted in Figure 3.

Minimum control effort approach

The minimum control effort–based path plan is designed considering the solution to the optimal control problem for finding the minimum energy controller. The cost function associated with this problem is given by

$\underset{u\in {\Re }^m}{\text{min}}J=\frac{1}{2}{\displaystyle {\int }_{t_0}^{t_f}{u}^{\text{T}}u\hspace{0.17em} \text{d}t}$

7

subject to

$\begin{array}{l}\dot{x}=Ax+Bu\\ x\left(t_0\right)=x_0\\ x\left(t_f\right)=x_f\end{array}$

Within the context of the grid-based path planner, the above expression represents the cost going from one grid point at t₀ to a neighboring grid point at t_f (denoted as $x_0$ and $x_f$ , respectively). The solution to this problem can be found in most optimal control textbooks such as the one by Lewis et al. ³⁵ The analytic solution for the open-loop, minimum energy controller for a linear model is given by

$u=B^T{\Phi }^T\left(t_f,t\right)W_R^{-1}\left(t_0,t_f\right)\left[\delta x_f-\Phi \left(t_f,t_0\right)\delta x_0\right]$

8

where $\delta x_f$ and $\delta x_0$ are perturbations of the state about the nominal trajectory at t_f and t₀, respectively. Also, $W_R\left(t_0,t_f\right)$ is the reachability Gramian and $\Phi \left(t_f,t\right)$ is the state transition matrix of the system. ³⁵ The state transition matrix is computed as

$\Phi \left(t_f,t\right)=e^{A(t_f-t)}$

and the reachability Gramian is computed by

$W_R\left(t_0, t_f\right)={\displaystyle {\int }_{t_0}^{t_f}}\hspace{0.17em}\Phi \left(t_f, \tau \right)B{B}^T{\Phi }^T\left(t_f, \tau \right)\text{d}\tau$

9

which needs to be non-singular in order for the state at t_f to be reachable.

Solving for the reachability Gramian using equation (9) is not straightforward; however, there are simplifications to find a solution which exist in the linear systems literature, such as the study by Chen. ³⁶ One method to find a solution to equation (9) is to look at the analytic solution of a differential Lyapunov equation. The solution to the differential Lyapunov equation of the form

$\dot{P}=AP+P{A}^{\text{T}}+B{R}^{-1}{B}^{\text{T}}$

10

is given by

$P(t)=\Phi \left(t,t_0\right)P\left(t_0\right){\Phi }^{\text{T}}\left(t,t_0\right)+{\displaystyle {\int }_{t_0}^t\Phi \left(t, \tau \right)B{R}^{-1}{B}^{\text{T}}{\Phi }^{\text{T}}\left(t, \tau \right)\text{d}\tau }$

11

Therefore, by taking $P\left(t_0\right)=0$ and setting R equal to the identity matrix, the right hand side of equation (11) is equal to the right hand side of equation (9). Hence, the reachability Gramian can be found by integrating the differential Lyapunov equation from t₀ to t_f, in equation (10), assuming $W_R\left(t_0,t_0\right)=0$ .

Additionally, the aspect of reachability for the system being studied is formally addressed with the following remark. The motivation for this assertion can be found in the discussions of chapter 6 in the study by Chen ³⁶ and chapter 3 in the study by Williams and Lawrence ³⁷ involving the concept of a system’s controllability and its implications with regards to reachability. The aim is to show that if the linear system is controllable, then it can be directed to successive local reachable states, with each one leading the system to the final objective reachable state at the end of the path plan.

Remark 1

Consider the nonlinear system in equation (1) and it’s linearization in equation (2) about the nominal state, ${\mathbf{x}}_n$ and $u_n$ . If the linear system is controllable, then the nonlinear system is locally controllable in the neighborhood of $\left(x_n\mathrm{,}{u}_n\right)$ . The linear and the nonlinear system are also locally reachable. Thus, successive state transitions of the linear system from controllable to controllable configurations ensure that the nonlinear system is transferred through locally reachable states to the goal location. ^36,37

In equation (2), the input vector, $u\in {\Re }^2$ , is defined as $u={\left[\delta u_1, \delta u_2\right]}^{\text{T}}$ , where $\delta u_1$ and $\delta u_2$ are the commanded heading angle turn rate and forward acceleration, respectively.

A and B for the linear kinematic model are obtained using the nominal trajectory. The values for the nominal trajectory $x_n$ are computed by specifying a sampling time, $\Delta t$ , and assume it is a straight line maneuver between the node position from $x_0$ (i.e. the values x₀ and y₀) and the node position from $x_f$ (i.e. the values x_f and y_f) within a grid as illustrated in Figure 4. Then, the nominal heading angle is taken as the direction pointing from the node position of $x_0$ to the node position of $x_f(i)$ within the grid, given by

${\psi }_n={\text{tan}}^{-1}\left(\frac{y_f-y_0}{x_f-x_0}\right)$

Figure 4.

Grid set up in two-dimensional space for a mobile robot model.

And the nominal velocity is computed as

$v_n=\sqrt{{\left(\frac{x_f-x_0}{\Delta t}\right)}^2+{\left(\frac{y_f-y_0}{\Delta t}\right)}^2}$

To compute the control effort from equation (8), the state information at t₀ and at t_f needs to be determined. Within the path planning algorithm, the final state at each level is taken as the ith state extracted from the kth list, that is, $x_f(i)$ , which is initially set as the desired final reachable state. The final objective reachable state is denoted as the vector $x_{\text{goal}}^{\text{MCE}}={\left[x_g, y_g, {\psi }_g, v_g\right]}^{\text{T}}$ . And, Figure 4 illustrates how the state is interpreted for the kinematic model within a discrete grid environment as considered in this algorithm.

At t₀, the state information is denoted as $x_0$ . And the state vector, $x_0$ , is found by considering the 2-neighbor points of $x_f(i)$ and is completed assuming that the vehicle at the neighbor location is approaching the state at t_f. In other words, the position information for $x_0$ is gathered from its position in the discrete grid. And the values that need to be computed in order to complete the state, $x_0$ , are the heading angle (ψ₀) and velocity (v₀). The heading angle for the neighboring points is computed by

${\psi }_0={\text{tan}}^{-1}\left(\frac{{\dot{y}}_0}{{\dot{x}}_0}\right)$

and the velocity as

$v_0=\sqrt{{\dot{x}}_0^2+{\dot{y}}_0^2}$

where the directional velocities are calculated as

${\dot{y}}_0=v_f\text{sin}\left({\psi }_f\right)+\frac{y_f-y_0}{\delta t}$

${\dot{x}}_0=v_f\text{cos}\left({\psi }_f\right)+\frac{x_f-x_0}{\delta t}$

where $\delta t=t_f-t_0$ .

Finally, in order to make use of the solution to the minimum energy controller in equation (8), the state perturbations $\delta x_f$ and $\delta x_0$ need to be set. The values are determined by taking the difference between the state at t_f, or t₀, and the nominal values, $x_n$ , as

$\delta x_f=x_f-x_n$

$\delta x_0=x_0-x_n$

This procedure is done for each of the free neighbors of $x_f\left(i\right)$ . This approach will ultimately influence the shaping of the NF’s contours based on the final desired reachable state that considers the linear kinematic model of a mobile robot with the minimum energy controller solution.

Initialization block

Now, with an understanding of how the minimum energy controller influences the potential levels in the NF, an explanation of the modified wavefront expansion is given. The first step of the algorithm takes place with the initialization block, as shown in Figure 3.

The initialization block of this numerically constructed potential field begins by discretizing the workspace into an evenly spaced grid. This grid can be scaled to fit over any two-dimensional working environment. The discrete grid and the obstacle positions are then used to form a bitmap representation of the environment. This allows the free points to be identified and extracted. Note that the free space is denoted as $q_{free}$ in the algorithm flowchart in Figure 3, where q, in general, represents a configuration in the workspace. In the two-dimensional workspace considered in this article, $q\in {\Re }^2$ , and is denoted by the vector $q={\left[x, y\right]}^{\text{T}}$ .

Also, during initialization, the obstacle potential levels are set uniformly to a large number and these configurations are separated for the rest of the algorithm. The potential level of the desired final state is then set to zero, to ensure that it is the global minimum, and these values are inserted into a list, list _k , where the index k = 0 initially, that is, ${\text{list}}_0\left(x_{\text{goal}}\mathrm{,0}\right)$ .

Another attribute that is set during initialization is the model to base the NF generation. The model will motivate how to set the final objective state, $x_{\text{goal}}$ , how the state is approximated in the neighboring points, and how the cost associated with the control effort is computed. For the results in this article, the model is defined based on the linear kinematic equations for a two-wheeled differential drive mobile robot.

Main block

Once set, the information from the initialization block is passed into the algorithm’s main block to generate the NF. Within the algorithm, the states and potentials within list _k are used to evaluate the potential values of their neighbors, $x_0(j)$ , where the index j denotes the j^th neighbor point. For two-dimensional workspace examples, the algorithm considers each of the 2-neighbors of the ith state, $x_f(i)$ , in list _k . The 2-neighbors are defined as the configurations in the grid that have at most two coordinates differing from the central point. There are eight 2-neighbors for a configuration when considering a two-dimensional grid. ²

Next, it must be determined if the neighbor point is in the free space and if the potential has not yet been computed. If the neighbor point is both free and has not been visited by the wavefront expansion, then the algorithm continues; otherwise, that point is disregarded and the next neighbor point, $x_0(j+1)$ , is considered. This step ensures that there are no overlapping values and that the NF is only evaluated in the free space.

Also, for the free neighbor points, the algorithm completes the state information of $x_0(j)$ , since the only information known at this point is the position in the discrete grid. Additionally, the control effort, to go from $x_0(j)$ to $x_f(i)$ , is computed based on the method chosen. And the value for this control effort, $u\left(x_0(j), x_f(i)\right)$ , is calculated using equation (8).

The resulting control vector and position information are compared with the constraints

$||u_0||\le u_{\text{max}}$

12

$d\le d_{\text{eff}}$

13

where $u_0=u(x_0(j), x_f(i))$ and $u_{\text{max}}\in \Re$ and $d_{\text{eff}}\in \Re$ are scalar parameters denoting the maximum control limit and effective distance, respectively, which are set during initialization. The value of d, in equation (13), represents the Euclidean distance of the corresponding neighbor point, $x_0(j)$ , to the final desired position in the grid, $x_{\text{goal}}$ .

Effectively, the d_eff term in equation (13) does make this method partially dependent on a measure of distance. However, the d_eff term is associated solely with the maximum control effort constraint, u_max. The combination of the constraints in equations (12) and (13) is used to enforce the notion that it would be cost prohibitive to approach the final desired state from certain configurations, such as facing the opposite direction at close proximity. The d_eff parameter in this sense restricts the u_max constraint to only affect grid points within a certain neighborhood of the goal and has no bearing on the potential levels through the rest of the free space.

Now, if the constraints in equations (12) and (13) are violated at a given neighbor point, $x_0(j)$ , then its associated cost is set equal to that of the obstacle configurations, that is, $J(x_0\left(j\right))=J\left(\text{obs}\right)$ . But, if the cost for the neighboring states do not violate the constraints, then it is computed by

$J(x_0(j))=\frac{1}{2}\left({u_0}^{\text{T}}{u}_0\right)\delta t+J(x_f(i))$

14

where $\delta t$ is a time step set during initialization for the maneuver to occur. The cost computed in equation (14) is used to create the contour levels making up the NF.

There are a couple of considerations with the cost function in equation (14). The first is that the cost is always increasing from one level to the next with the wavefront expansion, and this feature is what enables us to claim that there is a unique and global minimum at the goal. Second, the choice of grid resolution, or the spacing between the grid points, and the time-step parameter $\delta t$ , does affect the magnitude of the cost through the grid; however, they do not affect the overall shape of the potential field and its resulting path plan.

Finally, when the cost for $x_0(j)$ is computed, it is appended to the bottom of a new list, indexed at k + 1, written as

${\text{list}}_{k+1}(x_0\left(j\right),J(x_0\left(j\right)))$

in Figure 3. This process is continued for each neighboring configuration of $x_f(i)$ .

If all the neighbors have been visited, then the next final state and cost in list _k are used, that is, $x_f\left(i+1\right)$ and $J(x_f\left(i+1\right))$ . However, if all the elements in list _k have been used, then the values from the next list, ${\text{list}}_{k+1}$ are considered. The algorithm continues until ${\text{list}}_k$ is empty. When ${\text{list}}_k$ is empty, then all the points in $q_{free}$ that are connected to the objective position have been visited by the wavefront expansion and the NF generation is finished.

Once the NF algorithm finishes, the reference path, $x_r$ , is obtained through a best-first graph search following the negative gradient of the resulting potential field from a given initial location to the objective location. Through this method, the paths are attained by observing the potential values of the neighboring points and then choosing the neighbor with the lowest value. This is done in an iterative manner until the objective is found. In other words, the path is found by starting at some initial location in the environment and continuing along a path of minimal control effort until it reaches the goal.

The resulting path plan is used to provide a set of reference position coordinates, $(x_r\mathrm{,}{y}_r)\subseteq x_r$ , used to guide the robot safely through the environment and approach a final desired state. A trajectory can then be determined along the path plan, as described in Appendix 1. The trajectory is used to provide the reference values needed by the two guidance techniques covered in this article.

SDC-based NMPC

The primary guidance technique discussed in this article uses a NMPC approach and is presented in this section. The procedure is motivated by the work of Ru and Subbarao, ³⁴ where the performance of the traditional approach of NMPC is compared with that of a SDC approach. The objective in this section is to present this method for an autonomous mobile robot and verify its trajectory tracking capability in simulation and then compare its performance with the results from a different and widely researched nonlinear guidance approach.

SDC representation of the vehicle kinematics

Given a general nonlinear system of the form

$\dot{x}=f(x)+g(x)u$

a state-space representation of the system is obtained wherein the system matrices are given as functions of the current state of the system as

$\begin{array}{c}\dot{x}=A(x)x+B(x)u\\ y=C(x)x+D(x)u\end{array}$

15

where $x\in {\Re }^n$ , $u\in {\Re }^m$ , and $y\in {\Re }^{n_o}$ are, respectively, the state, input, and output vectors, and $A(x)\in {\Re }^{n\times n}$ , $B(x)\in {\Re }^{n\times m}$ , $C(x)\in {\Re }^{n_o\times n}$ , and $D(x)\in {\Re }^{n_o\times m}$ are the continuous state dependent system matrices in the SDC factored form. The pairs $(A\left(x\right),B\left(x\right))$ and $(A\left(x\right),C\left(x\right))$ are considered controllable and observable $\forall x\in {\Re }^n$ , respectively.

In general, the formation of SDC matrices for the system are not unique unless observing a scalar system. ^{22 –25} Therefore, different SDC forms may be obtained for a given system and solutions to the optimization problems posed may vary. Two particular SDC factorizations of the kinematics, from equation (1), are derived for the results in this article.

For the results, it is assumed that the full state is available at each instant, that is, the system output matrix $C(x)=C$ is considered as the constant identity matrix, that is, $I\in {\Re }^{4\times 4}$ . Also, from the kinematic equations, it is evident that $D(x)=0$ and the input matrix $B(x)=B$ is a constant matrix given by

$B=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 1& 0\\ 0& 1\end{array}\right]$

16

And one possible solution for the SDC factored $A\left(x\right)$ matrix can be given by

$A\left(x\right)=\left[\begin{array}{cc}{0}_{2\times 2}& A_{12}(x)\\ 0_{2\times 2}& 0_{2\times 2}\end{array}\right]$

where

$A_{12}\left(x\right)=\left[\begin{array}{cc}v\left(\frac{{\psi }^3}{4!}-\frac{\psi }{2!}\right)& 1\\ v\text{cos}\left(\frac{\psi }{2}\right)\left(\frac{1}{2}-\frac{{\psi }^2}{2^{3}3!}+\frac{{\psi }^4}{2^{5}5!}\right)& \text{sin}\left(\frac{\psi }{2}\right)\text{cos}\left(\frac{\psi }{2}\right)\end{array}\right]$

17

This result is derived using the following trigonometric identity and expansions

$\begin{array}{c}\text{sin}\hspace{0.17em}(2\psi )=2\text{sin}\hspace{0.17em}(\psi )\text{cos}\hspace{0.17em}(\psi )\\ \text{sin}\hspace{0.17em}(\psi )=\psi -\frac{{\psi }^3}{3!}+\frac{{\psi }^5}{5!}-\mathrm{...}\\ \text{cos}\hspace{0.17em}(\psi )=1-\frac{{\psi }^2}{2!}+\frac{{\psi }^4}{4!}-\mathrm{...}\end{array}$

The above derivation of the $A_{12}$ quadrant in equation (17) has some issues in implementation where the heading angle is limited from $-\frac{\pi }{2}\hspace{0.17em} \le \hspace{0.17em} \psi \hspace{0.17em} \le \hspace{0.17em}\hspace{0.17em}\frac{\pi }{2}$ . Therefore, another formulation of the $A_{12}$ quadrant is derived as

$A_{12}\left(x\right)=\left[\begin{array}{cc}v\left(\frac{{\psi }^3}{4!}-\frac{\psi }{2!}\right)& 1\\ v\left(-\frac{{\psi }^2}{3!}+\frac{{\psi }^4}{5!}\right)& \psi \end{array}\right]$

18

Note that with both SDC matrix results from equations (17) and (18), the psuedo-linear system $A(x)$ matrix becomes rank deficient when $v=0$ . This complication can be addressed by adding a state constraint on the velocity so that $v>0$ in the solution. Also, note that if $\psi =0$ , the resulting SDC matrix with $A_{12}$ coming from equation (18) becomes uncontrollable. Therefore, in the simulations, the $A(x)$ matrix is switched from using the $A_{12}$ quadrant from equation (18) to using the formulation in equation (17), whenever $\parallel \psi \parallel \le {\psi }_{tol}$ . Where ${\psi }_{\text{tol}}$ is a tolerance value defined in the interval $\left[-\pi /2,\pi /2\right]$ .

Next, a discrete-time equivalent of the system shown in equation (15) can be obtained by introducing a zero-order-hold (ZOH) with a specified sample time ( $\Delta t$ ). With the ZOH, a sampled point is held constant over a sampled time interval. The smaller the sample time, the more accurate the approximation of the continuous signal. The discrete-time equivalent system is given in the following form

$\begin{array}{c}{x}_{k+1}=\Phi (x_k)x_k+\Gamma u_k\\ y_k=C{x}_k\end{array}$

19

where $\Phi (x_k)$ and $\mathrm{\Gamma }$ are discrete approximations of the continuous $A(x)$ and B matrices respectively. The discrete system matrices in equation (19) are of the SDC form and can be considered as constants over the sampling time, $\Delta t=t_{k+1}-t_k$ , where the interval is given by $[t_k\mathrm{,}{t}_{k+1})$ .

The discrete-time system in equation (19) can be placed in batch form for an N-step prediction horizon as

$X_k=F(x_k)x_k+H(x_k)U_k$

20

where $X_k$ , $F(x_k)$ , $H(x_k)$ , and $U_k$ are defined as

$X_k=\left[\begin{array}{c}{x}_k\\ x_{k+1}\\ ⋮\\ x_{k+N-1}\end{array}\right],U_k=\left[\begin{array}{c}{u}_k\\ u_{k+1}\\ ⋮\\ u_{k+N-1}\end{array}\right],F(x_k)=\left[\begin{array}{c}I\\ \Phi (x_k)\\ ⋮\\ {\Phi }^{N-1}(x_k)\end{array}\right]$

$H(x_k)=\left[\begin{array}{ccccc}0& & & & \\ \Gamma & 0& & & \\ \Phi (x_k)\Gamma & \Gamma & 0& & \\ ⋮& ⋮& \ddots & \ddots & \\ {\Phi }^{N-2}(x_k)\Gamma & {\Phi }^{N-3}(x_k)\Gamma & \dots & \Gamma & 0\end{array}\right]$

And the terminal state at the end of the prediction horizon is defined as

$x_{k+N}={\Phi }^N(x_k)x_k+\overline{\Gamma }(x_k)U_k$

21

where

$\overline{\Gamma }(x_k)\triangleq \left[\begin{array}{ccccc}{\Phi }^{N-1}(x_k)\Gamma & {\Phi }^{N-2}(x_k)\Gamma & \cdots & \Phi (x_k)\Gamma & \Gamma \end{array}\right]$

NMPC design

The guidance commands are obtained as the solution to the minimization of the finite-horizon linear quadratic tracking cost function with free final state subject to state and input constraints. The cost function is given as

$J(x_k,x_k^r,u_k)={\displaystyle \sum _{j=0}^{N-1}}\left[{(x_{k+j}-x_{k+j}^r)}^{\text{T}}Q(x_{k+j}-x_{k+j}^r)+u_{k+j}^{\text{T}}R{u}_{k+j}\right]+{(x_{k+N}-x_{k+N}^r)}^{\text{T}}{Q}_f(x_{k+N}-x_{k+N}^r)$

22

where $x_k^r\in {\Re }^n$ in general denotes reference trajectory at the kth time step. The above cost function can be placed into batch form for the SDC system as

$J(x_k,x_k^r,u_k)={\left(X_k-X_k^r\right)}^{\text{T}}{\overline{Q}}_N\left(X_k-X_k^r\right)+U_k^{\text{T}}{\overline{R}}_N{U}_k+{\left(x_{k+N}-x_{k+N}^r\right)}^{\text{T}}{Q}_f\left(x_{k+N}-x_{k+N}^r\right)$

where

$X_k^r=\left[\begin{array}{c}{x}_k^r\\ x_{k+1}^r\\ ⋮\\ x_{k+N-1}^r\end{array}\right]$

is the batch form of the reference trajectory over the N-step horizon and ${\overline{Q}}_N=\text{diag}\left\{Q\mathrm{,}\cdots \mathrm{,}Q\right\}$ and ${\overline{R}}_N=\text{diag}\left\{R\mathrm{,}\cdots \mathrm{,}R\right\}$ are block diagonal matrices consisting of the state and input weighting matrices, respectively.

After proper substitution, the objective function can be rewritten in a quadratic-like form as

$\begin{array}{c}J\left(x_k\mathrm{,}{x}_k^r\mathrm{,}{u}_k\right)=U_k^{\text{T}}\left(H(x_k{)}^{\text{T}}{\overline{Q}}_{N}H(x_k)+{\overline{R}}_N+\overline{\Gamma }{(x_k)}^{\text{T}}{Q}_f\overline{\Gamma }(x_k)\right)U_k\\ +2\left[{\left(F(x_k)x_k-X_k^r\right)}^{\text{T}}{\overline{Q}}_{N}H(x_k)+{\left(\Phi {(x_k)}^N{x}_k-x_{k+N}^r\right)}^{\text{T}}{Q}_f\overline{\Gamma }(x_k)\right]U_k\\ +{\left(F(x_k)x_k-X_k^r\right)}^{\text{T}}{\overline{Q}}_N\left(F(x_k)x_k-X_k^r\right)+{\left(\Phi {(x_k)}^N{x}_k-x_{k+N}^r\right)}^{\text{T}}{Q}_f\left(\Phi {(x_k)}^N{x}_k-x_{k+N}^r\right)\end{array}$

23

The objective function depends on the current state of the system and is calculated at the beginning of every sample interval. Also, the weight matrix for the final state, $Q_f$ , is obtained by solving the state-dependent discrete algebraic Riccati equation (SDDARE) at time instant k. The SDDARE is of the form

$P(x_k)=\Phi {(x_k)}^{\text{T}}P(x_k)\hspace{0.17em}\Phi (x_k)-\Phi {(x_k)}^{\text{T}}P(x_k)\hspace{0.17em}\Gamma {\left(R+{\text{G}}^{\text{T}}P(x_k)\hspace{0.17em}\Gamma \right)}^{-1}{\Gamma }^{\text{T}}P(x_k)\hspace{0.17em}\Phi (x_k)+Q$

24

where $Q_f$ is set equal to the solution $P(x_k)$ . The solution to equation (24) is found at each sample instant k.

Input and state constraints

The constraints are used in this formulation to ensure that the tracking capabilities of the system are feasible in simulation and in practice. The nonlinear kinematic equations for the vehicle represented in equation (1) enable constraints to be placed on the heading angle turn rate and acceleration of the vehicle without added complication. Overall, the form of the kinematics chosen enables us to place constraints on the system’s velocity, heading angle, its turn rate, and its forward acceleration. In general, the input and state constraints are represented by the following inequalities

$\begin{array}{llllll}{u}_{lb}\hfill & \le \hfill & u_{k+j}\hfill & \le \hfill & u_{ub},\hfill & j=0,1,\cdots ,N-1\hfill \\ g_{lb}\hfill & \le \hfill & G{x}_{k+j}\hfill & \le \hfill & g_{ub},\hfill & j=0,1,\cdots ,N\hfill \end{array}$

25

where the subscripts lb and ub denote the lower and upper bounds, respectively, of the constraints and the matrix G is considered as an output matrix for the states that are constrained. The constraints can be placed in batch form as

$\left[\begin{array}{c}{u}_{lb}\\ u_{lb}\\ ⋮\\ u_{lb}\end{array}\right]\le U_k\le \left[\begin{array}{c}{u}_{ub}\\ u_{ub}\\ ⋮\\ u_{ub}\end{array}\right],\left[\begin{array}{c}{g}_{lb}\\ g_{lb}\\ ⋮\\ g_{lb}\end{array}\right]\le {\overline{G}}_N\left(F{x}_k+H{U}_k\right)\le \left[\begin{array}{c}{g}_{ub}\\ g_{ub}\\ ⋮\\ g_{ub}\end{array}\right]$

26

where ${\overline{G}}_N=\text{diag}\left\{G\mathrm{,}\cdots \mathrm{,}G\right\}$ is a block diagonal matrix formed by the output matrix G. Then, the constraints of the system need to be incorporated into a single matrix equation of the form

$M(x_k)U_k\le \Upsilon (x_k)$

27

where

$M(x_k)=\left[\begin{array}{c}{M}_U\\ {\overline{G}}_{N}H(x_k)\end{array}\right],\Upsilon (x_k)=\left[\begin{array}{c}{U}_b\\ g-{\overline{G}}_{N}F(x_k)\end{array}\right]$

and

$M_U=\left[\begin{array}{llll}\left[\begin{array}{r}\hfill I_{m\times m}\\ \hfill -I_{m\times m}\end{array}\right]\hfill & \hfill & \hfill & \hfill \\ \hfill & \left[\begin{array}{r}\hfill I_{m\times m}\\ \hfill -I_{m\times m}\end{array}\right]\hfill & \hfill & \hfill \\ \hfill & \hfill & \ddots \hfill & \hfill \\ \hfill & \hfill & \hfill & \left[\begin{array}{r}\hfill I_{m\times m}\\ \hfill -I_{m\times m}\end{array}\right]\hfill \end{array}\right]$

where $I_{m\times m}$ is the $m\times m$ identity matrix. Also,

$U_b=\left[\begin{array}{c}\left[\begin{array}{r}\hfill u_{ub}\\ \hfill -u_{lb}\end{array}\right]\\ \\ \left[\begin{array}{r}\hfill u_{ub}\\ \hfill -u_{lb}\end{array}\right]\\ ⋮\\ \left[\begin{array}{r}\hfill u_{ub}\\ \hfill -u_{lb}\end{array}\right]\end{array}\right],\hspace{1em}\hspace{1em}g=\left[\begin{array}{c}\left[\begin{array}{r}\hfill g_{ub}\\ \hfill -g_{lb}\end{array}\right]\\ \\ \left[\begin{array}{r}\hfill g_{ub}\\ \hfill -g_{lb}\end{array}\right]\\ ⋮\\ \left[\begin{array}{r}\hfill g_{ub}\\ \hfill -g_{lb}\end{array}\right]\end{array}\right]$

The guidance commands are synthesized by minimizing the quadratic cost function in equation (23), subject to the input and state constraints in equation (27) using quadratic programming.

Guidance command synthesis using the linear matrix inequality form

For readability with the following linear matrix inequality (LMI) formulation, the notation for state dependence is changed from using brackets to a subscript, in other words $(\cdot )(x_k)$ will be cast as ${(\cdot )}_k$ . First, note that the quadratic-like cost function in equation (23) can be decomposed into the following form

$J\left(x_k,x_k^r,u_k\right)=J_1\left(x_k,x_k^r,u_k\right)+J_2\left(x_k,x_k^r,u_k\right)$

where

$\begin{array}{c}{J}_1(x_k,x_k^r,u_k)=U_k^{\text{T}}{W}_k{U}_k+{\omega }_k^{\text{T}}{U}_k+{\left[F_k{x}_k-X_k^r\right]}^{\text{T}}{\overline{Q}}_{Nk}\left[F_k{x}_k-X_k^r\right]\\ J_2(x_k,x_k^r,u_k)={\left({\Phi }_k^N{x}_k+{\overline{\Gamma }}_k{U}_k-x_{k+N}^r\right)}^{\text{T}}{Q}_{fk}\left({\Phi }_k^N{x}_k+{\overline{\Gamma }}_k{U}_k-x_{k+N}^r\right)\end{array}$

and $W_k$ and ${\omega }_k$ are defined as

$\begin{array}{c}{W}_k=H_k^{\text{T}}{\overline{Q}}_{Nk}{H}_k+{\overline{R}}_N\\ {\omega }_k=2H_k^{\text{T}}{\overline{Q}}_{Nk}^{\text{T}}\left[F_k{x}_k-X_k^r\right]\end{array}$

Then, for each $x_k$ , there must exist a set of $(Q_{fk}\mathrm{,}{Q}_k\mathrm{,}{R}_k\mathrm{,}{U}_k)$ that satisfy the following conditions

$\begin{array}{l}{J}_1(x_k,x_k^r,u_k)\le {\gamma }_1\\ J_2(x_k,x_k^r,u_k)\le {\gamma }_2\end{array}$

28

$\left[\begin{array}{cc}{\gamma }_1-2x_k^{\text{T}}{F}_k^{\text{T}}{\overline{Q}}_{Nk}{H}_k{U}_k-x_k^{\text{T}}{F}_k^{\text{T}}{\overline{Q}}_{Nk}{F}_k{x}_k& U_k^{\text{T}}\\ U_k& {(H_k^{\text{T}}{\overline{Q}}_{Nk}{H}_k+{\overline{R}}_{Nk})}^{-1}\end{array}\right]\ge 0$

29

$\left[\begin{array}{cc}{\gamma }_2& {\left[{\Phi }_k^N{x}_k+\overline{\Gamma }{U}_k\right]}^{\text{T}}\\ {\Phi }_k^N{x}_k+\overline{\Gamma }{U}_k& Q_{fk}^{-1}\end{array}\right]\ge 0$

30

$\left[\begin{array}{cccc}{Q}_{fk}^{-1}& {\left({\Phi }_k{Q}_{fk}-\Gamma Y_k\right)}^{\text{T}}& {\left(\sqrt{Q_k}{Q}_{fk}^{-1}\right)}^{\text{T}}& {\left(\sqrt{R_k}{Y}_k\right)}^{\text{T}}\\ {\Phi }_k{Q}_{fk}-\Gamma Y_k& Q_{fk}^{-1}& 0_{n\times n}& 0_{n\times n}\\ \sqrt{Q_k}{Q}_{fk}& 0_{n\times n}& I_{n\times n}& 0_{n\times n}\\ \sqrt{R_k}{Y}_k& 0_{n\times n}& 0_{n\times n}& I_{n\times n}\end{array}\right]\ge 0$

31

$M(x_k)U_k\le \Upsilon (x_k)$

32

where $Y_k=K_k{Q}_{fk}^{-1}$ , and $I_{n\times n}$ , $0_{n\times n}$ denote the $n\times n$ identity matrix and $n\times n$ null matrix, respectively. The set of LMIs given above are used to establish the feasibility of the control design and are important for proving stability. When a suitable set of $(Q_{fk}\mathrm{,}{Q}_k\mathrm{,}{R}_k)$ satisfying the above conditions are obtained, the input over the N-step horizon, $U_K^{*}$ , can then be found through the following quadratic programming problem

$U_k^{*}=\underset{U_k}{\text{min}}\hspace{0.17em}{\gamma }_1+{\gamma }_2$

subject to the constraints in equations (29) to (32). And upon solving the quadratic programming problem, $u_k^{*}$ can be obtained by

$u_k^{*}=[I_{m\times m}\hspace{0.17em}{0}_{m\times m}\hspace{0.17em}\cdots \hspace{0.17em}{0}_{m\times m}]U_k^{*}$

33

which will extract the guidance commands corresponding to the kth time step to the system in equation (1). The stability of the closed-loop system using the SDC-based NMPC guidance law is given in Appendix 1.

Nonlinear guidance law

The second guidance technique investigated with this research is a backstepping-based approach which is chosen for comparison with the NMPC design for generating guidance commands for the mobile robot. Unlike the guidance law design using NMPC, the derivation in this section will not explicitly introduce constraints into the formulation.

Using the kinematic equations for the vehicle defined in equation (1), the guidance commands u₁ and u₂ influence the turn rate and forward acceleration of the robot. The guidance commands defined using the backstepping-based approach are

$u_1={\dot{\psi }}_d-{\lambda }_{\psi }{e}_{\psi }$

34

$u_2={\dot{v}}_d-{\lambda }_v{e}_v$

35

where ${\lambda }_{\psi }>0, \hspace{0.17em}{\lambda }_v>0$ , and where ${\psi }_d, \hspace{0.17em}{v}_d, \hspace{0.17em}{\dot{\psi }}_d ,\hspace{0.17em}\text{and} \hspace{0.17em}{\dot{v}}_d$ are nonlinear functions of the reference trajectory, the state and the state tracking errors; also, $e_{\psi }=\psi -{\psi }_d$ and $e_v=v-v_d$ .

The guidance commands are formulated to achieve stability in the sense of Lyapunov while tracking a reference trajectory. Note, the position tracking errors are given by $e_x=x-x_r$ and $e_y=y-y_r$ , and their time derivatives are ${\dot{e}}_x=\dot{x}-{\dot{x}}_r$ and ${\dot{e}}_y=\dot{y}-{\dot{y}}_r$ . The desired heading angle and velocity signals, ψ_d and v_d, respectively, are prescribed such that the position tracking error dynamics decay exponentially, that is, ${\dot{e}}_x=-{\lambda }_x{e}_x$ and ${\dot{e}}_y=-{\lambda }_y{e}_y$ for ${\lambda }_x>0$ and ${\lambda }_y>0$ . It follows that

$\begin{array}{l}{v}_d\text{cos}\left({\psi }_d\right)={\dot{x}}_r-{\lambda }_x{e}_x\\ v_d\text{sin}\left({\psi }_d\right)={\dot{y}}_r-{\lambda }_y{e}_y\end{array}$

36

Then, using equation (36), the desired heading angle and velocity signals are obtained as

${\psi }_d={\text{tan}}^{-1}\left(\frac{y_r-{\lambda }_y{e}_y}{x_r-{\lambda }_x{e}_x}\right)$

37

$v_d=\sqrt{{\left({\dot{x}}_r-{\lambda }_x{e}_x\right)}^2+{\left({\dot{y}}_r-{\lambda }_y{e}_y\right)}^2}$

38

The guidance commands are derived such that the heading angle and velocity tracking errors, $e_{\psi }=\psi -{\psi }_d$ and $e_v=v-v_d$ , also decay exponentially. Implying that the error dynamics should be ${\dot{e}}_{\psi }=-{\lambda }_{\psi }{e}_{\psi }$ and ${\dot{e}}_v=-{\lambda }_v{e}_v$ . Combining equation (1) with the state tracking error time derivatives given by ${\dot{e}}_{\psi }=\dot{\psi }-{\dot{\psi }}_d$ and ${\dot{e}}_v=\dot{v}-{\dot{v}}_d$ leads to

$\begin{array}{l}{u}_1={\dot{\psi }}_d-{\lambda }_{\psi }{e}_{\psi }\\ u_2={\dot{v}}_d-{\lambda }_v{e}_v\end{array}$

which are the terms introduced in equations (34) and (35).

The time derivatives for the desired heading angle and velocity values are obtained by differentiating equations (37) and (38) with respect to time, leading to

${\dot{\psi }}_d=\frac{1}{v_d}\left[\text{cos}\left({\psi }_d\right)\left({\ddot{y}}_r-{\lambda }_y{\dot{e}}_y\right)-\text{sin}\left({\psi }_d\right)\left({\ddot{x}}_r-{\lambda }_x{\dot{e}}_x\right)\right]$

39

${\dot{v}}_d=\text{cos}\left({\psi }_d\right)\left({\ddot{x}}_r-{\lambda }_x{\dot{e}}_x\right)+\text{sin}\left({\psi }_d\right)\left({\ddot{y}}_r-{\lambda }_y{\dot{e}}_y\right)$

40

where ${\dot{e}}_x=v\text{cos}\left(\psi \right)-{\dot{x}}_r$ and ${\dot{e}}_y=v\text{sin}\left(\psi \right)-{\dot{y}}_r$ .

Note that when v_d = 0, ${\dot{\psi }}_d$ is not defined. Therefore, a singularity avoidance scheme must be in place to handle this situation in practice. To avoid this situation in simulation, the following singularity avoidance algorithm is implemented. First, take ${\dot{\psi }}_d=0$ when $v_d\le \epsilon$ , for some $\epsilon \ll 1$ . Also, assume that ψ_d is held constant as a reference value computed based on the reference position values the vehicle is traveling in-between (call it ψ_r, which can be obtained from the path plan or waypoints). Formally, the following algorithm is used to avoid the singularities and keep the vehicle on the path.

if $v_d\le \epsilon$ then

${\dot{\psi }}_d=0;$

${\psi }_r={\text{tan}}^{-1}\left(\frac{\Delta y}{\Delta x}\right);$

${\psi }_d={\psi }_r$

end if

The values of $\Delta y$ and $\Delta x$ are computed as a cell difference from the reference position values as

$\begin{array}{l}\Delta y=y_r(i+1)-y_r(i)\\ \Delta x=x_r(i+1)-x_r(i), \forall i=1,\mathrm{...},n-1\end{array}$

where n is the number of position points in the reference trajectory and the ordered pair $(x_r(i),y_r(i))$ denotes the $(x\mathrm{,}y)$ location of the $i\text{th}$ point of the reference path. The stability of the nonlinear system with the derived backstepping guidance commands is given in.

Simulation results

The examples presented in this article are based on a two-dimensional Euclidean workspace. It is assumed that knowledge of the workspace such as the obstacle regions, model properties, and the objective reachable state are known by the robot. It is important to note that the path plan in both scenarios would have to be regenerated if either the environment or objectives change, thereby making the algorithm inefficient. However, the focus of this article considers situations where there are little to no uncertainty in the environment, such as in a warehouse or an open space and the objective is well defined.

There will be two example environments shown. The first case will have no obstacles present which will illustrate the features of the potential field’s formation with the NF algorithm. Then, the second case will have obstacles present in the environment which will be used to demonstrate the obstacle avoidance capability of the algorithm. A trajectory is fitted over the path plans as described in Appendix 1 and is used to test the trajectory tracking performance of the two guidance designs for both cases.

In this section, the path plan algorithm results will be shown first. With these set of results, the path plan will be shown alongside a contour plot illustrating the potential levels the algorithm generates. Next, the simulation results of the different guidance laws are presented with the same two cases tested using each design. The simulation results are obtained using MATLAB 2017a and the quadratic programming problem is solved using MATLAB’s solver in the optimization toolbox.

Path plan results

The results of the path plan algorithm are given in this subsection. The resulting path plan and potential levels are highlighted in each of the plots. Case 1 is the example with no obstacles in the environment and case 2 depicts an example environment with obstacles present.

Case 1: No obstacles present

For the first example, the final desired reachable state is $x_{\text{goal}}^{\text{MCE}}={\left[30, \hspace{0.17em}30,\hspace{0.17em} \frac{\pi }{4},\hspace{0.17em} 0.5\right]}^{\text{T}}$ . This results in a path plan that would approach the objective location with a heading angle pointing toward the north-east corner of the workspace.

The NF algorithm is run considering the minimum control effort approach with a linear rover model, and the resulting potential field and path plan is illustrated in Figure 5.

Figure 5.

Minimum control energy path plan and potential field for case 1.

The value of $u_{\text{max}}$ is set as $u_{\text{max}}=7$ for the approach shown. The effects of the constraint $u_{\text{max}}$ are evident in both sets of results as the crescent shape peaked in the potential contours. This demonstrates that it will block paths from being formed that approach the objective state from unfavorable directions. In the crescent shaped region, the white areas in Figures 5 and 6, the potential level is set at an infinite value, equal to an obstacle location, and therefore does not register in the color-spectrum of the potential field.

Figure 6.

Minimum control energy path plan and potential field for case 2.

Case 2: Obstacles present

The following example demonstrates the obstacle avoidance capability of the path planner with multiple obstacles present. The final desired reachable state for this example is taken as $x_{\text{goal}}^{\text{MCE}}={\left[30, \hspace{0.17em}35, \hspace{0.17em}\frac{\pi }{2},\hspace{0.17em} 0.5\right]}^{\text{T}}$ . This objective reachable state would direct the path to a position in between two obstacles with a resulting heading angle pointed in the north direction, that is, ${\psi }_g=\pi /2$ . The resulting path plan and potential contour levels are shown in Figure 6.

The results with the path planner show that the paths generated will avoid collisions and direct the vehicle to a state in between two obstacles.

From the results, it can be seen that the potential fields only possess one global minimum. This is a result that comes from the wavefront expansion construction method used. This also implies that a path plan can be formed to the final state from any free configuration in the given environment.

Note that if a minimum distance based path plan approach were considered, particularly in the second result in Figure 6, the path plan would look different. The resulting path plan would go straight toward the objective and hug the boundary of the obstacles until it is safely around. With the minimum control effort-based approach, the robot is trying to reduce the control energy, making it so there are less changes in direction and that they are not as sharp or sudden.

Trajectory tracking results

For all the trajectory tracking results presented, the same path plan results are used. And the reference trajectory is generated along the path plan as described in Appendix 1 with a desired time to reach the goal set at $t_{\text{goal}}=180\hspace{0.17em}\text{s}$ (or 3 min).

For the NMPC guidance law, the sampling time used to discretize the SDC system is chosen as $\Delta t=0.1\hspace{0.17em}\text{s}$ and the prediction horizon length is chosen as $N=20$ time steps. Also, the input and state weighting matrices are chosen as

$R=\left[\begin{array}{cc}0.5& 0\\ 0& 0.5\end{array}\right],\hspace{1em}Q=\left[\begin{array}{cccc}5& 0& 0& 0\\ 0& 5& 0& 0\\ 0& 0& 5& 0\\ 0& 0& 0& 5\end{array}\right]$

The input constraints chosen for the simulation are chosen as

$\begin{array}{lllll}-15(°/\text{s})\hfill & \le \hfill & \dot{\psi }\hfill & \le \hfill & 15(°/\text{s})\hfill \\ -0.15(\text{m}/{\text{s}}^2)\hfill & \le \hfill & \dot{v}\hfill & \le \hfill & 0.15(\text{m}/{\text{s}}^2)\hfill \end{array}$

for the heading angle turn rate and forward acceleration, respectively. The state, or output, constraints are placed on the heading angle and velocity, respectively, as

$\begin{array}{rrrrr}\hfill -180°& \hfill \le & \hfill \psi & \hfill \le & \hfill 180°\\ \hfill 0.001(\text{m}/\text{s})& \hfill \le & \hfill v& \hfill \le & \hfill 1(\text{m}/\text{s})\end{array}$

These constraints are chosen based on experiments conducted with the experimental mobile robot platform in the Aerospace Systems Laboratory at the University of Texas at Arlington. However, the lower bounds of 0.001 m/s for the velocity are chosen due to the lack of controllability with the SDC form of the equations (from equation (15)) when the velocity is equal to zero.

Then, for the backstepping guidance law results, the control gains are chosen as constants where ${\lambda }_x={\lambda }_y=2$ and ${\lambda }_{\psi }={\lambda }_v=5$ .

Case 1: Framework simulation results

The simulation results for case 1 are presented in Figures 7 to 12. The results illustrate the reference path tracking performance in Figures 7 and 8. Then, the simulated heading angle and velocity signals are depicted in Figures 9 and 10, respectively. Last, the guidance command time histories are shown in Figures 11 and 12 for u₁ and u₂, respectively. For the simulation results presented, the subscript NL denotes the set of results from the backstepping-based nonlinear guidance law and the subscript NMPC denotes the set of results from the NMPC guidance law.

Figure 7.

Reference path tracking for case 1.

Figure 8.

x. and y position tracking for case 1.

Figure 9.

Heading angle plot for case 1.

Figure 10.

Velocity plot for case 1.

Figure 11.

Guidance command u₁ for case 1.

Figure 12.

Guidance command u₂ for case 1.

From the reference path tracking results in Figures 7 and 8, the performance of the two guidance laws are nearly identical. Both guidance laws tightly track the reference path given with each simulation result showing that the vehicle arrives at its objective location.

The different behavior of the two guidance designs is evident in the plots of the robot’s heading angle and velocity, Figures 9 and 10, respectively. In these plots, it can be seen that there are spikes in the signals which correlate to the corner points in the reference path where a sharp turn occurs. The NMPC signals spike higher than the backstepping signals at these times due to the constraints being considered. The constraints enforced (essentially the vehicle’s acceleration and the turn rate, see Figures 11 and 12) within the NMPC framework imply that it cannot settle to a steady value as suddenly as the backstepping designs, hence the overshoot in the signals. Additionally, there are oscillations in the velocity signal in Figure 10, these may be undesired and can be lessened with further tuning of the state and input weight matrices.

As highlighted previously, another important contrast between the two guidance laws is evident in the plots of the guidance command history in Figures 11 and 12. In these plots, the constraints enforced within the NMPC design are overlayed on the plot of the two signals. The results in Figures 11 and 12 illustrate that the guidance commands for the NMPC design obey the constraints of the system. In contrast, the backstepping guidance commands have sudden sharp changes over the course of the simulation which would violate these constraints.

Also, notice the behavior of the acceleration guidance command, u₂, at the end of the simulation. At the end of the simulation, the guidance command drops suddenly to a low negative value. This occurs because the guidance law is commanding the vehicle to decelerate and stop at the end of the trajectory. The NMPC commands the vehicle’s acceleration to the lower bounds of its capability, whereas the backstepping command tends to a value that violates this constraint. This feature is also evident in the guidance command plot for case 2 in Figure 18.

Case 2: Framework simulation results

The simulation results for case 2 are presented in Figures 13 to 18. As with the previous case, the results illustrate the reference path tracking performance first in Figures 13 and 14. Then, the simulated heading angle and velocity signals are depicted in Figures 15 and 16, respectively. Last, the guidance command time histories for case 2 are shown in Figures 17 and 18 for u₁ and u₂, respectively.

Figure 13.

Reference path tracking for case 2.

Figure 14.

x. and y position tracking for case 2.

Figure 15.

Heading angle plot for case 2.

Figure 16.

Velocity plot for case 2.

Figure 17.

Guidance command u₁ for case 2.

Figure 18.

Guidance command u₂ for case 2.

Similar to the simulation results in case 1, the reference path tracking performance with both guidance laws are indistinguishable, as illustrated in Figures 13 and 14. The differences of the two methods are most evident in the simulation results of the heading angle, velocity, and guidance command signals, depicted in Figures 15 to 18, respectively.

As with the results for case 1, the heading angle and velocity signals of the two simulations behave similarly with the signals spiking at time instants that correlate to when the robot is making sudden turns along the reference path. The difference lies in the time it takes for the signals to settle. The backstepping signals settle rather quickly whereas the NMPC signals do so gradually. This behavior with the NMPC design is attributed to the constraints being enforced within its design. Similar to the result shown in Figure 10, there are oscillations in the velocity signal in Figure 16, and this behavior again may be lessened with further tuning of the state and input weight matrices.

Also, the guidance command history plots, shown in Figures 17 and 18, display similar results to what was presented with case 1. The guidance commands originating from the NMPC design are able to adhere to the constraints on the system over the duration of the simulation. In comparison, the backstepping commands again provide sharp and sudden changes which, at times, exceed the capabilities of the vehicle.

Overall, the guidance commands from the backstepping approach adhere to the constraints of the system for most of the simulation. However, the presence of sudden sharp changes in its guidance commands, as well as heading angle and velocity signals, could present complications in applying the backstepping approach to a mobile robot platform.

Conversely, all the state and input signals corresponding to the NMPC guidance law operate within the defined capabilities of the vehicle as intended. These signals do have some sudden changes but are less amplified in comparison with the backstepping approach over the course of the simulation. Thus, the capability of the NMPC design to ensure that the vehicle can operate within its physical limitations could prove beneficial for the kinds of tasks it may face.

Conclusions

In this article, a procedure for developing a reliable path planning and guidance framework for autonomous mobile robots was presented. The article introduced a new model-based path planning algorithm in addition to a NMPC guidance design to form a reliable path planning and guidance framework that was verified in simulation. The path planning algorithm, essentially derived from a special form of an APF constructed based on the minimum control effort to a goal state was shown to be very effective in synthesizing trajectories by transitioning through locally reachable states. The trajectory design exploited the wavefront expansion construction procedure to form potential contours using a control effort-based metric, thus establishing a model-based minimum control effort trajectory. This approach enables one to find an obstacle-free path that not only guides a robot to its objective state but also takes into account the final approach of the vehicle to a reachable state. Additionally, this algorithm allows for the obstacle-free paths to be formed from anywhere in the environment. The guidance law using the SDC-based NMPC design explicitly accounts for the vehicle’s physical constraints to track the desired trajectory. Comparisons with a nonlinear backstepping-based guidance law show similar tracking performance but the NMPC clearly distinguished itself by being able to perform with realistic constraints imposed on the system.

Ultimately, the combination of the aspects described in this article form a model-based framework that can be used for path planning and guidance involving autonomous mobile robots.

Future work using the research presented in this article includes applying the derived methods to real-time mobile robot platforms. Additionally, a detailed comparison of the path planning algorithm with other techniques will be conducted and considered with the minimum control effort approach.