基于前轮误差反馈的路径跟踪Stanley算法|matlab|c++|Webots

未定义～

已于 2023-09-10 22:35:46 修改

阅读量456

点赞数

分类专栏：路径跟踪算法文章标签：算法 matlab c++

于 2023-09-05 10:10:34 首次发布

本文链接：https://blog.csdn.net/weixin_44519259/article/details/132669188

版权

路径跟踪算法专栏收录该内容

3 篇文章 0 订阅

订阅专栏

【路径跟踪】基于前轮误差反馈的路径跟踪Stanley算法个人总结|matlab|c++|Webots

参考资料
1. Stanley算法
2. matlab代码
3 移植到Webots仿真 | c++
- 3.1 主要部分改成C++代码
- 3.2 仿真结果
4. 总结

参考资料

1. Stanley算法

最近在做项目需要仿真一个前轮转向、驱动的自走剪叉式升降平台底盘，其中需要对路径跟踪，正好借此机会将之前了解过的路径跟踪类的知识应用一下。Stanley算法是比较早的基于几何法的一种路径跟踪方法。

1.1 算法思想

前轮反馈控制(Front wheel feedback)也就是常说的Stanley方法，其核心思想是基于车辆前轴中心点的路径跟踪偏差量对方向盘转向控制量进行计算，这一类算法往往运用于阿克曼转向底盘。

Stanley方法是一种基于横向跟踪误差（前轴中心到最近路径点的距离，该距离网上的计算方式存在差异，也是该算法的一个需要注意的点，笔者认为是前轴中心坐标到最近路径点对应切线的垂直距离）的非线性反馈函数,并且能实现横向跟踪误差指数收敛于0。

1.2 公式

这里不在阐述，参考的网上的数学公式推导
在这里插入图片描述

1.3 算法流程

输入：当前车辆位置（主要是前轮中心的位置，如果是后轮，需要转换到前轮：根据后轮坐标、前后轮轴距、车辆偏航角计算）；航向角 $\theta$ _yaw；速度v（注意这里用的是前轮的速度）；离车前轴中心最近目标路径点处的航向角，即切线角；
根据当前机器人速度v、和目标速度target_v，PID控制器单纯P控制器计算加速度
遍历循环路径上距离当前坐标最近的路径点，如果量大了，网上说可以用KD tree算法等搜索
计算Stanley算法，计算关键参数：横向误差，前轮中心到目标路径点切线垂直横向距离e_f，笔者是看了网上的几种代码里的计算，然后分析认为应该计算坐标点到参考路径点切线的垂直距离，学习到了利用向量计算的方式得到一些几何距离之类的，注意横向距离具有正负性。然后根据计算公式得到前轮转角 $\delta$ 。
然后更新机器人状态

2. matlab代码

2.1 车辆模型

四轮阿克曼转向底盘一般简化为双轮单车模型
在这里插入图片描述

2.2 参考路径生成

部分代码示意：


map_L = 4;
map_R = 5.3 / 2;
RefPos = zeros(50,2);
RefPos(:,1) = map_R*6;
RefPos(:,2) = linspace(0, 2*map_L - 0.5, 50);

theta = linspace(0,1*pi, 40);
x = map_R * 5 + map_R*cos(theta);
y = map_L*2 + map_R*sin(theta);
RefPos = [RefPos;[x' y']];

% 计算离散轨迹每个点的参考航向角
diff_x = diff(RefPos(:,1)) ;
diff_x(end+1) = diff_x(end);
diff_y = diff(RefPos(:,2)) ;
diff_y(end+1) = diff_y(end);
refHeading = atan2(diff_y , diff_x);
refHeading = mod((2*pi + refHeading), 2*pi);

在这里插入图片描述

2.3 初始参数设置

%% initialization相关参数定义
target_speed = 1;      % m/s
Kv =1;              % 横向误差增益系数 越大，越振荡，轨迹跟随越好，越小，越平滑，轨迹误差越大
Kp = 0.8;              % 速度P控制器系数
dt = 0.02;              % 时间间隔，单位：s
L = 2.24;               % 车辆轴距，单位：m

