一种用于移动机器人运动规划的碰撞检测方法

1.本发明属于移动机器人运动规划技术领域，具体公开了一种用于移动机器人运动规划的碰撞检测方法。


背景技术：

2.随着移动机器人智能化程度的不断提高，其已广泛应用于家庭服务、景区导游、车间搬运、移动加工、零件装配、无人驾驶等场景。这些应用场景的普遍特点为工作空间大、空间中障碍物多、运动路径复杂，因此快速高效安全的运动规划技术是提升移动机器人智能化水平的关键因素。基于概率采样的运动规划算法不受机器人c-空间维度限制，可广泛应用于复杂场景规划问题，其已在操作臂机器人运动规划问题中发挥重大应用价值。
3.概率采样规划算法方法简单，相较于图搜索、人工势函数等方法，该方法无需精确表示机器人c-空间障碍物，其主要包含2个步骤：(1)在机器人c-空间中随机采样，并搜索采样点与之前采样点之间的连接路径；(2)采样点与连接路径的碰撞检测。由概率采样规划算法的步骤可以看出，针对大空间多障碍物运动规划问题，碰撞检测效率是制约其广泛大规模应用的主要因素。因此，大空间范围内碰撞检测效率是移动机器人领域亟需解决的问题。


技术实现要素：

4.针对现有技术的缺陷和改进需求，本发明提供了一种用于移动机器人运动规划的碰撞检测方法，其目的在于提升大空间范围内碰撞检测的效率以扩展概率采样规划方法在移动机器人领域的应用能力。本发明利用平面凸多边形精确近似移动机器人与障碍物的几何形状，在现有概率采样规划算法的步骤中，利用碰撞检测结果构造危险圆，进而将常规复杂的碰撞检测问题简化为点-圆包含测试问题，而点-圆包含测试被认为是最简单的碰撞检测方法。因此本发明提供的快速碰撞检测方法可显著提升碰撞检测效率进而大大提升概率采样方法在大空间范围内的计算速度。
5.为实现上述目的，第一方面，本发明提供了一种用于移动机器人运动规划的碰撞检测方法，包括如下步骤：
6.s1，采用平面凸多边形近似移动机器人与各障碍物的几何形状，分别记为图形a和图形bm，并获取各图形的顶点坐标及每条边的外法矢；其中，m＝1,2,
…
,m，m为障碍物总数量；
7.s2，对图形a的位置随机采样，若数据集s
obs
为空，执行s3，否则，执行s4；所述数据集s
obs
为图形a和图形bm相交的采样点的集合；
8.s3，若图形a和图形bm在随机采样点x处相交，则移动机器人与障碍物发生碰撞，并对图形a中每一顶点，计算该顶点到图形bm所有边的符号距离的最大值，从各最大值中任取一个小于零的值，将该小于零的值对应的图形a的顶点作为渗入点，并以其绝对值为半径、以所述渗入点为圆心，构造危险圆，所述符号距离为图形bm某条边的顶点到所述该顶点的向量与所述某条边的外法矢的内积；再将x以及所述危险圆、渗入点存储在所述数据集s
obs
中；否则，不发生碰撞；
9.s4，从所述数据集s
obs
中提取与随机采样点x距离最近的采样点xo，并根据xo提取对应的危险圆和渗入点；若xo对应的渗入点通过运动学变换在x处的坐标在所述对应的危险圆内部，则移动机器人与障碍物在x处发生碰撞；否则，执行步骤s3；
10.s5，重复执行步骤s2至s4，直至完成运动规划。
11.进一步地，所述步骤s4中，从所述数据集s
obs
中提取与随机采样点x距离最近的采样点xo，包括：
[0012][0013][0014]
d(x,x
′
)＝d1(x,x
′
) 0.5d2(x,x
′
)
[0015][0016]
式中，(x，y)和(x
′
,y
′
)分别表示采样点x、x＇处的平移向量，t和t
′
分别表示采样点x、x＇处的旋转角度，d1(x,x
′
)表示采样点x、x＇的平移距离，d2(x
,
x
′
)表示采样点x、x＇的角度距离，d(x
,
x
′
)表示采样点x、x＇的距离，xo表示数据集s
obs
中与x距离最近的采样点。
[0017]
进一步地，所述步骤s3中，若图形a和图形bm在随机采样点x处相交，相应的渗入点通过以下方式求解：
[0018][0019][0020]
式中，和分别为图形a和图形bm在随机采样点x处各顶点坐标，na和nb表示顶点个数；n
bj
表示图形bm第j条边的外法矢，表示向量n
bj
与a
i-bj的内积，p
ai
表示图形bm所有顶点到图形a顶点ai的向量与n
bj
的内积的最大值，pa表示渗入点；
[0021]
图形bm中危险圆的构造方法为：以pb＝pa为圆心，rb＝||p
ai
||为半径构造危险圆sb。
[0022]
进一步地，所述步骤s4中，若满足下式，说明xo对应的渗入点通过运动学变换在x处的坐标在所述对应的危险圆内部：
[0023]
||r
xox
pa t
xox-pb||≤rb[0024]
式中，表示欧式距离，顶点pb是危险圆sb的圆心，rb为sb的半径，表示图形a由采样点xo(xo，yo，to)运动至x(x，y，t)的旋转变换矩阵，表示平移向量，计算公式为：
[0025]
[0026][0027]
进一步地，所述步骤s1中，平面凸多边形第i条边ei连接第i个顶点vi和第i 1个顶点v
i 1
，根据顶点坐标vi(xi，yi)和v
i 1
(x
i 1
，y
i 1
)，ei的外法矢方向ni的计算公式为：
[0028][0029]
进一步地，所述步骤s2中，图形a的随机采样点x(x，y，t)的产生方式为：
[0030]
x＝w
×
uniform(0,1),y＝h
×
uniform(0,1),t＝π
×
uniform(-1,1)
[0031]
上式中，w、h表示矩形约束空间的边长，uniform(a,b)表示产生a、b间均匀分布随机数，(x，y)表示移动机器人平移向量，t表示旋转角度。
[0032]
进一步地，所述步骤s3中，通过以下方式判断图形a和图形bm在随机采样点x处是否相交：
[0033]
若da和db均小于零，则图形a和图形bm在随机采样点x处相交，否则不相交；其中，
[0034][0035][0036]
式中，和分别为图形a和图形bm在随机采样点x处各顶点坐标，na和nb表示顶点个数；n
ai
表示图形a第i条边的外法矢，表示向量n
ai
与b
j-ai的内积，d
ai
表示图形a顶点ai到图形bm所有顶点的向量与n
ai
的内积的最小值，da表示由图形a所有边的外法矢计算得到的d
ai
中的最大值；n
bj
表示图形bm第j条边的外法矢，表示向量n
bj
与a
i-bj的内积，d
bj
表示图形bm顶点bj到图形a所有顶点的向量与n
bj
的内积的最小值，db表示由图形bm所有边的外法矢计算得到的d
bj
中的最大值。
[0037]
第二方面，本发明提供了一种用于移动机器人运动规划的碰撞检测装置，包括：
[0038]
获取模块，用于采用平面凸多边形近似移动机器人与各障碍物的几何形状，分别记为图形a和图形bm，并获取各图形的顶点坐标及每条边的外法矢；其中，m＝1,2,
…
,m，m为障碍物总数量；
[0039]
采样模块，用于对图形a的位置随机采样，若数据集s
obs
为空，执行第一检测模块的操作，否则，执行第二检测模块的操作；所述数据集s
obs
为图形a和图形bm相交的采样点的集合；
[0040]
所述第一检测模块，用于判断图形a和图形bm在随机采样点x处是否相交，若是，则移动机器人与障碍物发生碰撞，并对图形a中每一顶点，计算该顶点到图形bm所有边的符号距离的最大值，从各最大值中任取一个小于零的值，将该小于零的值对应的图形a的顶点作为渗入点，并以其绝对值为半径、以所述渗入点为圆心，构造危险圆，所述符号距离为图形bm
某条边的顶点到所述该顶点的向量与所述某条边的外法矢的内积；再将x以及所述危险圆、渗入点存储在所述数据集s
obs
中；若否，则不发生碰撞；
[0041]
所述第二检测模块，用于从所述数据集s
obs
中提取与随机采样点x距离最近的采样点xo，并根据xo提取对应的危险圆和渗入点；若xo对应的渗入点通过运动学变换在x处的坐标在所述对应的危险圆内部，则移动机器人与障碍物在x处发生碰撞；否则，执行所述第一检测模块的操作；
[0042]
重复模块，用于重复执行所述采样模块、第一检测模块和第二检测模块的操作，直至完成运动规划。
[0043]
第三方面，本发明提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序被处理器运行时控制所述计算机可读存储介质所在设备执行如第一方面所述的用于移动机器人运动规划的碰撞检测方法。
[0044]
总体而言，通过本发明所构思的以上技术方案，能够取得以下有益效果：
[0045]
(1)本发明根据两个相交的多边形，寻找移动机器人渗入障碍物中的顶点，并在障碍物内部构造危险圆，再将相应的采样点、渗入点和危险圆存入数据集中；然后，从数据集中提取与随机采样点x距离最近的采样点xo以及对应的危险圆和渗入点；接着，判断xo对应的渗入点通过运动学变换在x处的坐标是否仍在xo对应的危险圆内部(危险圆在障碍物内部，渗入点是移动机器人的顶点，若渗入点依然在危险圆内部，则移动机器人必然与障碍物发生碰撞)，进而将常规复杂的碰撞检测问题简化为点-圆包含测试问题，可显著提升碰撞检测效率进而大大提升概率采样方法在大空间范围内的计算速度。
[0046]
(2)内部危险圆的构造可看作是对复杂几何形状物体的球形近似，并且本发明提供的方法是一种在线构造方法。即本发明不需要提前将物体用球形近似，而是在算法执行过程中实时在线构造危险圆，并且只在需要的时候(即无法判断采样点是否碰撞)构造危险圆。因此，随着危险圆的数量不断增加，机器人c-空间危险区域被逼近的越来越准确，新的采样点通过危险圆方法即可判断其碰撞状态，而无需再采用常规复杂的碰撞检测方法。
附图说明
[0047]
图1为本发明实施方式提供的快速碰撞检测方法流程图；
[0048]
图2(a)和图2(b)为本发明实施方式提供的移动机器人与障碍物多边形近似示意图；
[0049]
图3为本发明实施方式提供的移动机器人c-空间采样与障碍物碰撞检测的说明示意图；
[0050]
图4(a)和图4(b)为本发明实施方式提供的两物体相交判断的示意图；
[0051]
图5为本发明实施方式提供的由相交两物体求解得到的障碍物内部危险圆；
[0052]
图6为本发明实施方式提供的快速碰撞检测准则示意图。
具体实施方式
[0053]
为了使本发明的目的、系统组成、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限制本发明。此外，下面所述的本发明的各个实施方式中所涉及到的技术特
征只要彼此之间未构成冲突就可以相互结合。
[0054]
在本发明中，本发明及附图中的术语“第一”、“第二”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。
[0055]
参阅图1，结合图2(a)至图6，本发明提供了一种用于移动机器人运动规划的碰撞检测方法，包括操作s1至s6。
[0056]
操作s1，采用平面凸多边形近似移动机器人与各障碍物的几何形状，分别记为图形a和图形bm，并获取各图形的顶点坐标及每条边的外法矢；其中，m＝1,2,
…
,m，m为障碍物总数量。
[0057]
如图2(a)和图2(b)所示，以三角形和矩形为例，多边形顶点采用逆时针顺序存储，具体地，多边形第i条边ei连接第i个顶点vi和第i 1个顶点v
i 1
，根据顶点坐标vi(xi，yi)和v
i 1
(x
i 1
，y
i 1
)，ei的外法矢方向ni的计算公式为：
[0058][0059]
操作s2，对图形a的位置随机采样，若数据集s
obs
为空，执行步骤s3，否则，执行步骤s4；所述数据集s
obs
为图形a和图形bm相交的采样点的集合。
[0060]
如图3所示，该环境为移动机器人可行使空间，全局坐标系定义在矩形约束空间左下角。移动机器人随机采样点x(x，y，t)的产生方式为：
[0061]
x＝w
×
uniform(0,1),y＝h
×
uniform(0,1),t＝π
×
uniform(-1,1)
[0062]
上式中，w、h表示矩形约束空间的边长，uniform(a,b)表示产生a、b间均匀分布随机数，(x，y)表示移动机器人平移向量，t表示旋转角度。
[0063]
由随机采样点x通过正运动学变换，可知移动机器人在工作空间中的位置形状，具体地，机器人顶点坐标vi′
与边界外法矢向量ni′
计算公式为：
[0064][0065]
操作s3，若图形a和图形bm在随机采样点x处相交，则移动机器人与障碍物发生碰撞，并对图形a中每一顶点，计算该顶点到图形bm所有边的符号距离的最大值，从各最大值中任取一个小于零的值，将该小于零的值对应的图形a的顶点作为渗入点，并以其绝对值为半径、以所述渗入点为圆心，构造危险圆，所述符号距离为图形bm某条边的顶点到所述该顶点的向量与所述某条边的外法矢的内积；再将x以及所述危险圆、渗入点存储在所述数据集s
obs
中；否则，不发生碰撞。
[0066]
以移动机器人在随机采样点x处为例，常规碰撞检测方法需要依次判断移动机器人与某个障碍物间是否碰撞，具体地；通过以下方式判断图形a和图形bm在随机采样点x处是否相交：
[0067]
[0068][0069]
式中，和分别为图形a和图形bm在随机采样点x处各顶点坐标，na和nb表示顶点个数；n
ai
表示图形a第i条边的外法矢，表示向量n
ai
与b
j-ai的内积，d
ai
表示图形a顶点ai到图形bm所有顶点的向量与n
ai
的内积的最小值，da表示由图形a所有边的外法矢计算得到的d
ai
中的最大值；n
bj
表示图形bm第j条边的外法矢，表示向量n
bj
与a
i-bj的内积，d
bj
表示图形bm顶点bj到图形a所有顶点的向量与n
bj
的内积的最小值，db表示由图形bm所有边的外法矢计算得到的d
bj
中的最大值。
[0070]
若da和db均小于零，则图形a和图形bm在随机采样点x处相交，否则不相交。
[0071]
以图4(a)和图4(b)所示两矩形相交测试为例，d
b1
的求解过程为：
[0072][0073][0074][0075]
图4(a)中d
b1
大于零，因此图形a和图形bm不相交。而图4(b)中d
b1
小于零，因此需要利用相同的方式计算出d
b2
，d
b3
，d
b4
，并得到db＝max(d
b1
，d
b2
，d
b3
，d
b4
)，以判断图形a和图形bm是否相交。
[0076]
进一步地，若图形a和图形bm在随机采样点x处相交，则说明移动机器人与障碍物发生碰撞，相应的渗入点通过以下方式求解：
[0077][0078][0079]
式中，和分别为图形a和图形bm在随机采样点x处各顶点坐标，na和nb表示顶点个数；n
bj
表示图形bm第j条边的外法矢，表示向量n
bj
与a
i-bj的内积，p
ai
表示图形bm所有顶点到图形a顶点ai的向量与n
bj
的内积的最大值，pa表示渗入点，即满足p
ai
小于零所对应的顶点ai。
[0080]
图形bm中危险圆的构造方法为：以pb＝pa为圆心，rb＝p
ai
为半径构造危险圆sb。图5展示了构造的危险圆sb，由上述计算可知sb完全在障碍物b的内部。
[0081]
以图4(b)所示的相交的矩形机器人a与矩形障碍物b为例，p
a1
的求解过程为：
[0082][0083][0084]
p
a1
＝max(p
a1-1
,p
a1-2
,p
a1-3
,p
a1-4
)
[0085]
由图4(b)可以看出，p
a1
小于零，即顶点a1在障碍物b的内部，因此pa＝a1。
[0086]
操作s4，从所述数据集s
obs
中提取与随机采样点x距离最近的采样点xo，并根据xo提取对应的危险圆和渗入点；若xo对应的渗入点通过运动学变换在x处的坐标在所述对应的危险圆内部，则移动机器人与障碍物在x处发生碰撞；否则，执行步骤s3。
[0087]
本实施例中，操作s4包括子操作s41和s42。
[0088]
在子操作s41中，通过以下方式从数据集中提取与随机采样点x距离最近的采样点xo：
[0089][0090][0091]
d(x,x
′
)＝d1(x,x
′
) 0.5d2(x,x
′
)
[0092][0093]
式中，(x，y)和(x
′
,y
′
)分别表示采样点x、x＇处的平移向量，t和t
′
分别表示采样点x、x＇处的旋转角度，d1(x,x
′
)表示采样点x、x＇的平移距离，d2(x
,
x
′
)表示采样点x、x＇的角度距离，d(x
,
x
′
)表示采样点x、x＇的距离，xo表示数据集s
obs
中与x距离最近的采样点。
[0094]
在子操作s42中，判断xo对应的渗入点通过运动学变换在x处的坐标是否仍在所述对应的危险圆内部。
[0095]
如图6所示，若满足下式，说明xo对应的渗入点通过运动学变换在x处的坐标在所述对应的危险圆内部：
[0096][0097]
式中，表示欧式距离，顶点pb是危险圆sb的圆心，rb为sb的半径，表示图形a由采样点xo(xo，yo，to)运动至x(x，y，t)的旋转变换矩阵，表示平移向量，计算公式为：
[0098][0099]
进一步地，若危险圆方法不足以判断新采样点是否碰撞，则说明危险圆覆盖区域不够大，则利用传统碰撞检测方法的结果实时在线扩大其覆盖区域，具体步骤参阅步骤s3中关于判断图形a和图形bm在随机采样点x处是否相交的具体实施方式，此处不再赘述。
[0100]
操作s5，重复执行步骤s2至s4，直至完成运动规划。
[0101]
需要说明的是，本发明中构造的障碍物内部危险圆是在概率采样规划算法执行过程中实时在线构造的；对于新的采样点，从已记录的发生碰撞的采样点中选择最近点，并根据最近碰撞点提取出相应的障碍物内部危险圆与移动机器人渗入点，然后通过运动学变换判断在新采样点处渗入点是否依然在危险圆内部；若危险圆方法不足以判断新采样点是否碰撞，则说明危险圆覆盖区域不够大，则利用传统碰撞检测方法的结果实时在线扩大其覆
盖区域。
[0102]
因此，随着危险圆的数量不断增加，机器人c-空间危险区域被逼近的越来越准确，新的采样点通过步骤s4即可判断其碰撞状态，而无需再经过常规复杂的步骤s3。
[0103]
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。