IEEE/CAA JOURNAL OF AUTOMATICA SINICA, VOL. 7, NO.
1, JANUARY 2020
A Stable Analytical Solution Method for Car-Like
Robot Trajectory Tracking and Optimization
Keyvan Majd, Mohammad Razeghi-Jahromi, and Abdollah Homaifar
   Abstract—In this paper, the car-like robot kinematic model tra-
jectory tracking and control problem is revisited by exploring an
optimal analytical solution which guarantees the global exponen-
tial stability of the tracking error. The problem is formulated in
the form of tracking error optimization in which the quadratic
errors of the position, velocity, and acceleration are minimized
subject to the rear-wheel car-like robot kinematic model. The in-
put-output linearization technique is employed to transform the
nonlinear problem into a linear formulation. By using the vari-
ational approach, the analytical solution is obtained, which is
guaranteed to be globally exponentially stable and is also appro-
priate for real-time applications. The simulation results demon-
strate the validity of the proposed mechanism in generating an
optimal trajectory and control inputs by evaluating the proposed
method in an eight-shape tracking scenario.
    Index Terms—Global asymptotic stability, input-output lineariza-
tion, optimal control, trajectory tracking.
I.  Introduction
ONTROL of the nonholonomic car-like robot (NCLR)
C 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 challen-
ging as these class of systems cannot be asymptotically stabil-
ized 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 sta-
bilization, the control task is to solve a stabilization problem
for an equilibrium point in the robot state space where the ro-
bot must reach from a given initial configuration. However, in
trajectory tracking, the vehicle must follow a desired refer-
Manuscript received April 26, 2019; revised August 13, 2019; accepted
September 26, 2019. This work was partially supported by the Air Force Re-
search Laboratory and Office of the Secretary of Defense (OSD) (FA8750-15-
2-0116), the US Department of Transportation (USDOT), and Research and
Innovative Technology Administration (RITA) under University Transporta-
tion Center (UTC) Program (DTRT13-G-UTC47). Recommended by Asso-
ciate Editor Dianwei Qian. (Corresponding author: Abdollah Homaifar.)
Citation: K. Majd, M. Razeghi-Jahromi, and A. Homaifar, “A stable analyt-
ical solution method for car-like robot trajectory tracking and optimization,”
IEEE/CAA J. Autom. Sinica, vol. 7, no. 1, pp. 39–47, Jan. 2020.
K. Majd is with the School of Computing, Informatics, and Decision Sys-
tems Engineering, Arizona State University, Tempe, AZ 85281 USA (e-mail:
majd@asu.edu).
M. Razeghi-Jahromi is with the ABB Corporate Research Center, Raleigh,
NC 27606 USA (e-mail: mohammad.razeghi-jahromi@us.abb.com).
A. Homaifar is with the Department of Electrical and Computer Engineer-
ing, North Carolina A&T State University, Greensboro, NC 27403 USA (e-
mail: homaifar@ncat.edu).
Color versions of one or more of the figures in this paper are available on-
line at http://ieeexplore.ieee.org.
Digital Object Identifier 10.1109/JAS.2019.1911816
39
ence path with an associated timing law starting from an ini-
tial point which may be on or off the reference path. The tra-
jectory tracking problem is of interest in the literature and it is
also the scope of this paper since the point-to-point stabiliza-
tion requires a copious amount of information and may pro-
duce sporadic and large control signals for a large initial con-
figuration 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
40 IEEE/CAA JOURNAL OF AUTOMATICA SINICA, VOL. 7, NO. 1, JANUARY 2020

trajectory which, indeed, results in local stability. Second, are as

