Ferdyan Dannes Krisandika Weekly Report

fdannes@gmail.com Report 58th

M10702802 January 2, 2020

Objective
1. Primary
Re-check my double collision checking code.

2. Secondary
Re-check my driving policies code and make improvement.
3. Tertiary
Test on the longer video sequences.
4. Quaternary
Debug the error in my code.

1 Introduction
This week, I tried to re-check my double collision check code, which is it used to get the second
best trajectory. Previously, my methods won’t work but this week I have successfully implement it into
my code. Next thing I did is I tried to re-check my driving policies code and make some improvement,
previously the pseudo-car is too far with ego car so it quite hard to see the pseudo car. Right now, I
have make some limitation so we could see the pseudo car. Last, I tried to test my code using the longer
video sequences, the result is quite good. And when I tested with the longer video sequences I also tried
to solve the error from my code because when I tested it, my code has lot of errors.

2 Methods
2.1 Frenet Optimize Trajectory
In Frenet coordinate system [1], they define their plane into longitudinal and lateral axis, respectively
denoted as S and D. It using minimum jerk trajectories defined in the Frenet coordinate space to solve
the path planning problem. Jerk is defined as the rate of change in acceleration over time. Meanwhile,
acceleration is defined as the rate of change in velocity over time. Basically jerk and acceleration are

Figure 1: Frenet system based on the longitudinal and lateral axis.

1
respectively derivatives of acceleration and velocity. As an illustration, when we be a passenger in the
car, we will feel the sudden changes of the acceleration in a vehicle. It is therefore essential to minimize
jerk when planning a trajectory.
To compute the jerk in the lateral axis, by using quintic polynomial, first, when we have a position,
we need to derivative it and it will become speed. This speed, we derivative it and it will become
acceleration. This acceleration result, we derivative it again and it will become jerk. So, to get the jerk
from position, we need to derivative it three times until we get the jerk.

P osition Lat = A1 ∗ t + A2 ∗ t2 + A3 ∗ t3 + A4 ∗ t4 + A5 ∗ T 5 (1)

Speed Lat = A1 + 2 ∗ A2 ∗ t + 3 ∗ A3 ∗ t2 + 4 ∗ A4 ∗ t3 + 5 ∗ A5 ∗ T 4 (2)

Acceleration Lat = 2 ∗ A2 + 6 ∗ A3 ∗ t + 12 ∗ A4 ∗ t2 + 20 ∗ A5 ∗ T 3 (3)

Jerk Lat = 6 ∗ A3 + 24 ∗ A4 ∗ t + 60 ∗ A5 (4)

n
X
Jp = Jerk Lat2 (5)
i=0

To compute the jerk in the longitudinal axis, the equation is similar like lateral, but there is some
different, they use quartic polynomial:

P osition Lon = A1 ∗ t + A2 ∗ t2 + A3 ∗ t3 + A4 ∗ t4 (6)

Speed Lon = A1 + 2 ∗ A2 ∗ t + 3 ∗ A3 ∗ t2 + 4 ∗ A4 ∗ t3 (7)

Acceleration Lon = 2 ∗ A2 + 6 ∗ A3 ∗ t + 12 ∗ A4 ∗ t2 (8)

Jerk Lon = 6 ∗ A3 + 24 ∗ A4 ∗ t (9)

n
X
Js = Jerk Lon2 (10)
i=0

After we get the position, speed, acceleration, and jerk, we need to calculate the cost functional for
both lateral motion and longitudinal motion planning.

CD = KJ ∗ Jp + KT ∗ T i + KD ∗ lat pos2 (11)

CV = KJ ∗ Js + KT ∗ T i + KD ∗ dif f speed (12)

CF = KLAT ∗ CD + KLON ∗ CV (13)

Based on the Wenda Xu et. al. work’s [2], they add some new cost functional in they code, they
have static cost and dynamic cost. But in this works I just add dynamic cost. The cost is including
speed cost, acceleration cost, and centripetal cost. Speed cost is has impact on the energy of the car,
acceleration cost and centripetal cost have impact on the comfort of the car. The cost functional equation
is some thing like this:
n
X
Ce = vi2 (14)
i=0

2
 n
X
Ca = ai2 (15)
i=0
n
X
Cca = vi2 ki (16)
i=0

