混合A*路径规划器：非完整约束车辆的实时路径规划算法

混合A*路径规划器：非完整约束车辆的实时路径规划算法

引用

CSDN

1.

https://m.blog.csdn.net/qq1309399183/article/details/144861830

混合A路径规划器是一种专门针对非完整约束车辆（如自动驾驶汽车）的实时路径规划算法。它通过在三维离散搜索空间中结合Dubin's路径和Dijkstra算法，实现了既考虑车辆运动约束又避开障碍物的最优路径规划。本文将详细介绍混合A算法的原理、实现方法及其在自动驾驶领域的应用场景。

项目背景

本项目是一个用于非完整约束车辆的实时路径规划代码，采用混合A*（Hybrid-A*）算法。该项目是自动地面车辆（AGV）研究小组的研究成果，并已在实验室测试车辆Eklavya上成功测试。实验中使用GPS航点作为目标，并通过安装在车辆前端的激光雷达（LIDAR）生成二进制障碍物地图。传感器接口方面，采用了ROS（机器人操作系统）。

算法描述

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

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

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

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

改进点

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

应用场景

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

以下是GUI类的部分代码实现：

#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;
}

请注意，上述内容基于原文进行了翻译和适当扩充，以确保信息完整性和表达清晰度。实际应用和技术细节可能会随着研究进展而有所变化。