A Stable Analytical Solution Method for Car-Like Robot Trajectory Tracking and Optimization

I. Introduction

CONTROL of the nonholonomic car-like robot (NCLR) kinematic model has been a cardinal research topic since the model is widely prevalent in autonomous driving control, motion planning, robotics, and so on. The nonholonomic nature of the robot constraints has made this problem challenging as these class of systems cannot be asymptotically stabilized around the equilibrium point by using any smooth time-invariant state feedback [1]. The existing tracking control techniques can be divided into two categories of point-to-point stabilization and trajectory tracking [2]. In point-to-point stabilization, the control task is to solve a stabilization problem for an equilibrium point in the robot state space where the robot must reach from a given initial configuration. However, in trajectory tracking, the vehicle must follow a desired reference path with an associated timing law starting from an initial point which may be on or off the reference path. The trajectory tracking problem is of interest in the literature and it is also the scope of this paper since the point-to-point stabilization requires a copious amount of information and may produce sporadic and large control signals for a large initial configuration error.

A Literature Review

The early study, in [3], employed the Lyapanov stability theory to design a locally asymptotically stable tracking controller for the linearized kinematic model around the reference trajectory. The dynamic feedback linearization techniques, as explained in [4] and [5], have motivated various recent control techniques, such as the linear parameter varying (LPV) control technique in [6], kinematic-dynamic cascade controller design in [7], and the event-triggered tracking control structure proposed in [8], to achieve the globally stable output tracking performance. The aforementioned frameworks solely have explored a stabilized solution; however, achieving a better control performance such as optimality of the trajectory and control inputs have been generally neglected in these works.

Recently, the model predictive control (MPC) technique has propelled to the forefront of the car-like robot control strategies (e.g., [9]–[11]) as it can provide an optimal solution by predicting the vehicle motion and control actions at future time instants [12]. A major drawback of MPC is the computational overhead it imposes to the system due to the receding horizon calculation. Analytical solutions, proposed in [11] and [13], made this method computationally inexpensive for real-time applications. Nevertheless, they reduced the solution space by linearizing the error dynamics around the reference trajectory which causes sub-optimality. The performance of the continuous nonlinear MPC (NMPC) and discrete NMPC are also analyzed in [14], which showed a large tracking error produced in discrete NMPC due to the variable sampling compared with the continuous NMPC.

B Motivation

Generally speaking, the methods that addressed the kinematic model control has mostly followed either of the two following strategies. First, are the methods that transformed the system error dynamics into the robot coordinate frame where the car inputs explicitly appear in the model, and consequently, be considered in the control, as in [3], [13], and [14]. However, for the purpose of continuous control design, the error dynamics are linearized around the reference trajectory which, indeed, results in local stability. Second, are the methods which employed the input-output linearization technique by defining a linear relation from the defined auxiliary input vector to the outputs [15]. This strategy guarantees the global stability as long as the internal dynamics are bounded. While the first method is more attractive in trajectory tracking due to the explicit consideration of control inputs, linearization around the reference trajectory causes sub-optimality in this strategy. To this end, in this paper, the input-output linearization technique is employed to guarantee the global stability of the trajectory tracking and an error minimization framework for the transformed model is designed to address the sub-optimality of generated trajectory by using the flatness property of kinematic model.

This paper proposes a novel globally exponentially stable trajectory optimization and tracking control framework to minimize the tracking error of position, velocity, and acceleration. The input-output linearization technique is employed to transform the car-like nonlinear system tracking error model into a linear differential map from the defined auxiliary input to the output. Unlike the aforementioned studies, the proposed analytical solution drastically lowers the computational cost. The variational approach is used to solve the trajectory optimization problem, and the simulation results show that our trajectory planning technique is capable of following any arbitrary reference function with continuous first and second derivatives. In addition, the error cost is significantly down compared to the numerical solution. The comparison of our proposed method with the three most well-known tracking control techniques in the literature is shown in Table I.

Table  I.  Comparison of Methods

