RRT与碰撞检测

RRT与碰撞检测

在基于采样的路径规划算法中，快速搜索随机树（RRT）可以说是一个最为经典的算法。围绕RRT的改进算法几乎每年都有，就如同基于搜索的规划算法中的A*。

RRT的规划算法流程很简单，如下所示：

1. 首先RRT会初始化一个列表V，用于存放候选路径。在最开始，算法会把起点$v_{start}$加入到列表V中；
2. 然后RRT会在地图的free区域上随机生成一个节点$v_{rand}$，通过V表找到与新节点$v_{rand}$距离最近的节点$v_{near}$作为父节点。连接这两个节点，就形成了一条边（edge）；
3. 接下来就是最为重要的一步，即碰撞检测，需要检查这条新边是否与障碍物碰撞。 若碰撞，则放弃这条边和新节点$v_{rand}$；若无碰撞，则将这个节点$v_{rand}$加入到V表中；
4. 不断重复步骤2~3，直到找到一个新节点，它与终点距离小于阈值。最后可以通过父子关系提取出一组从起点到终点的节点列表。

注：图片来自于PathPlanning

以上简要说明了RRT算法的大致流程，但本文的主要内容并不是介绍RRT算法的，而是选择介绍其中最为重要的一个模块——碰撞检测。因为我在初学RRT时，遇到的最大问题就是思考如何进行碰撞检测。我在网上找了很多资料，但都比较杂乱，因此在这里做一个总结和梳理。

在对路径做碰撞检测时，主要需要对顶点和边$(V,E)$进行检测。

1.对于顶点的碰撞检测

对于顶点，我们只需要检测它是否位在障碍物的内部即可。若顶点在障碍物内部，则说明该顶点是不合法的。反之，则说明该顶点是合法的。

1.1.障碍物是实心多边形

对于实心多边形的检测，无论它的结构有多复杂，都存在一个普遍的方法可以检查某点是否在其内部。假设存在一个如下图所示的五边形，其内部都视为障碍物。

在检测时，首先我们需要假设多边形的所有边都是有向边，方向为逆时针，如上图所示。接下来就可以很简单的判断一个顶点是否在其内部了。

• 若对于每条边，顶点都位于这条边的左侧，那么这个顶点就在多边形内部；
• 若存在一条边，顶点位于其右侧，那么这个顶点就在多边形外部。

接下来一个问题就是应该如何判断顶点与边的位置关系。关于这个判断方法有很多，接下来我会介绍两种判断方法。

1.1.1.通过叉积判断

关于两个向量的叉积，我们都知道结果也是一个向量，并且该向量均垂直于前两个向量。而对于二维向量的叉积，其结果却是一个标量，但其实也可以视为一个与z轴平行的向量，其z轴分量就是这个标量值（虽然二维平面没有z轴）。

若存在向量$\vec{a}=(x_1,y_1),\vec{b}=(x_2,y_2)$，则$\vec{a}$与$\vec{b}$的叉积为
$$\vec{a}\times \vec{b}=x_1{y}_2-x_2{y}_1$$

二维向量的叉积有一个性质，即可以通过叉积结果的正负号判断两个向量的位置关系。

• 若符号为正，则说明向量b位于a的逆时针方向；
• 若符号为负，则说明向量b位于a的顺时针方向；
• 若符号为零，则说明向量a、b平行。

根据这个性质，我们就可以判断顶点与边的位置关系。

假设存在一条有向边AB与一个顶点P，连接AP。然后我们就可以通过判断向量AB与向量AP的叉积符号来判断顶点P与边AB的位置关系。
$$\overrightarrow{AB}\times \overrightarrow{AP}=a$$

• 若$a<0$，则说明向量AP位于向量AB的顺时针方向，说明顶点P位于边AB的右侧；
• 若$a>0$，则说明向量AP位于向量AB的逆时针方向，说明顶点P位于边AB的左侧；

但是存在一种特殊情况需要分类讨论，就是顶点位于线段AB或其延长线上，此时$a=0$。当出现这种情况时，我们可以通过判断A、B、P的位置顺序来判断，这里就不再赘述。

1.1.2.通过点积判断

上式的2-norm(u)表示向量u的二范数。需要注意的是，用上述方法计算得到的d，不但可以根据符号判断点P与线段AB的位置关系，而且d的模就是点P到直线AB的最短距离。
$$d=\vec{D}\cdot \vec{n}=|\vec{D}||\vec{n}|\cos \theta =|\vec{D}|\cos \theta ,|\vec{n}|=1$$

1.2.障碍物是实心圆形

对于实心圆形的检测，只需要判断顶点与圆心的距离是否小于半径即可。

2.对于线段的碰撞检测

RRT的碰撞检测除了判断顶点是否在障碍物内外，常常还需要判断线段是否与障碍物相交。这里其实有一个比较简单的方法，就是可以采样线段上一定数量的点，然后可以用上文介绍的方法判断这些点是否在障碍物内。但是，当障碍物与线段的接触较小时，这种采样的方法会有误检的风险。

下面会介绍更加可靠的方法来实现线段的碰撞检测。

2.1.障碍物是实心多边形

事实上，当障碍物是实心多边形时，线段的碰撞检测与点的碰撞检测相同，都可以采用向量叉积的方式进行判断。对于一个实心多边形，只有当被检测线段与该多边形的所有边均没有相交时，我们就能判断该线段没有与障碍物碰撞。换句话说，只需要检测线段与多边形的所有边是否相交，就可以实现线段的碰撞检测。

