OPTIMIZE COARSE TRAJECTORY¶
Introduction¶
The goal of this part is to optimizes the initial trajectory in the open space. Open_space_trajectory_optimizer is able to call a variety of different optimization algorithms.
Where is the code¶
Please refer open_space_trajectory_optimizer.cc
Code Reading¶
Input: stitching trajectory is provided by the open_space_trajectory_provider, planned target point, boundary of x and y, rotation angle relative to the corner of parking space, the reference origin point, line segment of boundary.
Status OpenSpaceTrajectoryOptimizer::Plan(const std::vector<common::TrajectoryPoint>& stitching_trajectory, const std::vector<double>& end_pose, const std::vector<double>& XYbounds, double rotate_angle, const Vec2d& translate_origin, const Eigen::MatrixXi& obstacles_edges_num, const Eigen::MatrixXd& obstacles_A, const Eigen::MatrixXd& obstacles_b, const std::vector<std::vector<Vec2d>>& obstacles_vertices_vec, double* time_latency)
Before optimization, some unreasonable cases are exited from the optimization process and implement some preprocessing.
The unreasonable case is below:
Input data is empty.
if (XYbounds.empty() || end_pose.empty() || obstacles_edges_num.cols() == 0 || obstacles_A.cols() == 0 || obstacles_b.cols() == 0) { ADEBUG << "OpenSpaceTrajectoryOptimizer input data not ready"; return Status(ErrorCode::PLANNING_ERROR, "OpenSpaceTrajectoryOptimizer input data not ready"); }
Starting point of planning is near the end point.
if (IsInitPointNearDestination(stitching_trajectory.back(), end_pose, rotate_angle, translate_origin)) { ADEBUG << "Planning init point is close to destination, skip new " "trajectory generation"; return Status(ErrorCode::OK, "Planning init point is close to destination, skip new " "trajectory generation"); }
End of the stitching trajectory is rotated and translated, and the trajectory information is converted according to the corner of the parking space.
PathPointNormalizing(rotate_angle, translate_origin, &init_x, &init_y, &init_phi)
Generate the coarse trajectory based on the warm start technology which is Hybrid A* algorithm.
if (warm_start_->Plan(init_x, init_y, init_phi, end_pose[0], end_pose[1], end_pose[2], XYbounds, obstacles_vertices_vec, &result)) { ADEBUG << "State warm start problem solved successfully!"; } else { ADEBUG << "State warm start problem failed to solve"; return Status(ErrorCode::PLANNING_ERROR, "State warm start problem failed to solve"); }
According to FLAGS_enable_parallel_trajectory_smoothing to achieve different optimization process. When FLAGS_enable_parallel_trajectory_smoothing is false, the optimization process is as follows:
(x, y, phi, V) and (ster, a) of initial trajectory points in hybrid_a_star are stored into xws and UWS respectively through LoadHybridAstarResultInEigen() function, and xws and UWS are used to generate the subsequent smooth trajectory.
Generate the smooth trajectory by the GenerateDistanceApproachTraj() function.
LoadHybridAstarResultInEigen(&result, &xWS, &uWS); const double init_steer = trajectory_stitching_point.steer(); const double init_a = trajectory_stitching_point.a(); Eigen::MatrixXd last_time_u(2, 1); last_time_u << init_steer, init_a; const double init_v = trajectory_stitching_point.v(); if (!GenerateDistanceApproachTraj( xWS, uWS, XYbounds, obstacles_edges_num, obstacles_A, obstacles_b, obstacles_vertices_vec, last_time_u, init_v, &state_result_ds, &control_result_ds, &time_result_ds, &l_warm_up, &n_warm_up, &dual_l_result_ds, &dual_n_result_ds)) { return Status(ErrorCode::PLANNING_ERROR, "distance approach smoothing problem failed to solve"); }
When FLAGS_enable_parallel_trajectory_smoothing is true, the optimization process is as follows:
Trajectorypartition() function is used to segment the initial trajectory.
Use loadhybridastarresultineigen() function to store the partitioned trajetory into xws and UWS respectively.
Set the initial information(a,V) of each trajectory.
the initial information of the first trajectory is the end point of the stitching trajectory.
In the next trajectory, the initial information is set to zero. At the start of the trajectory, the vehicle is stationary.
if (!warm_start_->TrajectoryPartition(result, &partition_trajectories)) { return Status(ErrorCode::PLANNING_ERROR, "Hybrid Astar partition failed"); }
Use combinetrajectories() function to integrate the parameter information after segmented optimization.
CombineTrajectories(xWS_vec, uWS_vec, state_result_ds_vec, control_result_ds_vec, time_result_ds_vec, l_warm_up_vec, n_warm_up_vec, dual_l_result_ds_vec, dual_n_result_ds_vec, &xWS, &uWS, &state_result_ds, &control_result_ds, &time_result_ds, &l_warm_up, &n_warm_up, &dual_l_result_ds, &dual_n_result_ds)
Converting trajectory information to world coordinate system.
for (size_t i = 0; i < state_size; ++i) { PathPointDeNormalizing(rotate_angle, translate_origin, &(state_result_ds(0, i)), &(state_result_ds(1, i)), &(state_result_ds(2, i)));
}
8. The trajectory information is loaded by loadtrajectory() function. Because the current optimization does not consider the end point control state, the end-point control state of the trajectory is processed (Steer = 0, a = 0).
``` cpp
LoadTrajectory(state_result_ds, control_result_ds, time_result_ds)
```
9. Output: Optput is optimized trajectory information.
# Algorithm Detail
```cpp
LoadHybridAstarResultInEigen(&partition_trajectories[i], &xWS_vec[i],&uWS_vec[i])
```
The function is to transform the initial trajectory information into the form needed for optimization.
1. Parameter: the initial trajectory and parameter matrix.
2. Introduction: the trajectory information is transformed into matrix form.
3. Process detail:
1. Transform the x,y,phi,v,steer to the matrix combined with horizon.
2. Store the transformed information to the matrix.