机器人路径跟踪方法与流程

1.本发明涉及一种机器人路径跟踪方法，属于人工智能技术领域。
背景技术
2.随着科技的发展，各种机器人开始出现在各个生活场所，用于代替人工执行一些任务，服务机器人正逐步替代部分人工的工作。目前，已将机器人广泛应用于餐厅、酒店、医院、博物馆、文化馆、政府机构等场景中，提供配送、引导、讲解等服务。应用于上述场景中的机器人需要克服使用场地的限制，进行无轨道移动。在一些应用场景中，机器人需要按照用户指定的路径进行移动，不要移动到不必要的区域内。
3.在原始的纯路径追踪算法中，首先需要计算指定路径上点与当前机器人最近的点(reference point)，然后从该最近点向前(路径是有向的)计算指定l距离的前向点(forward point)，从而把计算的前向点作为目标点进行运动。此方法对直线路径完全适应，但是不适应于在有较大弯度的路径中，当路径有较大的转弯时，转弯特征会被忽略，从而导致机器人无法在较大转弯处很好地跟随路径。


技术实现要素：

4.本发明目的是提供了一种机器人路径跟踪方法，提高机器人在发布的路径上贴合度，解决在较大弯度的路径上机器人不贴合的问题。
5.本发明为实现上述目的，通过以下技术方案实现：
6.一种机器人路径跟踪方法，包括以下步骤：
7.s1.获取机器人的全局路径，根据这段行走的距离判断为直线模式或圆弧模式；
8.s2.直线模式采用pid控制算法来进行线速度与角速度的控制；
9.s3.在圆弧模式下行走，利用贝塞尔曲线对转弯点处进行插值；
10.s4.计算机器人当前位置到局部目标点之间拟合弧线的相对曲率；
11.s5.在直线行走的过程中，不停地判断机器人有没有接近当前的转弯点位置，如果到达当前的转弯点的前2米，那么减速；等到达转弯点位置后，停止运动，开始旋转，使机器人朝向新的局部目标点集，期间不停地判断机器人有没有到达转弯点的位姿，如果满足条件，那么开始加速前进，不停地判断机器人有没有到达最终的目标点，同样如果到达目标点前m米，减速，最后安全到达终点位置；
12.s6.在圆弧行走过程中，不停地判断机器人有没有到达最终的目标点位置，如果到达目标点前m米，那么减速，直到安全到达点位置后停止，在拐弧线模式下，机器人在拐弯处会根据当前路径的弯曲程度d，计算出一组线速度和角速度驱动机器人过弯。
13.所述机器人路径跟踪方法优选方案，步骤s1具体过程如下：
14.s11.根据机器人当前位置(x,y)向前搜索5米的距离，取出这段距离中所有的点来进行判断；
15.s12.将这段距离中的点位(xi,yi)与(x
i+1
,y
i+1
)进行计算，保存记录两个点之间的
斜率
16.s13.遍历这个两点之间的斜率，当前后n个k值保持相同的斜率范围内是，则这段行走的距离模式可判断为直线模式，当连续k＞α(α为斜率阈值)后，则这段行走的距离为圆弧模式。
17.所述机器人路径跟踪方法优选方案，步骤s2具体过程如下：
18.s21.在机器人当前位置以初始速度v向下一个点位前进，实时获取机器人当前位置，判断机器人在计算两点(x1,y1)与(x2,y2)之间的距离d及偏角β，并将直线的点拟合成直线，具体公式如下：
19.(x1,y1)与(x2,y2)两个位姿之间的距离；
20.β为(x1,y1)与(x2,y2)两个位姿的斜率；
21.拟合直线：ax+by+c＝0，其中(x1,y1)为当前位置坐标，(x2,y2)为目标点的位置坐标；
22.s22.机器人行走中，求取机器人位置(x0,y0)距离直线ax+by+c＝0：
23.s23.当距离小于阈值a时，机器人按照固定的速度行驶，当阈值大于a时，角速度w根据距离d来进行调整，k为比系数，纠正转角ω来靠近直线，按照速度v前进，知道终点位置(xe,ye)。
24.所述机器人路径跟踪方法优选方案，步骤s3具体过程如下：
25.s31.塞尔曲线公式p＝(1-t)2p0+2t(1-t)p1+t2p2，设置转弯距离λ，根据转弯点的下标和λ，计算控制点c
i1
，c
i2
的下标；
26.s32.输入全局路径、转弯点和两个控制点c
i1
，c
i2
；
27.s33.采用分段读取的方法获取所有的插值点和原路径点，不包含被插值的点，具体如下：总共段数为l＝2
×
i+1，迭代次数为l次，当l为偶数，则装入直线段；当l为奇数，则装入弧线段，即插值点。
28.所述机器人路径跟踪方法优选方案，步骤s4具体过程如下：
29.s41.利用纯追踪模型跟踪预设的全局路径，根据当前速度计算前向距离，设置最大前向距离lookaheadmax＝v
×
t+ζ和最小前向距离lookaheadmin＝v
×
t-ζ，式中t为模拟时间、ζ为增益系数；
30.s42.根据前向距离和当前机器人的位置，在局部目标点集中找到当前时刻需要跟踪的局部目标点，计算机器人当前位置到局部目标点之间拟合弧线的相对曲率即当前点与选择的前向点之间距离生成的直线的最远距离：
31.参考点(x0,y0),前向点(x1,y1)，在点的集合set{(x0,y0),......(x1,y1)}中，取最
大值为圆弧的相对曲率：
32.两点之间的直线：y-y1＝[(y
0-y1)/(x
0-x1)]*(x-x1)
[0033]
其中(xi,yi)为集合set中的点，max的取值的下标i即为转弯点；
[0034]
s43.计算当前时刻机器人的角速度ω＝t*v*γ。t为仿真时间，v为速度阈值，γ为增益系数。
[0035]
本发明的优点在于：
[0036]
对前向点的计算进行计算修改，提出相对曲率的概念，根据距离的大小来限制前向点的位置，提高机器人在发布的路径上贴合度，解决在较大弯度的路径上机器人不贴合的问题。
附图说明
[0037]
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。
[0038]
图1为本发明实施例的深度卷积神经网络架构示意图。
具体实施方式
[0039]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0040]
正如本发明