the methods which employed the input-output linearization [ ]
x(t)
technique by defining a linear relation from the defined z(t) = . (2)
auxiliary input vector to the outputs [15]. This strategy y(t)
guarantees the global stability as long as the internal dynamics
B. Trajectory Planning
are bounded. While the first method is more attractive in
trajectory tracking due to the explicit consideration of control The trajectory planning is formulated in the form of an
inputs, linearization around the reference trajectory causes optimal control problem as follows
sub-optimality in this strategy. To this end, in this paper, the Optimal Control Problem 1 (OPT1): Find an admissible and
input-output linearization technique is employed to guarantee bounded control input u∗ (t) that causes the system (1) to
the global stability of the trajectory tracking and an error follow an admissible trajectory x∗ (t) that minimizes the
minimization framework for the transformed model is performance measure
designed to address the sub-optimality of generated trajectory 1 w tf 2
by using the flatness property of kinematic model. J(u(t)) = [e (x(t), y(t), xr (t), yr (t))
2 t0 p
This paper proposes a novel globally exponentially stable
+ e2v ( ẋ(t), ẏ(t), ẋr (t), ẏr (t))
trajectory optimization and tracking control framework to
minimize the tracking error of position, velocity, and + e2a ( ẍ(t), ÿ(t), ẍr (t), ÿr (t))]dt, (3)
acceleration. The input-output linearization technique is where t f is the fixed final time and zr (t) = [xr (t) yr (t)]T
is the
employed to transform the car-like nonlinear system tracking twice differentiable geometric reference path that both are giv-
error model into a linear differential map from the defined en by the path planner.
auxiliary input to the output. Unlike the aforementioned The error terms are defined as
studies, the proposed analytical solution drastically lowers the [ ]T [ ][ ]
computational cost. The variational approach is used to solve x(t) − xr (t) q1 0 x(t) − xr (t)
e2p (·) =
the trajectory optimization problem, and the simulation results y(t) − yr (t) 0 q2 y(t) − yr (t)
show that our trajectory planning technique is capable of [ ]T [ ][ ]
ẋ(t) − ẋr (t) q3 0 ẋ(t) − ẋr (t)
following any arbitrary reference function with continuous ev (·) =
2
ẏ(t) − ẏr (t) 0 q4 ẏ(t) − ẏr (t)
first and second derivatives. In addition, the error cost is [ ]T [ ][ ]
significantly down compared to the numerical solution. The ẍ(t) − ẍr (t) r1 0 ẍ(t) − ẍr (t)
e2a (·) = , (4)
comparison of our proposed method with the three most well- ÿ(t) − ÿr (t) 0 r2 ÿ(t) − ÿr (t)
known tracking control techniques in the literature is shown in where q1 , q2 , q3 , q4 , r1 and r2 are positive weights. Taking the
Table I. derivative of ẋ(t) and ẏ(t) in (1) gives
The remainder of this paper is organized as follows: Section
1
II formulates the model and problem definition. The input- ẍ(t) = a(t) cos(ψ(t)) − v2 (t) tan(δ(t)) sin(ψ(t))
output linearization technique and the proposed solution ℓ
1 2
method are explained in Sections III and IV, respectively. In ÿ(t) = a(t) sin(ψ(t)) + v (t) tan(δ(t)) cos(ψ(t)), (5)
Section V, the simulation results are discussed. Finally, the ℓ
conclusions and future works are presented in Section VI. then by substituting ẋ(t) , ẏ(t), ẍ(t) , and ÿ(t) in (4),
[ ]
II.  Problem Formulation e2p (·) = q1 [x(t) − xr (t)]2 + q2 y(t) − yr (t) 2
[ ] [ ]
e2v (·) = q3 v(t) cos(ψ(t)) − ẋr (t) 2 + q4 v(t) sin(ψ(t)) − ẏr (t) 2
A. Model 1
The kinematics of a rear-wheel car-like robot model, as e2a (·) = r1 [a(t) cos(ψ(t)) − v2 (t) tan(δ(t)) sin(ψ(t)) − ẍr (t)]2
[ ] ℓ
shown in Fig. 1, includes four states x(t) = x(t) y(t) ψ(t) v(t) T 1
+ r2 [a(t) sin(ψ(t)) + v2 (t) tan(δ(t)) cos(ψ(t)) − ÿr (t)]2 .
and two control inputs u(t) = [δ(t) a(t)]T which can be ℓ
expressed as follows [16]: (6)

ẋ(t) = v(t) cos(ψ(t)) The terms e2v (·) and e2a (·) are added as the control inputs are
ẏ(t) = v(t) sin(ψ(t)) required to appear explicitly in the cost function, that allows
1 the optimal control u∗ (t) to be extracted without considering
ψ̇(t) = v(t) tan(δ(t)) any parametrized polynomial function representation for
ℓ
v̇(t) = a(t), (1) trajectory and control inputs. Therefore, [ ẋ(t) ẏ(t)] and
[ ẍ(t) ÿ(t)] should follow [ ẋr (t) ẏr (t)] and [ ẍr (t) ÿr (t)],
where t refers to time, (x, y) are longitudinal and lateral posi-
respectively, since the goal is for [x(t) y(t)] to follow
tions of the rear-wheel axis midpoint with the magnitude of
[xr (t) yr (t)], and the reference trajectory is assumed to be twice
velocity
( π πdenoted
) by v , ψ ∈ [−π, π] is heading angle,
differentiable. This framework makes the NCLR error
δ∈ − , is the steering angle of the front wheels, a refers minimization problem tractable for the higher dimensional
2 2
to the acceleration, and ℓ is the wheelbase length. For the tra- kinematic models by adding the higher order derivatives of
jectory planning problem, we define the output of the system position error to the cost function (3).
MAJD et al.: A STABLE ANALYTICAL SOLUTION METHOD FOR CAR-LIKE ROBOT TRAJECTORY TRACKING AND OPTIMIZATION 41

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

 
 a(t) 
Y   = G−1 (v, ψ)ζ(t), (10)
δ tan(δ(t))
ℓ
that results in two decoupled double integrators linearized
υ
from input to output as
y
ẍ(t) = ζ1 (t)
ÿ(t) = ζ2 (t). (11)
ψ
o x The{ system has the relative ( π π )}degree 2 in the region
X
Ω = x(t) ∈ R | v(t) , 0, δ ∈ − ,
4 that remains with two
2 2
Fig. 1. Rear-wheel car-like robot model. internal dynamics ψ(t) and v(t).
If we form ẍ(t) − ẍr (t) and ÿ(t) − ÿr (t) errors as
C. Boundary Conditions
The initial states x(t0 ) , y(t0 ), ψ(t0 ), and v(t0 ) follow the initial ẍ(t) − ẍr (t) = ζ1 (t) − ẍr (t) = η1 (t)
configuration of the vehicle, and they are not necessarily ÿ(t) − ÿr (t) = ζ2 (t) − ÿr (t) = η2 (t), (12)
aligned with the reference trajectory, but considering the fact and define the error vector e(t) as
that the error e2v (·) is expected to be zero at final time t f ,    
 e1 (t)   x(t) − xr (t) 