Method	Stability	Optimality	Model	Assumption	Description
Kanayama [3]	LAS*	No	Unicycle	Model linearization	State Feedback + Lyapanov Stability Function
Luca [4]	GAS*	No	Unicycle and bicycle	Nonlinear model	Dynamic Feedback Linearization
CMPC [14]	LAS	No	Unicycle	Model linearization	Reference Error Dynamic Tracking
Proposed method	GAS	Yes	Unicycle and bicycle	Nonlinear model	Input-Output Linearization + Analytical Solution
*LAS: Locally asymptotically stable, and GAS: Globally asymptotically stable

 | Show Table

DownLoad: CSV

The remainder of this paper is organized as follows: Section II formulates the model and problem definition. The input-output linearization technique and the proposed solution method are explained in Sections III and IV, respectively. In Section V, the simulation results are discussed. Finally, the conclusions and future works are presented in Section VI.

II. Problem Formulation

A Model

The kinematics of a rear-wheel car-like robot model, as shown in Fig. 1, includes four states ${\bf x}(t) = \left[x(t)\; y(t)\; \psi (t)\; v(t)\right]^T$ and two control inputs ${\bf u}(t) = \left[\delta (t)\; a(t)\right]^T$ which can be expressed as follows [16]:

Figure  1.  Rear-wheel car-like robot model.

DownLoad: Full-Size Img PowerPoint

where $t$ refers to time, $(x,y)$ are longitudinal and lateral positions of the rear-wheel axis midpoint with the magnitude of velocity denoted by $v$, $\psi \in [-\pi, \pi]$ is heading angle, $\delta \in \left(-\dfrac{\pi}{2}, \dfrac{\pi}{2}\right)$ is the steering angle of the front wheels, $a$ refers to the acceleration, and $\ell$ is the wheelbase length. For the trajectory planning problem, we define the output of the system as

B Trajectory Planning

The trajectory planning is formulated in the form of an optimal control problem as follows

Optimal Control Problem 1 (OPT1): Find an admissible and bounded control input ${\bf u}^*(t)$ that causes the system (1) to follow an admissible trajectory ${\bf x}^*(t)$ that minimizes the performance measure

where $t_f$ is the fixed final time and ${\bf z}_r(t) = [x_r(t)\; y_r(t)]^T$ is the twice differentiable geometric reference path that both are given by the path planner.

The error terms are defined as

where $q_1,q_2,q_3,q_4,r_1$ and $r_2$ are positive weights. Taking the derivative of $\dot{x}(t)$ and $\dot{y}(t)$ in (1) gives

then by substituting $\dot{x}(t)$, $\dot{y}(t)$, $\ddot{x}(t)$, and $\ddot{y}(t)$ in (4),

The terms $e^2_v(\cdot)$ and $e^2_a(\cdot)$ are added as the control inputs are required to appear explicitly in the cost function, that allows the optimal control $\mathbf u^*(t)$ to be extracted without considering any parametrized polynomial function representation for trajectory and control inputs. Therefore, $[\dot{x}(t)\; \dot{y}(t)]$ and $[\ddot{x}(t)\; \ddot{y}(t)]$ should follow $[\dot{x}_r(t)\; \dot{y}_r(t)]$ and $[\ddot{x}_r(t)\; \ddot{y}_r(t)]$, respectively, since the goal is for $[x(t)\; y(t)]$ to follow $[x_r(t)\; y_r(t)]$, and the reference trajectory is assumed to be twice differentiable. This framework makes the NCLR error minimization problem tractable for the higher dimensional kinematic models by adding the higher order derivatives of position error to the cost function (3).

C Boundary Conditions

The initial states $x(t_0)$, $y(t_0)$, $\psi(t_0)$, and $v(t_0)$ follow the initial configuration of the vehicle, and they are not necessarily aligned with the reference trajectory, but considering the fact that the error $e^2_v(\cdot)$ is expected to be zero at final time $t_f$,

Accordingly, $\psi (t_f)$ and $v(t_f)$ can be found as

III. Input-Output Linearization Technique

In order to simplify the problem formulation and circumvent dealing with the nonlinear differential equations, the input-output linearization technique is used as introduced in [4] to transform the nonlinear kinematic model (1) into a linear model from input to output. Since the system inputs explicitly appeared in the second derivative of the defined output ${\bf z}(t)$ in (5), it can be rewritten into the following matrix form

