一种基于光线跟踪的移动机器人路径规划方法

1.本发明涉及移动机器人技术领域，尤其涉及一种基于光线跟踪的移动机器人路径规划方法。


背景技术：

2.移动机器人的路径规划是指从一幅已知地图中规划出一条从起始位置到目标位置的连续无碰撞路径。目前存在诸多的路径规划算法，按照规划结果是否为最优可以分为最优化算法和非最优化算法，总体说来，目前存在的路径规划算法存在如下问题：
①
只考虑到了机器人在运动过程中位置变化的连续性，没有考虑机器人速度变化的连续性，限制了机器人的运动速度；
②
规划出的路径有可能距离障碍物很近而降低了机器人运动的安全性。


技术实现要素：

3.为解决上述问题，本发明公开了提出一种基于光线跟踪的移动机器人路径规划方法，其所规划出的路径均是由长且笔直的路径直线段与贝塞尔曲线段拼接而成，在其连接处机器人运动的速度不变，且路径远离障碍物和地图边界，因而可以实现机器人运动速度的连续变化，适合于机器人进行高速运动，并且大大提高了机器人运动过程中的安全性。
4.本发明中基于光线跟踪的移动机器人路径规划方法，包括两个阶段：
5.步骤(1)光线传播阶段；首先对起始点发射出的光线条数进行进行初始化，并对目标点进行膨胀使其覆盖一定的区域范围以便光线能够更加容易的照射上去，从而加快算法的收敛速度；然后在光线传播阶段，各条光线在环境之中沿直线传播并在障碍物和地图边界处发生漫反射，直到有一条光线成功经过目标区域；
6.将机器人的起始位置视作一个点光源，将机器人目标位置视作一个等待被照亮的物体并进行适当膨胀，其中被视作点光源的起始位置会发射出n条光线，n为正整数，该参数的设定随机器人所处的场景而定，如果机器人所处的场景较为宽敞，则该参数应设置得大一些，以保证尽可能多的覆盖全部区域，如果机器人所处的环境较为逼仄，则该参数应当设置的小一些，以避免不必要的计算量。同时n的大小对算法的收敛速度也具有一定影响，n越大则找到一条可行的原始路径的速度就越快，即算法收敛越快。
7.目标位置在初始化阶段应该进行适当的膨胀，以增大光线经过目标区域的概率，同时膨胀后的目标区域不能够包含有任何障碍物或者超出地图范围以外的区域。建议膨胀后的目标区域应该尽可能将其所在的自由通道(比如走廊)完全堵住，以保证不会有任何光线恰好从目标区域旁边经过，以至于错失找到可行路径的机会。
8.将地图的边界和障碍物视作粗糙的物体是指在这些区域发生的是漫反射，即光线新的传播方向是随机产生的，而不会像在光滑物体表面那样发生镜面反射，因为镜面反射的反射方向是固定的，而在粗糙物体表面的不同位置，漫反射的反射方向是随机的。
9.步骤(2)路径优化阶段；包括步骤(21)不相邻的入射点直接相连以祛除不必要的
入射点；检查某两个点之间是不是可以直接连接是指检查存在不存在一条光线从其中一点沿直线传播到另一点，如果能够直接连接，则说明这两点之间可以用一条光线直线连接，而无需等光线在多次反射之后才连接；如果不能够直接连接，说明以这两点为端点的线段会经过障碍物，因此光线必须在经过多次反射之后才能够连接这两点。这一步的目的是为了尽量避免机器人频繁的改变自己的前进方向。
10.步骤(22)路径直线段是相对于路径点来说的，由于光的直线传播，每两个入射点之间是通过一条直线段相连的，这一条直线段称为路径直线段，而将发生反射的位置即入射点称为路径点，使用贝塞尔曲线对各个路径直线段连接处的轨迹进行平滑。
11.进一步优选的，所述步骤1的具体操作包括：
12.步骤(11)：初始化设置：最开始，假设场景为一个完全黑暗的场景，将起始位置看做一个点光源，地图的边界和障碍物看做粗糙的物体，且会对入射的光线发生漫反射，因而光线会在场景中不断被反射；这一条经过目标区域的光线所走过的路径就是一条从起始位置到目标位置的一条原始可行路径；
13.步骤(12)：光线传播：在传播的过程中，在发生漫反射的位置会存在一个入射点，在生成的原始路径中，这些入射点将被称为路径点；使用一个vector容器来保存这些入射点，由于入射点与入射点之间一定是可以用直线直接连通的，因此最后对这些入射点之间的部分进行线性插值即可获得一个连续的可行路径；且为了模拟漫反射，在入射点处为入射光线随机选择反射方向。
14.进一步优选的，所述步骤(11)初始化具体包括：
15.步骤(111)设置从起始位置(点光源)发射出的光线条数为n；
16.步骤(112)设置对目标位置进行膨胀的幅度为dis
swell
，以目标位置为圆心，将其四周360
°
角按照一定的角度间隔α划分为n个sector，显然，α＝360
°
/n；
17.步骤(113)沿着每一个sector向圆外探索最大距离dis
swell
，如果中途遇到障碍物，则停止探索；最终所有探索过的区域共同组成目标区域。
18.进一步优选的，对每一条光线，首先，在入射点处为该光线随机选择反射方向，在开始的时候，入射点就是起始位置，这是为了模拟漫反射。同时，在同一个入射点处，即便是来自同一个方向的入射光线的反射方向也有可能是不同的，因为每一条光线的反射方向是随机产生的，这是为了增强算法对各种多样化环境的鲁棒性；然后，该光线将沿着随机产生的反射方向传播。
19.检查某两个点之间是不是可以直接连接是指检查存在不存在一条光线从其中一点沿直线传播到另一点，如果能够直接连接，则说明这两点之间可以用一条光线直线连接，而无需等光线在多次反射之后才连接；如果不能够直接连接，说明以这两点为端点的线段会经过障碍物，因此光线必须在经过多次反射之后才能够连接这两点。这一步的目的是为了尽量避免机器人频繁的改变自己的前进方向。
20.进一步优选的，在传播的过程中，每传播一个单位长度，执行如下操作：
21.步骤(121)：检查目前是不是经过目标区域，如果经过，则将目标位置压入对应光路ray_cluster，这一条光路中保存的是该光线所走过的路径上的所有入射点，并返回该条光路在所有光路中的索引并终止所有的光线的传播；
22.步骤(122)检查是不是碰到障碍物和地图边界，如果碰到，则将该位置作为入射点
压入对应光路ray_cluster，若为串行执行，那么此时需要暂时中止该光线的传播，并对下一条光线执行传播动作；若是并行实现，则需要为该光线重新选择传播方向。并行实现可以使得所有光线同时传播以加快算法的收敛速度。
23.步骤(123)否则，继续传播。
24.进一步优选的，步骤(2)中直到某一条光线经过目标区域是指此时存在一条光线在经过多次反射之后，能够成功地将起始位置和目标位置连接起来，这一条光线所走过的路径便是我们后续需要对其进行优化的原始路径。
25.进一步优选的，所述步骤21：具体的操作过程为：从某一个入射点处向着排在其前面的不相邻入射点直接发射一条光线，如果这条光线可以不经反射而直接到达不相邻的入射点，则说明这两个入射点其实是可以直接相连的，也就意味着这两个入射点之间的那些入射点均是不必要的入射点，应当予以祛除；不断地向位于其前面的入射点发射光线，直到遇到一个不能直接相连的入射点，则停止试探；针对其他入射点重复此过程即可消除所有的不必要入射点。
26.设置location为直线ray_cluster的最后一个元素，也就是目标位置，作为试探的起点；
27.(1)设置probe＝location-2来标记被试探位置，也就是试探的终点；
28.(2)从起点向终点发射一条试探光线；
29.(3)如果试探光线不经反射即成功到达被试探位置，且probe》0，则 probe
‑‑
，重复步骤3)；
30.(4)如果试探光线不经反射即成功到达被试探位置，且probe＝0，则删去ray_cluster中probe与location之间的所有入射点，此时，probe 与location直接相连；
31.(5)如果试探光线不经反射无法到达终点，则将porbe+1与location 之间的入射点删去使probe+1与location直接相连，同时设置 location＝probe+1，probe＝location-2，重复步骤3)。
32.进一步优选的，所述步骤22：相对于路径点来说的，由于光的直线传播，每两个入射点之间是通过一条直线段相连的，这一条直线段称为路径直线段，而将发生反射的位置即入射点称为路径点；对路径点附近的路径进行平滑。因为根据光的反射定律，入射点一定位于光的传导介质与反射镜面的交界处。因此，本算法中的入射点都具有这样的特性：1)靠近地图边界或者障碍物；2)在入射点处机器人的前进方向会发生剧烈改变。这两点对于机器人的运动是非常不利的。使用三次贝塞尔曲线对路径进行平滑，这是因为三次贝塞尔曲线具有如下优点：
33.(i)贝塞尔曲线不经过中间的控制点，可以很好地避开不安全的入射点；
34.(ii)贝塞尔曲线可以保证在起点处与第一个控制点具有相同的速度，在终点处与最后一个控制点具有相同的速度，因而可以保证机器人运动速度的连续变化。
35.(iii)相比于圆弧，三次贝塞尔曲线具有更小的曲率和更大的曲率半径，在机器人转弯时更加不容易发生侧翻，因而可以允许更大的转弯速度。
36.具体的平滑过程如：
37.在入射点e处做其角平分线，计算入射点e与其前一个入射点之间的距离dis1，与后一个点之间的距离dis2，并计算dis1和dis2分别在角平分线上的投影的最小值
38.dis
min_project
＝min{dis1cos(θ),dis2cos(θ)},
39.取dis
propa
＝min{dis
max_propagate
,dis
min_project
}，其中 dis
max_propagate
为在路径平滑阶段设定的光线最远传播距离，如果传播dis
propa
距离后仍然没有碰到障碍物，则认为足够安全，并返回这一段路径的中点f；
40.三次贝塞尔曲线的表达式为：
41.b(t)＝(1-t)3p0+3t(1-t)2p1+3t2(1-t)p2+t3p3ꢀꢀ
t∈[0,1]
[0042]
(1)
[0043]
其中，p0、p1、p2、p3是三次贝塞尔曲线的四个控制点，分别称为起点、中间点1、中间点2、终点；存在
[0044][0045]
当f点为在入射点e处沿着角平分线传播一定距离后，返回的该段传播路径的中点；本算法需要根据点f、入射点e、入射光线以及反射光线，求解出三次贝塞尔曲线的控制点p0、p1、p2和p3；为了简便，我们假设控制点p0、p1与控制点p2、p3关于角平分线ef对称，那么可以过点f做角平分线的垂线分别交入射光线于p1，交反射光线于p2；在入射光线上找到一点p0，使得p1是p0e的中点；在反射光线上找到一点p3，使得p2是p3e的中点。那么有如下关系存在：
[0046]
p0p1＝p1e＝p2e＝p2p3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0047]
p1f＝p2f
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0048]
控制点p0、p1、p2和p3，由这四个控制点所决定的三次贝塞尔曲线将替代p
0-e-p3段路径以实现优化目的。
[0049]
本发明的有益效果：
[0050]
1、模拟现实世界中的照明现象和漫反射规律，利用光沿直线传播的物理学特性，首先获得一条连接起始位置和目标位置的原始路径，然后祛除不必要的入射点并利用三次贝塞尔曲线对路径进行平滑以提高路径的品质。
[0051]
2、本发明规划出的路径长且笔直，较少弯折，各个路径直线段之间以三次贝塞尔曲线平滑连接，可以保证机器人运动过程中的速度的连续变化，因而非常适合机器人进行高速运动；并且远离障碍物和地图边界，更加安全；
[0052]
3、在算法的运行速度方面，本算法的运行速度要快于rrt算法，且算法运行时间比rrt算法的运行时间更加稳定。
[0053]
4、本发明非常适合并行实现，使得各条光线同时进行传播可以使得算法的收敛速度进一步加快。
附图说明
[0054]
图1用于说明在路径优化阶段为什么需要首先祛除不必要的入射点((a)传播阶段所得路径；(b)优化阶段优化之后的路径)；
[0055]
图2用于说明的在祛除不必要的入射点之后，为什么需要对路径直线段交点附近的路径进行优化和优化之后的结果；
[0056]
图3(a)用于说明贝塞尔曲线的几何意义，(b)用于推导三次贝塞尔曲线的四个控制点p0、p1、p2、p3；
[0057]
图4是本发明中基于光线跟踪的移动机器人路径规划算法流程图；
[0058]
图5是本算法初始化阶段的伪代码；
[0059]
图6是本算法光线传播阶段的流程图；
[0060]
图7是本算法祛除不必要入射点阶段的流程图；
[0061]
图8是本算法使用三次贝塞尔曲线对路径进行平滑过程的流程图；
[0062]
图9、本发明算法的说明示意图；
[0063]
(a)算法说明示意图。本算法把路径规划问题转化为一个“照明”问题，起始位置作为光源发射光线，光线经过多次反射之后经过目标区域从而照亮目标区域。这一条光路就是本算法的原始路径；
[0064]
(b)光线传播阶段获得的一条原始路径；
[0065]
(c)祛除不必要入射点之后的路径；
[0066]
(d)经过三阶贝塞尔曲线平滑之后的最终路径。
具体实施方式
[0067]
下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。需要说明的是，下面描述中使用的词语“前”、“后”、“左”、“右”、“上”和“下”指的是附图中的方向，词语“内”和“外”分别指的是朝向或远离特定部件几何中心的方向。
[0068]
本实施例的一种基于光线传播的移动机器人路径规划算法，如图 4所示，首先对起始点发射出的光线条数进行进行初始化，并对目标点进行膨胀使其覆盖一定的区域范围以便光线能够更加容易的照射上去，从而加快算法的收敛速度；然后在光线传播阶段，各条光线在环境之中沿直线传播并在障碍物和地图边界处发生漫反射，直到有一条光线成功经过目标区域，本阶段即告终止；随后通过尝试将不相邻的入射点直接相连以祛除不必要的入射点；最后使用三次贝塞尔曲线对路径进行平滑以获得一条较好的路径。具体包括以下步骤：
[0069]
(1)初始化
[0070]
1)设置从起始位置(点光源)发射出的光线条数为n；
[0071]
2)设置对目标位置进行膨胀的幅度为dis
swell
，以目标位置为圆心，将其四周360
°
角按照一定的角度间隔α划分为n个sector，显然，α＝360
°
/n；
[0072]
3)沿着每一个sector向圆外探索最大距离dis
swell
，如果中途遇到障碍物，则停止探索；最终所有探索过的区域共同组成目标区域。
[0073]
该步骤的伪代码见图5所示。
[0074]
(2)光线传播
[0075]
针对每一条光线，首先，在入射点处为该光线随机选择反射方向，在开始的时候，入射点就是起始位置，这是为了模拟漫反射。同时，在同一个入射点处，即便是来自同一个方向的入射光线的反射方向也有可能是不同的，因为每一条光线的反射方向是随机产生的，这是为了增强算法对各种多样化环境的鲁棒性；然后，该光线将沿着随机产生的反射方
向传播。
[0076]
在传播的过程中，每传播一个单位长度，执行如下操作：
[0077]
1)检查目前是不是经过目标区域，如果经过，则将目标位置压入对应光路ray_cluster，这一条光路中保存的是该光线所走过的路径上的所有入射点，并返回该条光路在所有光路中的索引并终止所有的光线的传播；
[0078]
2)检查是不是碰到障碍物和地图边界，如果碰到，则将该位置作为入射点压入对应光路ray_cluster，若为串行执行，那么此时需要暂时中止该光线的传播，并对下一条光线执行传播动作；若是并行实现，则需要为该光线重新选择传播方向。并行实现可以使得所有光线同时传播以加快算法的收敛速度。
[0079]
3)否则，继续传播。
[0080]
该步骤的流程图如图6所示。
[0081]
祛除不必要的入射点，1)祛除不必要的入射点
[0082]
本阶段首先祛除不必要的入射点。这一步是非常必要的，因为如图1所示的类似于走廊的场景对于机器人来说是非常常见的。在走廊中，有一条光线经过多次漫反射之后成功照亮目标，但是很明显这一条路径并不是一条较好的路径，因为光线的多次反射也就意味光线所走过的路径中有多个入射点，而这些入射点并不都是必要的(比如图 1(a)中的a、b、c、d)，因此本算法必须祛除那些不必要的入射点，保留必要的入射点。
[0083]
在传播阶段，本算法由于使用一个vector容器ray_cluster来按顺序存储传播过程中的入射点，不可避免的存储了一些多余的入射点，祛除不必要的入射点即是要祛除这一个ray_cluster中的多余的入射点。该过程如下所述：
[0084]
1)设置location为直线ray_cluster的最后一个元素，也就是目标位置，作为试探的起点；
[0085]
2)设置probe＝location-2来标记被试探位置，也就是试探的终点；
[0086]
3)从起点向终点发射一条试探光线；
[0087]
4)如果试探光线不经反射即成功到达被试探位置，且probe》0，则probe
‑‑
，重复步骤3)；
[0088]
5)如果试探光线不经反射即成功到达被试探位置，且probe＝0，则删去ray_cluster中probe与location之间的所有入射点，此时， probe与location直接相连；
[0089]
6)如果试探光线不经反射无法到达终点，则将porbe+1与 location之间的入射点删去使probe+1与location直接相连，同时设置location＝probe+1，probe＝location-2，重复步骤3)；
[0090]
该步骤的流程图如图7所示；然后优化之后的结果如图2(b)虚线所示。
[0091]
路径优化阶段在祛除了不必要的入射点之后，还需要在各个路径直线段的连接处进行平滑，也就是对路径点附近的路径进行平滑。因为根据光的反射定律，入射点一定位于光的传导介质与反射镜面的交界处。因此，本算法中的入射点都具有这样的特性：1)靠近地图边界或者障碍物；2)在入射点处机器人的前进方向会发生剧烈改变。这两点对于机器人的运动是非常不利的。如图2黑色路径所示。
[0092]
(3)使用三次贝塞尔曲线对轨迹进行平滑；
[0093]
1)设定最远传播距离dis_max_propagation，获得ray_cluster中的入射点数量m；
[0094]
2)对location＝2、3、...、m-1，执行如下操作：
[0095]
计算入射边与出射边的长度length_inshot、length_outshot；
[0096]
计算入射边与出射边的角度theta_inshot、theta_outshot；
[0097]
计算入射边与出射边的角平分线的角度theta_bisector；
[0098]
计算入射边与出射边夹角的一半theta；
[0099]
分别计算入射边与出射边在角平分线上的投影长度的最小值 min_project；
[0100]
计算实际允许传播的最大距离dis_propagation；
[0101]
沿着角平分线向前试探dis_propagation距离，如果中途碰到障碍物则终止试探；
[0102]
返回试探过的路径的中点f；
[0103]
计算控制点p0、p1、p2、p3，并存入新的路径中；该步骤的流程图如图8所示。
[0104]
在入射点e处做其角平分线，如图2点虚线所示。计算入射点e与其前一个入射点之间的距离dis1，与后一个点之间的距离 dis2，并计算dis1和dis2分别在角平分线上的投影的最小值
[0105]
dis
min_project
＝min{dis1cos(θ),dis2cos(θ)}
[0106]
(4),取dis
propa
＝min{dis
max_propagate
,dis
min_project
}，其中 dis
max_propagate
为在路径平滑阶段设定的光线最远传播距离，如果传播dis
propa
距离后仍然没有碰到障碍物，则认为足够安全，并返回这一段路径的中点f。
[0107]
(5)三次贝塞尔曲线的表达式为：
[0108]
(6)b(t)＝(1-t)3p0+3t(1-t)2p1+3t2(1-t)p2+t3p3t∈[0,1]
ꢀꢀꢀꢀ
(1)
[0109][0110]
(7)其中，p0、p1、p2、p3是三次贝塞尔曲线的四个控制点，分别称为起点、中间点1、中间点2、终点；且如图3(a)所示，存在
[0111]
(8)
[0112]
(9)如图3(b)所示，
[0113]
(10)f点为在入射点e处沿着角平分线传播一定距离后，返回的该段传播路径的中点。本算法需要根据点f、入射点e、入射光线以及反射光线，求解出三次贝塞尔曲线的控制点p0、p1、p2和p3。为了简便，我们假设控制点p0、p1与控制点p2、p3关于角平分线ef对称，那么可以过点f做角平分线的垂线分别交入射光线于p1，交反射光线于p2。在入射光线上找到一点p0，使得p1是p0e的中点；在反射光线上找到一点p3，使得p2是p3e的中点。那么有如下关系存在：
[0114]
(11)p0p1＝p1e＝p2e＝p2p3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0115]
(12)p1f＝p2f
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0116]
(13)控制点p0、p1、p2和p3如图3(b)所示，由这四个控制点所决定的三次贝塞尔曲线将替代p
0-e-p3段路径以实现优化目的。优化之后的新路径如图2虚线弧部分所示。
[0117]
本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。