Content tagged math

Intro

The goal of this project is to plan a path for a car to make its way through highway traffic. You have at your disposal a map consisting of a set of waypoints. The waypoints follow the center of the road, and each of them comes with the normal vector. You also know the positions and velocities of nearby vehicles. Your car needs to obey the speed limit of 50 MPH (22.35 m/s), not collide with other traffic, keep acceleration within certain limits, and minimize jerk (the time derivative of acceleration). The path that you need to compute is a set of successive Cartesian coordinates that the car will visit perfectly every 0.02 seconds.

Here's the result I got. It's not completely optimal at times, but I used large safety margins and did not spend much time tweaking it.

Path Planning

Path planning

Udacity recommends using minimum jerk trajectories defined in the Frenet coordinate space to solve the path planning problem. This approach, however, struck me as suboptimal and hard to do well because of the distance distortions caused by the nonlinearity of the coordinate-space transforms between Cartesian and Frenet and back. Therefore, I decided to reuse the code I wrote for doing model-predictive control. However, instead of using actuations computed by the algorithm, I used the model states themselves as a source of coordinates that the vehicle should visit.

The controller follows a trajectory defined as a polynomial fitted to the waypoints representing the center of one of the lanes. In the first step, it computes 75 points starting from the current position of the car. In each successive step, it takes 25 points from the previously computed trajectory that the car still did not visit, it estimates the vehicle's state at the 25th point and uses this estimated state as an input to the solver. The points produced this way complement the path. This cycle repeats itself every 250 ms with the target speed given by:

\[ v = \begin{cases} v_l - 0.25 \cdot (25 - d_l) & d_l \leq 25 \\ 22.2 & d_l \gt 25 \\ \end{cases} \]

Where \( v_l \) is the speed of the leader and \( d_l \) is the car's distance from it. Subtracting the proximity penalty tends to make speed "bounce" less than when using fractional terms. I was too lazy to tune a PID controller for this task.

Lane selection

The most optimal lane is selected using a simple finite state machine with cost functions associated with state transitions. Every three seconds the algorithm evaluates these functions for all reachable states and chooses the cheapest move.

Lane Changing FSM
Lane Changing FSM

The cost function for keeping the current lane (the KL2KL transition) penalizes the difference in speed between the vehicle and the leader (if any) and their proximity. The one for changing lanes (the KL2CL transition) does a similar calculation for the target lane. Additionally, it increases the cost if a follower is close enough adding a further penalty proportional to its speed.

The start up state allows the car to ramp up to the target speed before the actual lane-change logic kicks in. Doing so avoids erratic behavior arising from the penalization of speed differences.

The exact logic is here.

Conclusions

Using the MPC approach has the advantage of letting you tune more parameters of the trajectory. You can choose how much acceleration and jerk you're going to tolerate, increasing or decreasing the perceived aggressiveness of lane changes.

I also tried incorporating the predicted trajectories of nearby vehicles into the solver's cost function, but the car ended up going out of the lane, accelerating or breaking aggressively to avoid collisions. It is something that you would do while driving in the real world, but it breaks the rules of this particular game. Another problem was that I used a Naïve Bayes Classifier to predict the behavior of other vehicles and it is not accurate enough. The mispredictions made the car behave erratically trying to avoid collisions with non-existing obstacles.

You can see the full code here.

The problem

I have been playing with one of my toy projects and ended up having to shuffle text many times in a file that is around 10MB long. A naïve implementation chopping a string and gluing it together again ended up being painfully slow:

\[ O(n \cdot k) \]

where n is the length of the string and k is the number of shuffling operations.

A solution

It turns out that the problem can be efficiently solved using splay trees, where each node holds:

  • the starting offset in the source string
  • the length of the substring it represents
  • the offset from the beginning of the substring taking into account all of the children on the left-hand-side
  • total length of the substring taking into account all the children on the right-hand side

The find operation looks for a node containing the offset and splits it in two, if the node does not start with the offset.

 1 static Node *Find(uint32_t offset, Node *root)
 2 {
 3   if(!root)
 4     return 0;
 5   if(offset >= root->offset && offset < (root->offset + root->length))
 6     return root;
 7   if(offset < root->offset)
 8     return Find(offset, root->left);
 9   return Find(offset-root->offset-root->length, root->right);
10 }
11 
12 static Node *STFind(uint32_t offset, Node *root)
13 {
14   Node *n = Find(offset, root);
15   Splay(n);
16 
17   if(n->offset < offset) {
18     Node *newNode = new Node();
19     newNode->start  = n->start;
20     newNode->length = offset - n->offset;
21     n->start  = newNode->start + newNode->length;
22     n->length -= newNode->length;
23     newNode->left = n->left;
24     update(newNode);
25     n->left = newNode;
26     update(n);
27   }
28   return n;
29 }

The actual shuffling boils down to splitting and merging sub-trees and producing the result to traversing the whole tree in-order.

 1 void process(int i, int j, int k)
 2 {
 3   Node *n, *t, *left, *right = 0;
 4   STSplit(left, n, i, pRoot);
 5   if(j+1 < pS.length())
 6     STSplit(n, right, j+1-i, n);
 7   t = STMerge(left, right);
 8   if(k < (t->offset + t->total_length))
 9     STSplit(left, right, k, t);
10   else {
11     right = 0;
12     left = t;
13   }
14   pRoot = STMerge(left, n);
15   pRoot = STMerge(pRoot, right);
16 }

The amortized complexity is:

\[ O(k \cdot log(k) + n) \]

See rope-splay.cpp.

A test

It all runs pretty nicely for a test case with 10M characters and 100k shuffle operations:

]==> ./testgen 10000000 100000 > test
]==> time ./rope-naive < test > test-o-naive
./rope-naive < test > test-o-naive  386.17s user 66.19s system 99% cpu 7:32.86 total
]==> time ./rope-splay < test > test-o-splay
./rope-splay < test > test-o-splay  0.71s user 0.04s system 99% cpu 0.752 total
]==> diff test-o-splay test-o-naive