report

Minimum-Time Path Planning for Multiple
Autonomous Ground Vehicles with Obstacle
Avoidance
Naga Venkata Murali, Veerapaneni∗
Sachan, Johny†
Jose, Garcia‡
Optimal Control problem, which is a dynamic optimization problem over a
time horizon, is a practical problem in determining control and state trajectories
to minimize a cost functional. In this article a Numerical trajectory optimiza-
tion method has been discussed, which discretizes the state and control to N
points in which a cubic polynomial approximation is used to approximate the
states between two successive nodal points and linear interpolation between two
successive nodes to approximate control. So the entire dynamic optimization
has been transcribed to a parameter optimization method where the parameters
being States and Control at the nodal points, which is nothing but Non-Linear
Programming.
I. Problem Description
The problem is about kinematic path planning problem for two autonomous ground vehicles.
The requirements for the two vehicles should be vehicles must reach a goal given their initial
positions and orientations, while not crossing the perimeters of any circular obstacles placed in
field.
Given the state dynamical equations, the solution to this problem must meet certain criteria.
Those are:
• Both vehicles must arrive at or within the perimeter of the goal.
• Neither vehicle should travel longer than the minimum final time of its partner (i.e. at
least one vehicle will have a minimum-time trajectory and its partner will arrive at the
same time).
• The paths should not cross obstacle perimeters
• The vehicles should take different paths.
∗
Graduate Student, Mechanical and Aerospace Engineering Department, University of Texas at Arlington.
†
Graduate Student, Mechanical and Aerospace Engineering Department, University of Texas at Arlington,
‡
Graduate Student, Mechanical and Aerospace Engineering Department, University of Texas at Arlington,
1 of 25
American Institute of Aeronautics and Astronautics

The differential equations for both the vehicles were given as,
˙xr = V cos φ sin θ
˙yr = V cos φ cos θ
˙θ = (V/L) sin φ
and the constraints for the steering angle is
−π/2 ≤ φ ≤ +π/2
where xr and yr are the cartesian coordinates of the vehicle and θ is the heading angle measured
positive from the vertical y-axis to the body line of the vehicle. The constant, L, is the wheelbase
of the vehicle, and V is the speed.
II. Numerical Trajectory Optimization
A. Direct Collocation:
This section briefly discusses about the direct collocation method which transcribes the dynamic
optimization problem to a nonlinear parameter optimization method. In direct collocation the
entire trajectory is divided into number of nodes called collocation points and a piecewise
continuous functions were used to approximate states and control.The control is approximated
using piecewise continuous linear function and states using piecewise continuous cubics. so the
entire problem now becomes finding the for these piecewise continuous functions which satisfies
the boundary conditions between each successive nodes.
The entire problem is discretized into N number of nodes,
0 = ti < ti+1 < ti+2 < . . . < tN = tf
where, ti, ti+1, . . . , tf are time at Nodes i, i + 1, . . . , N respectively and tf being final time.
The states and controls are also discretized into N parameters at N grid points along with final
time tN , which will be the parameters for nonlinear program.
P = (x(ti), x(ti+1), . . . , x(tN ), u(ti), u(ti+1), . . . , u(tN ), tN )
For each interval from i to i+1 the controls are chosen as piecewise linear interpolating functions,
u(t) = u(ti) +
t − ti
ti+1 − ti
(u(ti+1) − u(ti))
The states are chosen as piecewise continuous cubic polynomials between xi and xi+1,
x(t) = co + c1t + c2t2
+ c3t3
2 of 25

To calculate the polynomial coefficients between i and i + 1,
x(ti) = co + c1ti + c2t2
i + c3t3
i (1)
˙x(ti) = c1 + 2c2ti + 3c3t2
i (2)
x(ti+1) = co + c1ti+1 + c2t2
i+1 + c3t3
i+1 (3)
˙x(ti+1) = c1 + 2c2ti+1 + 3c3t2
i+1 (4)
where x(ti), x(ti+1) were obtained from Node points and ˙x(ti), ˙x(ti+1) were obtained from dif-
ferential equations by substituting x(ti), x(ti+1) respectively
So the coefficients were obtained as,





Co
C1
C2
C3





=





