一种动态环境下的避碰方法

1.本发明涉及一种动态环境下的避碰方法。


背景技术：

2.速度障碍(vo)：该方法在速度空间中产生一个锥形障碍(因此得名速度障碍)，只要自主装备的速度矢量在vo之外，就可以保证未来不会发生碰撞。
3.从开始往方向的射线表示为：
4.给定形状为a的自主装备以速度移动，给定形状为b的障碍物以速度移动，自主装备边界与障碍物边界的安全距离为d且则b的危险区域为
5.闵可夫斯基和：
6.镜射：
7.障碍物膨胀：当把a简化为点时，b相对于a的危险区域为
8.所以，b在a的速度空间中的vo为
9.b在a相对于b的相对速度空间中的vo为如图1所示为障碍物膨胀与速度障碍示意图。
10.目前大多数避碰方法由于自身的局限性只能应用于避开静态障碍物或单个动态障碍物等相对简单的场景，在需要避开多个动态障碍物的情况下无法提供可靠的避碰性能。而且，部分避碰方法并没有考虑避碰规则或者考虑到的避碰规则太过复杂繁琐，使其不太适合实际工程应用。


技术实现要素：

11.为解决以上现有技术存在的问题，本发明提出一种动态环境下的避碰方法，旨在自主装备在跟踪期望轨迹的前提下遭遇到单个或多个动态、静态障碍物时，应用所提出的避碰方法根据期望速度重新规划出符合避碰规则的最优避碰速度以避开障碍物。
12.本发明可通过以下技术方案予以实现：
13.一种动态环境下的避碰方法，包括以下步骤：
14.1)将自主装备检测到的障碍物投影到速度空间并膨胀形成voo；
15.2)通过碰撞风险评估确定所述自主装备检测到的障碍物是否有与其发生碰撞的危险，如果检测到的障碍物有与自主装备发生碰撞的危险，则将其状态标记为激活，否则将其状态标记为非激活；
16.3)更新所述自主装备的状态，只要存在已激活的障碍物，自主装备就处于避碰状态，否则处于非避碰状态并跟踪期望轨迹；
17.4)所述自主装备处于避碰状态时，根据其期望速度和所有已激活的障碍物信息确定最优偏置速度方向；
18.5)对于各个已激活的障碍物，根据所述自主装备的期望速度和所述最优偏置速度方向计算其锥形voo两侧对应的两个避碰速度；
19.6)确定避碰规则；
20.7)通过所提出的所述避碰规则建立代价函数，并从先前计算的所述避碰速度中找到最优避碰速度；
21.8)所述自主装备跟踪所述最优避碰速度，直到其状态更新为非避碰。
22.进一步地，所述步骤4)具体为：将定义为平行于偏置速度的单位矢量，所述自主装备的期望速度和障碍物速度分别为和所述自主装备相对于障碍物的期望相对速度为了使所述自主装备的重规划轨迹在避障的同时更接近期望轨迹，偏置速度应尽量小，所以与所在的直线的夹角应尽可能大，当所述自主装备遭遇单个已激活的障碍物时，设置当所述自主装备遭遇多个已激活的障碍物时，根据所述自主装备与每个已激活的障碍物之间的相对速度来确定的方向。
23.进一步地，所述的方向确定方法具体为：定义其中的每个元素为所述自主装备与每个已激活障碍物之间的相对速度，n为已激活障碍物的数量，并定义2
×
n矩阵a＝[a
1 a2]
t
，其中，
[0024][0025]
然后将矩阵a根据第一行从小到大排列得到矩阵b，当所述自主装备与每个已激活障碍物的相对速度平行时，与自主装备遇到单个已激活障碍物时相同，也就是说，当b(1，1)＝b(1，n)时，否则，定义2
×
n矩阵c＝[c
1 ... c
n-1 cn]，其中]，其中并且将矩阵c根据第二行从大到小排列得到矩阵d，此时，
[0026]
进一步地，所述步骤5)具体为：考虑自主装备的最大平移速度为u
max
，和分别表示在加上方向上的偏置速度后所能达到的两个极限速度，且对于已激活的障碍物，和分别表示在加上方向上的偏置速度后所能达到的左极限速度和右极限速度；表示所述自主装备从voo左侧进行避碰时的偏置速度，且为对应的相对速度，表示自主装备从voo右侧进行避碰时
的偏置速度，且为对应的相对速度，
[0027]
和由如下条件求得：
[0028][0029]
进一步的，由于考虑到自主装备最大平移速度u
max
的限制，和被约束如下：
[0030][0031][0032]
因此，可以得到自主装备分别从voo左侧和右侧进行避碰时的避碰速度和和
[0033]
进一步地，所述避碰规则为：当需要避开障碍物时，无论障碍物从哪个方向接近，且无论障碍物是否采取避碰措施，自主装备都优先从该障碍物的voo的右侧进行避让。
[0034]
进一步地，所述步骤7)具体为：
[0035]
定义其中每个元素代表每个已激活障碍物的速度，定义其中每个元素代表所述自主装备分别从每个障碍物的voo左侧或右侧进行避碰时的避碰速度，通过建立代价函数j＝j1+j2+j3，从v
c0
中选取最优避碰速度。
[0036]
进一步地，所述代价函数中：
[0037]
通过权重w1定义1
×
2n的矩阵j1，其中通过权重w2定义1
×
2n的矩阵j2，其中如果避碰速度v
c0
(i)会使所述自主装备处于在其他障碍物的voo内，即将发生碰撞的碰撞点为(v
c0
(i)-vo(i))与该voo的最近交点，用表示从所述自主装备的当前位置到碰撞点的位置矢量，且避碰反应时间为否则tc＝w3，优先选择更长的避碰反应时间，通过权重w3定义1
×
2n的矩阵j3，其中j3(i)＝w
3-tc；因此，定义2
×
2n矩阵e＝[v
c0 j]
t
，并且将矩阵e根据第2行从小到大排列得到矩阵f，最优避碰速度即为f(1，1)，所述自主装备跟踪所述最优避碰速度，直到其状态更新为非避碰，从而达到避碰目的。
[0038]
有益效果
[0039]
1)本发明所提出的避碰规则适合所有遭遇情况且比传统的简单。
[0040]
2)本发明所提出的避碰方法能在单个或多个动态、静态障碍物环境下提供稳定可靠的避碰性能，适用范围广且简单易于实现。
附图说明
[0041]
图1为背景技术中障碍物膨胀与速度障碍示意图；
[0042]
图2-1为本发明中自主装备选择从其中一个障碍物旁边绕过去的示意图；
[0043]
图2-2为本发明中分别对应于各个已激活障碍物的避碰速度示意图；
[0044]
图3为本发明避碰方法流程图；
[0045]
图4-1为本发明中自主装备遇到单个已激活障碍物时确定最优偏置速度方向示意图；
[0046]
图4-2为本发明中自主装备遇到多个已激活障碍物时第一种情况下的示意图；
[0047]
图4-3为本发明中自主装备遇到多个已激活障碍物时第二种情况下的示意图；
[0048]
图5-1为本发明中基于自主装备的最大平移速度，和在加上方向上的偏置速度后所能达到的极限速度示意图；
[0049]
图5-2为本发明中根据自主装备的期望速度和最优偏置速度方向计算出的能避开单个已激活障碍物的避碰速度示意图；
[0050]
图6为本发明中避碰规则，其中：(a)障碍物从右侧穿越。(b)障碍物从左侧穿越。(c)超越障碍物。(d)障碍物迎头接近。(e)被障碍物超越。
具体实施方式
[0051]
以下通过特定的具体实施例说明本发明的实施方式，本领域的技术人员可由本说明书所揭示的内容轻易地了解本发明的其他优点及功效。
[0052]
本发明针对自主装备在单个或多个动态、静态障碍物环境下的避碰问题提出了一种新的避碰方法。与目前已有的避碰方法不同，该方法基于自主装备跟踪期望轨迹的前提，当需要避碰时，通过在自主装备的期望速度的基础上增加一个偏置速度得到最优避碰速度，使其跟踪最优避碰速度即可实现避碰目的。通过碰撞风险评估将与自主装备有碰撞风险的障碍物标记为激活状态。偏置速度的方向是由所有已激活障碍物信息计算得到的。当自主装备遭遇多个障碍物时，其在不与任何障碍物发生碰撞的前提下，只会选择从其中一个障碍物旁边绕过去，如图2-1所示。因此，可以分别计算出能避开单个已激活障碍物的避碰速度，如图2-2所示。然后根据所提出的避碰规则建立代价函数并从中选取最优避碰速度。
[0053]
表示矢量的弧度且其取值范围为[0，2π)。符号
‘
\’表示取余。表示矢量的模。本发明的一种动态环境下的避碰方法的流程图如图3所示，且具体步骤如下：
[0054]
1)为了便于计算避碰速度，并使自主装备在跟踪最优避碰速度时能与障碍物保持安全距离，将检测到的障碍物投影到速度空间并膨胀形成voo；
[0055]
2)通过碰撞风险评估确定自主装备检测到的障碍物是否有与其发生碰撞的危险。如果检测到的障碍物有与自主装备发生碰撞的危险，则将其状态标记为激活，否则将其状
态标记为非激活；
[0056]
3)更新自主装备的状态，只要存在已激活的障碍物，自主装备就处于避碰状态，否则处于非避碰状态并跟踪期望轨迹；
[0057]
4)所述自主装备处于避碰状态时，根据其期望速度和所有已激活的障碍物信息确定最优偏置速度方向：
[0058]
将定义为平行于偏置速度的单位矢量。自主装备的期望速度和障碍物速度分别为和自主装备相对于障碍物的期望相对速度
[0059]
为了使自主装备的重规划轨迹在避障的同时更接近期望轨迹，偏置速度应尽量小，所以与所在的直线的夹角应尽可能大。当自主装备遭遇单个已激活的障碍物时，我们可以设置如图4-1所示。当自主装备遭遇多个已激活的障碍物时，需要根据自主装备与每个已激活的障碍物之间的相对速度来确定的方向。的确定方法具体如下：定义其中的每个元素为自主装备与每个已激活障碍物之间的相对速度，n为已激活障碍物的数量。并定义2
×
n矩阵a＝[a
1 a2]
t
，其中，
[0060][0061]
然后将矩阵a根据第一行从小到大排列得到矩阵b。当自主装备与每个已激活障碍物的相对速度平行时，与自主装备遇到单个已激活障碍物时相同。也就是说，当b(1，1)＝b(1，n)时，否则，定义2
×
n矩阵c＝[c
1 ... c
n-1 cn]，其中]，其中并且将矩阵c根据第二行从大到小排列得到矩阵d。此时，图4-2，4-3分别展示了两种情况下的
[0062]
5)对于各个已激活的障碍物，根据所述自主装备的期望速度和所述最优偏置速度方向计算锥形voo两侧对应的两个避碰速度：
[0063]
考虑自主装备的最大平移速度为u
max
，和分别表示在加上方向上的偏置速度后所能达到的两个极限速度，且对于已激活的障碍物，和分别表示在加上方向上的偏置速度后所能达到的左极限速度和右极限速度，如图5-1所示。表示自主装备从voo左侧进行避碰时的偏置速度，且为对应的相对速度。表示自主装备从voo右侧进行避碰时的偏置速度，且为对应的相对速度，如图5-2所示。
[0064]
和由如下条件求得：
[0065][0066]
进一步的，由于考虑到自主装备最大平移速度u
max
的限制，和被约束如下：
[0067][0068][0069]
因此，可以得到自主装备分别从voo左侧和右侧进行避碰时的避碰速度和和
[0070]
6)确定避碰规则
[0071]
在实际遭遇中，障碍物可能是静态或动态的，其中动态障碍物可能对自主装备采取或不采取避碰措施。为了使自主装备的避碰措施能适应所有遭遇情况，假设如下避碰规则：当需要避开障碍物时，无论障碍物从哪个方向接近，且无论障碍物是否采取避碰措施，自主装备都优先从该障碍物的voo的右侧进行避让，如图6所示(图中1为自主装备，2为障碍物，其中：(a)障碍物从右侧穿越。(b)障碍物从左侧穿越。(c)超越障碍物。(d)障碍物迎头接近。(e)被障碍物超越)。该避碰规则的优势主要有两点：1、不需要对遭遇情况进行判断，规则简单，有利于避碰方法的实现。2、如果遭遇双方都遵守该避碰规则，则它们会互相避让，从而降低避让方的避让压力。
[0072]
7)通过所提出的避碰规则建立代价函数，并从先前计算的避碰速度中找到最优避碰速度：定义其中每个元素代表每个已激活障碍物的速度。定义其中每个元素代表自主装备分别从每个障碍物的voo左侧或右侧进行避碰时的避碰速度。通过建立代价函数j＝j1+j2+j3，从v
c0
中选取最优避碰速度。代价函数考虑如下三个方面：
[0073]
1)偏置速度的大小
[0074]
为了降低自主装备的避碰压力，优先选择更小的偏置速度。通过权重w1定义1
×
2n的矩阵j1，其中
[0075]
2)避碰规则
[0076]
当自主装备需要避开动态障碍物时，优先从该障碍物的voo右侧进行避碰。通过权重w2定义1
×
2n的矩阵j2，其中
[0077]
3)即将与其他障碍物发生碰撞的反应时间
[0078]
如果避碰速度v
c0
(i)会使自主装备处于在其他障碍物的voo内，即将发生碰撞的碰撞点为(v
c0
(i)-vo(i))与该voo的最近交点，用表示从自主装备的当前位置到碰撞点的位置矢量，且避碰反应时间为否则tc＝w3。优先选择更长的避碰反应时间。通过权重w3定义1
×
2n的矩阵j3，其中j3(i)＝w
3-tc。
[0079]
因此，定义2
×
2n矩阵e＝[v
c0 j]
t
，并且将矩阵e根据第2行从小到大排列得到矩阵f。最优避碰速度即为f(1，1)。
[0080]
8)自主装备跟踪最优避碰速度，直到其状态更新为非避碰。
[0081]
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。