ẋr (t f ) = v(t f ) cos(ψ(t f ))  e (t)   y(t) − yr (t) 
e(t) =  2 = ,
 e3 (t)   ẋ(t) − ẋr (t) 
(13)
ẏr (t f ) = v(t f ) sin(ψ(t f )). (7)
e4 (t) ẏ(t) − ẏr (t)
Accordingly, ψ(t f ) and v(t f ) can be found as
( ∗ ) the tracking error state equation ė(t) = Ae(t) + Bη(t) can be
ẏr (t f )
ψ∗ (t f ) = arctan ∗ written as follows
ẋr (t f )    
 ẋ∗ (t ) ẏ∗r (t f )  0 0 1 0   0 0 

 π



r f
= , ψ∗ (t f ) , {0, ± , π} .  0 0 0 1   
 e(t) +  0 0  η(t),


 cos(ψ ∗ (t )) sin(ψ ∗ (t )) 2 e(t) =    1 0  (14)



f f
 0 0 0 0
∗  ẋr∗ (t f ), ψ∗ (t f ) = 0   
v (t f ) = (8) 0 0 0 0 0 1


 − ẋr∗ (t f ), ψ∗ (t f ) = π


 that is completely controllable. The OPT1 is reformulated as


 π
 sign(ψ∗ (t f ))ẏ∗r (t f ), ψ∗ (t f ) = ± . Optimal Control Problem 2 (OPT2): Find an admissible
2
control η∗ (t) that causes the system (14) to follow an
III.  Input-Output Linearization Technique admissible trajectory e∗ (t) that minimizes the performance
In order to simplify the problem formulation and measure
circumvent dealing with the nonlinear differential equations, 1 w tf [ T ]
the input-output linearization technique is used as introduced J(η(t)) = e (t)Qe(t)+ηT (t)Rη(t) dt, (15)
2 t0
in [4] to transform the nonlinear kinematic model (1) into a where
linear model from input to output. Since the system inputs
 
explicitly appeared in the second derivative of the defined  q1 0 0 0  [ ]
 0 q2 0 0  r 0
output z(t) in (5), it can be rewritten into the following matrix Q =   and R = 1 (16)
form  0 0 q3 0  0 r2
  0 0 0 q4
[ ]  cos(ψ(t)) − 1 v2 (t) sin(ψ(t))  [ ]
ẍ(t)  ℓ  a(t) are positive definite weighted matrices.
=   Clearly, the boundary conditions are changed to
ÿ(t)  1 2  tan(δ(t))
sin(ψ(t)) v (t) cos(ψ(t))     
[ ℓ ]  x(t0 ) − xr (t0 )   0 
 y(t0 ) − yr (t0 ) 
  
 and e(t f ) = 
a(t)  0  . (17)
= G(v, ψ) . (9) e(t0 ) = 
tan(δ(t))  v(t0 ) cos(ψ(t0 )) − ẋr (t0 )   0 
Defining an auxiliary input vector ζ(t) = [ζ1 (t) ζ2 (t)]T gives v(t0 ) sin(ψ(t0 )) − ẏr (t0 ) 0
42 IEEE/CAA JOURNAL OF AUTOMATICA SINICA, VOL. 7, NO. 1, JANUARY 2020