% pos = RefPos(1,:)+1;%初始位置
pos = [map_R*6, 0 + L];%初始位置
v = 0;%初始速度
heading = pi/2;%初始航向角

2.4 速度跟随

这里只需简单计算，用一个P控制器即可

function [a] = PIDcontrol(target_v, current_v, Kp)              %计算加速度的函数
a = Kp * (target_v - current_v);
end

2.5 搜索最近路径点

遍历即可，输出索引值target_index

function  [lookaheadPoint,idx_target] = findLookaheadPoint(pos, RefPos)

% 找到距离当前位置最近的一个参考轨迹点的序号
sizeOfRefPos = size(RefPos,1);%

for i = 1:sizeOfRefPos
    dist(i,1) = norm(RefPos(i,:) - pos);
end
[~,idx] = min(dist);

idx_target = idx; % 索引
lookaheadPoint = RefPos(idx,:);%某一个点的坐标
end

2.6 计算Stanley算法

计算横向误差
思路：计算一点到切线的垂直距离，可以利用向量法计算，设参考路径点A（x₀, y₀），当前车辆前轮中心P（x₁, y₁）， $\vec{AP}$ （x₁ - x₀, y₁ - y₀）为路径点指向当前坐标点的向量, $\vec{AB}$ 为路径点切线单位向量(cos( $\alpha$ )，sin( $\alpha$ ))， $\alpha$ 为路径点切线角，生成路径时可以求导计算。
然后利用向量叉乘的知识点点到直线最短距离–向量法，
在这里插入图片描述
P点到达单位向量 $\vec{AB}$ 的距离即为| $\vec{PC}$ |，其中| $\vec{AB}$ |为单位向量的模，即为1。故此横向误差e_f=| $\vec{AB}$ X $\vec{AP}$ |，该值为绝对值，差一个正负号判断，这里两个向量叉积得到的其实还是向量，本身带有方向，故此我们只需要将叉积方向与我们所需要的横向误差正负号对应即可。

继续分析：叉积得到的方向知识复习：通过叉积公式来判断一个点在矢量线段的那一侧的方法
在这里插入图片描述
从中我们可以知道叉积正负与两向量的顺逆时针方向是有关的。继续分析下图，P为当前坐标点，去掉取模符号后， $\vec {ef}$ = $\vec{AB}$ X $\vec{AP}$ ，叉积是有前后顺序的，当 $\vec {ef}$ = （X_ab*y_ap - X_ap*y_ab）< 0时，意味着AB在AP逆时针，符合下图情况，此时横向误差认为是正号，对应着前轮转角（逆正顺负）应该加上一个值。此时横向误差计算完毕。
在这里插入图片描述

function [delta,error] = Stanley_control(idx,pos, heading, v,RefPos,refHeading, Kv)
sizeOfRefPos = size(RefPos,1);%%=1223

if idx < sizeOfRefPos
    Point_temp = RefPos(idx, :);
else
    idx = sizeOfRefPos;
    Point_temp = RefPos(end,1:2);%从最后一行开始，从1开始取，取到第2个元素，也就是最后一个点的横纵坐标
end
% 向量法在工程计算中的应用，**关键点，复习**
% 通过最近轨迹点所在的切线单位向量与当前坐标点与最近轨迹点形成的向量，两个向量的叉积正负号判断坐标点在切线的哪一侧，则横向误差的正负号可确定
% if (cos(refHeading(idx)) * (pos(2)-Point_temp(2)) - sin(refHeading(idx)) * (pos(1)-Point_temp(1)) > 0)
%     error = -abs(sqrt((pos(1)-Point_temp(1))^2 + (pos(2)-Point_temp(2))^2));
%网上的一些error算错了，算成了两点距离了，应该算点到切线的距离，这个情况再直线上反应了，
%当机器人沿着一条直线走，前轮转角是为0的，但是当前坐标点到参考路径点是有距离的
% else
%     error = abs(sqrt((pos(1)-Point_temp(1))^2 + (pos(2)-Point_temp(2))^2 ));
% end
% 向量叉积计算横向误差方式：没有在网上看见这个的计算，自己琢磨出来的一个，效果是对的，其他博客里的Stanley说效果不好，有这个点的原因
    error = -(cos(refHeading(idx)) * (pos(2)-Point_temp(2)) - sin(refHeading(idx)) * (pos(1)-Point_temp(1)));

