this is the advanced driver module in Vi-sim simulation platform. this driver module algorithm pipeline:
1) a route plan by graph-search over the network of roads
2) rules based guiding trajectories generation(traffic and lane following rules)
3) set of candidate trajectories(control inputs) generation and evaluated by vehicle dynamic model and collision free model
4) most feasible trajectory evaluated through optimization
vehicle state space
the full state of a vehicle updates:
X = (x, y, v, theta, throttle, steering, behavior)
the vehicle updates its plan at a fixed palnning rate dt; at each pllaning step, the vehicle computes a target speed v and target steering theta to be achieved by the control system
S(u, X) determine if a set of control is feasible, given current state of the vehicle, S(u, X) will return false if the given input u cause a loss of traction or control.
sensing and perception
the sensing module provide an approximation of the center line of lane, closet point on the lane center to the ego-vehicle, and friction coefficient.
route choice and behavior state
behavior set includes merging, right turn, left run, keep straight. the behavior state of the vehicle is described as a finite-state machine(turn left, turn right, merge left, merge right), which restrict potential control decisions and adjust the weight of the cost function.
guiding path
the ego-vehicle computes a set of waypoints along the current lane at fixed time intervals. how to create the path based on waypoints
collision avoidance
define obstacles domain for each neighbor of the ego-vehicle, which is defined as all controls that could lead to collision. the obstacles domain and the set of dynamic infeasible domain form the boundary of collision-free space for the ego-vehicle.
trajectory sampling
the exact obstacle domain is not computing time efficent, instead here use a sampling strategy around theta and v to determin a feasible control. each sample is referred to as a candidata control u_c.
trajectory cost function
once the set of suitable control candidates has been computed, the most feasible control will be selected by minimizing the cost function for each sample point i :
C = sum_i{ C_path(i) + C_cmft(i) + C_mnvr(i) + C_prox(i)
1) path cost, defined as success at tracking its path and the global route.
2) comfort cost, C_cmfg = ||vel_acc|| + ||theta_acc||
3) maneuver cost, penalize lane changes C_mnvr = lane_change
4) proximity cost, prevent the ego vehicle from passing close to neighbors.
control input
one PID controller to driven current speed to match the target speed; another PID controller drives the current steering angle to match the target.