[发明专利]一种动态路径跟踪方法

说明书

本发明公开了一种动态路径跟踪方法，包括：利用B样条生成全局路径；在每一个控制周期内，获取速度空间；在速度空间内，通过对线速度和曲率或角速度进行采样，得到多组运动参数的组合；对于每一组运动参数，模拟出未来的一小段圆弧轨迹；通过评价函数对所有轨迹进行评价，采用评分最高或成本最低的轨迹及其相应的运动参数；选取最优路线，抵达目标点。基于采样和预测的动态路径跟踪方法，提高路径跟踪精度，与传统的集合方法相比，提供了一种泛华能力较强的通用方法，对不同形状的路径有一定的适应性，不需要动态调整预瞄点和预瞄距离，可以融合局部避障和局部路径规划。

技术领域

本发明属于移动机器人导航领域，涉及一种动态路径跟踪方法。

背景技术

可穿越锚定路径跟踪方法(英文：Traversability-anchored Dynamic PathFollowing，简称：TADPF)是一种AGV自主导航中的路径跟踪方法。动态窗口法(英文：Dynamic Window Approach，简称：DWA)是一种局部路径规划方法，都是将路径跟踪与局部路径规划相结合的基于采样和预测的方法。而目前的AGV在使用这两种方法进行路径规划时，路径跟踪精度不高。

发明内容

本发明目的是：提供一种基于采样和预测的动态路径跟踪方法，提高路径跟踪精度。

本发明的技术方案是：一种动态路径跟踪方法，包括：

利用B样条生成全局路径；

在每一个控制周期内，获取速度空间；

在速度空间内，通过对线速度和曲率或角速度进行采样，得到多组运动参数的组合；

对于每一组运动参数，模拟出未来的一小段圆弧轨迹；

通过评价函数对所有轨迹进行评价，采用评分最高或成本最低的轨迹及其相应的运动参数；

选取最优路线，抵达目标点。

其进一步的技术方案是：所述评价函数的评价标准至少包括避障能力、与全局路径的贴合程度、运动的平顺程度。

其进一步的技术方案是：所述对线速度和曲率或角速度进行采样，包括：

在速度空间内采用平均采样的方法，采样步长由采样数N_{v_sam}和N_{k_sam}决定，线速度和曲率的样本计算方法如下：

其进一步的技术方案是：所述对于每一组运动参数，模拟出未来的一小段圆弧轨迹，包括：

对于每一组运动参数(v,k)，在理想状态下，下一个控制周期以v为线速度，以为曲率圆半径的圆弧轨迹运动，k的正负性指示角速度的方向，正为逆时针，负为顺时针；

将圆弧轨迹上平均分成N_{p_sam}段，每一段对应的轨迹点坐标的计算方法如下：

当k不为0时：

当k为0时：

其中，(x₀,y₀,θ₀)为从定位程序获取的当前位置坐标，t_sam表示采样周期。

其进一步的技术方案是：在控制周期很短的情况下，轨迹模拟通过直线代替圆弧，对于每条轨迹，只取末端时刻的终点作为采样点。

其进一步的技术方案是：所述通过评价函数对所有轨迹进行评价，包括：