纯路径跟踪中的自主规划绕障方法与流程

1.本发明涉及机器人路径检测领域，尤其涉及一种纯路径跟踪中的自主规划绕障方法。


背景技术：

2.清洁机器人在运动控制中需要按照指定的路径进行清扫，以达到满足要求的清扫效率，而不需要进行点对点的自由导航。在其他类型的机器人应用中，在特定的环境下需要进行安全行驶时，也需要指定安全路径进行行驶，而不需要让机器人按照自由导航的模式进行规划行走，因此路径的纯跟踪技术就需要应用于机器人中。无论是给定机器人的行驶路径，还是机器人根据当前环境进行自主规划的路径，机器人都需要进行路径的纯跟踪行驶。但是当跟踪的路径遇到障碍物，机器人就需要进行绕障。但是机器人绕障后还需要回到给定的路径上，就需要进行给定路径和绕障路径的结合。
3.清洁机器人在纯跟踪模式下进行工作，将对指定的路径进行清扫。在指定路径的纯跟踪清扫模式下，机器人遇到障碍物时不能只是停障不进行作业，需要进行自主的绕开障碍物，回到需要清扫的路径上继续作业。如何对跟踪清扫路径上的障碍物进行绕障，并使得机器人回到需要清扫的路径上是一个需要解决的问题。
4.现有的纯跟踪技术是一种动态规划跟踪方法，在自主跟随的过程中总是在实时的规划全新的路径，无法满足清扫需要达到的要求。清扫的路径是相对固定的指定路径，需要对指定区域全覆盖的清理，在这个过程中遇到障碍物时需要避开障碍物，继续回到指定路径进行清扫任务。清扫时跟踪的路径是静态的路径，不需要进行变动，即使是障碍物在清扫路径上，全局跟踪路径也不需要变化。那么在静态路径上出现障碍物的时候，就需要解决机器人局部的路径规划，让机器人绕开障碍物回到跟随的全局路径上继续进行清洁作业。


技术实现要素：