Defining an auxiliary input vector ${ \zeta}(t) = [\zeta_1(t)\; \zeta_2(t)]^T$ gives

that results in two decoupled double integrators linearized from input to output as

The system has the relative degree 2 in the region $\Omega = \left\{{\bf x}(t)\in \mathbb{R}^4\mid v(t)\neq 0, \delta\in \left(-\dfrac{\pi}{2}, \dfrac{\pi}{2}\right)\right\}$ that remains with two internal dynamics $\psi(t)$ and $v(t)$.

If we form $\ddot{x}(t)-\ddot{x}_r(t)$ and $\ddot{y}(t)-\ddot{y}_r(t)$ errors as

and define the error vector ${\bf e}(t)$ as

the tracking error state equation $\dot{{\bf e}}(t) = {\bf A}{\bf e}(t)+{\bf B}{ \eta}(t)$ can be written as follows

that is completely controllable. The OPT1 is reformulated as

Optimal Control Problem 2 (OPT2): Find an admissible control ${ \eta^*}(t)$ that causes the system (14) to follow an admissible trajectory ${\bf e}^*(t)$ that minimizes the performance measure

where

are positive definite weighted matrices.

Clearly, the boundary conditions are changed to

Remark 1: It should be noted that OPT2 is not a linear quadratic regulator (LQR) problem since, unlike the LQR problem formulation, ${\bf e}(t_f)$ is assumed to be fixed.

IV. Analytical Solution Method

Since the problem is minimizing the cost functional (15) subject to the tracking error state equations (14), the variational method is employed to solve the optimization. The proposed solution in this section is not only optimal but also guarantees the exponential stability of trajectory and control inputs.

A Variational Method

Lemma 1: An optimal control ${ \eta}^*(t)$ for $t\in [t_0,t_f]$ which gives the optimal state ${\bf e}^*(t)$ that minimizes the cost function (15) is the solution of the following two-point boundary value (TPBV) state linear differential equations

and co-state linear differential equations

with boundary conditions in (17). The optimal control laws are also given by

Proof: To start solving the minimization (15), we form the Hamiltonian as follows

where the vector ${\bf p}(t) = [p_1(t)\; p_2(t)\; p_3(t)\; p_4(t)]^T$ includes the Lagrange multipliers.

The necessary conditions of optimality are [17]

State and co-state equations are derived from the substitution of ${\cal H}(\cdot)$ in (25) and (26), respectively. Also, (27) gives

from which the optimal control laws in (23) are obtained. ■

Taking the second and third derivative of $p^*_1(t)$ and $p^*_2(t)$ from (19) and (20) gives

where $\dot{e}^*_j(t)$ is given from substitution of (23) in (18). Similarly, substituting $\dot{p}^*_j(t)$ into the forth derivative of $p^*_i(t)$ gives

From (28), we have $e^*_j(t) = -\dfrac{1}{q_i}\ddot{p}^*_i(t)$. Thus, equation (29) can be rewritten as

which is a homogeneous linear ordinary differential equation. To solve the linear differential (30), the Laplace transform of $p^*_i(t)$ is first obtained as

where $p_{it_0} = p^*_i(t)\vert_{t = t_0}$. From (28), $\eta^*_i(t)$ is found as

Transforming (32) into the Laplace domain gives

Substitution of (31) in (33) gives

Taking the inverse Laplace transform of (31) and substituting the resulted $p^*_i(t)$ into the state and co-state equations gives the analytical optimal solution of the OPT2 problem with eight unknown parameters of $p^*_{it_0}$, $\dot{p}^*_{it_0}$, $\ddot{p}^*_{it_0}$, and $\dddot{p}^*_{it_0}$, which can be found to satisfy the eight initial and final boundary conditions in (17). However, (34) shows that the resulted $\eta^*_i(t)$ is unbounded due to the right-half plane roots of the polynomial $\Big(s^2-2m_is+\sqrt{\dfrac{q_i}{r_i}}\Big)$. Although, this analytical solution can address the OPT2 assumptions, it fails to guarantee the stability of the system.

