Stanley 轨迹跟踪算法研究

Stanley 轨迹跟踪算法研究

理论基础

首先还是阅读论文

《基于改进鲸鱼优化算法的Stanley算法研究》
《复杂环境下的移动机器人路径规划技术研究》
《Stanley:The Robot that Won the DARPA Grand Challenge》
《无人驾驶汽车轨迹跟随控制研究》
《基于混合算法的校园智能车路径规划与跟踪控制方法研究》

公式推导部分都不是很详细，第一篇论文中的推导算是比较详细的

需要注意的是 Stanley 算法不仅可以用于轨迹跟踪控制，也可以用于纵向车速控制

又读了一些博客

Stanley method无人车轨迹追踪
基于车辆运动学模型的轨迹跟踪方法之----Stanley法
基于运动学约束的轨迹跟踪算法（Pursuit法，Stanley法）

博客中对于公式的解释和推导还是比较清晰的，可以参考第一篇博客

我理解的Stanley方法就是：首先我们在横向控制里面可以控制的只有一个前轮转角（暂时不考虑纵向速度），那么这么前轮转角可以分成两部分，一部分去补偿横向距离误差，写成ed呀，d呀，ey呀啥样都行，另外一部分去补偿航向角误差，也是写成啥都行
Pure pursuit 与 Stanley method 两个方法都是基于对前轮转角进行控制来消除横向误差，因此在上文中还结合了一个P控制器用作纵向的速度控制，以达到车辆跟踪期望根轨迹的效果。由于这两种算法实际都是基于运动学模型的纯几何跟踪方法 ，因此算法在高速环境下的实际表现不会像上文中这样优异。但当其用作中低速场景时，还是有比较简单可靠的 。其中PP算法的关键在于预瞄点的选取：其距离越短，控制精度越高，但可能会产生震荡；预瞄距离越长，控制效果趋于平滑，震荡减弱 。实际调试只需根据上述规则以及应用需求调整预瞄系数即可。相反，Stanley method的控制效果取决于控制增益且缺少PP算法的规律性，通过仿真环节可以发现这个值无论是偏大还是偏小都很难有一个较好的控制效果，调试时需要花一定精力去找那个合适的值
stanley比pure优势在于考虑到了预瞄点和真实车辆的heading angle偏差， 作为一个前馈控制，提前补足了在转弯过程中的车辆转角，即可以根据需求，对前馈值进一步增加增益系数

同时，还推荐使用与速度相关的可变增益系数，可以更好的弥补此类仅考虑车辆运动学，而忽略车辆动力学的模型的不足。增益系数的设定方法可以参考PID系数整定法

Stanley算法在平滑路径上具有准确的跟踪性能，但在路径曲率处的快速变化会导致较大的跟踪误差，适合曲率是连续的低速场景

仿真参考

让 ChatGPT 写了一个仿真，stanley_chatgpt.m

% Stanley 轨迹跟踪算法仿真示例

% 车辆参数
L = 2.5;  % 车辆轴距

% 目标路径
path = [0:0.1:10; sin(0:0.1:10)];  % 示例目标路径，可以根据需要自定义

% 初始化车辆状态
x = 0;    % 初始横向位置
y = 0;    % 初始纵向位置
theta = 0;  % 初始航向角
v = 5;    % 初始车速

% 控制参数
K = 0.1;    % 控制增益
Kp = 1.0;   % 偏航角控制增益

% 仿真参数
dt = 0.1;   % 仿真步长
T = 10;     % 仿真时长
num_steps = T / dt;  % 仿真步数

% 计算参考轨迹（双移线轨迹）
ref_path = [path(1,:); path(2,:) + 1; path(2,:) - 1];  % 横向偏移为1和-1的参考轨迹

% 仿真循环
for i = 1:num_steps
    % 计算横向误差（横向位置偏差）
    idx = find(ref_path(1,:) >= x, 1, 'first');
    if isempty(idx)
        idx = length(ref_path(1,:));
    end
    desired_y = ref_path(2,idx);
    e = desired_y - y;
    
    % 计算航向角误差
    desired_theta = atan2(ref_path(2,idx+1) - desired_y, ref_path(1,idx+1) - x);
    theta_e = wrapToPi(desired_theta - theta);
    
    % 控制指令计算
    delta = atan2(K * e, v) + Kp * theta_e;
    
    % 更新车辆状态
    x = x + v * cos(theta) * dt;
    y = y + v * sin(theta) * dt;
    theta = theta + v / L * tan(delta) * dt;
    
    % 可视化车辆轨迹
    plot(x, y, 'ro');   % 绘制车辆当前位置
    hold on;
    plot(path(1,:), path(2,:), 'b');   % 绘制目标路径
    plot(ref_path(1,:), ref_path(2,:), 'g--');  % 绘制参考轨迹
    plot([x, x + 2*cos(theta)], [y, y + 2*sin(theta)], 'r');  % 绘制车辆朝向线
    hold off;
    axis equal;
    xlim([-10, 20]);
    ylim([-10, 10]);
    xlabel('X');
    ylabel('Y');
    title('Stanley Trajectory Tracking');
    drawnow;
    
    % 仿真暂停一段时间，以观察结果
    pause(0.1);