% 前轮转角计算公式，这里其实存在v开始等于0，导致除数为零异常c++里，可以加上一个v==0的判断
delta = (refHeading(idx) - heading) + atan2(Kv * error, v);
%     float delta = 0;
%     if (v == 0)
%         delta = ref_yaw[idx] - yaw;
%     else
%         delta = ref_yaw[idx] - yaw + atan2(Kv * error, v);

end

2.7 更新车辆状态

function [pos_now, yaw, v] = update(pos_last, yaw, v, ai, delta,dt,carL)   %更新车辆状态的函数
pos_now(1) = pos_last(1) + v * cos(yaw + delta) * dt;%注意这里加上了delta，因为是前轮的速度，与车辆航向角存在一个转角值delta的差别，这也是其他博客里没有注意的地方
pos_now(2) = pos_last(2) + v * sin(yaw + delta) * dt;
yaw = yaw + v * sin(delta) / carL * dt; % 注意速度是前轮还是后轮，w = v/R,R = carL /前轮时：sin(delta) or 后轮时：tan(delta)
v = v + ai * dt;
end

2.8 主函数


i = 0;
while 100 > time
    i = i+1;
    % 下面三行函数调用计算
    ai = PIDcontrol(target_speed,v,Kp); %
    [Look_pos, target_index] = findLookaheadPoint(pos, RefPos);                 %输出距离车辆当前最近的点的位置
    [delta,Error] = Stanley_control(target_index,pos, heading, v,RefPos,refHeading, Kv);
    %下面是画图的一些变量
    path_look(i,:) = Look_pos;
    path_pos(i,:) = pos;
    path_Error(i,:) = [time,Error];
    path_v(i,:) = [time, v];
    path_yaw(i,:) = [time, heading, refHeading(target_index)];
    path_delta(i,:) = [time, delta * 180 / pi];
    %
    
    [pos, heading, v] = update(pos, heading, v, ai, delta,dt,L);   %更新车辆状态的函数
    time = time + dt;                          %时间过完一周期
    
    if abs(pos(1) - RefPos(end,1)) < 0.5 && abs(pos(2) - RefPos(end,2)) < 0.5
        break % 车辆已经到达最后点，停止
    end
end

2.9 运行效果

左上角为结果图，红色为机器人路径，绿色为跟踪点路径，从右下角开始运动的，跟踪效果良好，有小震荡，图2是横向误差，图3是航向角与参考航向角，图4是前轮转角，中间一根线是因为有没删除的（0，0）导致，忽略即可。
在这里插入图片描述

2.10 完整可运行matlab代码

% 基于前轮中心横向误差和航向角误差的Stanley算法
%% 轨迹点计算
clear all;
clc;
close all;
map_L = 4;
map_R = 5.3 / 2;
RefPos = zeros(50,2);
RefPos(:,1) = map_R*6;
RefPos(:,2) = linspace(0, 2*map_L - 0.5, 50);

theta = linspace(0,1*pi, 40);
x = map_R * 5 + map_R*cos(theta);
y = map_L*2 + map_R*sin(theta);
RefPos = [RefPos;[x' y']];

x = linspace(map_R*4, map_R*4, 30);
y = linspace(map_L*2 - 0.5, map_L + 0.5, 30);
RefPos = [RefPos;[x' y']];

