Hello everyone, I have moved the blog on CSDN to Zhihu. Welcome to pay attention to my column on Zhihu Slow carriage(https://zhuanlan.zhihu.com/c_1132958996826546176 ) Later, I will put my daily thinking on CSDN, and the essence of the article is put on knowledge. I hope you can learn more about each other and learn from each other.
Catalog
1. Comparison of Hybrid A * and smooth optimization
2. Call to Optimization Smoothing
1. Comparison of Hybrid A * and smooth optimizationIf I am in Previous posts As mentioned above, Baidu Apollo first uses Hybrid A * algorithm to plan a rough collision free trajectory, and then uses optimization method to smooth this trajectory. I built a very simple parking scene. The track planned by Hybrid A * and the track after sliding are shown as follows, dark green is rough track, light green is smooth track. Sometimes, the "smooth" is not obvious when we only look at the shape of path, but it is more obvious when we look at the speed v, the direction phi, the steer ing angle steel and the acceleration a after the smooth. (Note: Fig. 1-2 and Fig. 3-4 are not corresponding, here is just to compare the effect before and after optimization.)
Figure 0 the path of hybrid a * output has many turns 
Figure 1 path of hybrid a * output 
Figure 2 path of optimization smoothing output 
Figure 3 orientation angle of hybrid a * output path
Figure 4 orientation angle of optimized path
As can be seen from Figure 2, the output path of Hybrid A * guarantees no collision, because collision detection is done while expanding the path, while the smoothed path collides, because Apollo does not take the "distance from obstacles" into account in the optimized objective function, only the curve smoothing is considered. I'll explain this in more detail in the next optimization introduction.
2. Call to Optimization SmoothingThe call entry of the Open Space Planner is in the DistancePlan() of the Apollo 5.0/modules/planning/open space/tools/distance'approach'problem'wrapper.cc file.
bool DistancePlan(HybridAStar* hybridA_ptr, ObstacleContainer* obstacles_ptr, ResultContainer* result_ptr, double sx, double sy, double sphi, double ex, double ey, double ephi, double* XYbounds) { //Load the configuration file of Open Space Planner and set the parameter value //In Apollo / modules / planning / conf / Planner "open space" config.pb.txt ... //First call Hybrid A * to generate a preliminary track as warm start if (!hybridA_ptr->Plan(sx, sy, sphi, ex, ey, ephi, XYbounds_, obstacles_ptr->GetObstacleVec(), &hybrid_astar_result)) { ... } if (FLAGS_enable_parallel_trajectory_smoothing) { ... } else { ... //Then call DistanceSmoothing() to smooth. if (!DistanceSmoothing(planner_open_space_config_, *obstacles_ptr, sx, sy, sphi, ex, ey, ephi, XYbounds_, &hybrid_astar_result, &state_result_ds, &control_result_ds, &time_result_ds, &dual_l_result_ds, &dual_n_result_ds)) { return false; } ... } return true; }
DistanceSmoothing() sets the input information that is needed to optimize the state and control matrix, the initial state and the end state matrix, the vehicle size matrix, and then calls DistanceApproachProblem::Solve(). According to different methods, the algorithm can be set up to implement different optimization algorithms.
bool DistanceSmoothing(...) { // load Warm Start result(horizon is the "N", not the size of step points) size_t horizon_ = hybrid_a_star_result->x.size() - 1; ... //If the initial trace points are few, then interpolation if (horizon_ <= 10 && horizon_ > 2 && planner_open_space_config.enable_linear_interpolation()) { ... } else { //Initialize the state component and control component with the output of Hybrid A * ... } //State quantity and control quantity, pay attention to different meanings corresponding to different lines, and the order is very important Eigen::MatrixXd xWS = Eigen::MatrixXd::Zero(4, horizon_ + 1); Eigen::MatrixXd uWS = Eigen::MatrixXd::Zero(2, horizon_); xWS.row(0) = x; xWS.row(1) = y; xWS.row(2) = phi; xWS.row(3) = v; uWS.row(0) = steer; uWS.row(1) = a; //Initial state Eigen::MatrixXd x0(4, 1); x0 << sx, sy, sphi, 0.0; //End state Eigen::MatrixXd xF(4, 1); xF << ex, ey, ephi, 0.0; ... //Vehicle dimensions ego_ << front_to_center, right_to_center, back_to_center, left_to_center; // result for distance approach problem Eigen::MatrixXd l_warm_up; Eigen::MatrixXd n_warm_up; DualVariableWarmStartProblem* dual_variable_warm_start_ptr = new DualVariableWarmStartProblem(planner_open_space_config); if (FLAGS_use_dual_variable_warm_start) { bool dual_variable_warm_start_status = dual_variable_warm_start_ptr->Solve( ...); } else { l_warm_up = 0.5 * Eigen::MatrixXd::Ones( obstacles.GetObstaclesEdgesNum().sum(), horizon_ + 1); n_warm_up = 0.5 * Eigen::MatrixXd::Ones(4 * obstacles.GetObstaclesNum(), horizon_ + 1); } DistanceApproachProblem* distance_approach_ptr = new DistanceApproachProblem(planner_open_space_config); bool status = distance_approach_ptr->Solve(...); ... } return true; }
The DistanceApproachProblem class is an intermediate layer, which is responsible for matching different optimization algorithms for optimization problems according to the conf configuration file. This class has only one member function, Solve(). Similar to the use of "pointer and impl" mentioned in the Effective C + + series.
bool DistanceApproachProblem::Solve(...) { //The DistanceApproachInterface class is the base class for all distance approach s DistanceApproachInterface* ptop = nullptr; if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT) { ptop = new DistanceApproachIPOPTInterface(...); } else if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT_FIXED_TS) { ptop = new DistanceApproachIPOPTFixedTsInterface(...); } else if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT_CUDA) { ptop = new DistanceApproachIPOPTCUDAInterface(...); } else if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT_FIXED_DUAL) { ptop = new DistanceApproachIPOPTFixedDualInterface(...); } else if (distance_approach_mode() == DISTANCE_APPROACH_IPOPT_RELAX_END) { ptop = new DistanceApproachIPOPTRelaxEndInterface(...); } //The goal problem of constructing the solution required by IPOPT solver Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop; // Create an instance of the IpoptApplication Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory(); //Set parameters according to the use method of IPOPT ... //Initialize the IPOPT solver Ipopt::ApplicationReturnStatus status = app->Initialize(); // status = app->OptimizeTNLP(problem); ... ptop->get_optimization_results(state_result, control_result, time_result, dual_l_result, dual_n_result); ... }
A pointer of type DistanceApproachInterface is defined in DistanceApproachProblem::Solve(). The DistanceApproachInterface class inherits from the Ipopt::TNLP virtual base class, implements several virtual functions (Figure 5), and then uses IPOPT to solve optimization problems. For the use of IPOPT, please refer to the official documents. I haven't understood it yet. I may add another one later.
Figure 5 virtual functions in the Ipopt::TNLP to be implemented by inheritance Several distance approach es inherited from the DistanceApproachInterface class rewrite the above functions. As shown in Figure 6 and Figure 7, although there are many files and contents in this folder, it is not surprising that the logical structure is very simple and clear.
Figure 6 different optimization methods 
Fig. 7 various inheritance relationships
Let's take the DistanceApproachIPOPTInterface class as an example to see the setting of its target function. Our goal is to calculate a smooth and collision free trajectory based on the trajectory of Hybrid A *. Naturally, the objective function should cover at least two aspects. According to figure 5, eval_f() is responsible for the setting of the objective function, and there is no consideration of non-collision, so Figure 2 appears, which shows that Apollo does not have all the details of open source parking and warehousing trajectory planning, which can be referred to here Stanford's paper , use Voronoi Diagram to calculate the distance to the obstacle.
bool DistanceApproachIPOPTInterface::eval_f(int n, const double* x, bool new_x, double& obj_value) { eval_obj(n, x, &obj_value); return true; }
template <class T> void DistanceApproachIPOPTInterface::eval_obj(int n, const T* x, T* obj_value) { // Objective is : // min control inputs // min input rate // min time (if the time step is not fixed) // regularization wrt warm start trajectory //The note here indicates that no consideration has been given to keeping away from obstacles, i.e. no collision ... //Obj_value is the total cost value, and the target is min(obj_value) *obj_value = 0.0; // 1. objective to minimize state diff to warm up ... // 2. objective to minimize u square ... // 3. objective to minimize input change rate for first horizon ... // 4. objective to minimize input change rates, [0- horizon_ -2] ... // 5. objective to minimize total time [0, horizon_] ... }3. Add obstacles
The actual call of Open Space Planner takes place in the Apollo / modules / tools / open space visualization / distance approach visualizer.py file, which defines the start and end points of parking and warehousing, and sets several "line" obstacles. The shape is very similar to figure 1. The code for adding obstacles here is very strange, difficult to reuse and understand. I guess it's lazy...
# Define the vertex of obstacle in the form of double array, corresponding to Python list # We can only see 10 points here, but we can't see several lines ROI_distance_approach_parking_boundary = ( c_double * 20)(*[-13.6407054776, ... 5.61797800844]) # Using compiled Open Space Planner C + + library in Python files OpenSpacePlanner.AddObstacle(ROI_distance_approach_parking_boundary)
ObstacleContainer::AddObstacle() in Apollo / modules / planning / open space / tools / distance · approach · problem · wrapper.cc, the input parameter is the coordinates of all obstacle vertices. The vertices of a single obstacle are arranged in a clockwise direction, first x and then y.
void AddObstacle(const double* ROI_distance_approach_parking_boundary) { // the obstacles are hard coded into vertice sets of 3, 2, 3, 2 if (!(VPresentationObstacle(ROI_distance_approach_parking_boundary) && HPresentationObstacle())) { AINFO << "obstacle presentation fails"; } }
Addobserver() calls vprepresentationobserver() to convert the incoming double array into the form of line segment point, that is, STD:: vector < STD:: vector < Vec2d > > observers [vertices] VEC, Vec2d represents vertex, STD:: vector < Vec2d > represents a continuous group of line segments, that is, an obstacle. Here, four obstacles are hard coded, with two, one, two and one edges respectively, as shown in Figure 8 (line disassembly in Figure 1).
bool VPresentationObstacle( const double* ROI_distance_approach_parking_boundary) { //Four obstacles are defined obstacles_num_ = 4; obstacles_edges_num_.resize(4, 1); //The first obstacle has two sides, the second obstacle has one side, and so on obstacles_edges_num_ << 2, 1, 2, 1; size_t index = 0; for (size_t i = 0; i < obstacles_num_; i++) { std::vector<Vec2d> vertices_cw; //Get all vertices of a single obstacle for (int j = 0; j < obstacles_edges_num_(i, 0) + 1; j++) { //Get the x y coordinate of a single vertex Vec2d vertice = Vec2d(ROI_distance_approach_parking_boundary[index], ROI_distance_approach_parking_boundary[index + 1]); index += 2; vertices_cw.emplace_back(vertice); } obstacles_vertices_vec_.emplace_back(vertices_cw); } return true; }
Hprepresentationobject () is responsible for representing obstacles in the form of matrix, which I didn't understand.
Figure 8 representation of obstacles 
linxigjs Published 39 original articles, won praise and 10000 visitors+ Private letter follow