轨迹规划 | 图解最优控制LQR算法(附ROS C++/Python/Matlab仿真)

2024-04-08 13:44

本文主要是介绍轨迹规划 | 图解最优控制LQR算法(附ROS C++/Python/Matlab仿真),希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

目录

  • 0 专栏介绍
  • 1 最优控制理论
  • 2 线性二次型问题
  • 3 LQR的价值迭代推导
  • 4 基于差速模型的LQR控制
  • 5 仿真实现
    • 5.1 ROS C++实现
    • 5.2 Python实现
    • 5.3 Matlab实现

0 专栏介绍

🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。

🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法


1 最优控制理论

最优控制理论是一种数学和工程领域的理论,旨在寻找如何使系统在给定约束条件下达到最佳性能的方法。它的基本思想是通过选择合适的控制输入,以最小化或最大化某个性能指标来优化系统的行为。其中,系统的动态行为通常用状态方程描述,状态表示系统在某一时刻的内部状态。状态空间表示将系统的状态和控制输入表示为向量,通常用微分方程或差分方程来描述系统的演化。在最优控制理论中,会设置代价函数或者目标函数,用来衡量系统行为的好坏的函数。性能指标可以是各种形式,如最小化路径长度、最小化能量消耗、最大化系统稳定性等。最优控制理论在许多领域都有广泛的应用,包括航空航天、机器人学、经济学、生态学等。

2 线性二次型问题

若系统动力学特性可以用一组线性微分方程表示,且性能指标为状态变量和控制变量的二次型函数,则此类最优控制问题称为线性二次型问题线性二次调节器(Linear Quadratic Regulator, LQR)是求解线性二次型问题常用的求解方法之一,其假设系统零输入且期望状态为零

在这里插入图片描述

如图所示的全状态反馈控制系统。设控制误差 x k = z k − z k ∗ \boldsymbol{x}_k=\boldsymbol{z}_k-\boldsymbol{z}_{k}^{*} xk=zkzk,其中 z k \boldsymbol{z}_k zk z k ∗ \boldsymbol{z}_{k}^{*} zk分别是第 k k k个控制时间步的实际状态和期望状态,则全反馈控制律由误差驱动

v k = v k ∗ − K x k ⇔ u = v − v ∗ u k = − K x k \boldsymbol{v}_k=\boldsymbol{v}_{k}^{*}-\boldsymbol{Kx}_k\xLeftrightarrow{\boldsymbol{u}=\boldsymbol{v}-\boldsymbol{v}^*}\boldsymbol{u}_k=-\boldsymbol{Kx}_k vk=vkKxku=vv uk=Kxk

上式表明可以通过选取状态变量和输入变量,使系统等效为零输入(跟踪期望输入)且期望状态为零(消除状态误差),满足应用LQR进行最优控制的条件。定义代价函数

J = ∑ k = 0 N ( x k T Q x k + u k T R u k ) J=\sum_{k=0}^N{\left( \boldsymbol{x}_{k}^{T}\boldsymbol{Qx}_k+\boldsymbol{u}_{k}^{T}\boldsymbol{Ru}_k \right)} J=k=0N(xkTQxk+ukTRuk)

其中 Q \boldsymbol{Q} Q R \boldsymbol{R} R是用户设定的半正定矩阵,前者衡量了系统状态向期望轨迹的收敛速度,后者衡量了系统能量消耗的相对大小,二者互相制约——希望系统快速收敛往往需要加强控制力度,导致能量耗散。因此, 与 需要结合具体场景进行调节。

3 LQR的价值迭代推导

针对 J J J进行优化,引入价值迭代策略,价值迭代的理论基础请看Pytorch深度强化学习1-4:策略改进定理与贝尔曼最优方程详细推导

