无人驾驶控制算法LQR和MPC的仿真实现

2024-02-17 10:52

本文主要是介绍无人驾驶控制算法LQR和MPC的仿真实现,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

1. LQR控制器

1.1 问题陈述

考虑一个质量为 m m m 的滑块在光滑的一维地面上运动。初始时,滑块的位置和速度均为 0 0 0。我们的目标是设计一个控制器,基于传感器测得的滑块位置 x x x,为滑块提供外力 u u u,使其能够跟随参考点 x r x_r xr 运动。

在这里插入图片描述

为建立动力学模型,我们采用以下微分方程:

x ¨ = u m \ddot x = \frac{u}{m} x¨=mu

定义状态向量 x = [ x 1 x 2 ] = [ x x ˙ ] x= \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} =\begin{bmatrix} x \\\dot x \end{bmatrix} x=[x1x2]=[xx˙],其中 x 1 x_1 x1 表示位移, x 2 x_2 x2 表示速度。系统的状态方程为:

x ˙ = A x + B u \dot x= Ax+Bu x˙=Ax+Bu

其中:

A = [ 0 1 0 0 ] , B = [ 0 1 m ] A=\begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix}, \quad B=\begin{bmatrix} 0 \\ \frac{1}{m} \end{bmatrix} A=[0010],B=[0m1]

系统的开环矩阵 A A A 决定了系统是否稳定。若没有控制器,物块在光滑地面上将无法自行停止。

1.2 控制器设计

引入控制器,我们考虑以下形式:

u = − k x = − [ k 1 , k 2 , ⋯ ] [ x 1 x 2 ⋮ ] u=-kx=-\begin{bmatrix} k_1,k_2,\cdots \end{bmatrix}\begin{bmatrix} x_1 \\ x_2 \\ \vdots \end{bmatrix} u=kx=[k1,k2,] x1x2

从而得到新的闭环矩阵:

x ˙ = ( A − B k ) x = A c l x \dot x = (A-Bk)x = A_{cl}x x˙=(ABk)x=Aclx

通过选择 k k k,我们可以改变 A c l A_{cl} Acl 的特征值,从而控制系统的行为。因此,关键问题是如何选择最优的 k k k

1.3 LQR控制器

引入线性二次调节器(LQR)的思想,我们定义代价函数:

J = ∫ 0 ∞ ( x T Q x + u T R u ) d t J= \int_0^\infty{(x^TQx+u^TRu)}dt J=0(xTQx+uTRu)dt

其中 Q Q Q R R R 是权重矩阵。通过调整权重,LQR 在保持系统稳定性的同时,寻找使代价函数最小化的控制策略。

在这里插入图片描述

1.4 仿真建模

进行简单的仿真建模,通过设定初始值和目标值,LQR 控制系统能够有效地使小物块按照预设轨迹运动。

在这里插入图片描述
我们将初始值设在5,目标值设在1,最后得到
在这里插入图片描述

2. MPC控制器

2.1 模型的离散化

考虑同样的控制对象 x ˙ = A x + B u \dot x= Ax+Bu x˙=Ax+Bu,使用前向欧拉法将状态方程离散化:

x ( k + 1 ) = A ˉ x ( k ) + B ˉ u ( k ) x(k+1) =\bar Ax(k) + \bar B u(k) x(k+1)=Aˉx(k)+Bˉu(k)

其中

A ˉ = [ 1 T 0 1 ] , B ˉ = [ 0 T m ] \bar A=\begin{bmatrix} 1 & T \\ 0 & 1 \end{bmatrix},\quad \bar B=\begin{bmatrix} 0 \\ \frac{T}{m} \end{bmatrix} Aˉ=[10T1],Bˉ=[0mT]

这里的 T T T 是控制周期。

2.2 预测

MPC 的特点之一是需要对未来系统状态进行预测。在 k k k 时刻,我们预测未来 p p p 个控制周期内的系统状态,并定义预测时域内的控制量:

X k = [ x ( k + 1 ∣ k ) T x ( k + 2 ∣ k ) T ⋯ x ( k + p ∣ k ) T ] T X_k= \begin{bmatrix} x(k+1|k)^T & x(k+2|k)^T &\cdots& x(k+p|k)^T \end{bmatrix}^T Xk=[x(k+1∣k)Tx(k+2∣k)Tx(k+pk)T]T

2.3 优化

我们希望找到最佳的控制量 U k U_k Uk,使预测时域内的状态向量与参考值越接近越好。这导致一个开环最优控制问题,其数学描述为:

min ⁡ J ( U k ) = U k T ( Θ T Q Θ + W ) U k + 2 ( E T Q Θ ) U k + E T Q E \min J(U_k) = U_k^T (\Theta^T Q \Theta + W) U_k +2(E^TQ\Theta) U_k +E^TQE minJ(Uk)=UkT(ΘTQΘ+W)Uk+2(ETQΘ)Uk+ETQE

其中, Θ \Theta Θ E E E 分别为预测模型和误差。

在这里插入图片描述

2.4 仿真

对动力学方程进行拉普拉斯变换,得到传递函数 G ( s ) = 1 m s 2 G(s)=\frac{1}{ms^2} G(s)=ms21。通过仿真,可以验证 MPC 控制系统在固定值和正弦波输入情况下能够有效跟踪目标。

s 2 X ( s ) = 1 m F ( s ) s^2X(s)=\frac{1}{m}F(s) s2X(s)=m1F(s)
得到传递函数为:
G ( s ) = X ( s ) F ( s ) = 1 m s 2 G(s)=\frac{X(s)}{F(s)}=\frac{1}{ms^2} G(s)=F(s)X(s)=ms21
建立仿真:

