农机复杂路径跟踪中的选点问题（含倒车）

农机这玩意在田里大多数路径都比较简单，即我之前博客里画出来那种遍历式，但在转弯地方会比较复杂，比如鱼尾、Ω、C形啥的。这里只针对较复杂路段的点的选择上，之前写mpc那篇博客时候提到一个B站博主，他对于简单直线曲线啥的，追踪策略就是遍历整个路径，寻找当前点到目标路径中所有点的最小距离点，以此作为追踪点。但针对下面的路况，就会各种麻爪

这是我最近写的一篇论文里的路径，可以看到的是，其中包含交叉点，接近点，前进，倒车等复杂情况交叉。只靠最近点追踪逻辑，基本没法用。
这里给出一种适用于这种稍微复杂路况的追踪逻辑，其整体思路如下：
1.确认是不是上线（即横向距离是不是5cm以内），是的话就追踪下一个点就成
2.不是上线阶段，分为两种情况：
a)也快要上线了，且航向跟目标航向差距不大：追踪当前点

b)横向偏差太大：追踪最近点（这一般是初始点和目标路径距离太远时候)

针对前进和倒车情况，我经验是分段来，路径规划阶段即提供标志位，由标志位来判断是不是要前进还是后退，并在追踪时候，只追踪当前的前进段和后退段，一是需要遍历的总路径长度要小，二是可以在开始阶段就把那种前进后退很靠近的路段隔离开来

function target_idx = calc_target_index(x,y, refPos_x,refPos_y,yaw,last_idx,flag,segment_start,segment_end)
    target_idx = last_idx;
    %距离阈值
    dist_threshold=0.05;
    %是否上线的标志位
    is_onLine=0;
    mode_switching = false; % 新增模式切换标志
    len=segment_end-segment_start-1;
    % 寻找最近的路径点作为初始目标点
    min_dist = inf;
    closest_idx = last_idx;
    for i = segment_start + 1 : 1 : segment_end
        dist = sqrt((refPos_x(i) - x)^2 + (refPos_y(i) - y)^2);
        if dist < min_dist
            min_dist = dist;
            closest_idx = i;
        end
    end
    for i=segment_start+1:1:segment_end
        dist = sqrt((refPos_x(i)-x).^2 + (refPos_y(i)-y).^2);
        TargetHeading = atan2(refPos_y(i)-refPos_y(i-1) , refPos_x(i)-refPos_x(i-1));
        headingError = TargetHeading-yaw; 
        if(dist<=dist_threshold)
            is_onLine=1;%上线条件
        end
        % 检测模式是否需要切换
        if i > 1 && flag(i) ~= flag(i-1)
            mode_switching = true;
        end
        if mode_switching
            % 在模式切换时选择合适的目标点
            if flag(i) == -1 % 如果是后退模式
                target_idx = i; % 选择当前点作为目标
                break;
            end
        elseif(flag(i)==1)%车辆前进
            if (is_onLine~=1) %如果距离太大，就谁寻目标点中距离最小的那个点
                %超级远就选择第一个点作为跟踪点
                if(dist < 0.15 && abs(headingError)<pi/4)%小角度上线
                    target_idx = i;
                     break;
                elseif(dist>=0.15) %大偏转角度
                    target_idx = closest_idx;
                end
            else %上线了，就按照顺序一个一个追踪就成
                if(last_idx<length(refPos_x))
                    target_idx=last_idx+1;
                else
                    target_idx=last_idx;
                end
                break;
            end
        end
    end
end