MPC轨迹跟踪算法

2024-01-02 22:03| 来源: 网络整理| 查看: 265

MPC轨迹跟踪算法学习记录1 双移线绘制问题双移线参考轨迹绘制横坐标为X_phi的双移线结果对比

双移线绘制问题

双移线作为一种经典工况，在测试车辆稳定性和无人驾驶轨迹跟踪仿真分析中应用广泛。本人在学习龚老师《无人驾驶车辆模型预测控制》第五章时，首先发现配套代码中没有绘制出参考轨迹，这样就不明确追踪效果如何。因此，参考网上案例，调制出了双移线参考轨迹。

双移线参考轨迹

这里，本人是将X_predict作为参考双移线的横坐标，纵坐标是Y_ref。

figure(1); plot(X_predict, Y_ref,'r--','LineWidth',1); plot(X, Y,'cx','LineWidth',1); title('追踪结果对比'); xlabel('横向位置X（米）'); axis([0 150 -2 4]); %坐标轴 ylabel('纵向位置Y（米）'); legend('追踪轨迹','参考轨迹'); hold on;

网上也有博主写到参考双移线的横坐标是X_phi,见下图：在这里插入图片描述按照网上博主的介绍，如果将X_phi替换X_predict, 总是会出现报错，原因是参考网上的设置很难保证X_phi与Y_ref的向量长度保持一致，原代码中的（X_predict, Y_ref）是在迭代中同时计算出来的，这里本人还没有找出它到底输出的数据长度是多少，如果有同学清楚，烦请指点。

绘制横坐标为X_phi的双移线

首先，本人是在MATLAB中将单独绘制出以X_phi为横坐标，Y_ref为纵坐标的双移线，代码如下：

clc; clear all; N=300; T=0.05; shape=2.4;%参数名称，用于参考轨迹生成 dx1=25;dx2=21.95;%没有任何实际意义，只是参数名称 dy1=4.05;dy2=5.7;%没有任何实际意义，只是参数名称 Xs1=27.19;Xs2=56.46;%参数名称 phi_ref=zeros(N,1);%用于保存预测时域内的期望轨迹 Y_ref=zeros(N,1);%用于保存预测时域内的期望轨迹 X_phi=0.05:0.5:150; X_ref=X_phi'; for p=1:1:N %%双移线追踪 z1=shape/dx1*(X_ref(p,1)-Xs1)-shape/2; z2=shape/dx2*(X_ref(p,1)-Xs2)-shape/2; Y_ref(p,1)=dy1/2*(1+tanh(z1))-dy2/2*(1+tanh(z2)); phi_ref(p,1)=atan(dy1*(1/cosh(z1))^2*(1.2/dx1)-dy2*(1/cosh(z2))^2*(1.2/dx2)); Yita_ref_cell{p,1}=[phi_ref(p,1);Y_ref(p,1)]; end figure(1); plot(X_ref, Y_ref,'b--','LineWidth',1); title('双移线图例'); xlabel('横向位置X（米）'); axis([0 150 -2 4]); %直线工况 % axis([-50 50 0 100]); 圆形工况 ylabel('纵向位置Y（米）'); legend('参考轨迹'); hold on;

绘制出的参考轨迹如图所示：在这里插入图片描述

结果对比

这里单独生成的双移线会和之前绘制的以X_predict为横坐标的双移线有差别吗？感兴趣的同学可以把上述代码添加到书上第五章的代码中，这里本人绘制后发现两个参考轨迹并无差别，所以原代码中，直接可以用X_predict绘制横坐标，这样能直接保证它和纵坐标的向量长度保持一致。在这里插入图片描述

【本文地址】

公司简介

联系我们

今日新闻

推荐新闻

专题文章