J k ( x k , u k ) = min ⁡ u k [ x k T Q x k + u k T R u k + J k + 1 ( x k + 1 ) ] J_k\left( \boldsymbol{x}_k,\boldsymbol{u}_k \right) =\underset{\boldsymbol{u}_k}{\min}\left[ \boldsymbol{x}_{k}^{T}\boldsymbol{Qx}_k+\boldsymbol{u}_{k}^{T}\boldsymbol{Ru}_k+J_{k+1}\left( \boldsymbol{x}_{k+1} \right) \right] Jk(xk,uk)=ukmin[xkTQxk+ukTRuk+Jk+1(xk+1)]

即第 k k k步到终端的代价等于当前步的代价与第 k + 1 k+1 k+1步到终端的代价之和。根据 J J J的定义,其一定能表示成二次型 J k = x k T P k x k J_k=\boldsymbol{x}_{k}^{T}\boldsymbol{P}_k\boldsymbol{x}_k Jk=xkTPkxk,对于优化问题 u k = a r g min ⁡ u k J k ( x k , u k ) \boldsymbol{u}_k=\mathrm{arg}\min _{\boldsymbol{u}_k}J_k\left( \boldsymbol{x}_k,\boldsymbol{u}_k \right) uk=argminukJk(xk,uk),令

∂ J k ( x k , u k ) ∂ u k = ∂ ∂ u k ( x k T P k x k + u k T R u k + J k + 1 ( A x k + B u k ) ) = ∂ ∂ u k ( u k T R u k + ( A x k + B u k ) T P k + 1 ( A x k + B u k ) ) = 2 ( R + B T P k + 1 B ) u k + 2 B T P k + 1 A x k = 0 \begin{aligned}\frac{\partial J_k\left( \boldsymbol{x}_k,\boldsymbol{u}_k \right)}{\partial \boldsymbol{u}_k}&=\frac{\partial}{\partial \boldsymbol{u}_k}\left( \boldsymbol{x}_{k}^{T}\boldsymbol{P}_k\boldsymbol{x}_k+\boldsymbol{u}_{k}^{T}\boldsymbol{Ru}_k+J_{k+1}\left( \boldsymbol{Ax}_k+\boldsymbol{Bu}_k \right) \right) \\&=\frac{\partial}{\partial \boldsymbol{u}_k}\left( \boldsymbol{u}_{k}^{T}\boldsymbol{Ru}_k+\left( \boldsymbol{Ax}_k+\boldsymbol{Bu}_k \right) ^T\boldsymbol{P}_{k+1}\left( \boldsymbol{Ax}_k+\boldsymbol{Bu}_k \right) \right) \\&=2\left( \boldsymbol{R}+\boldsymbol{B}^T\boldsymbol{P}_{k+1}\boldsymbol{B} \right) \boldsymbol{u}_k+2\boldsymbol{B}^T\boldsymbol{P}_{k+1}\boldsymbol{Ax}_k\\&=0\end{aligned} ukJk(xk,uk)=uk(xkTPkxk+ukTRuk+Jk+1(Axk+Buk))=uk(ukTRuk+(Axk+Buk)TPk+1(Axk+Buk))=2(R+BTPk+1B)uk+2BTPk+1Axk=0

u k ∗ = − ( R + B T P k + 1 B ) − 1 B T P k + 1 A x k \boldsymbol{u}_{k}^{*}=-\left( \boldsymbol{R}+\boldsymbol{B}^T\boldsymbol{P}_{k+1}\boldsymbol{B} \right) ^{-1}\boldsymbol{B}^T\boldsymbol{P}_{k+1}\boldsymbol{Ax}_k uk=(R+BTPk+1B)1BTPk+1Axk,对比 u k = − K x k \boldsymbol{u}_k=-\boldsymbol{Kx}_k uk=Kxk可得

K k = ( R + B T P k + 1 B ) − 1 B T P k + 1 A \boldsymbol{K}_k=\left( \boldsymbol{R}+\boldsymbol{B}^T\boldsymbol{P}_{k+1}\boldsymbol{B} \right) ^{-1}\boldsymbol{B}^T\boldsymbol{P}_{k+1}\boldsymbol{A} Kk=(R+BTPk+1B)1BTPk+1A

