it网站设计,池州网站建设哪家好,求职网站网页模板下载,福建抖音代运营开户目录 零、任务介绍一、算法原理1.1 A* Algorithm1.2 启发函数 二、代码实现三、结果分析四、效果展示4.1 Dijkstra距离4.2 Manhatten距离4.3 欧几里德距离4.4 对角距离 五、后记 零、任务介绍
carla-ros-bridge/src/ros-bridge/carla_shenlan_projects/carla_shenlan_a_star_p… 目录 零、任务介绍一、算法原理1.1 A* Algorithm1.2 启发函数 二、代码实现三、结果分析四、效果展示4.1 Dijkstra距离4.2 Manhatten距离4.3 欧几里德距离4.4 对角距离 五、后记 零、任务介绍
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*则会导致算法得到次优路径。比较极端的情况有两种
h0Dijkstrahh*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 dManhattendxdydz 欧式距离定义为空间中的直线距离 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} dEuclideandx2dy2dz2 三维的对角距离可以理解为先按照xyz三个方向的距离中最短的一个构成的正方体的体对角线移动到和终点相同的平面内然后在面内沿正方形的对角线移动最后沿一个坐标轴方向移动。 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} dmaxdmindmiddDiagonalmax(dx,dy,dz)min(dx,dy,dz)(dxdydz)−dmax−dmin3 dmin2 (dmid−dmin)(dmax−dmid)
二、代码实现
计算启发函数
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::multimapdouble, GridNodePtropenSet.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; // 更新起点的状态为已加入openSetvectorGridNodePtr neighborPtrSets; // 存储邻居节点vectordouble 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.45651.3047右下259.77639.7067左上194.33834.1463
Manhatten距离
目标点位置规划时间ms路径长度m右上1.1548551.8406右下3.2998741.3636左上3.4943236.7279
欧几里德距离
目标点位置规划时间ms路径长度m右上5.2185551.3047右下12.243339.8031左上1.3394334.2426
对角距离
目标点位置规划时间ms路径长度m右上3.054651.8406右下7.6939841.9993左上4.6761336.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往期文章也可以在本专栏中阅读。 所谓师傅领进门修行在个人后续还需要进一步广泛的阅读相关文献将我之前在多智能体博弈方面积累的经验和自动驾驶结合起来努力推进自动驾驶技术和我个人的发展。 想说的就这么多也衷心祝愿看到这里的朋友们在各自的领域坚持下去早日实现理想