5.为了解决以上技术问题，本发明提供了一种纯路径跟踪中的自主规划绕障方法。解决跟踪静态路径l0过程中遇到障碍物的时候，如何生成绕开障碍物的路径，并回到跟踪路径上的问题。
6.本发明的技术方案是：
7.纯路径跟踪中的自主规划绕障方法，
8.通过机器人行驶路径的区域来对障碍物进行检测，综合各传感器来计算机器人与障碍物之间的距离；
9.检测障碍物距离机器人的距离，通过机器人当前的位置与障碍物后的位置来计算机器人规划的目标位置；
10.机器人当前位置与目标位置确认之后的重新路径规划，生成一条能够避开障碍物的全新规划路径；
11.全局跟随路径与重新规划的绕障路径进行结合，在运行过程中动态的生成新的跟
随路径。
12.进一步的，
13.清洁机器人在跟踪过程中根据传感器检测数据来做一个机器人安全检测区域，此区域保证清洁机器人行驶的安全性；
14.各传感器在机器人本体上都有着坐标位置，经过统一的tf变换之后，将相机、超声数据统一变换到激光雷达坐标系下，利用这一统一数据来进行区域障碍物的判断处理；此时以其中一个传感器为原点，构建任意的多边形区域n(n1,n2,...nn,i＝1,2,...n)。
15.转换后的传感器数据进行处理之后，在多边形区域内进行计算；这些检测到的数据值是否在构成的多边形区域n内，若传感器检测数据点大于n(n》1)，则认为清洁机器人行走区域内前方存在障碍物；若传感器检测数据点小于n，则判定为清洁机器人行走区域内前方没有障碍物，则继续执行跟随即可。
16.检测多边形区域设定为前方距离为s(s》0)m的区域，此区域划分为减速区和紧急停障区；当检测到运行区域存在障碍物的时候，就需要根据传感器的数据来计算障碍物距离机器人的实际距离。
17.再进一步的，
18.机器人在运行过程中，当障碍物在检测区域s/2范围内，记录机器人跟随路径上当前位置p(x0,y0)，分为以下情况处理：
19.1)障碍物距离机器人距离d,当0《d《s/2,机器人会停止运行，在等待t秒的时间之后，障碍物不存在了，则机器人继续的执行全局静态路径的跟随；t秒等待时间之后障碍物仍然存在，则机器人执行倒车的动作，倒退到距离障碍物km的位置；
20.此时机器人以当前的定位位置p(x1,y1)为起始位置，设定目标位置p(x2,y2)，重新进行路径规划，从而沿着绕开障碍物的(p(x1,y1),p(x2,y2))路径继续进行跟随；
21.2)障碍物距离机器人距离d,当s/2《d《k时，此时的机器人会停止清扫，等待障碍物变化；在t秒的时间之内，障碍物离开检测区域，则机器人继续按照清扫路线运行，当障碍物继续存在检测区域的时候，此时机器人可执行重新规划的算法；
22.此时记录机器人当前的位置p(x3,y3)，寻找障碍物后方前进路线的点位，此时的距离d1是障碍物距离清洁机器人的位置与障碍物膨胀的距离之和。
23.目标位置p(x2,y2)的计算：
24.首先根据机器人停止位置p(x0,y0)向着前进路径方向遍历要跟随路径上的点；此时的计算点位根据欧式距离进行，d＝(x
0-xi)2+(y
0-yi)2其中i是跟随路路径上的点位下标；
25.从当前停止点位p(x0,y0)开始计算，选择跟随路径上的目标点位p(x2,y2)，当找到大于设定阈值的点位坐标的时候，及选择此时的目标位置作为绕障的目标位置。此时的距离设置为p(p》s)m的位置，此时要考虑车体的长度和障碍物膨胀的区域，从而遍历点位坐标，得出跟随路径上的目标位置；
26.当遍历完所有的点位都不能满足要求时，可以把最终路径点位位置作为目标位置p(x2,y2)，当路径的终点位置作为目标位置，检测到此目标点是被障碍物占据的时候，即报规划失败，从而结束任务，其他情况下继续规划路径重新跟随。
27.再进一步的，
28.当前的定位位置p(x1,y1)与目标位置p(x2,y2)的路径规划，直接采用a*算法来进
行路径规划，此时的代价地图中有着所有的地图信息，从而调用a*规划算法，得出从起点p(x1,y1)与目标点p(x2,y2)之间的规划路径l；
29.全局跟随静态路径l0与绕障路径l进行拼接，从而重新生成一条清洁机器人需要进行跟随的路径。这两条路径拼接的计算方法：
30.1)从机器人的当前位置p(x0,y0)开始进行全局跟随静态路径l0的遍历，将p(x0,y0)到p(x2,y2)之间的路径点位全部删除，p(x2,y2)之后的点位全部保留；
31.2)遍历新的规划绕障路径l，将新的规划绕障路径l的点位全部插入到p(x0,y0)与p(x2,y2)之间，从而形成一条全新的静态跟踪路径l1；
32.生成全新的路径l1之后，运动控制器根据新的路径进行跟踪；在新路径跟踪过程中出现障碍物的情况下，重复此操作来进行新的路径更新。
33.目标位置p(x4,y4)的计算：
34.首先根据机器人停止位置p(x3,y3)向着前进路径方向遍历要跟随路径上的点；此时的计算点位根据欧式距离进行，d＝(x
3-xi)2+(y
3-yi)2其中i是跟随路路径上的点位下标；
35.从当前停止点位p(x3,y3)开始计算，选择跟随路径上的目标点位p(x4,y4)，当找到大于设定阈值的点位坐标的时候，及选择此时的目标位置作为绕障的目标位置。此时的距离d1是障碍物距离清洁机器人的位置与障碍物膨胀的距离之和，从而得出跟随路径上的目标位置。
36.得出起点与终点的坐标之后，重新规划此时需要绕障的路径(p(x3,y3),p(x4,y4))，利用a*算法在当前的地图中规划一条路径；从起点p(x3,y3)到目标点p(x4,y4)，利用当前的代价地图，重新规划出新的绕障路径l2；获得绕障路径之后，采用相同的路径拼接方法，将全局的静态路径与新的绕障路径拼接到一起，形成新的全局跟踪路径。
37.本发明的有益效果是
38.全局跟随路径与重新规划的绕障路径进行结合，在运行过程中动态的生成新的跟随路径。
39.全局跟踪路径的实时规划，目标路径保持不变，根据障碍物位置对跟随路径的影响来动态规划绕障路径。
40.采用a*算法来对局部绕障路径进行动态规划，结合局部的代价地图以及传感器信息。
附图说明
41.图1是本发明的工作过程效果示意图。
具体实施方式
42.为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
43.本发明提出一种在路径纯跟踪和自主规划相结合的路径跟踪绕障方法，以使得机器人能按照安全指定的路径行驶并绕障，能稳定的执行行驶任务。
44.将动态路径和静态路径相结合，根据清洁机器人跟踪路径设定的判断区域计算障碍物距离，结合距离与膨胀来进行路径上目标点位的选择，结合a*算法来进行绕障路径的规划。新的绕障路径和跟随的全局路径相结合，生成全新的跟随清扫路径，从而达到满足清扫和绕障的效果。
45.具体包括：
46.1、清洁机器人在跟踪过程中根据传感器(激光雷达、相机、超声)检测数据来做一个机器人安全检测区域，此区域保证清洁机器人行驶的安全性。三种传感器在机器人本体上都有着坐标位置，经过统一的tf变换之后，将相机、超声数据统一变换到激光雷达坐标系下，利用这一统一数据来进行区域障碍物的判断处理。此时以激光雷达为原点，构建任意的多边形区域n(n1,n2,...nn,i＝1,2,...n)。转换后的传感器数据进行处理之后，在多边形区域内进行计算。这些检测到的数据值是否在构成的多边形区域n内，若传感器检测数据点大于n(如10个点)，则认为清洁机器人行走区域内前方存在障碍物；若传感器检测数据点小于n，则判定为清洁机器人行走区域内前方没有障碍物，则继续执行跟随即可。
47.2、检测多边形区域设定为前方1.0m的区域，此区域可以划分为减速区和紧急停障区。当检测到运行区域存在障碍物的时候，就需要根据传感器的数据来计算障碍物距离机器人的实际距离。机器人在运行过程中，当障碍物在检测区域0.5m范围内，记录机器人跟随路径上当前位置p(x0,y0)，分为以下情况处理：
48.(1)障碍物距离机器人距离d,当0《d《0.5,机器人会停止运行，在等待四秒的时间之后，障碍物不存在了，则机器人继续的执行全局静态路径的跟随。四秒等待时间之后障碍物仍然存在，则机器人执行倒车的动作，倒退到距离障碍物1.0m的位置。此时机器人以当前的定位位置p(x1,y1)为起始位置，设定目标位置p(x2,y2)，重新进行路径规划，从而沿着绕开障碍物的(p(x1,y1),p(x2,y2))路径继续进行跟随。
49.目标位置p(x2,y2)的计算：首先根据机器人停止位置p(x0,y0)向着前进路径方向遍历要跟随路径上的点。此时的计算点位根据欧式距离进行，d＝(x
0-xi)2+(y
0-yi)2其中i是跟随路路径上的点位下标。
50.从当前停止点位p(x0,y0)开始计算，选择跟随路径上的目标点位p(x2,y2)，当找到大于设定阈值的点位坐标的时候，及选择此时的目标位置作为绕障的目标位置。此时的距离设置为1.5m的位置，此时要考虑车体的长度和障碍物膨胀的区域，从而遍历点位坐标，得出跟随路径上的目标位置。
51.当遍历完所有的点位都不能满足要求时，可以把最终路径点位位置作为目标位置p(x2,y2)，当路径的终点位置作为目标位置，检测到此目标点是被障碍物占据的时候，即报规划失败，从而结束任务，其他情况下继续规划路径重新跟随。
52.当前的定位位置p(x1,y1)与目标位置p(x2,y2)的路径规划，直接采用a*算法来进行路径规划，此时的代价地图中有着所有的地图信息，从而调用a*规划算法，得出从起点p(x1,y1)与目标点p(x2,y2)之间的规划路径l。
53.全局跟随静态路径l0与绕障路径l进行拼接，从而重新生成一条清洁机器人需要进行跟随的路径。这两条路径拼接的计算方法：
54.1、从机器人的当前位置p(x0,y0)开始进行全局跟随静态路径l0的遍历，将p(x0,y0)到p(x2,y2)之间的路径点位全部删除，p(x2,y2)之后的点位全部保留；
55.2、遍历新的规划绕障路径l，将新的规划绕障路径l的点位全部插入到p(x0,y0)与p(x2,y2)之间，从而形成一条全新的静态跟踪路径l156.生成全新的路径l1之后，运动控制器根据新的路径进行跟踪，从而使得机器人在清扫过程中既能清扫规划的路径，也能够实现在清扫路径上的绕障处理。在新路径跟踪过程中出现障碍物的情况下，重复此操作来进行新的路径更新。
57.(2)障碍物距离机器人距离d,当0.5《d《1.0时，此时的机器人会停止清扫，等待障碍物变化。在四秒的时间之内，障碍物离开检测区域，则机器人继续按照清扫路线运行，当障碍物继续存在检测区域的时候，此时机器人可以执行重新规划的算法。
58.此时记录机器人当前的位置p(x3,y3)，寻找障碍物后方前进路线的点位，此时的距离d1是障碍物距离清洁机器人的位置与障碍物膨胀的距离之和。
59.目标位置p(x4,y4)的计算：首先根据机器人停止位置p(x3,y3)向着前进路径方向遍历要跟随路径上的点。此时的计算点位根据欧式距离进行，d＝(x
3-xi)2+(y
3-yi)2其中i是跟随路路径上的点位下标。
60.从当前停止点位p(x3,y3)开始计算，选择跟随路径上的目标点位p(x4,y4)，当找到大于设定阈值的点位坐标的时候，及选择此时的目标位置作为绕障的目标位置。此时的距离d1是障碍物距离清洁机器人的位置与障碍物膨胀的距离之和，从而得出跟随路径上的目标位置。
61.得出起点与终点的坐标之后，重新规划此时需要绕障的路径(p(x3,y3),p(x4,y4))，利用a*算法在当前的地图中规划一条路径。从起点p(x3,y3)到目标点p(x4,y4)，利用当前的代价地图，重新规划出新的绕障路径l2。获得绕障路径之后，采用相同的路径拼接方法，将全局的静态路径与新的绕障路径拼接到一起，形成新的全局跟踪路径。
62.以上所述仅为本发明的较佳实施例，仅用于说明本发明的技术方案，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所做的任何修改、等同替换、改进等，均包含在本发明的保护范围内。

