草庐IT

自动驾驶-预瞄-Pure pursuit纯跟踪算法-MATLAB实现

VotaverUno 2023-04-10 原文

有空把引入、逻辑、原理介绍给写了，目前先给大家看看代码。

将来写大概会分成这么几块：

  • 汽车运动学自行车模型
  • 跟踪算法主流模型及特点
  • 纯跟踪算法原理推导
  • 代码介绍

代码原创，来之不易，请勿不注明转载。

喜欢点个赞吧！网上许多代码都跑不起来hh

clc;
clear;

% ------------form road------------
cx = 0:0.1:50;
cx = cx';
for i = 1:length(cx)
    cy(i) = sin(cx(i)/5)*cx(i)/2;
end

%--------form initial state---------
L = 2.9  ;% [m] wheel base of vehicle
i = 1;
target_speed = 2.5;
T = 80;
x = 0; y = -3; yaw = 0; v = 0;
time = 0;

%--------how to update--------------
lastIndex = length(cx)-1;
dt = 0.1  ;% [s]
Lfc = 1.0;  % look-ahead distance
k = 0.1;  % look forward gain
Kp = 1.0 ; % speed propotional gain
Lf = k * v + Lfc;
%--------show initial state---------

target_ind= calc_target_index(x,y,cx,cy,Lf)



%--------Update now keep on track-----
while T > time && target_ind < lastIndex-10
    target_ind= calc_target_index(x,y,cx,cy,Lf)
    ai = PIDcontrol(target_speed, v,Kp);
    di = pure_pursuit_control(x,y,yaw,v,cx,cy,target_ind,k,Lfc,L,Lf);
    
    [x,y,yaw,v] = update(x,y,yaw,v, ai, di,dt,L)
    
    time = time + dt;
    pause(0.1)
    subplot(1,2,1)
    plot(cx,cy,'b',x,y,'r-*')
    title(['time=' num2str(time), 'v=' num2str(v),'ind=' num2str(target_ind)])
    drawnow
    hold on
    box off
% should also show errors
end

%-----growing speed-----------------
function [a] = PIDcontrol(target_v, current_v, Kp)
    a = Kp * (target_v - current_v);
end
%-----pute pursuit-------------------
function [delta] = pure_pursuit_control(x,y,yaw,v,cx,cy,ind,k,Lfc,L,Lf)
    tx = cx(ind);
    ty = cy(ind);
    
    alpha = atan((ty-y)/(tx-x))-yaw;
%--------should also show lateral error---------
    latError =abs((ty-y)*cos(yaw) - (tx-x)*sin(yaw))     
    subplot(1,2,2)
    plot(x,latError,'b-*')
    title(['latError=' num2str(latError)])
    drawnow
    hold on
    box off
%------------------------------------------------
    Lf = k * v + Lfc;
    delta = atan(2*L * sin(alpha)/Lf)  ;
end
%-----update state-------------------
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
%-----find the nearest point---------
function [ind] = calc_target_index(x,y, cx,cy,Lf)
    N =  length(cx);
    %N =  length(cx)-11;
    Distance = zeros(N,1);
    for i = 1:N
        Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2);
    end
    [~, location]= min(Distance);
    ind = location;
    ind = ind + 10;

end

结果图是实时的，包含一个路径跟踪展示和横向误差图，大概如下（图形并不精美，展示原理所用）：


凑字数凑字数使得可以投稿！不用理会
凑字数凑字数使得可以投稿！不用理会
凑字数凑字数使得可以投稿！不用理会

pursuitMATLABtargetltgt

