一种针对静态场景的编队避障方法与流程

1.本发明涉及多智能体编队避障领域，更具体的，涉及一种基于速度锥准则的静态场景编队避障方法。


背景技术：

2.随着无人机和无人车等智能体应用的兴起，智能控制领域逐渐转向低成本、大规模的集群协同控制。集群协同控制可以实现单体无法实现的群体智能，具有更广阔的侦查视野和更有价值的载荷能力，同时还具有节约成本、提高容错度、执行任务更加机动灵活等特点。
3.在集群协同控制领域，主要考虑以下问题：如单体动力学建模、单体控制、编队控制、路径规划、障碍规避、任务分配等，其中障碍规避，也就是避障，是实现上层任务决策的基础。只有做好避障，才能保证编队智能体不会因为碰撞受损，才能确保决策的有效执行及编队队形的实现。
4.现有技术针对实时避障中直接给出可行轨迹的研究比较薄弱，特别是针对静态场景的编队避障方法，计算效率低，实用性不强，不能高效应对复杂场景。
5.因此，需要提供一种针对静态场景的编队避障方法。


技术实现要素：

6.为了克服上述现有技术的不足，本发明提出一种针对静态场景的避障方法，通过利用本机与障碍物点之间的角度关系及速度关系进行实时运算、实时避障规划，规划路径不存在畸变，计算效率高，实用性强，能够高效应对复杂场景。
7.本发明的目的在于提供一种针对静态场景的编队避障方法，该方法包括：
8.识别障碍物并进行标记，获得障碍物点集，其中，障碍物点集包括静态障碍物点集和编队内的非本机智能体点集；
9.计算静态障碍物点集和编队内的非本机智能体点集内所有的点相对于本机点的速度锥范围；
10.基于速度锥范围判断是否发生碰撞，并执行以下步骤：
11.若判断至少存在一个障碍物点将会发生碰撞，则重新规划航线；
12.若判断已经发生碰撞，则执行逃逸模式；
13.若判断不存在碰撞的障碍物点，则继续执行编队。
14.可选地，本机标记为p0，静态障碍物点集标记为a，编队内的非本机智能体点集标记为p，其中计算静态障碍物点集a和编队内的非本机智能体点集p内所有的点相对于本机p0的速度锥范围包括：
15.确定静态障碍物点集a中一静态障碍物的安全半径d
a
；
16.确定本机p0的安全半径d
p
；
17.确定该静态障碍物的安全圈，安全圈的圆心为该静态障碍物的坐标，安全圈的半
径为静态障碍物的安全半径d
a
与本机p0的安全半径d
p
的叠加之和；
18.确定速度锥的边界为本机p0到该静态障碍物的安全圈的两条切线；及
19.根据障碍物a的安全圈及速度锥的边界确定速度锥范围。
20.进一步可选地，基于速度锥范围判断是否发生碰撞包括：
21.确定静态障碍物的运动速度v
a
；
22.确定本机p0的运动速度v
p
；
23.确定本机p0到该静态障碍物的合速度v
pa
；
24.若合速度v
pa
落在速度锥范围内且本机p0位于静态障碍物的安全圈范围外，则判断将会发生碰撞；
25.若合速度v
pa
落在速度锥范围内且本机p0位于静态障碍物的安全圈范围内，则判断已经发生碰撞；及
26.若合速度v
pa
没有落在速度锥范围内，则判断不会发生碰撞。
27.进一步可选地，若判断至少存在一个障碍物点将会发生碰撞，则重新规划航线包括：
28.解除编队，计算可行区域；
29.提取可行区域的边界相位值；
30.选择目标边界及目标边界从属的目标障碍物；及
31.针对目标边界规划航线。
32.进一步可选地，探测范围表示为由探测半径和探测角度构成的扇形区域，其中该探测区域的最外侧边界相位值分比表示为b1和b2，其中b1小于b2；
33.计算可行区域包括：
34.基于探测范围和将会发生碰撞的静态障碍物的速度锥的相位范围计算可行区域，可行区域为[b1,b2]的子集且排除静态障碍物所对应的边界相位值。
[0035]
进一步可选地，提取可行区域的边界相位值包括：
[0036]
在上述可行区域的相位边界值中，选择距离本机速度方向差角最小的相位边界值作为可行区域的边界相位值。
[0037]
进一步可选地，选择目标边界及目标边界从属的目标障碍物包括：
[0038]
选择可行区域的边界相位值作为目标边界；及
[0039]
选择可行区域的边界相位值对应的障碍物点为目标障碍物。
[0040]
进一步可选地，针对目标边界规划航线包括：
[0041]
控制本机p0向目标障碍物方向行驶，其中，行驶方向与目标障碍物位于本机p0的方向相反；
[0042]
控制本机p0以转弯半径r进行转弯，使得本机p0沿着目标障碍物安全圈的一侧相切经过目标障碍物。
[0043]
进一步可选地，计算转弯半径包括：
[0044]
当本机p0右转时，转弯半径r计算公式为：
[0045]
[0046]
当本机p0左转时，转弯半径r计算公式为
[0047][0048]
其中，r为转弯半径，l为本机p0与目标障碍物的距离，d为目标障碍物的安全半径，ψ为本机p0与目标障碍物连线和本机p0的运动速度v
p
的夹角。
[0049]
进一步可选地，执行逃逸模式包括：
[0050]
采用所决定的转弯方向，以预设转弯速度和预设转弯半径规划路径，完成逃逸模式，其中，预设转弯速度大于第一转弯速度，预设转弯半径小于第一转弯半径。
[0051]
本发明的有益效果如下：
[0052]
本发明公开一种针对静态场景的编队避障方法，基于速度锥判断是否发生碰撞，运算只利用到本机与障碍物点之间的角度关系及速度关系进行计算，计算效率高，实用性强，能够有效的应用于编队控制中针对复杂静态场景的避障工况，可实时进行避障规划，能够同时对静态场景中障碍物及编队内智能体做出避障响应。另外，本发明中避障路线是连续的，不存在路径尖角畸变，不需要进行路径的二次平顺，可直接应用于无人机、无人车等控制环境中。
附图说明
[0053]
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0054]
图1示出根据本发明实施例的编队避障方法流程图。
[0055]
图2示出根据本发明实施例的静态障碍物示意图。
[0056]
图3示出根据本发明实施例的速度锥判断示意图。
[0057]
图4示出根据本发明实施例的路径规划示意图。
具体实施方式
[0058]
为使本发明的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。
[0059]
需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
[0060]
现有的避障技术主要涉及两种，一种是基于场景感知得到的信息，如视觉同步定位与建图(simultaneous localization and mapping,slam)等，事先进行安全路径规划，
这种方法不能应对时变的动态障碍物或场景；另一种是实时避障，如基于势能场原理的避障，这种方法可以有效地应对动态场景，但是其规划出来的轨迹并不光滑，需要进行二次处理，才能输出给智能体进行导航控制。
[0061]
在无人机、无人车等智能体的应用问题中，场景通常的多变的且复杂的，事先进行探测并规划好航线往往是实用性较弱的一种选择，并不能体现具有较高应变性的智能水平，因此实时避障技术尤为重要。
[0062]
在编队控制中，一是要确保编队内智能体不会发生碰撞，如在队形变换中、任务执行中等，二是要确保智能体对外界复杂环境进行实时有效避障，并且在避障的过程中实现编队内的相互避碰。
[0063]
另外，避障所规划出的轨迹应当满足智能体行驶所要求的物理约束，如最大转弯角速度、最小转弯半径等，避障轨迹应当是可实现的，这样避障效率更高，智能体行走路线与规划航迹之间误差更小，执行性更强。
[0064]
本发明公开一种针对静态场景的编队避障方法，该方法包括：识别障碍物并进行标记，获得障碍物点集，其中，障碍物点集包括静态障碍物点集和编队内的非本机智能体点集；计算静态障碍物点集和编队内的非本机智能体点集内所有的点相对于本机点的速度锥范围；基于速度锥范围判断是否发生碰撞，并执行以下步骤：若判断至少存在一个障碍物点将会发生碰撞，则重新规划航线；若判断已经发生碰撞，则执行逃逸模式；若判断不存在碰撞的障碍物点，则继续执行编队。
[0065]
本发明公开的针对静态场景的编队避障方法，基于速度锥判断是否发生碰撞，运算只利用到本机与障碍物点之间的角度关系及速度关系进行计算，计算效率高，实用性强，能够有效的应用于编队控制中针对复杂静态场景的避障工况，可实时进行避障规划，能够同时对静态场景中障碍物及编队内智能体做出避障响应。另外，本发明中避障路线是连续的，不存在路径尖角畸变，不需要进行路径的二次平顺，可直接应用于无人机、无人车等控制环境中。
[0066]
如图1所示，本发明中编队避障方法流程图可描述如下：
[0067]
1)识别障碍物并进行标记，记a系列代表从静态障碍物上识别到的障碍物点的集合，p系列代表非本机的编队之内的智能体；
[0068]
2)计算每一个障碍物点相对本机的速度锥范围，并利用速度锥方法判断是否会碰撞；
[0069]
3)若判断存在一个障碍物点会发生碰撞，则解除编队控制算法，获取速度锥相位角范围的集合，记为不可行区域，剩余相位角范围代表可行区域；
[0070]
4)提取可行区域的边界相位值，获取边界相位值距本机速度方向差值最小的一个，记该边界为a，记其从属的障碍物点为a；
[0071]
5)规划航迹，使本机擦着a的安全圈位于a一侧的切边驶过，输出下一时刻期望轨迹位置，其中安全圈是设定的避障缓冲区域；
[0072]
6)若已经发生碰撞，即本机位于其中一个障碍物的安全圈内，则转为逃逸模式，以恒定转弯速度、转弯半径及上一时刻转弯方向进行规划，输出下一时刻期望轨迹位置；
[0073]
7)返回步骤1)进行循环，若判断不存在会发生碰撞的障碍物点，则继续编队。
[0074]
下面结合具体实施例进行介绍
[0075]
在一个可选实施例中，该方法的具体实施步骤为：
[0076]
步骤1，在每一时刻，首先对障碍物进行识别。
[0077]
在本实施例中，如图2所示，探测范围由探测半径和探测角度构成的扇形区域表示。场景中有静态障碍物，由黑色矩形表示，可以代表墙面等规则障碍物。
[0078]
应理解的是，本方法所适用的静态场景所涉及的静态障碍物也不一定是规则形状，还可由不规则边界构成。
[0079]
另外，场景中同时考虑编队同伴所代表的动态障碍物，以防止相互碰撞。
[0080]
现有技术中，静态障碍物的位置可由视觉、雷达等传感器获取。但是如雷达等器件所获取的障碍物位置不一定是连续的，无法保证每一个相位角都能检测到障碍物。所以为了应对这个问题，本方法将连续障碍物边界进行离散化，即使传感器信息是间断的也不影响对于障碍物的识别。
[0081]
如图2所示，黑色矩形框附近的虚线圆圈代表的是接收到的障碍物上的点所伴随的安全圈的位置，安全圈的设定见步骤2。若在探测扇形范围内同一相位角附近有多个障碍物点，则考虑在相对于本机的相位误差1度范围内舍去距离较远的障碍物点。将从静态障碍物上识别到的障碍物点的集合表示为a，而将探测到的非本机的编队之内的智能体表示为p。假设本机编号为p0。
[0082]
步骤2，如图3所示，计算a和p集合内所有点相对p0点的速度锥范围。
[0083]
图3中d
a
代表障碍物a的安全半径，d
p
代表本机的安全半径，安全半径代表缓冲区域的大小，跟物体尺寸、物体运动速度有关，可视情况而定。
[0084]
若将d
a
和d
p
叠加到a点处，则速度锥的边界由本机p0点到a的安全圈的两条切线构成。设a的运动速度为v
a
，p的运动速度为v
p
，则从p到a的合速度v
pa
方向如图3所示。
[0085]
若v
pa
落在速度锥范围内则判断为会碰撞，若否，则判断为不会碰撞，若p已经位于a的安全圈范围内，则判断为已经碰撞。对障碍物点集合针对p0逐一进行判断，是否会碰撞或者是否已经碰撞。
[0086]
步骤3，若存在一个障碍物点判断为会碰撞，则切断本机的协同编队控制算法，运行本发明的避障算法。
[0087]
如图3所示，b1和b2代表的是探测区域的最外侧边界的相位值，而每一个障碍物点都可以计算得到其速度锥的相位范围，若将探测区域内所有速度锥的相位范围进行并运算，可得到区域：[a1,a2]和[a3,b2]，标记为不可行区域。而相反，实际可行区域则为探测角度排除不可行区域，得到[b1,a1]和[a2,a3]。
[0088]
步骤4，提取可行区域的边界，如例中b1、a1、a2和a3。取这些边界中距离本机速度方向v
p0
差角最小的边界，如例中a3。若有两个边界距离本机速度方向差角相等，则随机选择一个。获取此最近边界a3所从属的障碍物点，记为a3。
[0089]
步骤5，规划路径使本机向a3方位驶去，目的是使本机擦着障碍物a3安全圈的a3一侧的切边而过，这样避障所需的能量最小，路径平缓。若a3方位在v
p0
右侧，则右转行驶，若a3方位在v
p0
左侧，则左转。由于本机智能体可能为无人车、无人机等真实物理体，其运动轨迹应是连续弧线，不应存在无法实现的尖角路线，则在本方法中定义规划路径为分段圆弧。
[0090]
如图4所示，在p相对于a的坐标系中，设期望轨迹的圆弧，该圆弧与a的安全圈相切，在确定是左转弯或是右转弯的情况下，这样的期望轨迹是一定的。p在每一时刻情况下
都进行期望轨迹规划，容易得到：
[0091]
若右转，则转弯半径r为：
[0092][0093]
若左转，转弯半径r为：
[0094][0095]
其中ψ代表该时刻下速度相位值。
[0096]
通过转弯半径，可以计算得到下一时刻的期望位置，将期望位置输出给控制导航系统控制本机驶向目标位置，以实现期望航迹。
[0097]
步骤6，若判断为已碰撞，则直接采用上一时刻所决定的转弯方向，以恒定较大的转弯速度和转弯半径规划路径，完成逃逸模式。
[0098]
返回步骤1进行时间循环，若没有一个识别到的障碍物点反馈为会碰撞，则连接协同控制算法，恢复编队队形。
[0099]
本发明公开的针对静态场景的编队避障方法，基于速度锥判断是否发生碰撞，运算只利用到本机与障碍物点之间的角度关系及速度关系进行计算，计算效率高，实用性强，能够有效的应用于编队控制中针对复杂静态场景的避障工况，可实时进行避障规划，能够同时对静态场景中障碍物及编队内智能体做出避障响应。另外，本发明中避障路线是连续的，不存在路径尖角畸变，不需要进行路径的二次平顺，可直接应用于无人机、无人车等控制环境中。
[0100]
上述实施例中的集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在上述计算机可读取的存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在存储介质中，包括若干指令用以使得一台或多台计算机设备(可为个人计算机、服务器或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。
[0101]
在本申请的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
[0102]
在本申请所提供的几个实施例中，应该理解到，所揭露的客户端，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。
[0103]
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
[0104]
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
[0105]
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定，对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。