review-apollo (2).md

Posted on |

review apollo (2)

EMPlanner

in review apollo(1), we had went through perception, prediction and part of planning module. the ADS planning problem can be summaried with a few input conditions, we can get a few drivable driving path, now EMPlanner, here need to find out the best one.

input conditions:

  • perception and prediction obstacles and its future(in 5s) trajectory

  • current road status, e.g. special area, pedestrain cross e.t.c.

both path and speed need to be planned or optimized. the core of EMPlanner is Dynamic programming(DP), which is an classic algorithm to search the global optimized solution. the DP garantee each sub-group best solution accumulated to the final global best solution.

PathOptimizer

the DP used in path optimizer basic processes:

starting from current location, in every time, move in x-direction about next n meters(to next location), and sample in y-direction at the next location, calcuate the cost from current location to each of the sampling point at the next location, and save cost value for each path for this sub path segment. (this is the different from DP to greedy algorithm), finally we retrieve all the cost values tables for each sub-path, and get the final best solution for the whole path.

a few question about DP here:

  • how long in x-direction need to plan ?

    KMinSampleDistance = 40.0

  • what size of the interval in x-direction is good ?

1
step_length = math::Clamp(init_point.v() * SamplePointLookForwardTime, config_.step_length_min(), config_.step_length_max());
  • how many sample points in y-direction is needed ? 
1
num_sample_per_level = config_.sample_points_num_each_level();
  • how to sample in y-direction and x-direction ?

which is case by case, depends on the ego’s maneuver as well as the lanes info.

basically, we had RouteSegments/ReferenceLine set, which includes the filtered referencelines for ego vehicle. for each referenceLine, we can calcuate the cost, then figure out the min cost referenceline as planning module’s output.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
EMPlanner::Plan(&start_point, *frame)
{
	foreach reference_line_info in frame->reference_line_info():
		PlanOnReferenceLine();
}
EMPlanner::PlanOnReferenceLine(start_point, frame, reference_line_info){
	foreach optmizer in tasks_ :
		ret = optmizer->Execute(frame, reference_line_info);
}
PathOptimizer::Execute(*frame, *reference_line_info){
	ret = Process(speedData, reference_line, start_point, path_data);
}
DpPolyPathOptimizer::Process(SpeedData, ReferenceLine, init_point, path_data){
	dp_road_graph.FindPathTunnel();
}
DPRoadGraph::FindPathTunnel(init_point, &obstacles, *path_data){
	CalculateFrenetPoint(init_point, &init_frenet_frame_point_);
	GenerateMinCostPath(obstacles, &min_cost_path);
	foreach point in min_cost_path:
		frenet_path.append(transfer(point));
	path_data->SetFrenetPath(frenet_path);
}

point2point cost calcualation

e.g. from the left interval sample points(in y-direction) to current interval sample points(in y-direction).

during this cost calcuation, consider pathCost, staticObstacleCost and DynamicObstacleCost. 

1
2
3
4
5
TrajectoryCost::Calculate(){
	total_cost += CalculatePathCost();
	total_cost += CalculateStaticObstacleCost();
	total_cost += CalculateDynamicObstacleCost();
}
  • UpdateNode() 
1
2
3
4
5
6
7
8
9
10
DPRoadGraph::GenerateMinCostPath(){
	for(level=1; level < path_waypoints.size(); ++level){
		level_points = path_waypoints[level]; //all sample points in y-direction at current level 
		for(i=0; i<level_points.size(); ++i){
			cur_point = level_points[i];
			UpdateNode(prev_dp_points, level, &trajectory_cost, &cur_point);
		}
	}
}
  • PathCost