theta = linspace(0,-1*pi, 40);
x = map_R*3 + map_R*cos(theta);
y = map_L + map_R*sin(theta);
RefPos = [RefPos;[x' y']];

x = linspace(map_R * 2, map_R * 2, 30);
y = linspace(map_L+0.5, map_L*2 - 0.5, 30);
RefPos = [RefPos;[x' y']];

theta = linspace(0,1*pi, 40);
x = map_R + map_R*cos(theta);
y = map_L*2 + map_R*sin(theta);
RefPos = [RefPos;[x' y']];

x = linspace(0, 0, 30);
y = linspace(map_L*2-0.5, 3, 30);
RefPos = [RefPos;[x' y']];
% 计算离散轨迹每个点的参考航向角
diff_x = diff(RefPos(:,1)) ;
diff_x(end+1) = diff_x(end);
diff_y = diff(RefPos(:,2)) ;
diff_y(end+1) = diff_y(end);
refHeading = atan2(diff_y , diff_x);
refHeading = mod((2*pi + refHeading), 2*pi);
%% initialization相关参数定义
target_speed = 1;      % m/s
Kv =1;              % 横向误差增益系数 越大，越振荡，轨迹跟随越好，越小，越平滑，轨迹误差越大
Kp = 0.8;              % 速度P控制器系数
dt = 0.02;              % 时间间隔，单位：s
L = 2.24;               % 车辆轴距，单位：m

% pos = RefPos(1,:)+1;%初始位置
pos = [map_R*6, 0 + L];%初始位置
v = 0;%初始速度
heading = pi/2;%初始航向角
%% 主计算函数
time = 0;
path_look = zeros(5000,2);
path_pos = zeros(5000,2);
path_Error = zeros(5000,2);
path_v = zeros(5000,2);
path_yaw = zeros(5000, 3);
path_delta = zeros(5000, 2);

i = 0;
while 100 > time
    i = i+1;
    ai = PIDcontrol(target_speed,v,Kp); %
    [Look_pos, target_index] = findLookaheadPoint(pos, RefPos);                 %输出距离车辆当前最近的点的位置
    [delta,Error] = Stanley_control(target_index,pos, heading, v,RefPos,refHeading, Kv);
    
    path_look(i,:) = Look_pos;
    path_pos(i,:) = pos;
    path_Error(i,:) = [time,Error];
    path_v(i,:) = [time, v];
    path_yaw(i,:) = [time, heading, refHeading(target_index)];
    path_delta(i,:) = [time, delta * 180 / pi];
    
    [pos, heading, v] = update(pos, heading, v, ai, delta,dt,L);   %更新车辆状态的函数
    time = time + dt;                          %时间过完一周期
    
    if abs(pos(1) - RefPos(end,1)) < 0.5 && abs(pos(2) - RefPos(end,2)) < 0.5
        break % 车辆已经到达最后点，停止
    end
end
%% 画图
figure(1);
axis equal
set(gcf, 'unit', 'centimeters', 'position', [10 8 21 15]);

subplot(2,2,1)
hold on;
plot(path_look(:,1), path_look(:,2), 'go', 'LineWidth', 5);
axis equal;
plot(path_pos(:, 1), path_pos(:, 2), 'r-', 'LineWidth', 1);
legend('look_pos','pos');
hold on;
title("路径图");

subplot(2,2,2)
plot(path_Error(:,1), path_Error(:,2));
title('path_error / time')

subplot(2,2,3)
plot(path_yaw(:,1), path_yaw(:,2) * 180 / pi,'r*', path_yaw(:,1), path_yaw(:,3) * 180 / pi, 'go');
title("yaw robot & ref_yaw");
legend('yaw','ref_yaw');

subplot(2,2,4)
plot(path_delta(:,1), path_delta(:,2));
title("angle front");