技术特征：
1.纯路径跟踪中的自主规划绕障方法，其特征在于，包括：1)通过机器人行驶路径的区域来对障碍物进行检测，综合各传感器来计算机器人与障碍物之间的距离；2)检测障碍物距离机器人的距离，通过机器人当前的位置与障碍物后的位置来计算机器人规划的目标位置；3)机器人当前位置与目标位置确认之后的重新路径规划，生成一条能够避开障碍物的全新规划路径；4)全局跟随路径与重新规划的绕障路径进行结合，在运行过程中动态的生成新的跟随路径。2.根据权利要求1所述的方法，其特征在于，清洁机器人在跟踪过程中根据传感器检测数据来做一个机器人安全检测区域，此区域保证清洁机器人行驶的安全性；各传感器在机器人本体上都有着坐标位置，经过统一的tf变换之后，将相机、超声数据统一变换到激光雷达坐标系下，利用这一统一数据来进行区域障碍物的判断处理；此时以其中一个传感器为原点，构建任意的多边形区域n(n1,n2,...n
n
,i＝1,2,...n)。转换后的传感器数据进行处理之后，在多边形区域内进行计算；这些检测到的数据值是否在构成的多边形区域n内，若传感器检测数据点大于n(n>1)，则认为清洁机器人行走区域内前方存在障碍物；若传感器检测数据点小于n，则判定为清洁机器人行走区域内前方没有障碍物，则继续执行跟随即可。3.根据权利要求2所述的方法，其特征在于，检测多边形区域设定为前方距离为s(s>0)m的区域，此区域划分为减速区和紧急停障区；当检测到运行区域存在障碍物的时候，就需要根据传感器的数据来计算障碍物距离机器人的实际距离。4.根据权利要求3所述的方法，其特征在于，机器人在运行过程中，当障碍物在检测区域s/2范围内，记录机器人跟随路径上当前位置p(x0,y0)，分为以下情况处理：1)障碍物距离机器人距离d,当0<d<s/2,机器人会停止运行，在等待t秒的时间之后，障碍物不存在了，则机器人继续的执行全局静态路径的跟随；t秒等待时间之后障碍物仍然存在，则机器人执行倒车的动作，倒退到距离障碍物km的位置；此时机器人以当前的定位位置p(x1,y1)为起始位置，设定目标位置p(x2,y2)，重新进行路径规划，从而沿着绕开障碍物的(p(x1,y1),p(x2,y2))路径继续进行跟随；2)障碍物距离机器人距离d,当s/2<d<s时，此时的机器人会停止清扫，等待障碍物变化；在t秒的时间之内，障碍物离开检测区域，则机器人继续按照清扫路线运行，当障碍物继续存在检测区域的时候，此时机器人可执行重新规划的算法；此时记录机器人当前的位置p(x3,y3)，寻找障碍物后方前进路线的点位，此时的距离d1是障碍物距离清洁机器人的位置与障碍物膨胀的距离之和。5.根据权利要求4所述的方法，其特征在于，目标位置p(x2,y2)的计算：
首先根据机器人停止位置p(x0,y0)向着前进路径方向遍历要跟随路径上的点；此时的计算点位根据欧式距离进行，d＝(x
0-x
i
)2+(y
0-y
i
)2其中i是跟随路路径上的点位下标；从当前停止点位p(x0,y0)开始计算，选择跟随路径上的目标点位p(x2,y2)，当找到大于设定阈值的点位坐标的时候，及选择此时的目标位置作为绕障的目标位置；此时的距离设置为p(p>s)m的位置，此时要考虑车体的长度和障碍物膨胀的区域，从而遍历点位坐标，得出跟随路径上的目标位置；当遍历完所有的点位都不能满足要求时，可以把最终路径点位位置作为目标位置p(x2,y2)，当路径的终点位置作为目标位置，检测到此目标点是被障碍物占据的时候，即报规划失败，从而结束任务，其他情况下继续规划路径重新跟随。6.根据权利要求5所述的方法，其特征在于，当前的定位位置p(x1,y1)与目标位置p(x2,y2)的路径规划，直接采用a*算法来进行路径规划，此时的代价地图中有着所有的地图信息，从而调用a*规划算法，得出从起点p(x1,y1)与目标点p(x2,y2)之间的规划路径l；全局跟随静态路径l0与绕障路径l进行拼接，从而重新生成一条清洁机器人需要进行跟随的路径。这两条路径拼接的计算方法：1)从机器人的当前位置p(x0,y0)开始进行全局跟随静态路径l0的遍历，将p(x0,y0)到p(x2,y2)之间的路径点位全部删除，p(x2,y2)之后的点位全部保留；2)遍历新的规划绕障路径l，将新的规划绕障路径l的点位全部插入到p(x0,y0)与p(x2,y2)之间，从而形成一条全新的静态跟踪路径l1；生成全新的路径l1之后，运动控制器根据新的路径进行跟踪；在新路径跟踪过程中出现障碍物的情况下，重复此操作来进行新的路径更新。7.根据权利要求4所述的方法，其特征在于，目标位置p(x4,y4)的计算：首先根据机器人停止位置p(x3,y3)向着前进路径方向遍历要跟随路径上的点；此时的计算点位根据欧式距离进行，d＝(x
3-x
i
)2+(y
3-y
i
)2其中i是跟随路路径上的点位下标；从当前停止点位p(x3,y3)开始计算，选择跟随路径上的目标点位p(x4,y4)，当找到大于设定阈值的点位坐标的时候，及选择此时的目标位置作为绕障的目标位置；此时的距离d1是障碍物距离清洁机器人的位置与障碍物膨胀的距离之和，从而得出跟随路径上的目标位置。8.根据权利要求7所述的方法，其特征在于，得出起点与终点的坐标之后，重新规划此时需要绕障的路径(p(x3,y3),p(x4,y4))，利用a*算法在当前的地图中规划一条路径；从起点p(x3,y3)到目标点p(x4,y4)，利用当前的代价地图，重新规划出新的绕障路径l2；获得绕障路径之后，采用相同的路径拼接方法，将全局的静态路径与新的绕障路径拼接到一起，形成新的全局跟踪路径。

技术总结
本发明提供一种纯路径跟踪中的自主规划绕障方法，属于机器人路径检测领域，本发明本方法结合静态全局跟踪路径和动态规划路径相结合的方式，绕开清扫路径上的障碍物执行清扫任务。动态路径和静态路径相结合，根据清洁机器人跟踪路径设定的判断区域计算障碍物距离，结合距离与膨胀来进行路径上目标点位的选择，结合A*算法来进行绕障路径的规划。新的绕障路径和跟随的全局路径相结合，生成全新的跟随清扫路径，从而达到满足清扫和绕障的效果。从而达到满足清扫和绕障的效果。从而达到满足清扫和绕障的效果。


技术研发人员：华逢彬 程瑶 马辰
受保护的技术使用者：山东新一代信息产业技术研究院有限公司
技术研发日：2023.05.17
技术公布日：2023/8/2