Remark 1: It should be noted that OPT2 is not a linear substitution of H(·) in (25) and (26), respectively. Also, (27)
quadratic regulator (LQR) problem since, unlike the LQR gives
problem formulation, e(t f ) is assumed to be fixed.
∂H(·)
= r1 (t)η∗1 (t) + p∗3 (t) = 0
IV.  Analytical Solution Method ∂η1
Since the problem is minimizing the cost functional (15)
∂H(·)
subject to the tracking error state equations (14), the = r2 (t)η∗2 (t) + p∗4 (t) = 0
∂η2
variational method is employed to solve the optimization. The
proposed solution in this section is not only optimal but also from which the optimal control laws in (23) are obtained. ■
guarantees the exponential stability of trajectory and control Taking the second and third derivative of p∗1 (t) and p∗2 (t)
inputs. from (19) and (20) gives
p̈∗i (t) = −qi ė∗i (t) = −qi e∗j (t)
A. Variational Method
...∗ q
Lemma 1: An optimal control η∗ (t) for t ∈ [t0 , t f ] which p i (t) = −qi ė∗j (t) = i p∗j (t), (i, j) = (1,3), (2,4), (28)
ri
gives the optimal state e∗ (t) that minimizes the cost function
(15) is the solution of the following two-point boundary value where ė∗j (t) is given from substitution of (23) in (18). Simil-
(TPBV) state linear differential equations arly, substituting ṗ∗j (t) into the forth derivative of p∗i (t) gives
....∗ q qq q
ė∗1 (t) = e∗3 (t) p i (t) = i ṗ∗j (t) = − i j e∗j (t) − i p∗i (t). (29)
ri ri ri
ė∗2 (t) = e∗4 (t) 1
From (28), we have e∗j (t) = − p̈∗i (t). Thus, equation (29) can
qi
ė∗3 (t) = η∗1 (t) be rewritten as
....
ė∗4 (t) = η∗2 (t). (18) ri p ∗i (t) − q j p̈∗i (t) + qi p∗i (t) = 0, (30)
and co-state linear differential equations which is a homogeneous linear ordinary differential equation.
ṗ∗1 (t) = −q1 e∗1 (t) (19) To solve the linear differential (30), the Laplace transform of
p∗i (t) is first obtained as
ṗ∗2 (t) = −q2 e∗2 (t) (20) qj ... qj
pit∗ 0 s3 + ṗ∗it0 s2 + ( p̈∗it0 − p∗it0 )s + p ∗it0 − ṗ∗it0
ri ri
ṗ∗3 (t) = −q3 e∗3 (t) − p∗1 (t) (21) P∗i (s) = √ )( √ ) , (31)
( q i q i
s2 + 2mi s + s2 − 2mi s +
ṗ∗4 (t) = −q4 e∗4 (t) − p∗2 (t), (22) ri ri
with boundary conditions in (17). The optimal control laws where pit0 = p∗i (t)|t=t0. From (28), η∗i (t) is found as
are also given by ...∗
p i (t) = −qi ė∗j (t) = −qi η∗j (t). (32)
1
η∗1 (t) = − p∗3 (t) = ė∗3 (t) Transforming (32) into the Laplace domain gives
r1
s3 P∗i (s) − s2 p∗it0 − s ṗ∗it0 − p̈∗it0 = −qi H ∗j (s). (33)
1
η∗2 (t) = − p∗4 (t) = ė∗4 (t). (23) Substitution of (31) in (33) gives
r2
Proof: To start solving the minimization (15), we form the 1 ... qj ( ∗ ) qj ∗ qj ∗
− p ∗it0 s3 − p̈it0− p∗it0 s2 + ṗit0 s+ p̈it0
Hamiltonian as follows q q r q r q i ri
H ∗j (s) =
i i i i i
( √ √ .
H(e(t), η(t), p(t)) = qi )( 2 qi )
s2 + 2mi s + s − 2mi s +
ri ri
1 T 1 ( ) (34)
e (t)Qe(t)+ ηT(t)Rη(t)+pT(t) Ae(t)+Bη(t) , (24)
2 2 Taking the inverse Laplace transform of (31) and
where the vector p(t) = [p1 (t) p2 (t) p3 (t) p4 (t)]T includes the substituting the resulted p∗i (t) into the state and co-state
Lagrange multipliers. equations gives the analytical optimal solution of the OPT2
The necessary conditions of optimality are [17] problem with eight unknown parameters of p∗it0, ṗ∗it0, p̈∗it0, and
...∗
p it , which can be found to satisfy the eight initial and final
∂H ∗
ė∗ (t) = (e (t), η∗ (t), p∗ (t))
0
(25) boundary conditions in (17). However, (34) shows that the
∂p
resulted η∗i (t) is unbounded due to the right-half plane roots of
( √ )
∂H ∗ qi
−ṗ∗ (t) = (e (t), η∗ (t), p∗ (t)) (26) the polynomial s2 − 2mi s + . Although, this analytical
∂e ri
solution can address the OPT2 assumptions, it fails to
∂H ∗
0= (e (t), η∗ (t), p∗ (t)). (27) guarantee the stability of the system.
∂η We approach the instability issue by relaxing e(t f ) to be free
State and co-state equations are derived from the while setting four of the initial conditions in (31) to cancel the
MAJD et al.: A STABLE ANALYTICAL SOLUTION METHOD FOR CAR-LIKE ROBOT TRAJECTORY TRACKING AND OPTIMIZATION 43

 
unstable poles. The new assumption automatically changes the  q1 0 0 0  [ ]
 0 q2 0 0 
original problem to the infinite-time LQR problem formulated
Q =   and R = r1 0
(36)
 0 0 q3 0  0 r2
as follows. 
Optimal Control Problem 3 (OPT3): Find an admissible 0 0 0 q4
control η∗ (t) that causes the origin e∗ = 0 of the closed loop are positive definite diagonal weighted matrices satisfying the
system (14) to be globally exponentially stable with an
boundary conditions
admissible trajectory e∗ (t) that minimizes the performance
measure    
 x(t0 ) − xr (t0 )   0 
   0 
1 w tf [ T ]
e(t0 ) = 
y(t0 ) − yr (t0 )  and e(t f ) =  
J(η(t)) = e (t)Qe(t)+ηT (t)Rη(t) dt, (35)  v(t0 ) cos(ψ(t0 )) − ẋr (t0 )   0  (37)
2 t0   
v(t0 ) sin(ψ(t0 )) − ẏr (t0 ) 0
where
Underdamped fi > 0 :
ni −mi (t−t0 ) [ (√ ) (√ )]
e∗i (t) = √ e ai ni sin fi (t − t0 ) − bi cos fi (t − t0 ) + αi (38)
qi fi
n2i −m (t−t ) [ (√ ) (√ )]
e∗j (t) = √ e i 0 ai ni cos fi (t − t0 ) + αi + bi sin fi (t − t0 ) + 2αi (39)
qi fi
−n3i −m (t−t ) [ (√ ) (√ )]
η∗i (t) = √ e i 0 ai ni sin fi (t − t0 ) + 2αi − bi cos fi (t − t0 ) + 3αi . (40)
qi fi
Critically damped fi = 0 :
−1 [ −mi (t−t0 ) ]
ei∗ (t) = bi e − mi (mi ai + bi )te−mi (t−t0 ) (41)
qi
mi [ ]
e∗j (t) = (mi ai + 2bi )e−mi (t−t0 ) − mi (mi ai + bi )te−mi (t−t0 ) (42)
qi
−m2i [ ]
η∗i (t) = (2mi ai + 3bi )e−mi (t−t0 ) − mi (mi ai +bi )te−mi (t−t0 ) . (43)
qi
Overdamped fi < 0 :
ai ( ) bi ( )
e∗i (t) = √ c1i c2i e−c1i (t−t0 ) −e−c2i (t−t0 ) + √ c1i e−c1i (t−t0 ) −c2i e−c2i (t−t0 ) (44)
2qi | fi | 2qi | fi |
ai ( ) bi ( )
e∗j (t) = √ c1i c2i c2i e−c2i (t−t0 ) −c1i e−c1i (t−t0 ) + √ c22i e−c2i (t−t0 ) −c21i e−c1i (t−t0 ) (45)
2qi | fi | 2qi | fi |
ai ( ) bi ( )
η∗i (t) = √ c1i c2i c21i e−c1i (t−t0 ) −c2i
2 −c2i (t−t0 )
e + √ c31i e−c1i (t−t0 ) −c32i e−c2i (t−t0 ) . (46)
2qi | fi | 2qi | fi |

as t f approaches infinity.
TABLE II
B. Globally Exponentially Stable Analytical Solution ai and bi for Three Different Cases
Theorem 1: The globally exponentially stable analytical Damping cases ai and bi coefficients
solutions of the OPT3 are given by (38)–(46), where √
qi fi [ e jt0 ]
( √ ) √ √ ai = 2 + 2eit0 sin(αi )
ni cos(αi ) ni
1 qi q j 1 qi q j Underdamped fi > 0 √
fi = 2 − , mi = 2 + qi fi eit0
4 ri ri 2 ri ri bi = − ,
√ √   ni cos(αi )
4 qi  mi 
ni = fi + mi =
2 , αi = arctan  √  1 [ ]
ri fi ai = qi e jt0 + 2mi qi eit0 ,
√ √ Critically damped fi = 0 m2i
c1i = mi − | fi | > 0, c2i = mi + | fi | > 0, (47) bi = −qi eit0 ,
and ai and bi are given as in Table II, which satisfy the initial [ ]
1
boundary condition e(t0 ) in (17). ai = qi e jt0 + 2mi qi eit0
Overdamped fi < 0 m2i − | fi |
Proof: As mentioned in Section IV-A, in order to guarantee
bi = −qi eit0 .
the exponential stability of the origin in the closed loop
44 IEEE/CAA JOURNAL OF AUTOMATICA SINICA, VOL. 7, NO. 1, JANUARY 2020

system (14), the initial conditions in (31) are tuned so as to x∗ (t) = xr (t) + e∗1 (t)
cancel the unstable poles related to the polynomial
√ y∗ (t) = yr (t) + e∗2 (t)
qi ( ∗ )
(s − 2mi s +
2 ) . To do so, the initial conditions of (31) are ẏ (t)
ri ψ∗ (t) = arctan ∗
assigned as ẋ (t)
 ∗ (t)

 ẋ ẏ∗ (t) π
p∗it0 = ai 

 = ; ψ∗ (t) , {0, ± , π}


 cos(ψ (t)) sin(ψ∗ (t))
∗ 2
ṗ∗it0 = bi 

 ∗ ψ∗ (t) = 0
 ẋ (t);
√ v∗ (t) = 
 (55)
qj qi 

 − ẋ∗ (t); ψ∗ (t) = π
p̈∗it0 = −2mi (bi + 2mi ai ) + ai + ai 


ri ri 

 π
√  sign(ψ∗ (t))ẏ∗ (t); ψ∗ (t) = ± ,
...∗ qi q j 2
p it = (bi + 2mi ai ) + bi , (48)
0 ri ri where
which gives ẋ∗ (t) = ẋr (t) + e∗3 (t)
ai s + bi + 2mi ai ẏ∗ (t) = ẏr (t) + e∗4 (t). (56)
P∗i (s) = , (49)
(s + mi )2 + fi The optimal control inputs are found from (10) as
where ai and bi should be determined by the initial boundary  
[ ]  cos(ψ∗ (t)) sin(ψ∗ (t)) 
conditions. Since P∗i (s) is a strictly-proper rational function of ∗
a (t) 
  ∗
s , p∗i (t) is a time-invariant function [18]. Location of poles in =  ℓ ℓ  ζ (t),
tan(δ∗ (t))  − 2 sin(ψ∗ (t)) cos(ψ∗ (t)) 
(49) leads in three underdamped, critically damped, and over- ∗
v (t) ∗2
v (t)
damped solution cases: (57)
[ ]
1) Underdamped fi > 0 : If P∗i (s) has complex roots, the ẋ(t) − ẋr (t)
inverse Laplace transform of (49) gives p∗i (t) for t ∈ [t0 , t f ] as where ζ ∗ (t) = η∗ (t) + .
ẏ(t) − ẏr (t)
1 [ (√ ) Remark 2: Since ẋ∗ (t) and ẏ∗ (t) are stable( and arctan(·) is the
p∗i (t) = √ e−mi (t−t0 ) ai ni cos fi (t − t0 ) − αi π π)
fi continuous function of its argument in − , , it can be
(√ )] 2 2
+ bi sin fi (t − t0 ) . (50) concluded from (55) that the internal dynamics ψ∗ (t) and v∗ (t)
are bounded.
By substituting the first derivative of p∗i (t) in (19) and (20),
ni [ (√ ) C. Boundary Conditions Satisfaction
−qi e∗i (t) = − √ e−mi (t−t0 ) ai ni sin fi (t − t0 )
fi The parameters ai and bi should be set to satisfy the
(√ )] boundary conditions. Substituting the initial conditions (37)
− bi cos fi (t − t0 ) + αi , (51)
into e∗i (t0 ) and e∗j (t0 ) gives ai and bi as illustrated for each case
which gives e∗i (t) as (38). Consequently, e∗j (t) is found by tak- in Table II. As mentioned in Section IV-A, the final time
ing the first derivative of e∗i (t) as in (39). boundary conditions may not be fulfilled since two unstable
2) Critically Damped fi = 0 : This case occurs when the poles are eliminated in (31) to guarantee the stability, and the
weights follow the equality q2j − 4ri qi = 0. In this case, the unknown parameters ai and bi are used up to satisfy the initial
inverse Laplace transform of (49) gives p∗i (t) for t ∈ [t0 , t f ] as boundary conditions. However, the solution is a good
approximation to satisfy e∗ (t f ) ≈ 0 due to the terms e−c1i (t f −t0 )
p∗i (t) = ai e−mi (t−t0 ) + (mi ai + bi )te−mi (t−t0 ) . (52) and e−c2i (t f −t0 ) in overdamped, and e−mi (t f −t0 ) in underdamped
Substitution of its first and second derivatives in (19), (20), and critically-damped cases. Thus, there exists a trade off
and (28) gives the closed form solution for e∗i (t) and e∗j (t) as in between guaranteeing the stability, as proved for the OPT3
(41) and (42). problem, and meeting the exact final time conditions as
3) Overdamped fi < 0 : Equation (49) can be rewritten as elaborated in the solutions of OPT2.
Remark 3: From (47) it can be concluded that for the fixed
ai s + bi + 2mi ai
P∗i (s) = , (53) value of ri, if the values of qi, and q j increase by the factors of
(s + mi )2 − | fi |
β2i and βi (for βi > 1), respectively, the values of mi for the
where | fi | is the absolute value of fi . By taking the inverse underdamped and critically damped cases, c1i and c2i for the
Laplace of P∗i (s), overdamped case will be increased as
ai ( ) √ √ old
p∗i (t) = √ c2i e−c1i (t−t0 ) − c1i e−c2i (t−t0 ) = βi mold
mnew i , c1i,2i = βi c1i,2i ,
new
2 | fi | i (58)
bi ( −c1i (t−t0 ) −c2i (t−t0 ) ) which determines how fast the tracking errors converge to
+ √ e −e . (54) zero.
2 | fi |
Similarly as the former cases, e∗i (t) and e∗j (t) are found as in V.  Simulation Results
(44) and (45). ■ In this section, the effect of the weight change in controller
Finding the errors give us the optimal states as performance, and tracking error is evaluated. At the end, the
MAJD et al.: A STABLE ANALYTICAL SOLUTION METHOD FOR CAR-LIKE ROBOT TRAJECTORY TRACKING AND OPTIMIZATION 45