Need to remember, that we have lateral and longitudinal axis, so we need to calculate the Ce, Ca,
and Cca for both axis. Then the each cost we multiply it by the weight. The equation is like the equation
below:

CE = KCE LAT ∗ Ce Lat + KCE LON ∗ Ce Lon (17)

CA = KCA LAT ∗ Ca Lat + KCA LON ∗ Ca Lon (18)

CCA = KCCA LAT ∗ Cca Lat + KCCA LON ∗ Cca Lon (19)
Where CE is speed cost functional, CA is the acceleration cost functional, and CCA is the centripetal
cost functional. KCE, KCA, and KCCA is the weight cost for that cost. The final cost function, is like
this equation:

CF = CF + KU N V ∗ CE + KU N V ∗ CA + KU N V ∗ CCA (20)
KUNV is the weight cost for CE, CA, and CCA. The final cost functional (CF), will used to find
the minimum cost path, if the CF value if less than the minCost, it will compute as the best path of the
pseudo car.

2.2 Second Best Trajectory

My second best trajectory methods is shown like in figure 2. This method is used also in the Moritz
et. al [1]. We could see that from the output of Frenet, I tried to get the state of the Frenet’s output,
it will determine whether it able to get the first best trajectory or not. If the Frenet has been get the
best first trajectory path, then the state will declared as ”True”, it means that Frenet could generate the
path and the next step is plot the position. When the state is ”False” it means that in the first Frenet
get the path, it failed to generate the path, hence I tried to change the parameters until it found the
second best trajectory (until the state is ”True”).

Figure 2: Second best trajectory flowchart.

3
 ]

Figure 3: New driving condition to test.

Table 1: Driving scenario result based on the updated code.

Scenario Result
1 X
2 X
3 X
4 X
5 X
6 X
7 X
8 X
9 X
10 X
11 X

2.3 Driving Scenario

In the new driving scenario, totally we have 11 driving scenario to test our system. The driving
scenario is shown like in figure 3, we could see that every scenario is includes low traffic condition until
crowd traffic condition. The idea behind made this scenario is because is some accident videos we don’t
have this scenario. To prevent some unusual traffic like in this scenario, I tried to make it. This scenario
will determine the driving policy of the pseudo-car.

3 Experimental and Result

In the experiment, first I will show the result from my driving scenario based on the updated code.
From the result from the table 1. From the 11 driving scenario that I have made, all of the driving
scenario has been run successfully. Next experiment is I tried to test my second best trajectory is work
or not, to test it I tried to visualize it, the result is shown like in figure !4. We could see that it looks like
has two different path, the upper side and bottom side. The upper side is come from the finding the first
best trajectory, but we could see that there isn’t any good trajectory, sign by the green color. Hence, in
the bottom side, is the result from the finding the second best trajectory, we could see it able to generate
the best trajectory sign by the blue color and the other possible path is sign with green color.
To test the code able to run on the long sequences video or not, I tried to run it on the dataset

4
 ]

Figure 4: Fail case in my experiment.

that has more than 1000 frames. The result we could see like in figure 5 and 6. The left side is the
on the first frame and the right side is the last frame. We could see in first experiment is successfully
generate the position but in the second experiment it failed to generate the pseudo car position until the
end of frames. This error is caused by the curvy road from the image sequences, on the some first frame
the road is straight way but in the middle until last frame the road is curvy, that’s why the pseudo car
position is disappear.

4 Conclusion and Next Step

4.1 Conclusions
1. Our system can test on the long sequences but only in the straight way.
2. Our system fail when the dataset has a curvy road.

4.2 Next Step

1. Add some part in the code to prevent the curvy road problem.
2. Add one more pseudo car in my code to generate the accident trajectory from pseudo car.

Figure 5: First experiment result.

5
 ]

Figure 6: Second experiment result.

References
[1] Moritz Werling, Julius Ziegler, Sören Kammel, and Sebastian Thrun. Optimal trajectory generation
for dynamic street scenarios in a frenét frame. 2010 IEEE International Conference on Robotics and
Automation, pages 987–993, 2010.
[2] Wenda Xu, Junqing Wei, John M. Dolan, Huijing Zhao, and Hongbin Zha. A real-time motion
planner with trajectory optimization for autonomous vehicles. 2012 IEEE International Conference
on Robotics and Automation, pages 2061–2067, 2012.