有关自动驾驶-预瞄-Pure pursuit纯跟踪算法-MATLAB实现的更多相关文章

  1. ruby-on-rails - 使用 Ruby on Rails 进行自动化测试 - 最佳实践 - 2

    很好奇，就使用ruby​​onrails自动化单元测试而言，你们正在做什么？您是否创建了一个脚本来在cron中运行rake作业并将结果邮寄给您？git中的预提交Hook？只是手动调用？我完全理解测试，但想知道在错误发生之前捕获错误的最佳实践是什么。让我们理所当然地认为测试本身是完美无缺的，并且可以正常工作。下一步是什么以确保他们在正确的时间将可能有害的结果传达给您？ 最佳答案 不确定您到底想听什么，但是有几个级别的自动代码库控制:在处理某项功能时，您可以使用类似autotest的内容获得关于哪些有效，哪些无效的即时反馈。要确保您的提

  2. ruby - RuntimeError(自动加载常量 Apps 多线程时检测到循环依赖 - 2

    我收到这个错误:RuntimeError(自动加载常量Apps时检测到循环依赖当我使用多线程时。下面是我的代码。为什么会这样？我尝试多线程的原因是因为我正在编写一个HTML抓取应用程序。对Nokogiri::HTML(open())的调用是一个同步阻塞调用，需要1秒才能返回，我有100,000多个页面要访问，所以我试图运行多个线程来解决这个问题。有更好的方法吗？classToolsController0)app.website=array.join(',')putsapp.websiteelseapp.website="NONE"endapp.saveapps=Apps.order("

  3. ruby - 如何根据特征实现 FactoryGirl 的条件行为 - 2

    我有一个用户工厂。我希望默认情况下确认用户。但是鉴于unconfirmed特征，我不希望它们被确认。虽然我有一个基于实现细节而不是抽象的工作实现，但我想知道如何正确地做到这一点。factory:userdoafter(:create)do|user,evaluator|#unwantedimplementationdetailshereunlessFactoryGirl.factories[:user].defined_traits.map(&:name).include?(:unconfirmed)user.confirm!endendtrait:unconfirmeddoenden

  4. Matlab imread（）读到了什么 （浅显 当复习文档了） - 2

    matlab打开matlab，用最简单的imread方法读取一个图像clcclearimg_h=imread('hua.jpg');返回一个数组（矩阵），往往是a*b*cunit8类型解释一下这个三维数组的意思，行数、数和层数，unit8：指数据类型，无符号八位整形，可理解为0~2^8的数三个层数分别代表RGB三个通道图像rgb最常用的是24-位实现方法，即RGB每个通道有256色阶（2^8）。基于这样的24-位RGB模型的色彩空间可以表现256×256×256≈1670万色当imshow传入了一个二维数组，它将以灰度方式绘制；可以把图像拆分为rgb三层，可以以灰度的方式观察它figure(1

  5. 区块链之加解密算法&数字证书 - 2

    目录一.加解密算法数字签名对称加密DES(DataEncryptionStandard)3DES(TripleDES)AES(AdvancedEncryptionStandard)RSA加密法DSA(DigitalSignatureAlgorithm)ECC(EllipticCurvesCryptography)非对称加密签名与加密过程非对称加密的应用对称加密与非对称加密的结合二.数字证书图解一.加解密算法加密简单而言就是通过一种算法将明文信息转换成密文信息，信息的的接收方能够通过密钥对密文信息进行解密获得明文信息的过程。根据加解密的密钥是否相同，算法可以分为对称加密、非对称加密、对称加密和非

  6. 华为OD机试用Python实现 -【明明的随机数】 2023Q1A - 2

    华为OD机试题本篇题目：明明的随机数题目输入描述输出描述：示例1输入输出说明代码编写思路最近更新的博客华为od2023|什么是华为od，od薪资待遇，od机试题清单华为OD机试真题大全，用Python解华为机试题|机试宝典【华为OD机试】全流程解析+经验分享,题型分享,防作弊指南华为o

  7. 基于C#实现简易绘图工具【100010177】 - 2

    C#实现简易绘图工具一.引言实验目的:通过制作窗体应用程序(C#画图软件),熟悉基本的窗体设计过程以及控件设计,事件处理等,熟悉使用C#的winform窗体进行绘图的基本步骤,对于面向对象编程有更加深刻的体会.Tutorial任务设计一个具有基本功能的画图软件**·包括简单的新建文件,保存,重新绘图等功能**·实现一些基本图形的绘制,包括铅笔和基本形状等,学习橡皮工具的创建**·设计一个合理舒适的UI界面**注明:你可能需要先了解一些关于winform窗体应用程序绘图的基本知识,以及关于GDI+类和结构的知识二.实验环境Windows系统下的visualstudio2017C#窗体应用程序三.

  8. ruby-on-rails - 从应用程序中自定义文件夹内的命名空间自动加载 - 2

    我们目前正在为ROR3.2开发自定义cms引擎。在这个过程中，我们希望成为我们的rails应用程序中的一等公民的几个类类型起源，这意味着它们应该驻留在应用程序的app文件夹下，它是插件。目前我们有以下类型:数据源数据类型查看我在app文件夹下创建了多个目录来保存这些:应用/数据源应用/数据类型应用/View更多类型将随之而来，我有点担心应用程序文件夹被这么多目录污染。因此，我想将它们移动到一个子目录/模块中，该子目录/模块包含cms定义的所有类型。所有类都应位于MyCms命名空间内，目录布局应如下所示:应用程序/my_cms/data_source应用程序/my_cms/data_ty

  9. MIMO-OFDM无线通信技术及MATLAB实现（1）无线信道：传播和衰落 - 2

     MIMO技术的优缺点优点通过下面三个增益来总体概括：阵列增益。阵列增益是指由于接收机通过对接收信号的相干合并而活得的平均SNR的提高。在发射机不知道信道信息的情况下，MIMO系统可以获得的阵列增益与接收天线数成正比复用增益。在采用空间复用方案的MIMO系统中，可以获得复用增益，即信道容量成倍增加。信道容量的增加与min(Nt,Nr)成正比分集增益。在采用空间分集方案的MIMO系统中，可以获得分集增益，即可靠性性能的改善。分集增益用独立衰落支路数来描述，即分集指数。在使用了空时编码的MIMO系统中，由于接收天线或发射天线之间的间距较远，可认为它们各自的大尺度衰落是相互独立的，因此分布式MIMO

  10. 【Java入门】使用Java实现文件夹的遍历 - 2

    遍历文件夹我们通常是使用递归进行操作，这种方式比较简单，也比较容易理解。本文为大家介绍另一种不使用递归的方式，由于没有使用递归，只用到了循环和集合，所以效率更高一些！一、使用递归遍历文件夹整体思路1、使用File封装初始目录，2、打印这个目录3、获取这个目录下所有的子文件和子目录的数组。4、遍历这个数组，取出每个File对象4-1、如果File是否是一个文件，打印4-2、否则就是一个目录，递归调用代码实现publicclassSearchFile{publicstaticvoidmain(String[]args){//初始目录Filedir=newFile("d:/Dev");Datebeg

随机推荐