u k = − K x k \boldsymbol{u}_k=-\boldsymbol{Kx}_k uk=Kxk代入 J k J_k Jk可得

J k = x k T P k x k = x k T ( Q + K k T R K k + ( A − B K k ) P k + 1 ( A − B K k ) ) x k J_k=\boldsymbol{x}_{k}^{T}\boldsymbol{P}_k\boldsymbol{x}_k=\boldsymbol{x}_{k}^{T}\left( \boldsymbol{Q}+\boldsymbol{K}_{k}^{T}\boldsymbol{RK}_k+\left( \boldsymbol{A}-\boldsymbol{BK}_k \right) \boldsymbol{P}_{k+1}\left( \boldsymbol{A}-\boldsymbol{BK}_k \right) \right) \boldsymbol{x}_k Jk=xkTPkxk=xkT(Q+KkTRKk+(ABKk)Pk+1(ABKk))xk

从而

P k = Q + A T P k + 1 A − A T P k + 1 B ( R + B T P k + 1 B ) − 1 B T P k + 1 A \boldsymbol{P}_k=\boldsymbol{Q}+\boldsymbol{A}^T\boldsymbol{P}_{k+1}\boldsymbol{A}-\boldsymbol{A}^T\boldsymbol{P}_{k+1}\boldsymbol{B}\left( \boldsymbol{R}+\boldsymbol{B}^T\boldsymbol{P}_{k+1}\boldsymbol{B} \right) ^{-1}\boldsymbol{B}^T\boldsymbol{P}_{k+1}\boldsymbol{A} Pk=Q+ATPk+1AATPk+1B(R+BTPk+1B)1BTPk+1A

称为离散迭代黎卡提方程。根据贝尔曼最优原理,在迭代过程中 P k \boldsymbol{P}_k Pk会逐步收敛。

4 基于差速模型的LQR控制

根据差分机器人运动学模型

p ˙ = [ x ˙ y ˙ θ ˙ ] = [ v cos ⁡ θ v sin ⁡ θ ω ] = [ f 1 f 2 f 3 ] \boldsymbol{\dot{p}}=\left[ \begin{array}{c} \dot{x}\\ \dot{y}\\ \dot{\theta}\\\end{array} \right] =\left[ \begin{array}{c} v\cos \theta\\ v\sin \theta\\ \omega\\\end{array} \right] =\left[ \begin{array}{c} f_1\\ f_2\\ f_3\\\end{array} \right] p˙= x˙y˙θ˙ = vcosθvsinθω = f1f2f3

选择状态量 p = [ x y θ ] T \boldsymbol{p}=\left[ \begin{matrix} x& y& \theta\\\end{matrix} \right] ^T p=[xyθ]T和状态误差量 x = [ x − x r y − y r θ − θ r ] T \boldsymbol{x}=\left[ \begin{matrix} x-x_r& y-y_r& \theta -\theta _r\\\end{matrix} \right] ^T x=[xxryyrθθr]T,控制量 s = [ v ω ] T \boldsymbol{s}=\left[ \begin{matrix} v& \omega\\\end{matrix} \right] ^T s=[vω]T和控制误差量 u = [ v − v r ω − ω r ] T \boldsymbol{u}=\left[ \begin{matrix} v-v_r& \omega -\omega _r\\\end{matrix} \right] ^T u=[vvrωωr]T,可得

x ( k + 1 ) = ( T A + I ) x ( k ) + T B u ( k ) \boldsymbol{x}\left( k+1 \right) =\left( T\boldsymbol{A}+\boldsymbol{I} \right) \boldsymbol{x}\left( k \right) +T\boldsymbol{Bu}\left( k \right) x(k+1)=(TA+I)x(k)+TBu(k)

其中

