Safe feasible region generation method and system based on effective obstacles

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种基于有效障碍物的安全可行域生成方法及系统，用以降低安全可行域的维度，提高效率。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例一

如图1所示，本发明提供的基于有效障碍物的安全可行域生成方法，包括以下步骤：

步骤101：采集无人车的姿态信息，并利用多边形对无人车进行描述。

本方法主要目的是求解某个指定无人车姿态信息下的安全可行域，因此无人车姿态信息是事先给定的。

采集无人车目标姿态信息，定义如下：

 (1)

式(1)中，k表示无人车多边形顶点编号；p为无人车在行驶环境中的坐标，该坐标在以固定原点建立的绝对坐标系下，后续步骤将以p为基础求解安全可行域；表示无人车在行驶环境中的坐标的横坐标值和纵坐标值；表示无人车多边形的第k个顶点相对坐标；表示第i个障碍物多边形顶点集合；本方法用四边形为例描述无人车，表示无人车多边形的第k个顶点绝对坐标。安全可行域是包含无人车且不包含障碍物的区域，即当无人车姿态信息中的p在内部时，无人车将不会与障碍物发生碰撞。

步骤102：采集障碍物的空间位姿信息，并利用凸多边形对障碍物进行描述。

从环境感知模块获得环境中动静态障碍物（简称障碍物）的空间位姿信息，利用凸多边形描述障碍物外形，障碍物的空间位姿信息表示为：

   (2)

其中，i为障碍物编号；j表示障碍物多边形顶点编号；q_i表示第i个障碍物在行驶环境中的坐标；W_i表示第i个障碍物的外形信息；表示第i个障碍物在行驶环境中的坐标的横坐标值和纵坐标值；，表示第i个障碍物的第j个多边形顶点坐标，该坐标为车体相对坐标系下的值；表示第i个障碍物多边形顶点集合；为障碍物数量，j_i为第i个障碍物多边形的顶点数量。由于本方法所求解的是针对某个时刻的环境瞬态信息建立的包含p且不包含障碍物的安全可行域，因此障碍物的空间位姿信息均可由式(2)表示。第i个障碍物为动态障碍物时，（q_i,W_i）则表示该动态障碍物某个确定时刻的几何中心坐标及外形信息。

步骤103：建立堆栈，堆栈当前为空。

堆栈是计算机领域的一种数据结构，本方法利用堆栈描述有效障碍物信息的储存和使用。

建立一个堆栈L_valid，当前的堆栈L_valid是空的，堆栈L_valid中将要储存的元素为(i,j,k)，分别表示障碍物序号、障碍物多边形顶点序号及无人车多边形顶点序号。给出对L_valid的四个操作：

(1)L_valid.push：入栈，将新的元素(i,j,k)加入栈中。如图2所示，将元素(1,2,1)和(2,4,3)压入堆栈L_valid，得到一个新的L_valid，堆栈中的元素顺序为先入栈元素在栈底，后入栈元素在栈顶。

(2)L_valid.getTop：返回栈顶元素(i,j,k)，但不出栈，其中L_valid.getTop(1)表示返回栈顶元素中的i，L_valid.getTop(2)表示返回栈顶元素中的j，L_valid.getTop(3)表示返回栈顶元素中的k。如图3所示，L_valid.getTop返回栈顶元(1,2,3)，L_valid.getTop(1)则返回数值2，L_valid.getTop(2)则返回数值4，L_valid.getTop(3)则返回数值3。

(3)L_valid(m)：返回栈底起第m个元素(i,j,k)，L_valid(m).(1)表示第m元素中的i，L_valid(m).(2)表示第m元素中的j，L_valid(m).(3)表示第m元素中的k。以图3堆栈L_valid为例，当m=1，L_valid(m)表示返回元素(1,2,1)，当m=2，L_valid(m)表示返回元素(2,4,3)。

(4)L_valid.length返回栈的长度。以图2为例，压入栈前，空栈长度为0，压入两个元素后，栈的长度为2。

此步骤得到堆栈L_valid，有效障碍物将在后续步骤压入栈中。

步骤104：基于无人车的姿态信息以及障碍物的空间位姿信息，将第1个障碍物的相关信息储存到堆栈L_valid中；第1个障碍物的相关信息包括第1个障碍物的编号、第1个障碍物距离无人车最近的障碍物多边形顶点以及无人车距离第1个障碍物最近的无人车多边形顶点。

将第1个障碍物相关信息(i，j_i,opt，k_i,opt)压入堆栈L_valid中，其中，i表示障碍物编号，j_i,opt、k_i,opt表示无人车和障碍物i的最近的顶点编号。以图4为例，无人车第4个顶点和障碍物i的第2个顶点距离最近，因此j_i,opt=2，k_i,opt=4。对于无人车和障碍物两个最近的顶点的判别公式如公式(3a)、(3b)和(3c)所示。

i=1    (3a)

  (3b)

   (3c)

