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

权利要求书

1.一种动态路径跟踪方法，其特征在于，包括：

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

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

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

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

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

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

2.根据权利要求1所述的动态路径跟踪方法，其特征在于，所述评价函数的评价标准至少包括避障能力、与全局路径的贴合程度、运动的平顺程度。

3.根据权利要求1所述的动态路径跟踪方法，其特征在于，所述对线速度和曲率或角速度进行采样，包括：

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

4.根据权利要求3所述的动态路径跟踪方法，其特征在于，所述对于每一组运动参数，模拟出未来的一小段圆弧轨迹，包括：

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

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

当k不为0时：

当k为0时：

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

5.根据权利要求4所述的动态路径跟踪方法，其特征在于，在控制周期很短的情况下，轨迹模拟通过直线代替圆弧，对于每条轨迹，只取末端时刻的终点作为采样点。

6.根据权利要求4所述的动态路径跟踪方法，其特征在于，所述通过评价函数对所有轨迹进行评价，包括：

针对与样条轨迹上最近点的距离偏差、与样条轨迹上最近点的角度偏差、速度的绝对值大小、与上一周期相比速度的变化量、与上一周期相比曲率的变化量进行成本考核；

对各项成本进行归一化处理，各项成本的归一化处理如下：

在叠加过程中，首先对每一个控制周期内的N_{p_sum}段的采样点与B样条上最近点的距离偏差和角度偏差进行累加：

然后将结果按采样点平均，并对其余各项成本进行叠加：

参考轨迹由均匀三阶B样条给出，由A_x,B_x,C_x,D_x,A_y,B_y,C_y,D_y,八个系数决定，参数方程为：

在每个控制周期一开始，将计算样条曲线上距小车当前位置最近点所对应的s值，记做currentS；在计算距采样点最近点的坐标时，从currentS开始采用逐步搜索的方法；

其中，为各项成本系数，决定各项因素的权重。

7.根据权利要求6所述的动态路径跟踪方法，其特征在于，所述选取最优路线，抵达目标点，包括：

得到各轨迹的成本后，从中选取成本最低的一条，并将其对应的运动参数转换为(v,ω)下发给下位机，其中，

当currentS接近1且未到达最后一段路径时，A_x,B_x,C_x,D_x,A_y,B_y,C_y,D_y切换为下一段的系数；

当currentS接近1且已在最后一段路径时，进入最终抵达阶段；

在最终抵达阶段的控制周期内，若计算所得currentS近似于1且/或当前位置坐标与终点坐标的距离、角度偏差都在允许范围内，则视作已经到达，将速度和角速度置零，各开关量复位。