proposed closed-form solution in this paper is compared with 2.0 Reference Trajectory
qi = 10, qj = 1, ri = 1, UD
qi = 1, qj = 10, ri = 1, OD
other well-known methods in the literature. The simulations qi = 0.2, qj = 2, ri = 5, CD
are developed in MATLAB. The reference time-parametrized 1.5

Latitude (m)
trajectory, for t ∈ [0, 30], is an eight-shaped Lissajous curve
1.0
expressed as .
( 2πt ) 0.5 Initial point
xr (t) = 1.1 + 0.7 sin
30
( 4πt ) 0
0.4 0.6 0.8 1.0 1.2 1.4 1.6 1.8
yr (t) = 0.9 + 0.7 sin . (59) Longitude (m)
30
The system starts from the initial configurations, x(0) = 1.1,
Fig. 3. Weighting parameters impact on final time errors for the eight-
y(0) = 0.8, v(0) = 1, and ψ(0) = 1.3, and the errors are expec-
shaped trajectory tracking.
ted to be zero at final time.
The tracking errors for three different cases are shown in
1.0
Fig. 2. The errors can remain in a certain bound by making a 3 Reference Trajectory
qi = 10, qj = 1, ri = 1, UD
trade-off among qi, q j , and ri weights. The impact of weight 2 0.8 qi = 1, qj = 10, ri = 1, OD

