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)];
備注：
- 為減少計算量，每次都選取100個點進行尋找，當軌迹點較為密集而預瞄距離又較大時，可以适當增大每批次點的個數；
- 程式沒經過完備的測試。
軌迹跟蹤_預描點尋找 (betheme.net)