Hybrid Astar 算法剖析和实现（七）_template using typevectorvecd = typename

在学习资料满天飞的大环境下，知识变得非常零散，体系化的知识并不多，这就导致很多人每天都努力学习到感动自己，最终却收效甚微，甚至放弃学习。我的使命就是过滤掉大量的无效信息，将知识体系化，以短平快的方式直达问题本质，把大家从大海捞针的痛苦中解脱出来。

0 前言

本篇介绍一种简单高效的凸多边形碰撞检测方案，以及代码实现。

1 AABB碰撞检测算法

1.1 原理

AABB(Axis-Aligned Bounding Box)碰撞检测方法法全称为基于轴向包围框的碰撞检测方法。

假设有A和B两个凸多边形，将它们的顶点投影到x轴，A对应的区间为 $[x_{AL},x_{AH}]$ ，B对应的区间为 $[x_{BL},x_{BH}]$。如果这两个区间不相交，则一定不会发生碰撞，如果相交，则再将它们的顶点投影到y轴进行判断，如果投影到y轴的两个区间不相交，则一定不会发生碰撞，如果相交，对于AABB方法来说，则认为会发生碰撞。

其实，A和B两个突多边形在x轴和y轴的投影分别都有重叠，也并不一定会发生碰撞，但对于粗略判断来说，可以认为发生碰撞。

因此，AABB方法只能作为碰撞的粗略检测方案。但它的优点是速度快，因为向x轴和y轴投影就是顶点的横纵坐标值。

1.2 代码实现

示例代码如下。代码比较简单就不过多解释了。

template<int dim>
using TypeVectorVecd = typename std::vector<Eigen::Matrix<double, dim, 1>,
        Eigen::aligned_allocator<Eigen::Matrix<double, dim, 1>>>;
typedef TypeVectorVecd<2> VectorVec2d;