Heading angle (rad)

qi = 0.2, qj = 2, ri = 5, CD

Velocity (m/s)
choice on the final time error can also be seen by decreasing 1 0.6
the control effort that will increase the tracking time and 0
consequently, it may unfavorably cause the final time error −1 0.4
not to be close to zero. However, the trajectory error is −2 0.2
globally exponentially stable and control inputs are −3
guaranteed to be in t ∈ [0, 30]. 0 10 20 30
0
0 10 20 30
Time (s) Time (s)
0.10 (a) (b)
0.4

0.05 0.2 Fig. 4. Eight-shaped trajectory tracking: (a) heading angle: ψ∗ (t) and ψr (t) ;
e*1

e*2

(b) velocity: v∗ (t) and vr (t).

0
0
0 10 20 30 0 10 20 30 0.2
Time (s) Time (s) 1.5
1.0 0
Steering angle (rad)

Acceleration (m/s2)
0.10 0.6
0.5 −0.2
0.4
0.05 0 −0.4
e*3

e*4

0.2
0 −0.5 −0.6
0
−1.0 qi = 10, qj = 1, ri = 1, UD
0 10 20 30 0 10 20 30 −0.8 qi = 1, qj = 10, ri = 1, OD
Time (s) Time (s) qi = 0.2, qj = 2, ri = 5, CD
−1.5 −1.0
0 0 0 10 20 30 0 10 20 30
Time (s) Time (s)
−0.1 (a) (b)
−1
η*1

