轨迹跟踪_预描点寻找

1 原理

当所有点到后轴中心的距离都大于预瞄距离时，选择最近的点；

当所有点到后轴中心的距离都小于预瞄距离时，选择最远的点；

否则，选择一个点满足要求：

即，该点与车辆后轴中心在车辆纵轴上的距离小于预瞄距离，而该点下一个点与车辆后轴中心在车辆纵轴上的距离大于预瞄距离。

如何计算轨迹点与车辆后轴中心在车辆纵轴上的距离

已知局部坐标到全局坐标的换算公式：

             

式中，xl,yl 为点在局部坐标系下的坐标；x,y 为点在全局坐标系下的坐标；x0,y0 为局部坐标系原点在全局坐标系下的坐标；α 为局部坐标系在全局坐标系下的角度，逆时针为正。

可得全局坐标到局部坐标的换算公式：

可用下列数据对公式进行验证：

2 参考文献

[1] 赵凯, 朱愿, 冯明月, et al. 基于多点序列预瞄的自动驾驶汽车路径跟踪算法研究 [J]. 汽车技术, 2018, 11): 1-5.

3 程序

%% 筛选预瞄点
function [XY_pre,D_rel,Id_out]  = SelectPreviewPoint(Id_pre,Seg,Center,D_pre,Yaw)
%% 参数定义
% Id_pre 上一时刻预瞄点id
% Seg 当前预瞄轨迹点坐标
x_pre = Seg(:,1); y_pre = Seg(:,2);
% Center 车辆后轴中心点坐标
x_r = Center(1); y_r = Center(2);
% D_pre 目标距离
% Yaw 车辆横摆角，单位弧度，横轴向右为x正,纵轴向上为y正
% XY_pre 预瞄点坐标
% D_rel 实际预瞄距离
% Id_out 预瞄点id
num_point = 99; % 一次搜索包含的轨迹点数量-1
%% 选取最近的点
if (Id_pre+num_point)>=size(Seg,1)id_end = size(Seg,1);
elseid_end = Id_pre+99;
end
d_m = sqrt((x_pre(Id_pre:id_end)-x_r).^2+(y_pre(Id_pre:id_end)-y_r).^2);
[d_min,id_min] = min(d_m);
[d_max,id_max] = max(d_m);
%% 搜寻预瞄点
if d_min>=D_preId_out = (Id_pre+id_min-1);
elseif d_max<D_preId_out = (Id_pre+id_max-1);
elsedx = x_r-x_pre(Id_pre:id_end);dy = y_r-y_pre(Id_pre:id_end);d_y = abs(dx*sin(Yaw)-dy*cos(Yaw));idx_d = (d_y(1:end-1)-D_pre).*(d_y(2:end)-D_pre)<0; % 预瞄点的索引Id_out = Id_pre+find(idx_d==1)-1;
end
%% 输出 
D_rel = sqrt((x_pre(Id_out)-x_r).^2+(y_pre(Id_out)-y_r).^2);
XY_pre = [x_pre(Id_out),y_pre(Id_out)];

备注：

1. 为减少计算量，每次都选取100个点进行寻找，当轨迹点较为密集而预瞄距离又较大时，可以适当增大每批次点的个数；
2. 程序没经过完备的测试。

轨迹跟踪_预描点寻找 (betheme.net)