A = [ 0 0 − v r sin ⁡ θ r 0 0 v r cos ⁡ θ r 0 0 0 ] , B = [ cos ⁡ θ r 0 sin ⁡ θ r 0 0 1 ] \boldsymbol{A}=\left[ \begin{matrix} 0& 0& -v_r\sin \theta _r\\ 0& 0& v_r\cos \theta _r\\ 0& 0& 0\\\end{matrix} \right] , \boldsymbol{B}=\left[ \begin{matrix} \cos \theta _r& 0\\ \sin \theta _r& 0\\ 0& 1\\\end{matrix} \right] A= 000000vrsinθrvrcosθr0 ,B= cosθrsinθr0001

接着按照LQR算法求解即可。

5 仿真实现

5.1 ROS C++实现

核心代码如下所示

Eigen::Vector2d LQRPlanner::_lqrControl(Eigen::Vector3d s, Eigen::Vector3d s_d, Eigen::Vector2d u_r)
{Eigen::Vector2d u;Eigen::Vector3d e(s - s_d);e[2] = regularizeAngle(e[2]);// state equation on errorEigen::Matrix3d A = Eigen::Matrix3d::Identity();A(0, 2) = -u_r[0] * sin(s_d[2]) * d_t_;A(1, 2) = u_r[0] * cos(s_d[2]) * d_t_;Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3, 2);B(0, 0) = cos(s_d[2]) * d_t_;B(1, 0) = sin(s_d[2]) * d_t_;B(2, 1) = d_t_;// discrete iteration Ricatti equationEigen::Matrix3d P, P_;P = Q_;for (int i = 0; i < max_iter_; ++i){Eigen::Matrix2d temp = R_ + B.transpose() * P * B;P_ = Q_ + A.transpose() * P * A - A.transpose() * P * B * temp.inverse() * B.transpose() * P * A;if ((P - P_).array().abs().maxCoeff() < eps_iter_)break;P = P_;}// feedbackEigen::MatrixXd K = -(R_ + B.transpose() * P_ * B).inverse() * B.transpose() * P_ * A;u = u_r + K * e;return u;
}

在这里插入图片描述

5.2 Python实现

核心代码如下所示

def lqrControl(self, s: tuple, s_d: tuple, u_r: tuple) -> np.ndarray:dt = self.params["TIME_STEP"]# state equation on errorA = np.identity(3)A[0, 2] = -u_r[0] * np.sin(s_d[2]) * dtA[1, 2] = u_r[0] * np.cos(s_d[2]) * dtB = np.zeros((3, 2))B[0, 0] = np.cos(s_d[2]) * dtB[1, 0] = np.sin(s_d[2]) * dtB[2, 1] = dt# discrete iteration Ricatti equationP, P_ = np.zeros((3, 3)), np.zeros((3, 3))P = self.Q# iterationfor _ in range(self.lqr_iteration):P_ = self.Q + A.T @ P @ A - A.T @ P @ B @ np.linalg.inv(self.R + B.T @ P @ B) @ B.T @ P @ Aif np.max(P - P_) < self.eps_iter:breakP = P_# feedbackK = -np.linalg.inv(self.R + B.T @ P_ @ B) @ B.T @ P_ @ Ae = np.array([[s[0] - s_d[0]], [s[1] - s_d[1]], [self.regularizeAngle(s[2] - s_d[2])]])u = np.array([[u_r[0]], [u_r[1]]]) + K @ ereturn np.array([[self.linearRegularization(float(u[0]))], [self.angularRegularization(float(u[1]))]])

在这里插入图片描述

5.3 Matlab实现

核心代码如下所示