%%
function [pos_now, yaw, v] = update(pos_last, yaw, v, ai, delta,dt,carL)   %更新车辆状态的函数
pos_now(1) = pos_last(1) + v * cos(yaw + delta) * dt;
pos_now(2) = pos_last(2) + v * sin(yaw + delta) * dt;
yaw = yaw + v * sin(delta) / carL * dt; % 注意速度是前轮还是后轮，w = v/R,R = carL /前轮时：sin(delta) or 后轮时：tan(delta)
v = v + ai * dt;
end
%%
function [a] = PIDcontrol(target_v, current_v, Kp)              %计算加速度的函数
a = Kp * (target_v - current_v);
end
%% 计算控制量，也就是前轮转角
function [delta,error] = Stanley_control(idx,pos, heading, v,RefPos,refHeading, Kv)
sizeOfRefPos = size(RefPos,1);%%=1223

if idx < sizeOfRefPos
    Point_temp = RefPos(idx, :);
else
    idx = sizeOfRefPos;
    Point_temp = RefPos(end,1:2);%从最后一行开始，从1开始取，取到第2个元素，也就是最后一个点的横纵坐标
end
% 向量法在工程计算中的应用，**关键点，复习** error-> = e-> X Q->  / |e->|
% 通过最近轨迹点所在的切线单位向量与当前坐标点与最近轨迹点形成的向量，两个向量的叉积正负号判断坐标点在切线的哪一侧，则横向误差的正负号可确定
% if (cos(refHeading(idx)) * (pos(2)-Point_temp(2)) - sin(refHeading(idx)) * (pos(1)-Point_temp(1)) > 0)
%     error = -abs(sqrt((pos(1)-Point_temp(1))^2 + (pos(2)-Point_temp(2))^2
%     ));%网上的这个error算错了，算成了两点距离了，应该算点到切线的距离
% else
%     error = abs(sqrt((pos(1)-Point_temp(1))^2 + (pos(2)-Point_temp(2))^2 ));
% end
    error = -(cos(refHeading(idx)) * (pos(2)-Point_temp(2)) - sin(refHeading(idx)) * (pos(1)-Point_temp(1)));

% 前轮转角
delta = (refHeading(idx) - heading) + atan2(Kv * error, v);

end
%%
function  [lookaheadPoint,idx_target] = findLookaheadPoint(pos, RefPos)

% 找到距离当前位置最近的一个参考轨迹点的序号
sizeOfRefPos = size(RefPos,1);%

for i = 1:sizeOfRefPos
    dist(i,1) = norm(RefPos(i,:) - pos);
end
[~,idx] = min(dist);
% 从该点开始向轨迹前方搜索，找到与预瞄距离最相近的一个轨迹点
% L_steps = 0;           % 参考轨迹上几个相邻点的累计距离
% Ld = Kv*v + Ld0;       % Ld0是预瞄距离的下限值；
%
% while L_steps < Ld && idx < sizeOfRefPos%%重点看
%     L_steps = L_steps + norm(RefPos(idx + 1,:) - RefPos(idx,:));
%     idx = idx+1;
% end
%
idx_target = idx; % 索引
lookaheadPoint = RefPos(idx,:);%某一个点的坐标
end

3 移植到Webots仿真 | c++

3.1 主要部分改成C++代码

从matlab移植到webots里仿真，以下是算法部分c++代码供参考。

#include <vector>
#include <cmath>
#include <algorithm>
#include <iostream>