η*2

−0.2 qi = 10, qj = 1, ri = 1, UD

−0.3
qi = 1, qj = 10, ri = 1, OD
qi = 0.2, qj = 2, ri = 5, CD
−2
0 10 20 30 0 10 20
Time (s) Time (s)
Fig. 2. Tracking errors compared for different weighting parameters (UD =
Underdamped, OD = Overdamped, and CD = Critically damped).
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 e(t f )
is almost zero at tf = 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 ψ∗ (t) are
also evident in these figures.
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),
Fig. 5. Eight-shaped trajectory tracking: (a) steering angle: δ∗ (t) ; (b) accel-
eration: a∗ (t).
30
i.e., e p (·)2 + ev (·)2 + ea (·)2 , is plotted over time in Fig. 6. The
weights are set to be qi = 1, q j = 1, and ri = 1 for both cases.
The cost function value J(·) for the analytical solution is 9.87
which shows more than 380% reduction compared to the
numerical case which gives 47.45.
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 u∗ (t) without considering any parametrized
geometric function representations for control inputs since
they explicitly appear in the cost function. The input-output
46 IEEE/CAA JOURNAL OF AUTOMATICA SINICA, VOL. 7, NO. 1, JANUARY 2020

TABLE III reality by applying the calculated optimal input signal.

