Hybrid-A-Star-U-Turn-Solution: A C++ repository from XiaoHuixxTian

问题描述

在城市道路驾驶中，我们经常会遇到需要车辆掉头的场景，请设计并实现一个轨迹生成类(一个或者几个class/struct组成)，能够生成一条位置、曲率平滑的轨迹，让车辆能够完成掉头运动。

输入：需要完成掉头的两条车道的中心线数据，中心线的每个点数据包含位置与朝向（x,y,theta）

输出：连接两条车道的完整轨迹，轨迹中每个点包含位置、朝向与曲率数据（x,y,theta,kappa）

方案：

使用A-Star来寻找可行驶轨迹。在离散的情况下A-Star给出的路径不是可行驶的，但是当我们加入汽车的动力学约束后，也许可以达到题目要求的结果。

使用动力学约束来计算A-Star的可扩展区域（格子）
1. 约束动力学模型的同时，HeuristicCost也需要根据U-Turn的场景进行适当的优化
删除不合理的区域（碰到障碍物，地图外，效率低等）
记录连续的车辆状态以及状态关联的离散格子
待得到搜索结果后取出路径相关联的连续状态数据（x，y，theta）
检查曲率是否平滑
可视化
1. 可视化部分使用python的matplotlib
测试代码部分并没有引入测试框架（gtest，boosttest等），而是用脚本配合c++ assert的轻量化方案，因为开发和测试过程中需要很多肉眼观测的辅助和批量的测试文件读取，使用c++对代码维护要求较高，而且无法做到和代码不耦合的可视化。

程序使用说明：

构建：

cd /path/to/U-Turn-Solution
mkdir build
cd build
cmake ..
make
cd ..

运行测试脚本：
```
# 测试单个数据并可视化
./test_and_viz_one.sh ./test_data/lane_data_1.txt
# 批量测试所有数据集
./test_all_and_quiet.sh
```
测试数据中的lane_data.3.txt是通过不了测试的，因为我给的数据中两个lane之间的距离过长，可以试试把程序的第一个参数放大到5以上，但是效果不是很好，针对这种长距离的规划我这边还需要优化

参数说明：

本程序采用命令行参数传递的形式设置参数，参数设置方法可参照：第1~6个参数：

    // load planner config from command line
    twx_algo::MotionConfig motion_config = {
        argc > 1 && argv[1] != NULL ? std::stod(argv[1]) : 1.0,  // desired_speed or a-star simulation speed
        argc > 2 && argv[2] != NULL ? std::stod(argv[2]) : 4.5,  // ego car wheelbase
        argc > 3 && argv[3] != NULL ? std::stod(argv[3]) : 40.0, // max steering angle
        argc > 4 && argv[4] != NULL ? std::stod(argv[4]) : -40}; // min steering angle

    twx_algo::hybrid_a_star::SearchConfig search_config = {
        argc > 5 && argv[5] != NULL ? std::stod(argv[5]) : 20.0, // explore steering angle step
        argc > 6 && argv[6] != NULL ? std::stod(argv[6]) : 1.0}; // time duration each exploration

第7个参数为环境信息即lane数据：

    if (argc > 7 && argv[7] != NULL)
    {
        lane_data_file_name = argv[7];
        std::ifstream lane_stream(argv[7], std::ifstream::in);
        ...
    }

所以在我的测试脚本中我是这么做的：

    ./build/u_turn_planner_test 1.0 3 40 -40 40 1.0 ./test_data/lane_data.txt

文件夹说明:

./test //可执行程序入口代码
./test_data //测试数据和可视化结果

方案1关键技术解释：

路径搜索使用了Hybrid A Star，‘Hybrid‘意味着路径搜索过程结合了车辆运动模型。也就是在grid map中每向前走一步，都只搜索车辆可到达的区域。
A Star中没有使用3维数组形式的真栅各地图，而是:
```
std::vector<std::map<Point, State>>
```
该形式有点像稀疏矩阵的表达方式，大大节约了空间消耗，也大大减轻的坐标系转换的工作。

车辆运动模型为：相关代码：

    double delta = M_PI * steering / 180.0;
    double omega = motion_config_.desired_speed / motion_config_.wheelbase * std::tan(delta);

    double next_theta = theta + omega * search_config_.time_step;
    double next_x = x + motion_config_.desired_speed * std::cos(theta) * search_config_.time_step;
    double next_y = y + motion_config_.desired_speed * std::sin(theta) * search_config_.time_step;

最终的轨迹经过了拟合以及重采样，为了让轨迹是平滑的，同时也是为了计算kappa kappa的计算根据该公式得出：

其他可能的方案1:

生成半圆曲线作为reference line, 然后使用qp进行优化。该方法没有比较合理的壁障方案。

其他可能的方案2：

使用目标lane的入口作为end-configuration space，然后使用Jerk minimize直接求解汽车可执行轨迹，然后检查轨迹是超过边界或存在碰撞。该方法可能导致计算出的trajectory不满足车子的物理特性，比如某些点的curve过大。下面给出一个最简单的JMT实现：

/**
 * jerk-minimized trajectory solver
 * @param start Init configuration
 * @param end Final configuration
 * @return JMT params
 */ 
TrajectoryParams generate_jerk_minimized_trajectory(
    const TrajectoryReference &start, 
    const TrajectoryReference &end
) {
    TrajectoryParams trajectory_params;

    trajectory_params.T = end.timestamp - start.timestamp;

    // rectify round trip behavior:
    double rectified_start_s = start.s;
    double rectified_end_s = end.s;
    if (rectified_start_s > rectified_end_s) {
        rectified_start_s -= 6945.554;
    }

    trajectory_params.s = solve_jerk_minimized_trajectory(
        rectified_start_s, start.vs, start.as,
          rectified_end_s,   end.vs,   end.as,
        trajectory_params.T
    );
    trajectory_params.d = solve_jerk_minimized_trajectory(
        start.d, start.vd, start.ad,
          end.d,   end.vd,   end.ad,
        trajectory_params.T
    );
    
    return trajectory_params;
}
std::vector<double> solve_jerk_minimized_trajectory(
    double x0, double vx0, double ax0,
    double x1, double vx1, double ax1,
    double T
) {
    // pre-computed exponentials of T:
    std::vector<double> T_exp{1.0, T, pow(T, 2.0), pow(T, 3.0),pow(T, 4.0), pow(T, 5.0)};
    
    // system matrix:
    Eigen::MatrixXd A(3, 3); 
    A <<   T_exp[3],    T_exp[4],    T_exp[5], 
         3*T_exp[2],  4*T_exp[3],  5*T_exp[4], 
         6*T_exp[1], 12*T_exp[2], 20*T_exp[3];
    
    // desired output:
    Eigen::VectorXd b(3);

    b <<  x1 - (x0 + vx0*T_exp[1] + 0.5*ax0*T_exp[2]),
         vx1 - (vx0 + ax0*T_exp[1]),
         ax1 - ax0;
    
    // polynomial coefficients:
    Eigen::VectorXd coeffs = A.colPivHouseholderQr().solve(b);
   
    return {x0, vx0, 0.5*ax0, coeffs(0), coeffs(1), coeffs(2)};
}

XiaoHuixxTian/Hybrid-A-Star-U-Turn-Solution

问题描述

方案：

程序使用说明：

方案1关键技术解释：

其他可能的方案1:

其他可能的方案2：