We approach the instability issue by relaxing ${\bf e}(t_f)$ to be free while setting four of the initial conditions in (31) to cancel the unstable poles. The new assumption automatically changes the original problem to the infinite-time LQR problem formulated as follows.

Optimal Control Problem 3 (OPT3): Find an admissible control ${ \eta^*}(t)$ that causes the origin ${\bf e}^* = 0$ of the closed loop system (14) to be globally exponentially stable with an admissible trajectory ${\bf e}^*(t)$ that minimizes the performance measure

where

are positive definite diagonal weighted matrices satisfying the boundary conditions

Underdamped $f_i> 0$:

Critically damped $f_i = 0$:

Overdamped $f_i < 0$:

as $t_f$ approaches infinity.

B Globally Exponentially Stable Analytical Solution

Theorem 1: The globally exponentially stable analytical solutions of the OPT3 are given by (38)–(46), where

and $a_i$ and $b_i$ are given as in Table II, which satisfy the initial boundary condition ${\bf e}(t_0)$ in (17).

Table  II.  $a_i$ and $b_i$ for Three Different Cases

Damping cases	ai and bi coefficients
Underdamped $f_i >0$	$\begin{aligned} & a_i = \frac{q_i\sqrt{f_i}}{n^2_i\cos(\alpha_i)}\Big[\frac{e_{jt_0}}{n_i}+2e_{it_0}\sin(\alpha_i)\Big]\\ & b_i = -\frac{q_i\sqrt{f_i}e_{it_0}}{n_i\cos(\alpha_i)}, \end{aligned}$
Critically damped $f_i = 0$	$\begin{aligned} & a_i = \frac{1}{m^2_i}\Big[q_ie_{jt_0}+2m_iq_ie_{it_0}\Big],\\ & b_i = -q_ie_{it_0}, \end{aligned}$
Overdamped $f_i < 0$	$\begin{aligned} & a_i = \frac{1}{m^2_i-|f_i|}\Big[q_ie_{jt_0}+2m_iq_ie_{it_0}\Big]\\ & b_i = -q_ie_{it_0}.\end{aligned}$

 | Show Table

DownLoad: CSV

Proof: As mentioned in Section IV-A, in order to guarantee the exponential stability of the origin in the closed loop system (14), the initial conditions in (31) are tuned so as to cancel the unstable poles related to the polynomial $(s^2-2m_is+\sqrt{\dfrac{q_i}{r_i}})$. To do so, the initial conditions of (31) are assigned as

which gives

where $a_i$ and $b_i$ should be determined by the initial boundary conditions. Since $P^*_i(s)$ is a strictly-proper rational function of $s$, $p^*_i(t)$ is a time-invariant function [18]. Location of poles in (49) leads in three underdamped, critically damped, and overdamped solution cases:

1) Underdamped $f_{i}>0$: If $P^*_i(s)$ has complex roots, the inverse Laplace transform of (49) gives $p^*_i(t)$ for $t\in[t_0,\; t_f]$ as

By substituting the first derivative of $p^*_i(t)$ in (19) and (20),

which gives $e^*_i(t)$ as (38). Consequently, $e^*_j(t)$ is found by taking the first derivative of $e^*_i(t)$ as in (39).

2) Critically Damped $f_{i}=0$: This case occurs when the weights follow the equality $q^2_j-4r_iq_i = 0$. In this case, the inverse Laplace transform of (49) gives $p^*_i(t)$ for $t\in[t_0,\; t_f]$ as

Substitution of its first and second derivatives in (19), (20), and (28) gives the closed form solution for $e^*_i(t)$ and $e^*_j(t)$ as in (41) and (42).

3) Overdamped $f_{i}<0$: Equation (49) can be rewritten as

where $|f_i|$ is the absolute value of $f_i$. By taking the inverse Laplace of $P^*_i(s)$,

Similarly as the former cases, $e^*_i(t)$ and $e^*_j(t)$ are found as in (44) and (45).■

Finding the errors give us the optimal states as

where

The optimal control inputs are found from (10) as

