Home
The graph search method is used to find a collision-free reference path around the reference points. In the image above, the red points are the reference points input to the program. The orange curve is the approximation of these points using B-spline. A set of points are sampled uniformly along the orange curve and shifted laterally to get the light blue points. Then we use Dijkstra's algorithm on these points to find a new reference path (the green curve). The cost function consists of two terms: the deviation from the global path, and the minimum distance to the obstacles.
The green curve is smoothed to get the yellow curve. There are several smoothing implementations in the program, but I haven't decided which is the best one So I'll just skip this part for now.
(1) Use four circles to represent the vehicle.
(2) Sample uniformly along the smoothed reference path to get n points, with a spacing of 0.3m. Each point will correspond to a vehicle state to form a path.
(3) For each point, assuming that the vehicle center (rear axle) coincides with that point, calculate the lower and upper bounds of the circles. These bounds will be used as constraints of the optimization later.
(4) Formulate QP.
The vehicle state in the Frenet frame is
. After the optimization, n vehicle states in the Frenet frame, corresponding to the n points on the reference path, are calculated to form a path.
State variables: z =
Control variables: u = (curvature's derivative w.r.t. the arc length s).
According to Trajectory planning under vehicle dimension constraints using sequential linear programming III-A, the linearized and discretized system dynamics are .