1 ti t2
i t3
i
0 1 2ti 3t2
i
1 ti+1 t2
i+1 t3
i+1
0 1 2ti+1 3t2
i+1





−1
∗





x(ti)
˙x(ti)
x(ti+1)
˙x(ti+1)





with boundary conditions xi at ti and xi+1 at ti+1
The approximating functions of the states have to satisfy the differential equations at the grid
points tk, k = 1, . . . , N. This is known as cubic collocation. The assumed approximation of x(t)
must satisfy the differential equations at the grid points tk. As the polynomial already fulfills
the differential equations at the grid points, the only constraints for the nonlinear program will
be,
• the midpoint of the cubic polynomial should be equal to the value of the differential
equation at tc =
(ti+1 − ti)
2
f(x(tc), u(tc), tc) − ˙xc(tc) = 0
which is nothing but the defect at the collocation point and it should be zero,
where ˙xc = c1 + 2c2tc + 3c3t2
c and f(x(tc), u(tc), tc) is differential equation evaluated at
xc =
xi+1 + xi
2
• and all the inequality constraints like obstacles will be added to the nodal points,
g(x(tk), u(tk), tk) ≥ 0, k = 1, . . . , N
• and the initial and end point constraints at t1, tN
Any nonlinear programming technique can be used to solve this problem like sequential quadratic
programming, interior-point method, trust region, active-set.
B. Performance Index:
This section talks about the cost function that should be minimized based on the parameters
and objectives. Those were given as,
• For reaching the goal in minimum time the cost that was used is,
Jf = α1(tf )
where α1 is the weight on the final time, to reduce the emphasis on final time α can be
0.1 or to keep more focus on final time α can be 10 to 25;
3 of 25

• To specify the goal position position, the cost function of distance between vehicle and
goal is used,
Jg = α2(x1(tN ) − xg)2
+ (y1(tN ) − yg)2
+ (x2(tN ) − xg)2
+ (y2(tN ) − xg)2
where (xg, yg) are goal location.
• For minimizing the rate of change of control, cost function of minimum control is,
Jc = α3(∆φ2
1 + ∆φ2
2)
• For adding penalty function, when any vehicle violates the obstacle constraint or Vehicle
to Vehicle distance constraint inverse square law is used,
Jp = β/(rj)2
if rj ≤specified distance and
Jp = 0
if rj ≥ given distance.
And β is some weighting term to increase or decrease the penalty term, β = 0.01 to 1 to
increase penalty and β = 1 to 1000 to decrease the penalty.
So the total performance index now becomes,
J = Jf + Jg + Jc + Jp
For the simulation α1 = 1, α2 = 10, α3 = 50, β = 10.
C. Constraints:
This section talks about the constraints on the system that should be added for each node. As
the both vehicles must avoid obstacles, the obstacles were added as constraints at each node for
every obstacle. As the any node can be placed anywhere so each obstacle constraint is given to
each node and it is given as,
g1obstacles =⇒ (rbj)2
− (x1k − xbj)2
+ (y1k − ybj)2
≤ 0
g2obstacles =⇒ (rbj)2
− (x2k − xbj)2
+ (y2k − ybj)2
≤ 0
where, g1obstacles, g2obstacles are obstacle constraint for vehicle 1 and 2 respectively, rbj is the
radius of the obstacle,(xbj, ybj) is the center of obstacles k = 1, . . . , N, j = 1, . . . ,number of
obstacles.
The constraint for maintaining the distance between two vehicles, so that they won’t collide
each other is given as,
gvehicle =⇒ (rvehicle)2
− (x1k − x2k)2
+ (y1k − y2k)2
≤ 0
where rvehicle is the distance between the vehicles that they should maintain.
III. Simulation Setup
The first vehicle is placed at the origin, and the second is placed directly below it at y =
4.Both vehicles are oriented parallel to the x-axis. The minimum allowable distance between
the vehicles is 2.25 units. The vehicle speed is a constant 0.2 units, and the steering wheel may
be deflected up to ±π/2 radians from the axis of the vehicle. Each vehicle has a wheelbase of 2
units. The goal is placed at (25, 15). The safe zone around the goal position is about a radius
2.5 units.
4 of 25

