自动驾驶控制与规划——Project 6: A* Route Planning

零、任务介绍

carla-ros-bridge/src/ros-bridge/carla_shenlan_projects/carla_shenlan_a_star_planner/src/Astar_searcher.cpp
中的 TODO部分
对⽐分析不同的启发函数的计算耗时，每次运⾏后在终端内会打印计算时间，需要截图放⼊⽂档中上传。

一、算法原理

1.1 A* Algorithm

A*算法的步骤如下：

启发函数有如下两条性质

可采纳性：启发式函数h(n)必须永远不超过从节点n到终点的实际成本（即h(n) ≤ h*(n)，其中h*(n)是从节点n到终点的真实成本）。
一致性：对于任意两个节点n和m，若存在一条从n到m的边，则启发式函数满足h(n) ≤ c(n, m) + h(m)，其中c(n, m)是从n到m的边成本。

当启发函数满足上述两条性质时，A算法能够保证路径的最优性，但实际应用中，启发函数可能不满足性质。若启发函数过于保守（远小于h），则会导致搜索效率低下，若启发函数过于激进（远大于h*），则会导致算法得到次优路径。比较极端的情况有两种：

h=0：Dijkstra
h>>h*：Greedy

1.2 启发函数

本项目中使用了三种常见的启发函数，分别为曼哈顿距离、欧氏距离和对角距离。

记起点到终点沿直角坐标系坐标轴方向的距离分别为 ( d x , d y , d z ) (d_x, d_y, d_z) (dx,dy,dz)，曼哈顿距离定义为每一步只能向坐标轴方向移动
d M a n h a t t e n = d x + d y + d z d_{Manhatten} = d_x + d_y + d_z dManhatten=dx+dy+dz

欧式距离定义为空间中的直线距离
d E u c l i d e a n = d x 2 + d y 2 + d z 2 d_{Euclidean} = \sqrt{d_x^2 + d_y^2 + d_z^2} dEuclidean=dx2+dy2+dz2

三维的对角距离可以理解为先按照x，y，z三个方向的距离中最短的一个构成的正方体的体对角线移动到和终点相同的平面内，然后在面内沿正方形的对角线移动，最后沿一个坐标轴方向移动。
d max ⁡ = max ⁡ ( d x , d y , d z ) d min ⁡ = min ⁡ ( d x , d y , d z ) d m i d = ( d x + d y + d z ) − d max ⁡ − d min ⁡ d D i a g o n a l = 3 d min ⁡ + 2 ( d m i d − d min ⁡ ) + ( d max ⁡ − d m i d ) \begin{aligned} d_{\max} &= \max(d_x, d_y, d_z) \\ d_{\min} &= \min(d_x, d_y, d_z) \\ d_{mid} &= (d_x + d_y + d_z) - d_{\max} - d_{\min} \\ d_{Diagonal} &= \sqrt{3}d_{\min} + \sqrt{2} (d_{mid} - d_{\min}) + (d_{\max} - d_{mid}) \end{aligned} dmaxdmindmiddDiagonal=max(dx,dy,dz)=min(dx,dy,dz)=(dx+dy+dz)−dmax−dmin=3 dmin+2 (dmid−dmin)+(dmax−dmid)

二、代码实现

计算启发函数

cpp 复制代码

double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2) {
    /*
    choose possible heuristic function you want
    Manhattan, Euclidean, Diagonal, or 0 (Dijkstra)
    Remember tie_breaker learned in lecture, add it here ?
    */
    bool tie_breaker = true;
    double distance_heuristic;
    Eigen::Vector3d node1_coordinate = node1->coord;
    Eigen::Vector3d node2_coordinate = node2->coord;

    // auto start = std::chrono::high_resolution_clock::now();
    Eigen::VectorXd abs_vec = (node1_coordinate - node2_coordinate).cwiseAbs();
    // **** TODO: Manhattan *****
    distance_heuristic = abs_vec.sum();

    // **** TODO: Euclidean  *****
    distance_heuristic = abs_vec.norm();

    // **** TODO: Diagonal  *****
    double min_coeff = abs_vec.minCoeff();
    double max_coeff = abs_vec.maxCoeff();
    double mid_coeff = abs_vec.sum() - min_coeff - max_coeff;

    distance_heuristic = 0.0;
    distance_heuristic += min_coeff * sqrt(3.0);
    distance_heuristic += (mid_coeff - min_coeff) * sqrt(2.0);
    distance_heuristic += (max_coeff - mid_coeff);

    if (tie_breaker) {
        distance_heuristic = distance_heuristic * (1.0 + 1.0 / 100.0);
    }

    return distance_heuristic;
}

A*路径搜索函数

cpp 复制代码