end

并不太好

参考下面的博客，stanley_blog.m

Stanley轨迹跟踪算法Python/Matlab算法实现

clc
clear all

k = 0.1;  % look forward gain
global Kp ;
Kp = 1.0 ; % speed propotional gain
dt = 0.1  ;% [s]
L = 2  ;% [m] wheel base of vehicle

cx = 0:0.1:200; % sampling interception from 0 to 100, with step 0.1
for i = 1:500% here we create a original reference line, which the vehicle should always follow when there is no obstacles;
    cy(i) = -sin(cx(i)/10)*cx(i)/8;
end
for i = 501: length(cx)
    cy(i) = -sin(cx(i)/10)*cx(i)/8; %cy(500);
end
p = [cx', cy'];

%计算一阶导数
for i = 1 : length(cx)-1
    pd(i) = (p(i+1,2)-p(i,2))/(p(i+1,1)-p(i,1));
end
pd(end+1) = pd(end);
%计算二阶导数
for i = 2 : length(cx)-1
    pdd(i) = (p(i+1,2)-2*p(i,2) + p(i-1,2))/(0.5*(-p(i-1,1)+p(i+1,1)))^2;
end
pdd(1) = pdd(2);
pdd(length(cx)) = pdd(length(cx)-1);
%计算曲率
for i  = 1:length(cx)-1
    k(i) = (pdd(i))/(1+pd(i)^2)^(1.5);
end

%计算每个路径点处的航向角
cyaw = atan(pd);
ck = k;

i = 1;
target_speed = 30/3.6;
T = 60;
lastIndex = length(cx);
x = 0; y = 0; yaw = 0; v = 0;
time = 0;
target_ind = 0;

figure
while T > time && target_ind <length(cx)    
    [error, target_ind]= calc_target_index(x,y,yaw,cx,cy,L)
    
    if abs(error)> 4
        fprintf('mayday mayday!\n')
        break;
    end
    
    ai = PIDcontrol(target_speed,v,Kp);
    di = stanley_control(x,y,yaw,v,cx,cy,cyaw,target_ind,ck,L, error);
    
    [x,y,yaw,v] = update(x,y,yaw,v,ai,di,dt,L);
    time = time + dt;
    %     pause(0.1)
    subplot(3,1,1)
    plot(cx,cy,'b',x,y,'r-*','LineWidth',0.2)
    hold on
    subplot(3,1,2)
    plot( i, error,".b")
    hold on
    subplot(3,1,3)
    plot( i, yaw,".r")
    
    drawnow
    hold on
    i  = i + 1;
end

function [x, y, yaw, v] = update(x, y, yaw, v, a, delta,dt,L)
    x = x + v * cos(yaw) * dt;
    y = y + v * sin(yaw) * dt;
    yaw = yaw + v / L * tan(delta) * dt;
    v = v + a * dt;
end

function [a] = PIDcontrol(target_v, current_v, Kp)
    a = Kp * (target_v - current_v);
end

function [delta] = stanley_control(x,y,yaw,v,cx,cy,cyaw,ind,ck,L,error)
    tx = cx(ind + 5);
    ty = cy(ind + 5);
    %delta_yaw做什么用
    delta_yaw = v * 0.1 * sin(yaw) / L;
    %     th_e = pipi(yaw + delta_yaw - cyaw(ind));
    %alpha角的计算感觉有问题
    alpha = pipi(atan((ty-y)/(tx-x)) -yaw);
    gain_k = 1;
    theta = atan(gain_k* error/v);
    delta = alpha + theta;
end

function [error, ind] = calc_target_index(x,y,yaw,cx,cy,L)
    %计算前轴位置
    x = x + L * cos(yaw);
    y = y + L * sin(yaw);
    %计算每个路径点到前轴的距离
    N =  length(cx);
    Distance = zeros(N,1);
    for i = 1:N
        Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2);
    end
    %ind是最近点索引
    [value, location]= min(Distance);
    ind = location;
    %error是横向误差，根据实际位置与参考点的y值判断
    if y<cy(ind)
        error = -value;
    else
        error = value;
    end
end

function [angle] = pipi(angle)
    if (angle > pi)
        angle =  angle - 2*pi;
    elseif (angle < -pi)
        angle = angle + 2*pi;
    else
        angle = angle;
    end
end

效果还不错