IV. Results
This section talks about the results that was obtained from the implementation of above
mentioned technique. The results were divided into two sections, one talks about effect of num-
ber of nodes and another talks about the effect of weight on the minimum control cost.
Effect of Number of nodes:
This section talks about the effect of number of nodes on the overall result without adding the
vehicle to vehicle distance penalty. The increase in number of nodes increases the accuracy of
the result, as the nodes were placed closely together and as cubic polynomial is assumed between
them, the distance between the nodes and the defect is so small, that the defect evaluation at
the mid interval is accurate enough.
The algorithm has been implemented for different number of nodes. It turns out that less
number of nodes were not sufficient enough to consider or avoid obstacles, as the nodes were
spaced far apart from each other, the nodes were not able to see the obstacles. So a good
amount of nodes were required to account for all the obstacles that was given.
5 of 25

Figure 1. Obstacle avoidance of two vehicles above one with 37 nodes and below one with 65
nodes
By looking at the fig.1., it turns out that the 37 nodes were actually good enough to avoid
the obstacles. As the two figures (37 nodes and 65 nodes) does look the same, but the result
that was obtained after applying the optimal control which is obtained from the fmincon to
the actual system(differential equations) and integrate the equations forward, the trajectories
of System and Fmincon does not match, which is shown in following figures.
6 of 25

Figure 2. Obstacle avoidance of two vehicles with 37 nodes fig.a is the path of the system, and
fig.b is the path from fmincon
From fig.2 it turns out that the paths from the system and fmincon does not match. The
reason for the two paths not matching is because of the steering angle .
7 of 25

Figure 3. Steering Angle(Control) of both the vehicles with 37 nodes.
From ﬁg.3 it is clear that there was a lot of jumps of steering angle between the successive
nodes, So because of this reason the solution deviated from the fmincon, after integrating the
system with the obtained Optimal control. This can be rectiﬁed by increasing the weight on
the minimum control cost function or by increasing the number of nodes.
8 of 25

Figure 4. Obstacle avoidance path of two vehicles from the system and from Fmincon
From ﬁg.4 it turns out that the paths from the system and fmincon does match each other.
It is because of the increasing the number of nodes decreased the jumps between the steering
angle of successive nodes. This was shown in ﬁg.5.
9 of 25

Figure 5. Steering Angle(Control) of both the vehicles with 65 nodes.
10 of 25

Figure 6. Comparision of Trajectories from System and Fmincon for Vehicle One and Two
11 of 25

Figure 7. Comaprision of Heading angle from System and Fmincon for Vehicle one and Two
The fig.6 and fig.8, shows the comparision between Trajectory of states obtained from in-
tegrating the differential equation after obtaining the optimal control and from fmincon for 65
nodes.
12 of 25

Effect of weight on Minimum Control cost function:
This section talks about the effect of weight on the Minimum control cost function. Mini-
mum control cost function is the cost function which reduces the rate of change of steering
angle(Control). By increasing the weight on that cost function, the optimizer will reduce that
cost function even further so that corresponding cost function will be much lesser than before
without any weight on it.
So by increasing the weight on the Minimum control cost function will reduce the actuator
rate even further so that we will see smooth curve for Control trajectory.
Figure 8. Obstacle Avoidance of the two vehicles after increasing the weight on the control cost
function to 50 and 67 nodes.
By comparing fig.8 and fig.2 it is clear that after increasing the weight on the control cost
function, we can see that the vehicles path has a lot of curving than from fig.2(No weight on
control). It is because that as the weight decreased the control cost much further, it decreased
the actuator rates so the vehicles don’t have much of an option to turn instantaneously as
before. But adding weight on the control also increases the final time. As the vehicle have to
take longer turn than before, the final time increased a lot than before. Final time with no
weight on control cost function came out to be 177.74 seconds and after adding the weight on
the control the final time turns out to be 235.5138 seconds.
13 of 25

Figure 9. Comparision of Trajectories of both the vehicles with System and Fmincon with 67
nodes
14 of 25

Figure 10. Comparision of Trajectories of Heading angle for both the vehicles with System and
Fmincon with 67 nodes
15 of 25