where ${{ \zeta }^*}(t) = {{ \eta }^*}(t) + \left[ {\begin{array}{*{20}{c}} {\dot x(t) - {{\dot x}_r}(t)}\\ {\dot y(t) - {{\dot y}_r}(t)} \end{array}} \right]$.

Remark 2: Since $\dot{x}^*(t)$ and $\dot{y}^*(t)$ are stable and $\arctan(\cdot)$ is the continuous function of its argument in $\left(-\dfrac{\pi}{2},\dfrac{\pi}{2}\right)$, it can be concluded from (55) that the internal dynamics $\psi^*(t)$ and $v^*(t)$ are bounded.

C Boundary Conditions Satisfaction

The parameters $a_i$ and $b_i$ should be set to satisfy the boundary conditions. Substituting the initial conditions (37) into $e^*_i(t_0)$ and $e^*_j(t_0)$ gives $a_i$ and $b_i$ as illustrated for each case in Table II. As mentioned in Section IV-A, the final time boundary conditions may not be fulfilled since two unstable poles are eliminated in (31) to guarantee the stability, and the unknown parameters $a_i$ and $b_i$ are used up to satisfy the initial boundary conditions. However, the solution is a good approximation to satisfy ${\bf e}^*(t_f)\approx{\bf 0}$ due to the terms $e^{-c_{1i}(t_f-t_0)}$ and $e^{-c_{2i}(t_f-t_0)}$ in overdamped, and $e^{-m_i(t_f-t_0)}$ in underdamped and critically-damped cases. Thus, there exists a trade off between guaranteeing the stability, as proved for the OPT3 problem, and meeting the exact final time conditions as elaborated in the solutions of OPT2.

Remark 3: From (47) it can be concluded that for the fixed value of $r_i$, if the values of $q_i$, and $q_j$ increase by the factors of $\beta^2_i$ and $\beta_i$ (for $\beta_i>1$), respectively, the values of $m_i$ for the underdamped and critically damped cases, $c_{1i}$ and $c_{2i}$ for the overdamped case will be increased as

which determines how fast the tracking errors converge to zero.

V. Simulation Results

In this section, the effect of the weight change in controller performance, and tracking error is evaluated. At the end, the proposed closed-form solution in this paper is compared with other well-known methods in the literature. The simulations are developed in MATLAB. The reference time-parametrized trajectory, for $t\in[0, 30]$, is an eight-shaped Lissajous curve expressed as

The system starts from the initial configurations, $x(0) = 1.1$, $y(0) = 0.8$, $v(0) = 1$, and $\psi(0) = 1.3$, and the errors are expected to be zero at final time.

The tracking errors for three different cases are shown in Fig. 2. The errors can remain in a certain bound by making a trade-off among $q_i$, $q_j$, and $r_i$ weights. The impact of weight choice on the final time error can also be seen by decreasing the control effort that will increase the tracking time and consequently, it may unfavorably cause the final time error not to be close to zero. However, the trajectory error is globally exponentially stable and control inputs are guaranteed to be in $t\in [0,30]$.

Figure  2.  Tracking errors compared for different weighting parameters (UD = Underdamped, OD = Overdamped, and CD = Critically damped).

DownLoad: Full-Size Img PowerPoint

The vehicle trajectory, velocity, heading angle, and control variables, for three different cases, are illustrated in Figs. 3-5, respectively. As shown in Table III, the measured error ${\bf e}(t_f)$ is almost zero at t_f = 30 s, 40 s, and 50 s for the three simulated cases, and converges to the zero for large enough $t_f$ due to the exponential stability of the solution. Boundedness of the control effort and internal dynamics $v^*(t)$ and $\psi^*(t)$ are also evident in these figures.

Figure  3.  Weighting parameters impact on final time errors for the eight-shaped trajectory tracking.

DownLoad: Full-Size Img PowerPoint

Figure  4.  Eight-shaped trajectory tracking: (a) heading angle: $\psi^* (t)$ and $\psi_r (t)$; (b) velocity: $v^*(t)$ and $v_r(t)$.

DownLoad: Full-Size Img PowerPoint

