Stanley 轨迹跟踪算法研究

news2024/11/14 15:44:14

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

效果还不错

在这里插入图片描述

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/902233.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

【kafka】-分区-消费端负载均衡

一.为什么kafka要做分区&#xff1f; 因为当一台机器有可能扛不住&#xff08;类比&#xff1a;就像redis集群中的redis-cluster一样&#xff0c;一个master抗不住写&#xff0c;那么就多个master去抗写&#xff09;&#xff0c;把一个队列的单一master变成多个master&#xf…

学习嵌入式

最近&#xff0c;打算了解一些关于嵌入式的知识。这里先了解嵌入式到底是什么。

[ MySQL ] — 常见函数的使用

目录 日期函数 current_date — 获取当前日期 current_time — 获取当前时间 current_timestamp — 获取当前时间戳 date — 获取参数的日期部分 ​编辑 date_add — 在日期或时间的基础上进行增加 date_sub — 在日期或时间的基础上进行减少 datediff — 计算两个日期相差…

快速上手Linux核心命令:Linux系统信息相关命令

前言 这期呢主要说一说Linux中与系统相关的命令&#xff0c;一共包含10个命令 uname 显示系统信息 1、简介 uname命令用于显示系统相关信息&#xff0c;比如内核版本号、硬件架构等 2、语法格式 uname [参数选项] 3、参数说明 参数参数说明-a显示系统所有相关信息-m显示计算…

通过重构来加深理解——DDD

要想成功地开发出实用的模型&#xff0c;需要注意以下三点&#xff1a; &#xff08;1&#xff09;复杂巧妙的领域模型是可以实现的&#xff0c;也是值得我们去花费力气实现的。 &#xff08;2&#xff09;这样的模型离开不断的重构是很难开发出来的&#xff0c;重构需要领域专…

2. Linux Server 20.04 Qt5.14.2配置Jetson Orin Nano Developer Kit 交叉编译环境

最近公司给了我一块Jetson Orin Nano的板子&#xff0c;先刷了系统&#xff08;1.Jetson Orin Nano Developer Kit系统刷机&#xff09;又让我搭建交叉编译环境&#xff0c;所以有了下面的文章 一 :Qt5.14.2交叉编译环境安装 1.准备 1.1设备环境 1.1.1 Server: Ubuntu20.0…

md文本学习

这里写自定义目录标题 欢迎使用Markdown编辑器新的改变功能快捷键合理的创建标题&#xff0c;有助于目录的生成如何改变文本的样式插入链接与图片如何插入一段漂亮的代码片生成一个适合你的列表创建一个表格设定内容居中、居左、居右SmartyPants 创建一个自定义列表如何创建一个…

腾讯云轻量应用服务器配置_网站上线全流程

腾讯云轻量应用服务器CPU内存带宽配置高&#xff0c;成本很低&#xff0c;腾讯云百科来详细说下腾讯云服务器从购买、配置到网站上线全流程&#xff0c;包括轻量服务器配置选择、应用镜像选择、重置密码、防火墙开放端口教程等详细教程&#xff1a; 目录 一&#xff1a;注册腾…

年薪100w的项目组合和项目集经理与项目经理的区别

项目经理未来的发展是什么&#xff1f;很多人经常问&#xff0c;专业的路可以是项目集&#xff0c;项目组合经理&#xff0c;也可以是PMO等等。为什么项目集经理&#xff0c;项目组合经理就挣得比较多呢&#xff1f;今天为大家一一揭秘&#xff01; 项目经理、项目集经理和项目…

【2023年11月第四版教材】《第5章-信息系统工程(合集篇)》

《第5章-信息系统工程&#xff08;合集篇&#xff09;》 章节说明1 软件工程1.1 架构设计1.2 需求分析1.3 软件设计1.4 软件实现&#xff3b;补充第三版教材内容&#xff3d; 1.5 部署交付 2 数据工程2.1 数据建模2.2 数据标准化2.3 数据运维2.4 数据开发利用2.5 数据库安全 3 …

攻防世界-backup

原题 解题思路 备份文件后缀大多是bak、git、svn、swp等&#xff0c;尝试index.php.bak就有文件下载了:

文件型数据库Derby操作示例

1.下载db-derby-10.14.2.0-bin.zip 2.启动数据库&#xff0c;两种方式 1) 可执行程序 Windows可以运行ij.bat, Linux可以运行ij Ij.bat, ij在bin目录里 2&#xff09;通过执行jar包形式启动 Derbyrun.jar在lib目录里 执行Java -jar derbyrun.jar ij 启动 2. 连接数据库 …

除了提升编码能力,养成这4个工作习惯,让你成为更优秀的前端开发者

如果你想成为一名优秀的前端开发者&#xff0c;你需要拥有不仅仅是技术知识。如今&#xff0c;由于谷歌和其他资源的存在&#xff0c;获取技术知识比以前更容易了。而真正能够产生差异的是习惯。就像心理强度对于个人成长很重要一样&#xff0c;有些习惯可以让你成为一个更有效…

七夕最强Python表白代码来了

点击上方Python小二&#xff0c;选择星标公众号 干货速达&#xff0c;不迷路 快到七夕了&#xff0c;大家都懂&#xff0c;这里不过多解释了&#xff0c;送大家几段节日专属Python代码。 玫瑰 毫无疑问&#xff0c;玫瑰一直都是七夕、520......这类节日的专属&#xff0c;带文字…

最长公共子序列——力扣1143

解法:动态规划 int longestCommonSubsequence(string text1, string text2){int m=text1.size(), n=text2.size

高等数学上册 第九章 多元函数微分法及其应用 知识点总结

多元函数微分法及其应用 &#xff08; 1 &#xff09;多元函数的极限&#xff1a; 用“ ε − δ ”语言描述&#xff0c;二元函数的极限叫二重极限 二重极限存在&#xff1a; { 1 、 P ( x , y ) 一定要以任何方式趋于 ( x 0 , y 0 ) 时&#xff0c; f ( x , y ) 无限趋近于 A…

flutter:webview_flutter和flutter_inappwebview的简单使用

前言 最近在研究如何在应用程序中嵌入Web视图&#xff0c;发现有两个库不错。 一个是官方维护、一个是第三方维护。因为没说特别的需求&#xff0c;就使用了官方库&#xff0c;实现一些简单功能是完全ok的 webview_flutter 不建议使用&#xff0c;因为效果不怎么样&#xf…

wps设置其中几页为横版

问题&#xff1a;写文档的时候&#xff0c;有些表格列数太多&#xff0c;纵向显示内容不完整&#xff0c;可以给它改成横向显示。 将鼠标放在表格上一页的底部&#xff0c;点击‘插入-分页-下一页分节符’。 将鼠标放在表格页面的底部&#xff0c;点击‘插入-分页-下一页分节符…

qt显示图片并转换成灰度图及伪彩图

写了个程序&#xff0c;可在途图片&#xff0c;并切换成灰度图及伪彩图显示&#xff0c;主要代码如下&#xff1a; #include "mainwindow.h" #include "ui_mainwindow.h"MainWindow::MainWindow(QWidget *parent): QMainWindow(parent), ui(new Ui::MainW…

Java日常的String、Date、计算问题

一、String相关类 三者执行速度&#xff1a;StringBuilder > StringBuffer > String 1.1、String 每次对 String 类型改变的时&#xff0c;都会生成一个新的 String 对象&#xff0c;指针指向新的 String 对象。 适用于字符串不常变的&#xff0c;少量的数据场景中&am…