背景技术：
在一些应用场景中，机器人需要按照用户指定的路径进行移动，不要移动到不必要的区域内，在较大弯度的路径上机器人存在不贴合的问题。
[0041]
参考图1，引入了相对曲率的概念，从而修正前向点的计算。假设基准点和前进点的线段为s，基准点和前进点之间的路径曲线距离s的最远距离为d，则相对曲率为d/s，在改进的算法中，相对曲率越大，前进点相对基准点曲线距离d越小，反之越大，这样机器人跟随前进点时，就能更好的跟踪各种曲率半径。
[0042]
另外，为了让机器人在追踪前进点的同时，能够考虑最终的姿态，则以前进点处路径曲线的切线为前向点期望方向。
[0043]
本发明具体采用以下技术方案：
[0044]
一种机器人路径跟踪方法，包括以下步骤：
[0045]
s1.机器人行走分为直线行走模式和圆弧行走模式，机器人在运行的时候，获取机器人的全局路径，根据这段行走的距离判断为直线模式或圆弧模式；
[0046]
s2.直线模式采用pid控制算法来进行线速度与角速度的控制；
[0047]
s3.在圆弧模式下行走，利用贝塞尔曲线对转弯点处进行插值；
[0048]
s4.计算机器人当前位置到局部目标点之间拟合弧线的相对曲率；
[0049]
s5.在直线行走的过程中，不停地判断机器人有没有接近当前的转弯点位置，如果
到达当前的转弯点的前2米，那么减速；等到达转弯点位置后，停止运动，开始旋转，使机器人朝向新的局部目标点集，期间不停地判断机器人有没有到达转弯点的位姿，如果满足条件，那么开始加速前进，不停地判断机器人有没有到达最终的目标点，同样如果到达目标点前m米，减速，最后安全到达终点位置，在走直线模式下，在转弯点前提前减速，到达位置后旋转，然后起再起步加速，最终将线速度和角速度分开控制，且旋转过程加入加速和减速，给机器人一定的缓冲，保证了机器人在转弯时候的安全性；
[0050]
s6.在圆弧行走过程中，不停地判断机器人有没有到达最终的目标点位置，如果到达目标点前m米，那么减速，直到安全到达点位置后停止，在拐弧线模式下，机器人在拐弯处会根据当前路径的弯曲程度d，计算出一组线速度和角速度驱动机器人过弯，计算出一组线速度和角速度驱动机器人过弯，拐弯全程的速度均由机器人计算而得，并非设定的常数，这样一方面保证了机器人在拐弯时不越弯，安全可靠，另一方面使得机器人在拐弯过程中很平稳，轨迹平滑，不出现速度的跳变。
[0051]
本实施例中，步骤s1具体过程如下：
[0052]
s11.根据机器人当前位置(x,y)向前搜索5米的距离，取出这段距离中所有的点来进行判断；
[0053]
s12.将这段距离中的点位(xi,yi)与(x
i+1
,y
i+1
)进行计算，保存记录两个点之间的斜率
[0054]
s13.遍历这个两点之间的斜率，当前后n个k值保持相同的斜率范围内是，则这段行走的距离模式可判断为直线模式，当连续k＞α(α为斜率阈值)后，则这段行走的距离为圆弧模式。
[0055]
本实施例中，步骤s2具体过程如下：
[0056]
s21.在机器人当前位置以初始速度v向下一个点位前进，实时获取机器人当前位置，判断机器人在计算两点之间的距离d及偏角β并将直线的点拟合成直线，具体公式如下：
[0057]
与(x2,y2)两个位姿之间的距离；
[0058]
β为(x1,y1)与(x2,y2)两个位姿的斜率；
[0059]
拟合直线：ax+by+c＝0，其中(x1,y1)为当前位置坐标，(x2,y2)为目标点的位置坐标；
[0060]
s22.机器人行走中，求取机器人位置(x0,y0)距离直线ax+by+c＝0：
[0061]
s23.当距离小于阈值a时，机器人按照固定的速度行驶，当阈值大于a时，角速度ω根据距离d来进行调整，k为比系数，纠正转角ω来靠近直线，按照速度v前进，知道终点位置(xe,ye)。
[0062]
本实施例中，步骤s3具体过程如下：
[0063]
s31.塞尔曲线公式p＝(1-t)2p0+2t(1-t)p1+t2p2，设置转弯距离λ，根据转弯点的下标和λ，计算控制点c
i1
，c
i2
的下标；
[0064]
s32.输入全局路径、转弯点和两个控制点c
i1
，c
i2
；
[0065]
s33.采用分段读取的方法获取所有的插值点和原路径点，不包含被插值的点，具体如下：总共段数为l＝2
×
i+1，迭代次数为l次，当l为偶数，则装入直线段；当l为奇数，则装入弧线段，即插值点。
[0066]
本实施例中，步骤s4具体过程如下：
[0067]
s41.计算所述机器人当前位置的偏航角和局部目标点集之间的角度偏差并对其取平均值，将线速度设置为0，确保当前机器人转弯时不平移；计算角速度使机器人旋转至局部目标点集位置，这就确保了机器人在起步时的安全性；利用纯追踪模型跟踪预设的全局路径，根据当前速度计算前向距离，设置最大前向距离lookaheadmax＝v
×
t+ζ和最小前向距离lookaheadmin＝v
×
t-ζ，式中t为模拟时间、ζ为增益系数；
[0068]
s42.根据前向距离和当前机器人的位置，在局部目标点集中找到当前时刻需要跟踪的局部目标点，所述目标点集的阈值为ε＝max(l,w)/2，式中l、w分别表示所述局部代价地图的长和宽，说明局部目标点集永远在局部代价地图上，计算机器人当前位置到局部目标点之间拟合弧线的相对曲率即当前点与选择的前向点之间距离生成的直线的最远距离：
[0069]
参考点(x0,y0),前向点(x1,y1)，在点的集合set{(x0,y0),......(x1,y1)}中，取最大值为圆弧的相对曲率：
[0070]
两点之间的直线：y-y1＝[(y
0-y1)/(x
0-x1)]*(x-x1)
[0071]
其中(xi,yi)为集合set中的点，max的取值的下标即为转弯点；
[0072]
s43.计算当前时刻机器人的角速度ω＝t*v*γ。t为仿真时间，v为速度阈值，γ为增益系数。
[0073]
最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。