vector<vector<float>> ref_pos = {{15.9000000000000, 0}, {15.9000000000000, 0.163265306122449};
vector<float> ref_yaw = {1.57079632679490, 1.57079632679490};
float target_speed = 1; // m / s
float Kv = 1;          // % 横向误差增益系数 best:1 ;越大，越振荡，轨迹跟随越好，越小，越平滑，轨迹误差越大
float Kp = 0.8;         // % 速度P控制器系数
float L = 2.24;         // % 车辆轴距，单位：m
float path_latError;
vector<float> look_pos;
float out_ld;

float PID_vel_control(float target_v, float current_v, float kp)
{
    return kp * (target_v - current_v);
}

float Stanley_control(int idx, vector<float> pos, float yaw, float v, vector<vector<float>> ref_pos, vector<float> ref_yaw, float Kv)
{
    int sizeOfRefPos = ref_pos.size();
    vector<float> Point_temp;

    if (idx < sizeOfRefPos)
        Point_temp = ref_pos[idx];
    else
    {
        Point_temp = ref_pos[sizeOfRefPos];
        idx = sizeOfRefPos;
    }
    float error = -(cos(ref_yaw[idx]) * (pos[1] - Point_temp[1]) - sin(ref_yaw[idx]) * (pos[0] - Point_temp[0]));

    path_latError = error;
    // % 前轮转角
    float delta = 0;
    if (v == 0)
        delta = ref_yaw[idx] - yaw;
    else
        delta = ref_yaw[idx] - yaw + atan2(Kv * error, v);
    printf("error: %4.2f, delta: %4.2f\n", error, Rad2Deg(delta));
    return delta;
}

int findLookaheadPoint(vector<float> pos, float v, vector<vector<float>> RefPos, float Kv)
{
    //    % 找到距离当前位置最近的一个参考轨迹点的序号
    int sizeOfRefPos = RefPos.size();
    vector<float> dist;
    for (int i = 0; i < sizeOfRefPos; i++)
        dist.push_back(hypot(RefPos[i][0] - pos[0], RefPos[i][1] - pos[1]));

    int idx = min_element(dist.begin(), dist.end()) - dist.begin();
    return idx;
}

float run(float time, float &v, float yaw, vector<float> pos, int dt)
{
    float ai = PID_vel_control(target_speed, v, Kp);
    int target_index = findLookaheadPoint(pos, v, ref_pos, Kv); // % 输出距离车辆当前最近的点的位置
    float delta = Stanley_control(target_index, pos, yaw, v, ref_pos, ref_yaw, Kv);

    vector<vector<float>> path_look, path_pos;
    path_look.push_back(ref_pos[target_index]);
    path_pos.push_back(pos);

    v = v + ai * dt * 0.001;
    cout << "v:" << v << " ai: " << ai << endl;
    delta = Limit(delta, Deg2Rad(77.5), Deg2Rad(-77.5));
    look_pos = ref_pos[target_index];
    return delta;
}

3.2 仿真结果

在webots仿真环境中实验路径跟踪：
请添加图片描述
跟踪效果：良好的图：
在这里插入图片描述
效果不好的图：

横向误差：
在这里插入图片描述

4. 总结

Stanley算法是基于几何方法的，跟踪的是前轴中心，适用于中低速、路径平滑的环境中，算法实施简单，效果良好。在实验过程中发现由于跟踪前轴中心，导致车身航向角收敛较慢，横向误差收敛效果好。如果需要航向角也较好跟踪，可能更建议用纯跟踪法PurePursuit算法，跟踪后轮中心更符合实际车辆情况。
该算法中核心应该就是横向误差的计算，以及注意跟踪的时前轮中心，一些运动学计算需要注意。以上只是个人总结，如个人理解有误，见谅。

未定义～

关注

0
点赞
踩
7

收藏

觉得还不错? 一键收藏
1
评论
基于前轮误差反馈的路径跟踪Stanley算法|matlab|c++|Webots

Stanley算法是基于几何方法的，跟踪的是前轴中心，适用于中低速、路径平滑的环境中，算法实施简单，效果良好。在实验过程中发现由于跟踪前轴中心，导致车身航向角收敛较慢，横向误差收敛效果好。如果需要航向角也较好跟踪，可能更建议用纯跟踪法PurePursuit算法，跟踪后轮中心更符合实际车辆情况。该算法中核心应该就是横向误差的计算，以及注意跟踪的时前轮中心，一些运动学计算需要注意。以上只是个人总结，如个人理解有误，见谅。
复制链接

扫一扫