百度 Apollo 自动驾驶控制算法分析(Matlab代码实现）

 💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、数据、文章

---

💥1 概述

Apollo 是由百度发起的一个高效、灵活的基于自动驾驶的测试和研发的平台。Apollo 的主要逻辑为局部规划 +轨道跟踪。在局部规划时，由低精度模型 + 长周期规划组成。在跟踪控制层面上是有高精度模型 + 短周期规划组成。本文主要是对于 Apollo 控制算法中的 MPC（模型预测控制）和 LQR（线性二次调节器）这两种路径跟踪控制器进行分析与比较。

MPC（模型预测控制）是一种先进的过程控制方法，在满足一定约束条件的前提下，用于实现控制的过程。他的实现依赖于过程的动态线性模型。在控制时域内，MPC 将主要对当前时刻进行优化，但同时也会考虑未来时刻，求取当前时刻的最优解，然后反复优化，从而实现在整个时

域内的优化求解。LQR（线性二次调节器）是现代控制理论中发展十分成熟的一种状态空间设计方法。LQR 算法可得到状态线性反馈的最优控制规律，易于形成闭环最优控制。LQR 的最优设计是指设计出的状态反馈器 K 要使二次型目标函数J 取最小值，而 K 有权矩阵 Q 与 R 唯一决定，在汽车不同的速度下，Q 与 R 的不同会对算法的效果有着重要的影响，所以 Q，R 的选值十分重要。

MPC 的研究对象是以现代控制理论中的以状态空间方程的形式给出的线性系统。对于 MPC 控制器，他的研究对象既可以是线性系统，也可以是非线性系统。LQR 的研究对象是以状态空间方程形式给出的线性系统。然而由于对效率和时耗的考虑，通常会将非线性系统线性化进行计

算。

在 Apollo 算法中，LQR 和 MPC 都选用单车动力学模型作为研究对象。单车系统为非线性系统，但 LQR 和MPC 控制器的目的是为了求最优控制解，在优化过程中都会将其线性化，所以也可以说 Apollo 中研究的对象都为线性系统。

📚2 运行结果

可视化：
 

subplot(2,3,4)
load('reference.mat')

load('sp10pp.mat')
a=1:size(real_ref,1);
plot(realcmd(a),'--','Color','r')
hold on
load('sp10lqrap.mat')
plot(realcmd(a),'-.','Color','g')
load('sp10mpc.mat')
plot(realcmd(a),':','Color','b')

hold off
%xlim([0 80])
%ylim([-20 20])
xlabel('time (s)')
ylabel('steer cmd (degree)')
title('算法跟踪下发角度')
legend('pp','lqr','mpc')

%% d dcmd/dx difference cmd
subplot(2,3,5)
load('reference.mat')
%plot(refx,refy)


load('sp10pp.mat')
a=1:size(real_ref,1);
b=size(real_ref,1);
T=2;
for i=1:b-T-2
    kpp(i)=(realcmd(i+T)-realcmd(i))/(a(i+T)-a(i));%k=dy/dx=Δy/Δx
end
plot(kpp,'--','Color','r')

hold on
load('sp10lqrap.mat')
for i=1:b-T-2
    klqr(i)=(realcmd(i+T)-realcmd(i))/(a(i+T)-a(i));%k=dy/dx=Δy/Δx
end
plot(klqr,'-.','Color','g')

load('sp10mpc.mat')
for i=1:b-T-2
    kmpc(i)=(realcmd(i+T)-realcmd(i))/(a(i+T)-a(i));%k=dy/dx=Δy/Δx
end
plot(kmpc,':','Color','b')
hold off
xlim([0 180])
%ylim([-20 20])
xlabel('time (s)')
ylabel('gradient (degree/s)')
title('下发角度斜率')
legend('pp','lqr','mpc')

%% E time cost

subplot(2,3,6)
load('reference.mat')
%plot(refx,refy)


load('sp10pp.mat')
a=1:size(real_ref,1);
plot(real_t(a),'--','Color','r')
sumpp=sum(real_t);
hold on
load('sp10lqrap.mat')
plot(real_t(a),'-.','Color','g')
sumlqr=sum(real_t);
load('sp10mpc.mat')
plot(real_t(a),':','Color','b')
summpc=sum(real_t);
hold off
%xlim([0 80])
xlim([0 180])
%xlabel('step ()')
ylabel('time cost (s)')
xlabel('time (s)')
title(['算法耗时 ','pp:',num2str(sumpp),', lqr:',...
    num2str(sumlqr),', mpc:',num2str(summpc),...
   ])
legend('pp','lqr','mpc')


%% F wave affect

subplot(2,3,4)
load('reference.mat')

load('sp10pp.mat')
a=1:size(real_ref,1);
plot(realcmd(a),'--','Color','r')
hold on
load('sp10lqrap.mat')
plot(realcmd(a),'-.','Color','g')
load('sp10mpc.mat')
plot(realcmd(a),':','Color','b')

hold off
%xlim([0 80])
%ylim([-20 20])
xlabel('time (s)')
ylabel('steer cmd (degree)')
title('算法跟踪下发角度')
legend('pp','lqr','mpc')

%% d dcmd/dx difference cmd
subplot(2,3,5)
load('reference.mat')
%plot(refx,refy)

load('sp10pp.mat')
a=1:size(real_ref,1);
b=size(real_ref,1);
T=2;
for i=1:b-T-2
    kpp(i)=(realcmd(i+T)-realcmd(i))/(a(i+T)-a(i));%k=dy/dx=Δy/Δx
end
plot(kpp,'--','Color','r')

hold on
load('sp10lqrap.mat')
for i=1:b-T-2
    klqr(i)=(realcmd(i+T)-realcmd(i))/(a(i+T)-a(i));%k=dy/dx=Δy/Δx
end
plot(klqr,'-.','Color','g')

load('sp10mpc.mat')
for i=1:b-T-2
    kmpc(i)=(realcmd(i+T)-realcmd(i))/(a(i+T)-a(i));%k=dy/dx=Δy/Δx
end
plot(kmpc,':','Color','b')
hold off
xlim([0 180])
%ylim([-20 20])
xlabel('time (s)')
ylabel('gradient (degree/s)')
title('下发角度斜率')
legend('pp','lqr','mpc')

%% E time cost

subplot(2,3,6)
load('reference.mat')
%plot(refx,refy)

load('sp10pp.mat')
a=1:size(real_ref,1);
plot(real_t(a),'--','Color','r')
sumpp=sum(real_t);
hold on
load('sp10lqrap.mat')
plot(real_t(a),'-.','Color','g')
sumlqr=sum(real_t);
load('sp10mpc.mat')
plot(real_t(a),':','Color','b')
summpc=sum(real_t);
hold off
%xlim([0 80])
xlim([0 180])
%xlabel('step ()')
ylabel('time cost (s)')
xlabel('time (s)')
title(['算法耗时 ','pp:',num2str(sumpp),', lqr:',...
    num2str(sumlqr),', mpc:',num2str(summpc),...
   ])
legend('pp','lqr','mpc')

%% F wave affect

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

🌈4 Matlab代码、数据、文章