混合A路径规划器 (Hybrid A Path Planner）

qq1309399183

于 2025-01-01 09:29:24 发布

阅读量288

点赞数 5

分类专栏：计算机视觉实战项目集合文章标签：混合A路径规划器 hybrid astar AGV路径规划路径规划 Astar

本文链接：https://blog.csdn.net/qq1309399183/article/details/144861830

版权

计算机视觉实战项目集合专栏收录该内容

823 篇文章

订阅专栏

混合A路径规划器 (Hybrid A Path Planner)

本仓库包含了一个用于非完整约束车辆（non-holonomic vehicles）的实时路径规划代码，该代码使用了混合A*（Hybrid-A*）算法。关于混合A*算法的描述，请参见《自动驾驶路径规划中的实用搜索技术》。

项目背景

此代码是自动地面车辆（AGV）研究小组的一个项目成果。我们已经在实验室的测试车辆Eklavya上成功测试了这段代码。在实验中，我们使用GPS航点作为目标，并通过安装在车辆前端的激光雷达（LIDAR）生成二进制障碍物地图。传感器接口方面，我们采用了ROS（机器人操作系统）。

算法描述

混合A算法使用了一个三维离散搜索空间，但与传统的A不同的是，它为每个网格单元关联了一个连续的三维车辆状态。这样可以确保生成的路径是可行驶的（而标准A*只能产生分段线性的路径）。

搜索过程由两个启发式规则指导：

无障碍物非完整约束（Non-Holonomic Without Obstacles）：采用Dubin’s路径长度计算方法，忽略障碍物的存在。
有障碍物完整约束（Holonomic With Obstacles）：使用Dijkstra算法在二维平面上计算最短路径，同时忽略了车辆的完整约束条件。

为了提高搜索速度，算法会在接近目标时，利用Dubins路径分析扩展节点，并检查这些路径是否与当前的障碍物地图发生碰撞。

在这里插入图片描述

改进点

3D离散搜索空间：相较于传统A*，混合A*不仅考虑位置信息，还考虑了方向和姿态等连续状态变量，从而保证了路径的实际可执行性。
双重启发式规则：结合了非完整约束下的最优路径估算以及考虑障碍物的最短路径规划，提高了搜索效率和路径质量。
快速收敛机制：针对靠近目标区域的节点，采用解析方法进行扩展，减少了不必要的计算开销，加快了搜索进程。

应用场景

#include "../include/Gui.hpp"

GUI::GUI(int rows, int cols){
	display_size=cv::Size(rows, cols);
	display=Mat(display_size, CV_8UC3, Scalar(220,220,220));
}

void GUI::drawCar(State src){
	RotatedRect rotRect = RotatedRect(Point2f(src.x*display_size.width/MAPX, src.y*display_size.height/MAPY), Size2f(BOT_L*display_size.width/MAPX, BOT_W*display_size.height/MAPY), src.theta*Theta_Res);
	Point2f vert[4];
	rotRect.points(vert);
	for(int i=0;i<4;i++)
		line(display, vert[i], vert[(i+1)%4], Scalar(200, 0, 0));

	circle(display, Point2f((src.x + 17*cos(src.theta*2.0*PI/Theta))*display_size.width/MAPX, (src.y + 17*sin(src.theta*2.0*PI/Theta))*display_size.width/MAPX), 5, Scalar(255, 0, 0));
}

void GUI::drawObs(Map map){
	for(int i=0;i<MAPX;i++)
		for(int j=0;j<MAPY;j++)
			if(map.obs_map[i][j]){

				for(int k=i*display_size.width/MAPX;k<(i+1)*display_size.width/MAPX;k++)
					for(int l=j*display_size.height/MAPY;l<(j+1)*display_size.height/MAPY;l++){
						display.at<Vec3b>(l,k)={128, 128, 128};
					}
			}
}

void GUI::markPoint(int i, int j){
	if(i<0 || i>MAPX || j<0 || j>MAPY)
		return;

	for(int k=i*display_size.width/MAPX;k<(i+1)*display_size.width/MAPX;k++)
		for(int l=j*display_size.height/MAPY;l<(j+1)*display_size.height/MAPY;l++){
						display.at<Vec3b>(k,l)={0, 255, 0};
					}
}

void GUI::show(int t){
	Mat temp=display;
	//resize(display, temp, display_size);
	imshow("Display", temp);
	waitKey(t);
	return;
}

该路径规划器适用于各种需要精确路径规划的应用场景，特别是在复杂环境下的自主导航任务中表现出色。例如，在城市道路、工业厂房或农业领域内，对于无人配送车、自动叉车或者智能农机等设备来说，混合A*算法能够提供安全可靠的路径解决方案。
在这里插入图片描述