from each point at prevous level to current point at current level, consider the length, 1st order(speed), 2sec order(acc) should be smooth, can give a 5th-order polyfit, same as what’d done in referenceLine smoother. for each interval/curve polyfit(by curve.Evaluate()), we can re-sampling it more density, so at each new sampling point, calculate its (position, speed, acc)l, dl, ddl

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
TrajectoryCost::CalculatePathCost(curve, start_s, end_s, curr_level){
	foreach sampling_point in (end_s - start_s):
		l = curve.Evalaute(0, sampling_point)
		// cost from l 
		path_cost +=  l * l * config_.path_l_cost()
		// cost from dl 
		dl = curve.Evaluate(1, sampling_point)
		path_cost +=  dl * dl * config_.path_dl_cost()
		// cost from ddl 
		ddl = curve.Evaluate(2, sampling_point)
		path_cost +=  ddl * ddl * config_.path_ddl_cost()		
}
```	
		
* StaticObstacleCost
StaticObstacleCost() used the same idea from PathCost, basically, to resampling the interval, and calculate a cost on each of the sampling point. 
```c++
TrajectoryCost::CalculateStaticObstalceCost(curve, start_s, end_s){
	foreach curr_s in (end_s - start_s):
		curr_l = curve.Evaluate(0, curr_s, start_s);
		foreach obs_boundary :: static_obstacle_boundaries:
			obstacle_cost += GetCostFromObs(curr_s, curr_l, obs_boundary);
}
TrajectoryCost::GetCostFrmObs(){
	delta_l = fabs(adc_l - obs_center); 
	obstacle_cost.safety_cost += config_.obstacle_collision_cost() * Sigmoid(config_.obstacle_collision_distance() - delta_l) ;
  • DynamicObstacleCost 
1
2
3
4
5
6
7
8
9
10
11
TrajectoryCost::CalculateDynamicObstacleCost(curve, start_s, end_s){
	foreach timestamp:
		foreach obstacle_trajectory in dynamic_obstacle_boxes_:
			obstacle_cost += GetCostBetweenObsBoxes(ego_box, obstacle_trajectory.at(timestamp));
}
TrajectoryCost::GetCostBetweenObsBoxes(ego_box, &obs_box){
	distance = obstacle_box.DistanceTo(ego_box);
	if(distance > config_.obstacle_ignore_distance())
		return obstacle_cost ;
	obstacle_cost.safety_cost += ( config_.obstacle_collision_cost() + 20) * Sigmoid(config_.obstacle_collision_distance() - distance);

static obstacles cost calculating is like one space-dimension, only consider cost from the fixed/static obstacle to the fixed sampling points. for dynamic obstacles case, as the obstacle is moving, so we need a time-dimension, to calculate the cost at each time interval, from the current obstacle location to the fixed sampling points.

for dynamic obstacle cost, there is a risk cost (20), which need to take care.

min cost path

we got the cost matrix from the start_s level to the target_s/final level. but the final level in y-direction has 7 sampling points, all of which are drivable. so backward propagation, to get the best path (or a NODE in DAG)

1
2
3
4
DPRoadGraph::GenerateMinCostPath(){
	for(cur_dp_node : graph_nodes.back()){
		fake_head.UpdateCost()
	}

SpeedOptimizer

the previous PathOptimier give the best (s, l) location of ego should arrived at each interval section along the referenceline. so what’s the best speed to reach these interval section ?

to do speed optimzier, two parts:

  • constraint speed conditions, e.g. speed limit, upper/lower boundary of drivable area along the refereneline due to obstacle trajectory

  • speed planning

BoundaryMapper()

for each obstalce’s trajectory, sampling in time-dimension, if its trajectory has overlapped with ego’s trajectory, mark the boundary box.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
StBoundaryMapper::CreateStBoundary(){
	foreach trajectory_point in trajectory:
		obs_box = obstacle.GetBoundingBox(trajectory_point);
		for(path_s=0.0, path<s < discretized_path.Length(); path_s += step_length){
			curr_adc_path_point = discretized_path.EvaluateUsingLinearApproximation();
			if( CheckOverlap(curr_adc_path_point, obs_box, st_boundary_config_.boundary_buffer()) )
			{
			}
		}
}
// only when obs trajectory overlapped with ego's trajectory
StBoundaryMapper::MapWithDecision(path_obstacle, decision){
	boundary = StBoundary::GenerateStBoundary(lower_points, upper_points);
	path_obstacle->SetStBoundary(boundary);
	// assign different boundary type 
}

SpeedLimitDecider

  • speed limit

  • centripetal acc limit

  • centripetal force limit

so far we have info from boundaryMapper, which tells lower_s and upper_s of safe driving area along reference line, excluding the overlaping area of obstacle’s trajectories; as well as speed limit. so now in the driving safe area, which speed ego should take at each point is also done by DP, speed can be represent in time-space two dimension, in a T-S table.

so speed DP can transfer to find the min cost [T,S] althrough the whole planning drive path. 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
DpStGraph::Search(speed_data){
	InitCostTable().ok();
	CalculateTotalCost().ok();
	RetriveSpeedProfile(speed_data).ok();
}
```	
### QpSplineSpeedOptimizer 
DPSpeedOptimizer output the value pair of (time, location)(t, s) at each interval, then can get the velocity, acc, angular velocity by differencial among the neighbor intervals. QPSpline is a better way to get a polyfit 2nd-order smoothed speed profile along the referenceLine, which is higher-precision than DP.
### SpeedOptimizer and PathOptimizer fusion
PathOptimizer outputs driving path in (accumulated_distance, section_direction distance)(s, l), SpeedOptimizer outputs driving path in (time, accumulated_distance)(t, s). now to get the best/optmized path along the referenceLine, need combine these two outputs.
```c++
for(cur_rel_time=0.0; cur_rel_time < speed_data_.TotalTime(); cur_rel_time += TimeSec){
	speed_data_.EvaluateByTime(cur_rel_time, &speed_point);
	if(speed_point.s() > path_data_.discretized_path().Length()) break; 
	path_data.GetPathPointWithPathS(speed_point.s(), &path_point);
	path_point.set_s(path_point.s() + start_s);
	
	trajectory_point.mutable_path_point()->CopyFrom(path_point);
	 trajectory_point.set_v(speed_point.v());
    trajectory_point.set_a(speed_point.a());
    trajectory_point.set_relative_time(speed_point.t() + relative_time);
    ptr_discretized_trajectory->AppendTrajectoryPoint(trajectory_point);
   
}

the above fusion is on one ReferenceLine, for all referenceLine, we can reach out the min cost referenceline as following:

Frame::FindDriveReferenceLineInfo(){
    foreach reference_line_info in reference_line_info_ :
        if(reference_line_info.IsDrivable() && reference_lien_info.Cost < min_cost){
            drive_reference_line_info_ = &reference_line_info;
            min_cost = reference_line_info.Cost();
    }

    return drive_reference_line_info_ ;
}

Planning in summary

Planning module has three components, ReferenceLineProvider, Frame and EMPlanner. Frame has ReferencelineInfo, which has everything about once-time planning, including the min-cost ReferenceLineInfo from EMPlanner. EMPlanner do DP and QP for PathPlan and SpeedPlan, seperately, which give a best driving path for each RefereneLine. in the whole, we can reach out the global best(min-cost) referenceLine from EMPlanner, and store back in Frame.

Control

Control module readin the planning ReferenceLine (Trajectory, VehicleStatus) and Localization, output control command.

ControlComponent::Proc(){
    chassis_msg = chassis_reader_->GetLatestObserved();
    OnChassis(chassis_msg);
    trajectory_msg = trajectory_reader_-> GetLatestObserved();
    localization_msg = localization_reader_-> GetLatestObserved();

    status = ProduceControlCommand(&control_command);
    control_cmd_writter_->Write();
}

simulation

simulation is a system level verification/test tool, there are a few topics:

simulate the world

how to make a virtual world in simulation, quickly, easily, close to real and massively deploy-friendly.

currently most commericial tools, e.g. PreScan, CarMaker, are supporting well real map(.osm), and on-line/distributed/cloud service, which is good way.

a few pioneers are trying to make the virtual world by 3D build through image/point cloud scanning. which is maybe the closest way to the physical world, but need large computing resource.

integrate ADS system

how to integrate ADS system to simulation, easily.

most simulation software/platform is independent from ADS core.

for in-house prototype team, they both develop ADS and simulate in Matlab. which is pretty common in most OEM teams.

in product phase, it’s common to package ADS core as ros node, and talk to the simulation software by ROS.

friendly API

system level simulation has plenty of scenarios, requiring the simulation platform has friendly way to produce massively scenarios. including env, ego/npc behaviors, special cases e.t.c

most common APIs are Python and Matlab.

massively deployment

it’s back to the test purpose, simulation needs to cover as much as possible scenarios, which requires IT infrastructure to support massively deployment. e.g. cloud deployment

`