A_c=(q_i-p)'

式(3a)中，A_c表示以无人车中心为起点障碍物i为终点的二维向量，由于本步骤是针对第1个障碍物进行讨论，因此i=1；式(3b)中表示第1个障碍物的第顶点离无人车最近；将式(3b)结果代入到式(3c)，式(3c)中，表示无人车距离第1个障碍物第顶点最近。,具体值取决于无人车与第1障碍物相对位置，这里不再给出具体值。最后将（i,,）压入堆栈L_valid中。

步骤105：基于无人车的姿态信息以及障碍物的空间位姿信息，对剩余障碍物的有效性进行判断，将有效障碍物的相关信息储存到堆栈L_valid中。

基于更新的堆栈L_valid，对障碍物i进行筛选。过程如步骤1051-步骤1053叙述所示：

步骤1051：对第i（i≥2）个障碍物使用步骤104进行处理，可以得到(i，j_i,opt，k_i,opt)，公式如下：

 (4a)

   (4b)

步骤1052：利用栈顶元素信息，对第i个障碍物的有效判断，判断公式如下：

   (5)

式(5)中：L_valid.getTop见步骤103定义的操作(2)。

如果不等式(5)成立，则第i个障碍物为有效障碍物，将(i，j_i,opt，k_i,opt)压入堆栈L_valid。

如果不等式(5)不成立，则第i个障碍物为无效障碍物，对堆栈L_valid不做任何操作。

对于不等式(5)有效性判断效果中，当障碍物m被判断为无效障碍物，障碍物m相对于无人车，在第1个障碍物的“后面”；当障碍物n被判断为有效障碍物，障碍物n相对于无人车，在第1个障碍物的“前方”。简单来说，无人车在行驶的过程中，只需要关注周围离自己最近的障碍物，而障碍物后方的障碍物是无需考虑的，图5中障碍物m即为障碍物1后方的障碍物，为无效障碍物；障碍物n则相反。

步骤1053：重复步骤1051-步骤1052，直到对所有剩余障碍物完成有效性判断，并将有效障碍物的相关信息依次放入堆栈中。

步骤106：基于堆栈中所有有效障碍物生成安全可行域。

有效障碍物的数量可以用返回栈的长度得到，并且满足：

     (6)

将L_valid依次取出，基于所有有效障碍物进行安全可行域集合构建，公式描述如下：

  (7)

其中：

  (8)

式(8)中，表示安全可行域集合；A表示无人车指向有效障碍物的向量；表示安全可行域集合内的点；表示围成安全可行域集合的凸多边形；b表示围成安全可行域集合的凸多边形中每一条边的偏移量；表示第个障碍物在行驶环境中的坐标；表示堆栈中第m个元素中的i；表示第个障碍物多边形的第个顶点相对坐标；表示堆栈中第m个元素中的j；表示无人车多边形的第个顶点相对坐标；表示堆栈中第m个元素中的k；表示堆栈的长度。

至此，得到安全可行域，如式(7)所示。从式(6)可知，式(7)得到的安全可行域维度是小于等于障碍物数量的，在障碍物密集情况下，效果更为显著。安全可行域的维度降低可以提高安全走廊等诸多方法在使用安全可行域时的效率。

本方法在生成安全可行域的过程中考虑了无人车外形和障碍物外形，通过筛除无效障碍物降低安全可行域维度。从步骤104-步骤105可以看出，从第1个障碍物开始，本方法会将后一个障碍物、前一个障碍物、无人车三者空间位置进行比较，比较公式为式(5)，即如果前一个障碍物挡在后一个障碍物和无人车中间，那么式(5)不满足，后一个障碍物则为无效障碍物，反之则是有效障碍物。简单来说，无人车在行驶的过程中，只需要关注周围离自己最近的障碍物，而障碍物后方的障碍物是无需考虑的。

实施例二

为了执行上述实施例一对应的方法，以实现相应的功能和技术效果，下面提供了一种基于有效障碍物的安全可行域生成系统，包括：

无人车姿态信息采集模块，用于采集无人车的姿态信息，并利用多边形对无人车进行描述；

障碍物空间位姿信息采集模块，用于采集障碍物的空间位姿信息，并利用凸多边形对障碍物进行描述；

堆栈建立模块，用于建立堆栈，堆栈当前为空；

储存模块，用于基于无人车的姿态信息以及障碍物的空间位姿信息，将第1个障碍物的相关信息储存到堆栈中；第1个障碍物的相关信息包括第1个障碍物的编号、第1个障碍物距离无人车最近的障碍物多边形顶点以及无人车距离第1个障碍物最近的无人车多边形顶点；

判断模块，用于基于无人车的姿态信息以及障碍物的空间位姿信息，对剩余障碍物的有效性进行判断，将有效障碍物的相关信息储存到堆栈中；

安全可行域生成模块，用于基于堆栈中所有有效障碍物生成安全可行域。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。