写在前面

在之后的时间内,我将会从事自动驾驶的轨迹规划的相关学习,轨迹规划的算法有很多,类似于A_这种,那么这个系列就先分享一下A_算法的原理以及实现。

需求场景

曾经有一个机器人的轨迹规划项目,示意图如下所示:

图中排成一排的机器人是障碍物,那么我的任务就是控制一台机器人按照红线的轨迹从起点运行到终点,经过对比之后,我选了A*算法。

场景转换

A*算法是基于格点来走的,所以我们首先需要将实地场景进行算法转换,方法如下所示:

const int width = 30; 
const int height = 40;
const int FAIL = -99;
const int BLOCK = 1; //障碍
const int WALK = 0; //可以走的路
const int PATH = 2; //最后得出的路径
const int offseti[8]{ -1, -1, -1, 0, 0, 1, 1, 1 }; //8邻域偏移量
const int offsetj[8]{ -1, 0, 1, -1, 1, -1, 0, 1 };
int map[height][width] = { 0 };

//场上精度问题进行切换
void Accuracy(const int& i, const int& j, const float& x_accuracy, const float& y_accuracy, int map[][width]){
    if (x_accuracy<8 && i>0){
        map[i - 1][j] = BLOCK;
    }
    if (y_accuracy<8 && j>0){
        map[i][j-1] = BLOCK;
    }
    if (x_accuracy>12 && i<29){
        map[i + 1][j] = BLOCK;
    }
    if (y_accuracy>12 && j<29){
        map[i][j + 1] = BLOCK;
    }
}

//把场上起点和终点的坐标转换成数组
void Change( const point2f& point_on_field, Node& point_on_array){
    point_on_array.i = (300 + ((int)(point_on_field.x))) / 20;
    point_on_array.j = (200 + ((int)(point_on_field.y))) / 20;
}

//把场上障碍物的坐标转换成数组
void Change(const point2f& point_on_field, int map[][width]){
    Node point_on_array;
    float x_accuracy, y_accuracy;
    x_accuracy = abs((int)(point_on_field.x)) % 20;
    y_accuracy = abs((int)(point_on_field.y)) % 20;
    point_on_array.i = (300 + ((int)(point_on_field.x))) / 20;
    point_on_array.j = (200 + ((int)(point_on_field.y))) / 20;
    map[point_on_array.i][point_on_array.j] = BLOCK;
    Accuracy(point_on_array.i, point_on_array.j, x_accuracy, y_accuracy, map);
}

//把场上的坐标转换成数组
void Adaptation(const point2f& _start, const point2f& _end, const point2f* obstacle, int map[][width],
        Node& start,Node& end){
    for (int i=0;i<9;i++){
        if (i==0){
            //cout << endl << "接下来进入起点的坐标转换" << endl;
            Change( _start, start);
        }
        else if (i==1){
            //cout << endl << "接下来进入终点的坐标转换" << endl;
            Change( _end, end);
        }
        else{
            //cout << endl << "接下来进入障碍物的坐标转换" << endl;
            Change(obstacle[i - 2],map);
        }
    }
}

上述几个函数主要是将实际场地的尺寸换成算法里的数组内容,我的思路是将整个场地均分成若干个方块(根据机器人的实际尺寸来确定的),并且为了保证机器人在运行的时候不会碰到障碍物,我对障碍物的范围进行了扩大。

因为障碍物放置的时候是不知道位置的,因此障碍物所在的格子可能影响路径格子,因此为了保证机器人在运行的时候不会碰到障碍物,我将障碍物附近的格子都进行了“障碍物化”处理,即在规划轨迹的时候是不会考虑的,如下图所示:

经过上述的函数处理之后就可以得到一个数组,数组元素即对应着场上的机器人的可选路径和障碍物以及起点和终点。