Sensors 2018, 18, 2185 6 of 29
Lipp et al. [
3
] presented a convex-optimization-based general minimum time speed planning
method over the fixed path based on the approach proposed by [
2
]. The friction circle constraint is well
considered as a convex set constraint acting on the problem formulation, which leads to an elegant
solution. Not only the capacity of mobility of cars are fully explored, but also the total time travelling
along the path is explicitly and analytically represented as a soft constraint to achieve time efficiency.
The problem is solved by a customized interior point method using log barrier functions efficiently.
Thanks to the preserving convexity of the problem formulation, the global optimality of solutions
is guaranteed. However, smoothness of the speed profile is not consider, which most likely results
in the same issues that we mentioned about Dakibay’s work regarding tracking performance and
safety concerns. In addition, the use of customized Newton-based solver requires that constraints
and objective functions are all at least twice differentiable, which seems very restrictive on the type of
constraints that users can impose in convex optimization. Convex problems with non-differentiable
constraint terms cannot be solved by their framework.
Liu et al. [
11
] recently introduced a temporal optimization approach, optimizing time stamps
for all waypoints along a fixed path with respect to time window constraints at each point, and then
using a slack convex feasible set algorithm to solve it iteratively. Smoothness of the speed profile and
time efficiency are taken into account in the problem formulation. However, the time efficiency is
considered in an indirect way that optimizes IoD with respect to a reference speed over the path. Their
formulation leads to a highly nonlinear and non-convex problem and is solved by a local optimization
method, thus only local optimality is guaranteed. They addressed some important constraints in
Table 1 such as smoothness, time window and comfort box constraints in their formulation but left
out the friction circle constraint, which does not fully exploit the acceleration capacity of the vehicle.
In addition, since they optimized timestamps directly, we do not see a quick way to impose a path
constraint or a point constraint as a hard one to manipulate speed profiles.
3. Problem Formulation
Assuming a curvature continuous path has been generated by a hierarchical motion planning
framework like [
9
,
22
], the speed planning is to find a time-efficient, safe, and smooth speed profile
travelling along the fixed path with respect to both safety and performance constraints.
To solve the proposed problem, we optimize the performance criterions from three aspects,
smoothness
J
S
, time efficiency
J
T
, and speed deviation
J
V
from a desired speed, with others left as
hard constraints or semi-hard constraints. We first introduce the path representation and explain
the relationship of an arc-length parametrized path and a time parametrized path, then present
mathematical expressions of all the constraints, and pose the optimization problem at the end.
3.1. Path Representation
The goal of speed planning is to find a speed profile along a fixed path. Since the path is known,
we need to reconstruct the mapping between the known path and the speed profile, then represent the
speed profile with parameters determined by the prescribed path. A rich set of parameterized path
representations has been proposed in the literature, including B-spline [
23
,
24
], Bezier curve [
25
,
26
],
clothoid [
27
,
28
], polynomial curve [
29
] and polynomial spiral [
30
,
31
]. It is trivial to convert all the
listed curve models to a simple waypoints representation, but not vice versa. To avoid the non-trivial
converting between curve models, we use the general waypoints parametrization to represent a
fixed path, with the orientation and curvature encoded implicitly by the path. Formally, we define a
waypoints parametrized curve as a workspace path. A workspace path, r, of the body point, b, at the
center of the rear axle with footprint,
A
, is defined as
r : [
0,
s
f
] → R
2
. More specifically, we consider
the following arc-length parametric form in Cartesian coordinate system,
r(s) = (x(s), y(s)), s ∈ [0, s
f
],
(1)