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.
Please refer open_space_trajectory_optimizer.cc
-
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)
- The unreasonable case is below:
-
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.