一种动态路径跟踪方法与流程

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


背景技术：

2.可穿越锚定路径跟踪方法(英文：traversability
‑
anchored dynamic path following，简称：tadpf)是一种agv自主导航中的路径跟踪方法。动态窗口法(英文：dynamic window approach，简称：dwa)是一种局部路径规划方法，都是将路径跟踪与局部路径规划相结合的基于采样和预测的方法。而目前的agv在使用这两种方法进行路径规划时，路径跟踪精度不高。


技术实现要素：

3.本发明目的是：提供一种基于采样和预测的动态路径跟踪方法，提高路径跟踪精度。
4.本发明的技术方案是：一种动态路径跟踪方法，包括：
5.利用b样条生成全局路径；
6.在每一个控制周期内，获取速度空间；
7.在速度空间内，通过对线速度和曲率或角速度进行采样，得到多组运动参数的组合；
8.对于每一组运动参数，模拟出未来的一小段圆弧轨迹；
9.通过评价函数对所有轨迹进行评价，采用评分最高或成本最低的轨迹及其相应的运动参数；
10.选取最优路线，抵达目标点。
11.其进一步的技术方案是：所述评价函数的评价标准至少包括避障能力、与全局路径的贴合程度、运动的平顺程度。
12.其进一步的技术方案是：所述对线速度和曲率或角速度进行采样，包括：
13.在速度空间内采用平均采样的方法，采样步长由采样数n
v_sam
和n
k_sam
决定，线速度和曲率的样本计算方法如下：
[0014][0015]
其进一步的技术方案是：所述对于每一组运动参数，模拟出未来的一小段圆弧轨迹，包括：
[0016]
对于每一组运动参数(v,k)，在理想状态下，下一个控制周期以v为线速度，以为曲率圆半径的圆弧轨迹运动，k的正负性指示角速度的方向，正为逆时针，负为顺时针；
[0017]
将圆弧轨迹上平均分成n
p_sam
段，每一段对应的轨迹点坐标的计算方法如下：
[0018]
当k不为0时：
[0019][0020]
当k为0时：
[0021][0022]
其中，(x0,y0,θ0)为从定位程序获取的当前位置坐标，t
sam
表示采样周期。
[0023]
其进一步的技术方案是：在控制周期很短的情况下，轨迹模拟通过直线代替圆弧，对于每条轨迹，只取末端时刻的终点作为采样点。
[0024]
其进一步的技术方案是：所述通过评价函数对所有轨迹进行评价，包括：
[0025]
针对与样条轨迹上最近点的距离偏差、与样条轨迹上最近点的角度偏差、速度的绝对值大小、与上一周期相比速度的变化量、与上一周期相比曲率的变化量进行成本考核；
[0026]
对各项成本进行归一化处理，各项成本的归一化处理如下：
[0027][0028][0029][0030][0031][0032]
在叠加过程中，首先对每一个控制周期内的n
p_sum
段的采样点与b样条上最近点的
距离偏差和角度偏差进行累加：
[0033]
然后将结果按采样点平均，并对其余各项成本进行叠加：
[0034][0035]
参考轨迹由均匀三阶b样条给出，由a
x
,b
x
,c
x
,d
x
,a
y
,b
y
,c
y
,d
y
,八个系数决定，参数方程为：
[0036]
在每个控制周期一开始，将计算样条曲线上距小车当前位置最近点所对应的s值，记做currents；在计算距采样点最近点的坐标时，从currents开始采用逐步搜索的方法；
[0037]
其中，为各项成本系数，决定各项因素的权重。
[0038]
其进一步的技术方案是：所述选取最优路线，抵达目标点，包括：
[0039]
得到各轨迹的成本后，从中选取成本最低的一条，并将其对应的运动参数转换为(v,ω)下发给下位机，其中，
[0040]
当currents接近1且未到达最后一段路径时，a
x
,b
x
,c
x
,d
x
,a
y
,b
y
,c
y
,d
y
切换为下一段的系数；
[0041]
当currents接近1且已在最后一段路径时，进入最终抵达阶段；
[0042]
在最终抵达阶段的控制周期内，若计算所得currents近似于1且/或当前位置坐标与终点坐标的距离、角度偏差都在允许范围内，则视作已经到达，将速度和角速度置零，各开关量复位。
[0043]
本发明的优点是：
[0044]
基于采样和预测的动态路径跟踪方法，提高路径跟踪精度，与传统的集合方法相比，提供了一种泛华能力较强的通用方法，对不同形状的路径有一定的适应性，不需要动态调整预瞄点和预瞄距离，可以融合局部避障和局部路径规划。
附图说明
[0045]
下面结合附图及实施例对本发明作进一步描述：
[0046]
图1是本技术提供的动态路径跟踪方法的流程图。
具体实施方式
[0047]
实施例：本技术提供了一种动态路径跟踪方法，如图1所示，该方法包括以下步骤。
[0048]
步骤s1，利用b样条生成全局路径。
[0049]
步骤s2，在每一个控制周期内，获取速度空间。
[0050]
速度空间即当前线速度和角速度(或曲率)的取值范围，由当前速度和最大加(减)速度、用户设定的最大/最小速度，以及硬件能达到的最大/最小速度三个边界条件共同决定。在叉车的单舵轮运动模型中，近似的认为加/减速度无穷大，只考虑用户允许的最大/最
小速度v
max
/v
min
，用户允许的最大/最小曲率k
max
/k
min
。
[0051]
步骤s3，在速度空间内，通过对线速度v和曲率k或角速度ω进行采样，得到多组运动参数(v,k)的组合。
[0052]
对于采样，在速度空间内采用平均采样的方法，采样步长由采样数n
v_sam
和n
k_sam
决定，线速度和曲率的样本计算方法如下：
[0053][0054]
步骤s4，对于每一组运动参数，模拟出未来的一小段圆弧轨迹。
[0055]
对于每一组运动参数(v,k)，在理想状态下，下一个控制周期以v为线速度，以为曲率圆半径的圆弧轨迹运动，k的正负性指示角速度的方向，正为逆时针，负为顺时针。
[0056]
将圆弧轨迹上平均分成n
p_sam
段，每一段对应的轨迹点坐标的计算方法如下：
[0057]
当k不为0时：
[0058][0059]
当k为0时：
[0060][0061]
其中，(x0,y0,θ0)为从定位程序获取的当前位置坐标，t
sam
表示采样周期。
[0062]
鉴于控制周期很短，模拟轨迹可以通过直线代替圆弧，对于每条轨迹，只取末端时刻的终点作为采样点，从而在不影响跟踪效果的前提下降低运算量。
[0063]
步骤s5，通过评价函数对所有轨迹进行评价，采用评分最高或成本最低的轨迹及其相应的运动参数。
[0064]
可选的，评价函数的评价标准至少包括避障能力、与全局路径的贴合程度、运动的平顺程度。
[0065]
本技术主要分五个部分进行成本考核，即与样条轨迹上最近点的距离偏差、与样条轨迹上最近点的角度偏差、速度的绝对值大小、与上一周期相比速度的变化量、与上一周期相比曲率的变化量。由于在运动控制部分不需要局部避障，暂不考虑障碍物的影响。最后将五个部分成本按权重线性叠加起来：其中，为各项成本系数，决定各项因素的权重。一般而言，与样条轨迹角度和速度偏差较小、速度较快、速度和角速度变化量较小的轨迹会获得较低的成本。
[0066]
成本系数使用动态参数，譬如在急转弯，或快到达目标点时，降低对于平顺度的要求，提高对距离和角度偏差的要求。
[0067]
对于单舵轮模型，运动中心的速度和曲率存在耦合。选取驱动轮的线速度和转角作为采样对象也许更恰当。
[0068]
在进行叠加之前，需要对各项成本进行归一化处理，目的是使各项成本的数值均匀、平滑。
[0069]
各项成本的归一化处理如下：
[0070][0071][0072][0073][0074][0075]
在叠加过程中，首先对每一个控制周期内的n
p_sum
段的采样点与b样条上最近点的距离偏差和角度偏差进行累加：
[0076]
然后将结果按采样点平均，并对其余各项成本进行叠加：
[0077][0078]
参考轨迹由均匀三阶b样条给出，由a
x
,b
x
,c
x
,d
x
,a
y
,b
y
,c
y
,d
y
,八个系数决定，参数方程为：
[0079]
在每个控制周期一开始，将计算样条曲线上距小车当前位置最近点所对应的s值，记做currents；在计算距采样点最近点的坐标时，从currents开始采用逐步搜索的方法，步
长可以人为设定。
[0080]
步骤s6，选取最优路线，抵达目标点。
[0081]
得到各轨迹的成本后，从中选取成本最低的一条，并将其对应的运动参数转换为(v,ω)下发给下位机(plc)，其中，
[0082]
当currents接近1(如大于0.95)且未到达最后一段路径时，a
x
,b
x
,c
x
,d
x
,a
y
,b
y
,c
y
,d
y
切换为下一段的系数；
[0083]
当currents接近1(如大于0.9)且已在最后一段路径时，进入最终抵达阶段；
[0084]
在最终抵达阶段的控制周期内，若计算所得currents近似于1(如|currents
‑
1|<0.05)且/或当前位置坐标与终点坐标的距离、角度偏差都在允许范围内，则视作已经到达，将速度和角速度置零，各开关量复位。
[0085]
综上所述，本技术提供的动态路径跟踪方法，基于采样和预测的动态路径跟踪方法，提高路径跟踪精度，与传统的集合方法相比，提供了一种泛华能力较强的通用方法，对不同形状的路径有一定的适应性，不需要动态调整预瞄点和预瞄距离，可以融合局部避障和局部路径规划。
[0086]
术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或隐含所指示的技术特征的数量。由此，限定的“第一”、“第二”的特征可以明示或隐含地包括一个或者更多个该特征。在本技术的描述中，除非另有说明，“多个”的含义是两个或者两个以上。
[0087]
上述本技术实施例序号仅仅为了描述，不代表实施例的优劣。
[0088]
本领域普通技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成，也可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，上述提到的存储介质可以是只读存储器、磁盘或光盘等。
[0089]
以上所述仅为本技术的较佳实施例，并不用以限制本技术，凡在本技术的精神和原则之内所作的任何修改、等同替换、改进等，均应包含在本技术的保护范围之内。