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

零、任务介绍

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

一、算法原理

1.1 A* Algorithm

A*算法的步骤如下：

启发函数有如下两条性质

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

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

1. h=0：Dijkstra
2. h>>h*：Greedy

1.2 启发函数

本项目中使用了三种常见的启发函数，分别为曼哈顿距离、欧氏距离和对角距离。
记起点到终点沿直角坐标系坐标轴方向的距离分别为$(d_x,d_y,d_z)$，曼哈顿距离定义为每一步只能向坐标轴方向移动
$$d_{Manhatten}=d_x+d_y+d_z$$
欧式距离定义为空间中的直线距离
$$d_{Euclidean}=\sqrt{d_x^2+d_y^2+d_z^2}$$
三维的对角距离可以理解为先按照x，y，z三个方向的距离中最短的一个构成的正方体的体对角线移动到和终点相同的平面内，然后在面内沿正方形的对角线移动，最后沿一个坐标轴方向移动。
$$\begin{array}{rl}{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{array}$$

二、代码实现

计算启发函数

double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2) {/*choose possible heuristic function you wantManhattan, 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*路径搜索函数

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_pointVector3i start_idx = coord2gridIndex(start_pt);    // 坐标和体素地图索引互转Vector3i end_idx = coord2gridIndex(end_pt);goalIdx = end_idx;// 此处转换完成后的start_pt和end_pt是体素中心的坐标// position of start_point and end_pointstart_pt = gridIndex2coord(start_idx);end_pt = gridIndex2coord(end_idx);// Initialize the pointers of struct GridNode which represent start node and goal nodeGridNodePtr 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_listGridNodePtr currentPtr = NULL;GridNodePtr neighborPtr = NULL;// put start node in open setstartPtr->gScore = 0;                           // 起点到起点的距离为0startPtr->fScore = getHeu(startPtr, endPtr);    // 计算起点到终点的启发函数startPtr->id = 1;startPtr->coord = start_pt;openSet.insert(make_pair(startPtr->fScore, startPtr));    // 将起点加入openSetGridNodeMap[start_idx[0]][start_idx[1]][start_idx[2]]->id = 1;    // 更新起点的状态为已加入openSetvector<GridNodePtr> neighborPtrSets;    // 存储邻居节点vector<double> edgeCostSets;// this is the main loopwhile (!openSet.empty()) {/* Remove the node with lowest cost function from open set to closed set */// 将openSet中f最小的节点从openSet移动到closedSetcurrentPtr = 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 goalif (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 successionAstarGetSucc(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 expandedIMPORTANT NOTE!!!neighborPtrSets[i]->id = -1 : expanded, equal to this node is in close setneighborPtrSets[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;}}}
}

三、结果分析

本次实验中使用的地图如下：

Dijkstra算法的结果为最优解，作为下面不同启发函数的对照。

目标点位置	规划时间（ms）	路径长度（m）
右上	341.456	51.3047
右下	259.776	39.7067
左上	194.338	34.1463

Manhatten距离

目标点位置	规划时间（ms）	路径长度（m）
右上	1.15485	51.8406
右下	3.29987	41.3636
左上	3.49432	36.7279

欧几里德距离

目标点位置	规划时间（ms）	路径长度（m）
右上	5.21855	51.3047
右下	12.2433	39.8031
左上	1.33943	34.2426

对角距离

目标点位置	规划时间（ms）	路径长度（m）
右上	3.0546	51.8406
右下	7.69398	41.9993
左上	4.67613	36.7276

对比上述启发函数，可以发现，欧氏距离作为其中最保守的距离，最终规划得到的路径长度接近Dijkstra算法得到的最优解，而曼哈顿距离和对角距离得到的路径长度略大于最优解，但是规划耗时较小。三种启发函数的A*规划时间都远远小于Dijkstra。
规划耗时：曼哈顿距离<对角距离<欧几里德距离
路径长度：欧几里德距离<对角距离$\approx$曼哈顿距离

四、效果展示

Dijkstra演示视频

自动驾驶控制与规划——Project 5(Dijkstra)

A*演示视频

自动驾驶控制与规划——Project 5(A*)

4.1 Dijkstra距离

4.2 Manhatten距离

Manhatten距离，目标点右上

Manhatten距离，目标点右下

Manhatten距离，目标点左上

4.3 欧几里德距离

欧几里德距离，目标点右上

欧几里德距离，目标点右下

欧几里德距离，目标点左上

4.4 对角距离

对角距离，目标点右上

对角距离，目标点右下

对角距离，目标点左上

五、后记

自动驾驶控制与规划是我在深蓝学院完完整整学完的第一门课，能坚持下来和我从无人机到航天器再到自动驾驶的心路历程转变密不可分，其中有的原因牵涉单位利益，不便多说。总而言之，无论在个人发展还是薪资待遇等方面，自动驾驶这个行业的前景还是非常可观的。
在短短一个多月的时间内熟练掌握自动驾驶必然是不可能的，但是这门课程让我了解了自动驾驶的常用动力学、控制、规划等基本原理和设计思路，已经成功入门了。学习过程中的代码全部开源在我的github上：AutonomousVehiclePnCCourseProjects，往期文章也可以在本专栏中阅读。
所谓师傅领进门，修行在个人，后续还需要进一步广泛的阅读相关文献，将我之前在多智能体博弈方面积累的经验和自动驾驶结合起来，努力推进自动驾驶技术和我个人的发展。
想说的就这么多，也衷心祝愿看到这里的朋友们在各自的领域坚持下去，早日实现理想！！！