void AstarPathFinder::AstarGraphSearch(Vector3d start_pt, Vector3d end_pt) {
    // ros::Time time_1 = ros::Time::now();

    rclcpp::Time time_1 = this->now();    // 计时开始时间
    // ->now();
    // rclcpp::Time end_mpc;
    // start_mpc = this->now();

    // RVIZ中点选的目标点坐标不一定是体素的中心
    // index of start_point and end_point
    Vector3i start_idx = coord2gridIndex(start_pt);    // 坐标和体素地图索引互转
    Vector3i end_idx = coord2gridIndex(end_pt);
    goalIdx = end_idx;
    // 此处转换完成后的start_pt和end_pt是体素中心的坐标
    // position of start_point and end_point
    start_pt = gridIndex2coord(start_idx);
    end_pt = gridIndex2coord(end_idx);

    // Initialize the pointers of struct GridNode which represent start node and goal node
    GridNodePtr startPtr = new GridNode(start_idx, start_pt);
    GridNodePtr endPtr = new GridNode(end_idx, end_pt);

    // openSet is the open_list implemented through multimap in STL library
    // openSet的类型是std::multimap<double, GridNodePtr>
    openSet.clear();
    // currentPtr represents the node with lowest f(n) in the open_list
    GridNodePtr currentPtr = NULL;
    GridNodePtr neighborPtr = NULL;

    // put start node in open set
    startPtr->gScore = 0;                           // 起点到起点的距离为0
    startPtr->fScore = getHeu(startPtr, endPtr);    // 计算起点到终点的启发函数
    startPtr->id = 1;
    startPtr->coord = start_pt;
    openSet.insert(make_pair(startPtr->fScore, startPtr));    // 将起点加入openSet

    GridNodeMap[start_idx[0]][start_idx[1]][start_idx[2]]->id = 1;    // 更新起点的状态为已加入openSet

    vector<GridNodePtr> neighborPtrSets;    // 存储邻居节点
    vector<double> edgeCostSets;

    // this is the main loop
    while (!openSet.empty()) {
        /* Remove the node with lowest cost function from open set to closed set */
        // 将openSet中f最小的节点从openSet移动到closedSet
        currentPtr = openSet.begin()->second;    // 从openSet移除当前节点
        openSet.erase(openSet.begin());
        Eigen::Vector3i current_index = currentPtr->index;
        GridNodeMap[current_index[0]][current_index[1]][current_index[2]]->id = -1;    // 将当前节点标记为已扩展(ClosedSet)

        // if the current node is the goal
        if (currentPtr->index == goalIdx) {
            // ros::Time time_2 = ros::Time::now();

            rclcpp::Time time_2 = this->now();    // 计时结束时间
            terminatePtr = currentPtr;
            std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
            std::cout << "[A*]{sucess}  Time in A*  is " << (time_2 - time_1).nanoseconds() / 1000000.0 << "ms, path cost is " << currentPtr->gScore * resolution << "m." << std::endl;
            ;
            std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;

            return;
        }
        // get the succession
        AstarGetSucc(currentPtr, neighborPtrSets, edgeCostSets);

        /*
        For all unexpanded neigbors "m" of node "n"
        */
        for (int i = 0; i < (int)neighborPtrSets.size(); i++) {
            /*
            Judge if the neigbors have been expanded
            IMPORTANT NOTE!!!
            neighborPtrSets[i]->id = -1 : expanded, equal to this node is in close set
            neighborPtrSets[i]->id = 1 : unexpanded, equal to this node is in open set
            */
            neighborPtr = neighborPtrSets[i];
            if (neighborPtr->id == 0)    // discover a new node, which is not in the closed set and open set
            {
                /*
                As for a new node, put neighbor in open set and record it
                */
                neighborPtr->gScore = currentPtr->gScore + edgeCostSets[i];
                neighborPtr->fScore = neighborPtr->gScore + getHeu(neighborPtr, endPtr);
                neighborPtr->cameFrom = currentPtr;
                openSet.insert(make_pair(neighborPtr->fScore, neighborPtr));
                neighborPtr->id = 1;
                continue;
            } else if (neighborPtr->id == 1)    // this node is in open set and need to judge if it needs to update
            {
                /*
                As for a node in open set, update it , maintain the openset ,and then put neighbor in open set and record it
                */
                if (neighborPtr->gScore > (currentPtr->gScore + edgeCostSets[i])) {
                    neighborPtr->gScore = currentPtr->gScore + edgeCostSets[i];
                    neighborPtr->fScore = neighborPtr->gScore + getHeu(neighborPtr, endPtr);
                    neighborPtr->cameFrom = currentPtr;
                }
                continue;
            } else    // this node is in closed set
            {
                continue;
            }
        }
    }
}