Figure  5.  Eight-shaped trajectory tracking: (a) steering angle: $\delta^* (t)$; (b) acceleration: $a^*(t)$.

DownLoad: Full-Size Img PowerPoint

Table  III.  Error Measurements at Three Time Instances ($t_f = 30\;{\rm s}$, $t_f = 40\; {\rm s}$, and $t_f = 50\; {\rm s}$).

Error	Case	tf = 30 s	tf = 40 s	tf = 50 s
$e^*_1(t)$	UD	–1.35E–20	2.69E–25	3.93E–31
	OD	3.09E–06	1.29E–07	5.36E–09
	CD	5.41E–06	8.23E–08	1.18E–09
$e^*_2(t)$	UD	–7.47E–20	1.49E–24	2.18E–30
	OD	1.71E–05	7.13E–07	2.97E–08
	CD	3.00E–05	4.57E–07	6.52E–09
$e^*_3(t)$	UD	–2.64E–19	–5.76E–25	–3.23E–31
	OD	–9.82E–07	–4.09E–08	–1.70E–09
	CD	–2.24E–06	–3.48E–08	–5.02E–10
$e^*_4(t)$	UD	–1.47E–18	–3.19E–24	–1.79E–30
	OD	–5.44E–06	–2.27E–07	–9.45E–09
	CD	–1.24E–05	–1.93E–07	–2.78E–09
$\eta^*_1(t)$	UD	7.58E–19	7.06E–25	–3.69E–31
	OD	3.12E–07	1.30E–08	5.41E–10
	CD	9.20E–07	1.46E–08	2.14E–10
$\eta^*_2(t)$	UD	4.20E–18	3.92E–24	–2.05E–30
	OD	1.73E–06	7.21E–08	3.00E–09
	CD	5.10E–06	8.11E–08	1.19E–09

 | Show Table

DownLoad: CSV

Finally, the proposed analytical solution in this paper is compared with our previous numerical solution (based on MATLAB two-point boundary value problem solution package BVP4C) in [19]. The integrand of cost function (3), i.e., $e_p(\cdot)^2+e_v(\cdot)^2+e_a(\cdot)^2$, is plotted over time in Fig. 6. The weights are set to be $q_i = 1$, $q_j = 1$, and $r_i = 1$ for both cases. The cost function value $J(\cdot)$ for the analytical solution is $9.87$ which shows more than 380% reduction compared to the numerical case which gives $47.45$.

Figure  6.  Comparison of analytical and numerical methods based on their measured cost function integrand, i.e., $e_p(\cdot)^2+e_v(\cdot)^2+e_a(\cdot)^2$, over time.

DownLoad: Full-Size Img PowerPoint

VI. Conclusions and Future Work

In this work, a novel globally exponentially stable trajectory optimization and tracking control formulation is proposed to generate the optimal trajectory and tracking control inputs for the car-like robot kinematic model.

The velocity and acceleration errors are minimized in addition to the position error, which allows us to extract the optimal control ${\bf u}^*(t)$ without considering any parametrized geometric function representations for control inputs since they explicitly appear in the cost function. The input-output linearization technique is employed to linearize the system from the defined auxiliary input to output. The closed-form optimal trajectories and control inputs are obtained by the calculus of variation technique, and boundedness of the solution and internal dynamics are also proved.

The proposed framework can be easily extended to tackle the optimal trajectory optimization problem of a higher dimensional kinematic model with more states by adding the higher order derivatives of the $x(t)$ and $y(t)$ into the cost function and assuming that the reference trajectory is smooth.

Our proposed optimal planning framework is an open loop control strategy which yields continuous control signals (sequence of inputs) as a function of the initial state of the vehicle. Thus, if the states evolves exactly according to the model then the predicted state trajectory is exactly obtained in reality by applying the calculated optimal input signal. However, if the model is inaccurate and the system is subject to disturbance not included in the model, the result of applying the open-loop control signal may be different than what is expected. Thus, in our future work, we aim to apply the repeated application of this open-loop strategy in discrete-time domain to obtain the same feedback effect of recursive techniques, known as receding horizon strategy in the literature.