Figure 11. Steering angle of both the vehicles with 67 nodes
By looking at the fig.11(weight on control) and fig.5(No weight on control) it looks like fig.5
have lesser oscillations than fig.11. But by looking at the difference in angle φ(tf ) − φ(t0) it
turns out that the δφ for fig.5 is 30o compared to δφ for fig.11 which is 18o.
16 of 25

After adding Penalty for Vehicle to Vehicle distance constraint:
This section talks about the obstacle avoidance path after adding the Vehicle to Vehicle dis-
tance penalty function. The simulation is conducted with 67 nodes by adding the inequality
constraints for Vehicle to Vehicle distance maintenance. Penalty function is introduced into the
cost function for violations of the vehicle to vehicle distance constraint with weight on Penalty
100. The constraint was removed when the rover is 5 units within the goal position. The
following were the results obtained after implementing this simulation case,
Figure 12. Comparision of Obstacle avoidance path of two rovers between system (After integrat-
ing) and Fmincon with 67 nodes and 10 weight on Minimum Control Cost.
From the above ﬁgure, it turns out that the Fmincon violated the Vehicle to Vehicle con-
straints with penalty function, but the actual system tends to maintain some distance between
two vehicles.
17 of 25

Figure 13. Comparision of Trajectory of two rovers between system (After integrating) and
Fmincon with 67 nodes and 10 weight on Minimum Control Cost.
18 of 25

Figure 14. Comparision of Trajectory of Heading angle of two rovers between system (After
integrating) and Fmincon with 67 nodes and 10 weight on Minimum Control Cost.
19 of 25

Figure 15. Trajectory of steering angle(Control) of two rovers with 67 nodes and no weight on
Minimum Control Cost.
With diﬀerent Initial Conditions:
This section talks about the what the algorithm tries to do if one vehicle is close to the goal
and another vehicle is far apart. The position of rover one is changed to (30,15) just 5 units
beside the goal and the rover two position is at (0,-4). It is expected that the vehicle close to
the goal has to travel away from goal and come back to achieve the minimum time, same time
20 of 25

condition.
The following figure demonstrates the above mentioned result.
Figure 16. Comparision of Obstacle Avoidance Trajectory of both vehicles obtained from sys-
tem(Intgeration) and Fmincon
From the above figure, it is clear that because the first vehicle is close to goal and it cannot
reach the goal before the second obstacle it did deviated from the goal position went through
all the unnecessary path and then both the vehicles reached the goal position at the same time.
21 of 25

Both the trajectories did not matched perfectly because when we integrate the system with a
step size of 0.1 the path kind of deviated because of the lot of jumps in steering angle. As
this simulation is done without using any weight on the control, this can be rectiﬁed by adding
weight on the Minimum control cost function.
Figure 17. Trajectory of both vehicles obtained from system(Intgeration) and Fmincon
22 of 25

Figure 18. Trajecttory of Heading angle for both the rover’s system and fmincon
23 of 25

Figure 19. Trajectory of steering angle of both the vehicles
24 of 25

V. Team Member Contributions to project
Team Member Contribution(%)
V.N.V.Murali 80
Sachan,Johny 10
Jose,Garcia 10
VI. Conclusion
From the above mentioned technique and results the direct collocation method is ﬂexible
enough to handle complex problems. The solution accuracy of this collocation method can be
improved much further by using orthogonal polynomials to approximate the state and control
trajectories instead of using cubic polynomials to approximate and this method of approximation
is called orthogonal collocation. The same technique can be used for path planning of vehicles
in dynamic obstacles environment. While collocation is good it takes much longer to converge
as the better solution needs more number of nodes, and this collocation technique cannot be
implemented in real-time, atleast not with Fmincon.
References
1. Oskar von Stryk,”Numerical Solution of Optimal Control Problems by Direct Collocation”,
Calculus of Variations, Optimal Control Theory and Numerical Methods, International
Series of Numerical Mathematics, Vol. 111, p.129-143, Birkhauser, 1993
2. Oskar von Stryk and R. Bulirsch, ”Direct and Indirect Methods for Trajectory Optimiza-
tion”, Mathematisches Institut, Technische Universitat Munchen, Postfach 20 24 20, D-
8000 Munchen 2, Germany.
25 of 25

report

Recommended

Recommended

More Related Content

What's hot

What's hot (20)

Similar to report

Similar to report (20)

report