在这里插入图片描述

我们得到在固定值和sinwave的情况下基本都可以跟踪的比较好(参数还可继续优化)

在这里插入图片描述

在这里插入图片描述

其中MPC代码为:

function u = Controller(pos_ref, pos, vel)
%参数设置
m    = 1.05;                %滑块质量,增加了5%作为建模误差
T    = 0.01;                %控制周期10ms
p    = 40;                  %控制时域(预测时域)
Q    = 10*eye(2*p);         %累计误差权重
W    = 0.0001*eye(p);       %控制输出权重
umax = 100;                 %控制量限制,即最大的力
Rk   = zeros(2*p,1);        %参考值序列
Rk(1:2:end) = pos_ref;    
Rk(2:2:end) = vel;          %参考速度跟随实际速度
%构建中间变量
xk    = [pos;vel];          %xk
A_    = [1 T;0 1];          %离散化预测模型参数A
B_    = [0;T/m];            %离散化预测模型参数B
psi   = zeros(2*p,2);       %psi
for i=1:1:ppsi(i*2-1:i*2,1:2)=A_^i;
end
theta = zeros(2*p,p);       %theta
for i=1:1:pfor j=1:1:itheta(i*2-1:i*2,j)=A_^(i-j)*B_;end
end
E = psi*xk-Rk;              %E
H = 2*(theta'*Q*theta+W);   %H
f = (2*E'*Q*theta)';        %f
%优化求解
coder.extrinsic('quadprog');
Uk=quadprog(H,f,[],[],[],[],-umax,umax);
%返回控制量序列第一个值
u = 0.0;                    %显示指定u的类型
u = Uk(1);

以上是一个简单的物理系统的 LQR 和 MPC 控制系统的设计和仿真。

这篇关于无人驾驶控制算法LQR和MPC的仿真实现的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/717554

相关文章

使用Sentinel自定义返回和实现区分来源方式

《使用Sentinel自定义返回和实现区分来源方式》:本文主要介绍使用Sentinel自定义返回和实现区分来源方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录Sentinel自定义返回和实现区分来源1. 自定义错误返回2. 实现区分来源总结Sentinel自定

Java实现时间与字符串互相转换详解

《Java实现时间与字符串互相转换详解》这篇文章主要为大家详细介绍了Java中实现时间与字符串互相转换的相关方法,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 目录一、日期格式化为字符串(一)使用预定义格式(二)自定义格式二、字符串解析为日期(一)解析ISO格式字符串(二)解析自定义

opencv图像处理之指纹验证的实现

《opencv图像处理之指纹验证的实现》本文主要介绍了opencv图像处理之指纹验证的实现,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面随着小编来一起学... 目录一、简介二、具体案例实现1. 图像显示函数2. 指纹验证函数3. 主函数4、运行结果三、总结一、

Springboot处理跨域的实现方式(附Demo)

《Springboot处理跨域的实现方式(附Demo)》:本文主要介绍Springboot处理跨域的实现方式(附Demo),具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不... 目录Springboot处理跨域的方式1. 基本知识2. @CrossOrigin3. 全局跨域设置4.

Spring Boot 3.4.3 基于 Spring WebFlux 实现 SSE 功能(代码示例)

《SpringBoot3.4.3基于SpringWebFlux实现SSE功能(代码示例)》SpringBoot3.4.3结合SpringWebFlux实现SSE功能,为实时数据推送提供... 目录1. SSE 简介1.1 什么是 SSE?1.2 SSE 的优点1.3 适用场景2. Spring WebFlu

基于SpringBoot实现文件秒传功能

《基于SpringBoot实现文件秒传功能》在开发Web应用时,文件上传是一个常见需求,然而,当用户需要上传大文件或相同文件多次时,会造成带宽浪费和服务器存储冗余,此时可以使用文件秒传技术通过识别重复... 目录前言文件秒传原理代码实现1. 创建项目基础结构2. 创建上传存储代码3. 创建Result类4.

SpringBoot日志配置SLF4J和Logback的方法实现

《SpringBoot日志配置SLF4J和Logback的方法实现》日志记录是不可或缺的一部分,本文主要介绍了SpringBoot日志配置SLF4J和Logback的方法实现,文中通过示例代码介绍的非... 目录一、前言二、案例一:初识日志三、案例二:使用Lombok输出日志四、案例三:配置Logback一

Python如何使用__slots__实现节省内存和性能优化

《Python如何使用__slots__实现节省内存和性能优化》你有想过,一个小小的__slots__能让你的Python类内存消耗直接减半吗,没错,今天咱们要聊的就是这个让人眼前一亮的技巧,感兴趣的... 目录背景:内存吃得满满的类__slots__:你的内存管理小助手举个大概的例子:看看效果如何?1.

Python+PyQt5实现多屏幕协同播放功能

《Python+PyQt5实现多屏幕协同播放功能》在现代会议展示、数字广告、展览展示等场景中,多屏幕协同播放已成为刚需,下面我们就来看看如何利用Python和PyQt5开发一套功能强大的跨屏播控系统吧... 目录一、项目概述:突破传统播放限制二、核心技术解析2.1 多屏管理机制2.2 播放引擎设计2.3 专

Python实现无痛修改第三方库源码的方法详解

《Python实现无痛修改第三方库源码的方法详解》很多时候,我们下载的第三方库是不会有需求不满足的情况,但也有极少的情况,第三方库没有兼顾到需求,本文将介绍几个修改源码的操作,大家可以根据需求进行选择... 目录需求不符合模拟示例 1. 修改源文件2. 继承修改3. 猴子补丁4. 追踪局部变量需求不符合很