function u = lqrControl(s, s_d, u_r, robot, param)dt = param.dt;% state equation on errorA = eye(3);A(1, 3) = -u_r(1) * sin(s_d(3)) * dt;A(2, 3) = u_r(1) * cos(s_d(3)) * dt;B = zeros(3, 2);B(1, 1) = cos(s_d(3)) * dt;B(2, 1) = sin(s_d(3)) * dt;B(3, 2) = dt;% discrete iteration Ricatti equationP = param.Q;% iterationfor i=1:param.lqr_iterationP_ = param.Q + A' * P * A - A' * P * B / (param.R + B' * P * B) * B' * P * A;if max(P - P_) < param.eps_iterbreak;endP = P_;end% feedbackK = -inv(param.R + B' * P_ * B) * B' * P_ * A;e = [s(1) - s_d(1); s(2) - s_d(2); regularizeAngle(s(3) - s_d(3))];u = [u_r(1); u_r(2)] + K * e;u = [linearRegularization(robot, u(1), param), angularRegularization(robot, u(2), param)];
end

在这里插入图片描述

完整工程代码请联系下方博主名片获取


🔥 更多精彩专栏

  • 《ROS从入门到精通》
  • 《Pytorch深度学习实战》
  • 《机器学习强基计划》
  • 《运动规划实战精讲》

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇

这篇关于轨迹规划 | 图解最优控制LQR算法(附ROS C++/Python/Matlab仿真)的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



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

相关文章

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

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

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

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

Python中随机休眠技术原理与应用详解

《Python中随机休眠技术原理与应用详解》在编程中,让程序暂停执行特定时间是常见需求,当需要引入不确定性时,随机休眠就成为关键技巧,下面我们就来看看Python中随机休眠技术的具体实现与应用吧... 目录引言一、实现原理与基础方法1.1 核心函数解析1.2 基础实现模板1.3 整数版实现二、典型应用场景2

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

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

SpringBoot实现MD5加盐算法的示例代码

《SpringBoot实现MD5加盐算法的示例代码》加盐算法是一种用于增强密码安全性的技术,本文主要介绍了SpringBoot实现MD5加盐算法的示例代码,文中通过示例代码介绍的非常详细,对大家的学习... 目录一、什么是加盐算法二、如何实现加盐算法2.1 加盐算法代码实现2.2 注册页面中进行密码加盐2.

python+opencv处理颜色之将目标颜色转换实例代码

《python+opencv处理颜色之将目标颜色转换实例代码》OpenCV是一个的跨平台计算机视觉库,可以运行在Linux、Windows和MacOS操作系统上,:本文主要介绍python+ope... 目录下面是代码+ 效果 + 解释转HSV: 关于颜色总是要转HSV的掩膜再标注总结 目标:将红色的部分滤

Python 中的异步与同步深度解析(实践记录)

《Python中的异步与同步深度解析(实践记录)》在Python编程世界里,异步和同步的概念是理解程序执行流程和性能优化的关键,这篇文章将带你深入了解它们的差异,以及阻塞和非阻塞的特性,同时通过实际... 目录python中的异步与同步:深度解析与实践异步与同步的定义异步同步阻塞与非阻塞的概念阻塞非阻塞同步

Python Dash框架在数据可视化仪表板中的应用与实践记录

《PythonDash框架在数据可视化仪表板中的应用与实践记录》Python的PlotlyDash库提供了一种简便且强大的方式来构建和展示互动式数据仪表板,本篇文章将深入探讨如何使用Dash设计一... 目录python Dash框架在数据可视化仪表板中的应用与实践1. 什么是Plotly Dash?1.1

在C#中调用Python代码的两种实现方式

《在C#中调用Python代码的两种实现方式》:本文主要介绍在C#中调用Python代码的两种实现方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录C#调用python代码的方式1. 使用 Python.NET2. 使用外部进程调用 Python 脚本总结C#调

Python下载Pandas包的步骤

《Python下载Pandas包的步骤》:本文主要介绍Python下载Pandas包的步骤,在python中安装pandas库,我采取的方法是用PIP的方法在Python目标位置进行安装,本文给大... 目录安装步骤1、首先找到我们安装python的目录2、使用命令行到Python安装目录下3、我们回到Py