那么该如何判断两条线段是否相交呢？

可以通过向量叉积的方式进行判断。假设线段AB与线段CD相交，那么它们必然满足以下两个条件：

• 顶点C、D分别位于线段AB的两端；
• 顶点A、B分别位于线段CD的两端。

假设要证明顶点C、D位于线段AB的两侧，那么可以连接AC、AD，分别计算向量AB与向量AC、AD的叉积，只要两者的叉积结果异号，那么说明它们分别位于线段AB两端。

$$\overrightarrow{AB}\times \overrightarrow{AC}=m,\overrightarrow{AB}\times \overrightarrow{AD}=n$$

• 若$m\times n>0$，说明两者同号，顶点C、D位于AB的同侧；
• 若$m\times n<0$，说明两者异号，顶点C、D位于AB的异侧。

当然与上文相同，这里也会存在两种特殊情况：

• 顶点C或顶点D在线段AB上；
• 线段CD与线段AB共线。

这两种情况都会出现向量叉积为0的问题，但第一种情况还是可以判断的，因为这种情况说明两条线段必然相交。只要我们把上文判断线段相交的条件改为小于等于即可，如下所示：

• 若$m\times n>0$，说明顶点C、D位于AB的同侧，两线段不相交；
• 若$m\times n\le 0$，说明顶点C、D位于AB的异侧或某点在线段AB上，两线段相交。

但是对于第二种情况，则需要分类讨论，因为这种情况下两条线段不一定会相交。比如下图的第一种情况，两条线段并没有相交。此时就需要讨论这四个顶点的顺序关系了，这里就不再赘述。

线段相交的判断逻辑图整理后如下图所示。

当然，对于判断线段与障碍物是否碰撞而言，以上两种特殊情况（点在线段上和两线段共线）其实可以视为是不碰撞的。也就是说，我们只要判断$m\times n<0$是否成立即可，只要不成立就认为不碰撞。 若成立，则需要再判断顶点A、B是否位于线段CD的两侧，若仍然成立，才可以判断这两条线段相交。

function result = cross_2d(vec1,vec2)
    % 计算二维向量的叉积
    result = vec1(1)*vec2(2)-vec1(2)*vec2(1);
end
function is_intersect = intersect_check(A,B,V1,V2)
    % 检测线段AB与线段V12是否相交
    % 判断V1,V2与线段AB的位置
    AB = B - A;
    AV1 = V1 - A;
    AV2 = V2 - A;
    result1 = MapConfig.cross_2d(AB,AV1);  % 叉积
    result2 = MapConfig.cross_2d(AB,AV2);
    if result1~=0 && result2~=0
        % 同号
        if result1*result2 > 0
            is_intersect = false;
            return;
        end
    % 两线段共线(result1=result2=0)视为不相交
    % 某点在线段上(result1=0 || result2=0)视为不相交
    else
        is_intersect = false;
        return;    
    end
    % 判断A,B与线段V12的位置
    V12 = V2 - V1;
    V1A = A - V1;
    V1B = B - V1;
    result1 = MapConfig.cross_2d(V12,V1A);
    result2 = MapConfig.cross_2d(V12,V1B);
    if result1~=0 && result2~=0
        % 同号
        if result1*result2 > 0
            is_intersect = false;
            return;
        end
    % 两线段共线(result1=result2=0)视为不相交
    % 某点在线段上(result1=0 || result2=0)视为不相交
    else
        is_intersect = false;
        return;    
    end
    is_intersect = true;
end

2.2.障碍物是实心圆形

若障碍物为实心圆形时，对于线段的碰撞检测，我们只需要计算圆心到直线。若是检测直线与圆，那么可以直接计算圆心到直线的距离，然后判断该距离是否大于圆半径即可。但是线段是有长度的，它并不一定会穿过圆，如下图的三种情况。当线段AB没有穿过圆时，最短距离就变成了线段OB的长度。当然为了避免下图中间的情况，可以先判断两个顶点是否在圆内，然后再计算圆心到直线的距离。但下图的右侧情况仍然无法排除。下面介绍一种方法可以排除第三种情况。

该方法的大致思路是先求出直线与圆的最短距离的那个点的坐标，即上图的G点。然后计算线段AG与AB的长度比值$\lambda$，根据$\lambda$就可以判断G点是否位于线段AB的中间。

• 若$0<\lambda <1$，说明G点位于AB中间，即上图的第一种情况；
• 若$\lambda >1$，说明G点位于AB外侧，即上图的第二、三种情况；

其中，第二种情况可以根据顶点A、B是否在圆内进行判断，第三种情况则可以根据$\lambda$的取值范围进行判断。

关于$\lambda$的计算方式如下所示：

1. 首先通过计算向量内积得到线段AG的长度，如下式：
   $$\begin{array}{rl}& \overrightarrow{AO}\cdot \overrightarrow{AB}=|\overrightarrow{AO}||\overrightarrow{AB}|\cos \theta \\ ⟹& |\overrightarrow{AG}|=|\overrightarrow{AO}|\cos \theta =\frac{\overrightarrow{AO}\cdot \overrightarrow{AB}}{|\overrightarrow{AB}|}\end{array}$$

2. 然后求出线段AG与线段AB的长度比值$\lambda$：
   $$\lambda =\frac{|\overrightarrow{AG}|}{|\overrightarrow{AB}|}=\frac{\overrightarrow{AO}\cdot \overrightarrow{AB}}{|\overrightarrow{AB}{|}^2}$$