path tracing for autonomous vehicle in controlled environment

KINEMATIC MODELLING

Entire car can be assumed as a bicycle

TESTING SIMULATIONS

Based on the kinematic modeling, the setup is tested for various cases. The above equations are formulated using Python and the developed code is tested for 1) Circular morion and 2)Wave like path

Case 1: For the circular motion, two constant velocity cases were studied, but the second case had velocity half of the first case

List of parameters used

• L = 5m

• lr =2.5m

• maximum angular velocity = 3 rad/sec

• Steer Angle = 10 deg

• sampling time = 1 milli second

• Total run time = 20 sec

Case 2:Traveling in a wave like path with fixed amplitude (similar to x+sin(x))

a) The path of the car can be seen as a wavelike or x+sin x form. The car is having a constant speed (low) and a constant steer rate.

b) This time I gave speed. Due to high speed, the car starts circling at one position. After a point, the steering angle dominates over velocity and changes the path of the car.

c) It is a mix of both the above plots. In the case, acceleration is also considered hence, v = 2t where t is time.

For initial cases, when time is small, the steer rate turns the car counter clock but as velocity increases, it suddenly pushes the car and then it starts to travel in negative y-direction unlike part a and b

IMPLEMENTATION OF .SVG

The main aim of the project is to run the car in a controlled environment. Meaning, the car should follow trajectories that we define. To do so, Inkspace is used to develop path trajectory with free hand. The output files of Inkspace are called Support Vector Graphics (SVG). A python code is developed to convert these hand sketched drawings to cartesian coordinates scaled on 1x1 area. These coordinates are then passed in the code.
Now since we have the cordinates of the path, we incorporate Cubic Spline fitting to trace these coordinates into a well defined path.

As shown in above schematic, the developed python code takes SVG files as an input and gives out x and y coordinates of the entire path. Now there are infinite ways to travel in between any two given points. Therefore, we need to define a trajectory connecting all the points that our car can follow.

The easiest way to do it is by using polyft function. However, there is a problem with using this fitting function. The main difficulty arises at sharp turns. Mathematically the derivatives have a discontinuity at sharp edges which means that the velocity (derivative of the position) is not defined at all of these points and so will the acceleration.

To improve the model and remove these discrepancies we can use CUBIC SPLINE. This method removes the sharp edges and discontinuities in the derivatives. A cubic spline is a piecewise third order polynomial which passes through a set of control points. The working is such that the second-order derivative at the start and end of the domain is set to zero.

Let us say we have a table of points [x_i, y_i] where i = 0, 1, 2…n for the function y = f(x). This makes a total of n+1 points and n intervals between them. The cubic spline interpolation is a piecewise continuous curve, passing through each of the values in the table. There is a separate cubic polynomial for each interval, each with its own coefficients:

Together, these polynomial segments are denoted S(x), the spline.

Since there are n intervals and four coefficients for each we require a total of parameters to define the spline S(x). We need to find independent conditions to fix them. We get two conditions for each interval from the requirement that the cubic polynomial matches the values of the table at both ends of the interval. Notice that these conditions result in a piecewise continuous function. But we still need more conditions. Since we would like to make the interpolation as smooth as possible, we require that the first and second derivatives also be continuous:

These conditions apply for I = 1, 2,….n-1, resulting in 2(n-1) constraints. So, we need two more conditions to completely fix the spline. One of the standard choices is to use:

S₀’’(xo) =0, S_n-1”(xn) =0

Other choices are possible if the function is periodic, which is best dependent on the application.

With coefficients and linear conditions, it is straightforward to work out the equations that determine them. Cubic splines are popular because they are easy to implement and produce a curve that appears to be seamless. As we have seen, a straight polynomial interpolation of evenly spaced data tends to build in distortions near the edges of the data table. Cubic splines avoid this problem, but they are only piecewise continuous, meaning that a sufficiently high derivative (third) is discontinuous. Therefore, if the application is sensitive to the smoothness of derivatives higher than a second, cubic splines may not be the best choice.

A* SEARCH ALGORITHM

A* search algorithm is a well established technique which is used to find the optimal (shortest) path between any two given points. As per Wikipedia the functioning of algorithm can be described as follows

A* is an informed search algorithm, or a best-first search, meaning that it is formulated in terms of weighted graphs: starting from a specific starting node of a graph, it aims to find a path to the given goal node having the smallest cost (least distance travelled, shortest time, etc.). It does this by maintaining a tree of paths originating at the start node and extending those paths one edge at a time until its termination criterion is satisfied.

At each iteration of its main loop, A* needs to determine which of its paths to extend. It does so based on the cost of the path and an estimate of the cost required to extend the path all the way to the goal. Specifically, A* selects the path that minimizes

f(n)=g(n)+h(n)

where n is the next node on the path, g(n) is the cost of the path from the start node to n, and h(n) is a heuristic function that estimates the cost of the cheapest path from n to the goal. A* terminates when the path it chooses to extend is a path from start to goal or if there are no paths eligible to be extended. The heuristic function is problem-specific. If the heuristic function is admissible, meaning that it never overestimates the actual cost to get to the goal, A* is guaranteed to return a least-cost path from start to goal.

Typical implementations of A* use a priority queue to perform the repeated selection of minimum (estimated) cost nodes to expand. This priority queue is known as the open set or fringe. At each step of the algorithm, the node with the lowest f(x) value is removed from the queue, the f and g values of its neighbors are updated accordingly, and these neighbors are added to the queue. The algorithm continues until a removed node (thus the node with the lowest f value out of all fringe nodes) is a goal node. The f value of that goal is then also the cost of the shortest path, since h at the goal is zero in an admissible heuristic.

The algorithm described so far gives us only the length of the shortest path. To find the actual sequence of steps, the algorithm can be easily revised so that each node on the path keeps track of its predecessor. After this algorithm is run, the ending node will point to its predecessor, and so on, until some node's predecessor is the start node.

As an example, when searching for the shortest route on a map, h(x) might represent the straight-line distance to the goal, since that is physically the smallest possible distance between any two points. For a grid map from a video game, using the Manhattan distance or the octile distance becomes better depending on the set of movements available (4-way or 8-way).

If the heuristic h satisfies the additional condition h(x) ≤ d(x, y) + h(y) for every edge (x, y) of the graph (where d denotes the length of that edge), then h is called monotone, or consistent. With a consistent heuristic, A* is guaranteed to find an optimal path without processing any node more than once and A* is equivalent to running Dijkstra's algorithm with the reduced cost d'(x, y) = d(x, y) + h(y) − h(x).

Here are several examples in which starting point, ending point and obstacles are defined and the algorithm finds the optimal path itself.

The Python codes are available here