Error Measurements at Three Time Instances However, if the model is inaccurate and the system is subject
(t f = 30 s , t f = 40 s , and t f = 50 s ).
to disturbance not included in the model, the result of
Error Case tf = 30 s tf = 40 s tf = 50 s
applying the open-loop control signal may be different than
UD –1.35E–20 2.69E–25 3.93E–31 what is expected. Thus, in our future work, we aim to apply
e∗1 (t) OD 3.09E–06 1.29E–07 5.36E–09 the repeated application of this open-loop strategy in discrete-
CD 5.41E–06 8.23E–08 1.18E–09 time domain to obtain the same feedback effect of recursive
UD –7.47E–20 1.49E–24 2.18E–30 techniques, known as receding horizon strategy in the
e∗2 (t) OD 1.71E–05 7.13E–07 2.97E–08 literature.
CD 3.00E–05 4.57E–07 6.52E–09
UD –2.64E–19 –5.76E–25 –3.23E–31 References
e∗3 (t) OD –9.82E–07 –4.09E–08 –1.70E–09 [1] R. W. Brockett et al., “Asymptotic stability and feedback stabilization,”
Differential Geometric Control Theory, vol. 27, no. 1, pp. 181–191,
CD –2.24E–06 –3.48E–08 –5.02E–10 1983.
UD –1.47E–18 –3.19E–24 –1.79E–30 [2] B. Paden, M. Čáp, S. Z. Yong, D. Yershov, and E. Frazzoli, “A survey
of motion planning and control techniques for self-driving urban
e∗4 (t) OD –5.44E–06 –2.27E–07 –9.45E–09 vehicles,” IEEE Trans. Intelligent Vehicles, vol. 1, no. 1, pp. 33–55,
CD –1.24E–05 –1.93E–07 –2.78E–09 2016.
[3] Y. Kanayama, Y. Kimura, F. Miyazaki, and T. Noguchi, “A stable
UD 7.58E–19 7.06E–25 –3.69E–31
tracking control method for an autonomous mobile robot, ” in Proc.
η∗1 (t) OD 3.12E–07 1.30E–08 5.41E–10 IEEE Int. Conf. Robotics and Automation, 1990.
CD 9.20E–07 1.46E–08 2.14E–10 [4] A. De Luca, G. Oriolo, and C. Samson, “Feedback control of a
nonholonomic car-like robot,” Robot Motion Planning and Control,
UD 4.20E–18 3.92E–24 –2.05E–30 pp. 171–253, 1998.
η∗2 (t) OD 1.73E–06 7.21E–08 3.00E–09 [5] B. d’Andréa Novel, G. Campion, and G. Bastin, “Control of nonholo-
nomic wheeled mobile robots by state feedback linearization,” The Int.
CD 5.10E–06 8.11E–08 1.19E–09
J. Robotics Research, vol. 14, no. 6, pp. 543–559, 1995.
[6] P. Gáspár, Z. Szabó, and J. Bokor, “LPV design of fault-tolerant control
for road vehicles,” Int. J. Applied Mathematics and Computer Science,
Analytical vol. 22, no. 1, pp. 173–182, 2012.
400 Numerical
[7] J. Fu, F. Tian, T. Chai, Y. Jing, Z. Li, and C.-Y. Su, “Motion tracking
350 control design for a class of nonholonomic mobile robot systems,” IEEE
300 Trans. Systems, Man, and Cybernetics: Systems, no. 99, pp. 1–7,
[8] R. Postoyan, M. C. Bragagnolo, E. Galbrun, J. Daafouz, D. Neši ć, and
250 E. B. Castelan, “Event-triggered tracking control of unicycle mobile
robots,” Automatica, vol. 52, pp. 302–308, 2015.
200
[9] D. Gu and H. Hu, “Receding horizon tracking control of wheeled
150 mobile robots,” IEEE Trans. Control Systems Technology, vol. 14, no. 4,
pp. 743–749, 2006.
100
[10] Y. Zhang, S. Li, and X. Liu, “Neural network-based model-free
50 adaptive near-optimal tracking control for a class of nonlinear systems,”
IEEE Trans. Neural Networks and Learning Systems, 2018.
0
0 0.5 1.0 1.5 2.0 2.5 [11] G. V. Raffo, G. K. Gomes, J. E. Normey-Rico, C. R. Kelber, and L. B.
Time (s) Becker, “A predictive controller for autonomous vehicle path tracking,”
IEEE Trans. Intell. Trans. Sys., vol. 10, no. 1, pp. 92–102, 2009.
Fig. 6. Comparison of analytical and numerical methods based on their [12] D. Q. Mayne, J. B. Rawlings, C. V. Rao, and P. O. Scokaert,
measured cost function integrand, i.e., e p (·)2 + ev (·)2 + ea (·)2 , over time. “Constrained model predictive control: stability and optimality,”
Automatica, vol. 36, no. 6, pp. 789–814, 2000.
linearization technique is employed to linearize the system [13] G. Klančar and I. Škrjanc, “Tracking-error model-based predictive
control for mobile robots in real time,” Robotics and Autonomous
from the defined auxiliary input to output. The closed-form Systems, vol. 55, no. 6, pp. 460–469, 2007.
optimal trajectories and control inputs are obtained by the [14] I. Škrjanc and G. Klančar, “A comparison of continuous and discrete
calculus of variation technique, and boundedness of the tracking-error model-based predictive control for mobile robots,”
solution and internal dynamics are also proved. Robotics and Autonomous Systems, vol. 87, pp. 177–187, 2017.
The proposed framework can be easily extended to tackle [15] H. K. Khalil and J. Grizzle, Nonlinear Systems. Prentice hall Upper
Saddle River, NJ, vol. 3, 2002.
the optimal trajectory optimization problem of a higher
[16] R. Rajamani, Vehicle Dynamics and Control. Springer Science &
dimensional kinematic model with more states by adding the Business Media, 2011.
higher order derivatives of the x(t) and y(t) into the cost [17] D. E. Kirk, Optimal Control Theory: An Introduction. Courier
function and assuming that the reference trajectory is smooth. Corporation, 2012.
Our proposed optimal planning framework is an open loop [18] W. J. Rugh, Linear System Theory. Prentice hall Upper Saddle River,
control strategy which yields continuous control signals NJ, vol. 2, 1996.
(sequence of inputs) as a function of the initial state of the [19] K. Majd, M. Razeghi-Jahromi, and A. Homaifar, “Optimal
Kinematicbased Trajectory Planning and Tracking Control of
vehicle. Thus, if the states evolves exactly according to the Autonomous Ground Vehicle Using the Variational Approach, ” in
model then the predicted state trajectory is exactly obtained in Proc. IEEE Intelligent Vehicles Symposium, 2018.
MAJD et al.: A STABLE ANALYTICAL SOLUTION METHOD FOR CAR-LIKE ROBOT TRAJECTORY TRACKING AND OPTIMIZATION
Keyvan Majd received the B.S. degree in electrical
engineering from Ferdowsi University of Mashhad,
Mashhad, Iran, in 2015, and the M.S. degree in elec-
trical engineering from North Carolina A&T State
University, Greensboro, NC, in 2019. He is cur-
rently pursuing the Ph.D. degree in Computer Sci-
ence at Arizona State University, Tempe, AZ. His re-
search interests include motion planning of autonom-
ous vehicles, optimal control theory and model pre-
dictive control.
Mohammad Razeghi-Jahromi received the B.S.
degree from Amirkabir University of Technology,
Tehran, Iran, in 1997, the M.S. degrees from Uni-
versity of Tehran, Tehran, Iran in 2000 and Uni-
versity of Rochester, Rochester, NY, USA in 2012,
respectively and the Ph.D. degree from University of
Rochester, Rochester, NY, USA in 2016, all in elec-
trical and computer engineering. He joined ABB
Corporate Research Center United States in 2017,
where he is currently a Research Scientist. His main
areas of research interest include control systems theory, optimization,
47
stochastic process, statistical signal processing, and machine learning.
Abdollah Homaifar received B.S. and M.S. degrees
from the State University of New York at Stony
Brook in 1979 and 1980, respectively, and his Ph.D.
degree from the University of Alabama in 1987, all
in electrical engineering. He is the NASA Langley
Distinguished Professor and the Duke Energy Emin-
ent Professor in the Department of Electrical and
Computer Engineering at North Carolina A&T State
University (NCA&TSU). He is the Director of the
Autonomous Control and Information Technology
Institute and the Testing, Evaluation, and Control of Heterogeneous Large-
scale Systems of Autonomous Vehicles (TECHLAV) Center at NCA & TSU.
His research interests include machine learning, unmanned aerial vehicles
(UAVs), testing and evaluation of autonomous vehicles, optimization, and
signal processing. He also serves as an Associate Editor of the Journal of In-
telligent Automation and Soft Computing and is a Reviewer for IEEE Trans-
actions on Fuzzy Systems, Man Machines and Cybernetics, and IEEE Trans-
actions on Neural Networks. He is a Member of the IEEE Control Society,
Sigma Xi, Tau Beta Pi, and Eta Kapa Nu.