bool checkCollisionAABB(const Eigen::MatrixXd& vehicle, const std::vector<VectorVec2d>& obstacles)
{
    std::vector<double> vhx{vehicle.block<8,1>(0,0)[0], 
                            vehicle.block<8,1>(0,0)[2], 
                            vehicle.block<8,1>(0,0)[4], 
                            vehicle.block<8,1>(0,0)[6]};
    std::vector<double> vhy{vehicle.block<8,1>(0,0)[1], 
                            vehicle.block<8,1>(0,0)[3], 
                            vehicle.block<8,1>(0,0)[5], 
                            vehicle.block<8,1>(0,0)[7]};

    double vhx_max = *max_element(vhx.begin(), vhx.end());
    double vhx_min = *min_element(vhx.begin(), vhx.end());

    double vhy_max = *max_element(vhy.begin(), vhy.end());
    double vhy_min = *min_element(vhy.begin(), vhy.end());

    for (auto obs: obstacles) {
        double obsx_max = std::numeric_limits<double>::min();
        double obsx_min = std::numeric_limits<double>::max();
        double obsy_max = std::numeric_limits<double>::min();
        double obsy_min = std::numeric_limits<double>::max();

        for (auto point: obs) {
            if (point.x() > obsx_max) 
                obsx_max = point.x();
            if (point.x() < obsx_min) 
                obsx_min = point.x();
            if (point.y() > obsy_max) 
                obsy_max = point.y();
            if (point.y() < obsy_min) 
                obsy_min = point.y();
        }
        
        if (vhx_min > obsx_max || 
            vhx_max < obsx_min || 
            vhy_min > obsy_max || 
            vhy_max < obsy_min) {
            continue;
        } else {
            return true;
        }

    }

    return false; 
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52

---

2 SAT碰撞检测算法

2.1 原理

基于SAT(Separating Axis Theorem)进行碰撞检测的原理其实也很简单，本质上是AABB方法的升级版。

都是做投影，只不过AABB是将顶点投影到x轴和y轴，而SAT是自己创建轴，然后再将两个凸多边形的顶点投影过去，查看是否有重叠区域。

创建轴的方法就是计算凸多变形每条边的法向量。

2.2 代码实现

示例代码如下。

其中，需要注意两点。一个是计算分离轴时不要忘记对闭合凸多边形的处理；另一个是计算法向量时需要对平行于坐标轴的情况进行特殊处理。

bool checkCollisionSAT(const Eigen::MatrixXd& vehicle, const VectorVec2d& obs)
{
    //1.find all axis of vehicle
    VectorVec2d vaxis;
    for (std::size_t i = 0; i < 2u; ++i) {
        auto vtemp0 = vehicle.block<2,1>(2*i,0);
        auto vtemp1 = vehicle.block<2,1>(2*(i+1),0);
        auto vaxi = vtemp1 - vtemp0;
        vaxis.push_back(vaxi);
    }

    //2.find all axis of obstacle
    for (std::size_t j = 0; j < obs.size(); ++j) {
        auto dx = obs[(j+1) % obs.size()][0] - obs[j][0]; //使用取余运算处理凸多边形首尾衔接问题
        auto dy = obs[(j+1) % obs.size()][1] - obs[j][1];

        Eigen::Vector2d oaxi;
        if (dx != 0)
            oaxi << dy/dx, -1;
        else //对平行于y轴的情况进行特殊处理
            oaxi << 1, 0;

        vaxis.push_back(oaxi);
    }

    //3.project
    for (auto axi: vaxis) {
        //3.1 cal projection of vehicle
        std::vector<double> vpros;
        for (std::size_t i = 0; i < 4u; ++i) { 
            auto vtemp = vehicle.block<2,1>(2*i,0);
            double pro = vtemp.dot(axi);
            vpros.push_back(pro);
        }

        //3.2 cal projection of obstacle
        std::vector<double> opros;
        for (std::size_t j = 0; j < obs.size(); ++j) {
            auto x = obs[j][0];
            auto y = obs[j][1];
            Eigen::Vector2d vec(x,y);
            double pro = vec.dot(axi);
                       opros.push_back(pro);
        }

        //3.3 judge collision
        auto vmax = *max_element(vpros.begin(), vpros.end());
        auto vmin = *min_element(vpros.begin(), vpros.end());
        auto omax = *max_element(opros.begin(), opros.end());
        auto omin = *min_element(opros.begin(), opros.end());
        if (vmax < omin || vmin > omax) {
            return false;
        }
    }
    
    return true; 
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57

3 加速方案

3.1 方案设计

最终的碰撞检测方案是将AABB和SAT方法结合使用。先使用AABB粗筛，然后使用SAT精确计算车辆和障碍物是否发生碰撞。

细节实现上可以通过下述技巧进行加速：

1. 在SAT中使用法向量计算公式，不使用反三角函数。
2. 在SAT中提前计算好所有障碍物的分离轴，保存在障碍物的数据结构中。
3. 在SAT中只考虑车辆周边有限范围内的障碍物（可以请定位同事给障碍物打上标签）。
4. 在SAT中去除平行分离轴，避免重复投影和判断。

3.2 代码实现

代码实现如下。

//入参是车辆当前位姿
bool checkCollision(const double &x, const double &y, const double &theta) 
{
    Mat2d R;
    R << std::cos(theta), -std::sin(theta),
            std::sin(theta), std::cos(theta);

    Eigen::MatrixXd vehicle_shape;
    vehicle_shape.resize(8, 1);

    //坐标变换
    for (unsigned int i = 0; i < 4u; ++i) {
        transformed_vehicle_shape.block<2, 1>(i * 2, 0) = 
            R * vehicle_shape_.block<2, 1>(i * 2, 0) + Vector2d(x, y);
    }

    //AABB
    if (!checkCollisionAABB(vehicle_shape, obstacles_)) {
        return false;
    }

    //SAT
    for (auto obs: obstacles_) {
        if (!checkCollisionSAT(vehicle_shape, obs)) {
            continue;
        } else {
            return true;
        }
    }
    return false;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

4 总结

本篇基于AABB和SAT实现了一种较快速的碰撞检测方案，并在实现细节上给出了建议。

恭喜你又坚持看完了一篇博客，又进步了一点点！如果感觉还不错就点个赞再走吧，你的点赞和关注将是我持续输出的哒哒哒动力~~