基于LQR算法的机器人轨迹跟踪控制详解

news2024/12/23 3:06:29

   本文摘要

   本文详细介绍了基于线性二次型调节器(LQR)算法的机器人轨迹跟踪控制方法。首先,文章通过建立基于运动学模型的离散状态方程,来描述机器人的当前状态与目标状态之间的关系,并利用此模型进行状态误差的计算。接着,通过设定状态量和控制量的权重矩阵(Q和R),使用LQR算法求解控制输入u,旨在最小化状态偏差和控制成本,以实现精确的轨迹跟踪。此外,文章还分析了LQR算法在ros_motion_planning运动规划库中的实现,通过源代码详解了误差量化、状态方程动态矩阵的建立、反馈控制增益的计算以及最终控制向量的生成等关键技术步骤。


   一、根据运动学模型建立离散的状态方程

   本部分内容已经在之前的文章中详细介绍过了,链接如下:


   车辆/单车运动学建模、线性化、离散化过程详细推导【点击可跳转】


   设机器人当前的位姿为 ( x c , y c , φ c ) (x_c, y_c, φ_c) xc,yc,φc, 其中 ( x c , y c , ) (x_c, y_c,) xc,yc, 为其当前位置坐标, φ c φ_c φc为其当前姿态角,当前前轮的转向角为 δ c δ_c δc,当前的后轮车速为 v c v_c vc,前轮车速为 v f v_f vf,前后轮距为 l l l,默认采用后轮驱动模式,即可控的是后轮车速 v c v_c vc,如下图所示:

   这里我们直接放一下上文的结论(想了解过程中,可以去看一下上面链接中的文章),将状态量和输入量取为当前量与期望值的差值,即

   X = [ x c − x r y c − y r φ c − φ r ] , u = [ v c − v r δ c − δ r ] {{X}}=\begin{bmatrix}{x_{c}}-x_{r}\\ {y_c}-{y}_{r}\\ {\varphi_c}-{\varphi}_{r}\end{bmatrix},\mathbf{{u}}=\begin{bmatrix}v_{c}-v_{r}\\ \delta_{c}-\delta_{r}\end{bmatrix} X= xcxrycyrφcφr ,u=[vcvrδcδr]

   此时:

   X ( k + 1 ) = [ 1 0 − T v r sin ⁡ ψ r 0 1 T v r cos ⁡ ψ r 0 0 1 ] X ( k ) + [ T cos ⁡ ψ r 0 T sin ⁡ ψ r 0 T tan ⁡ δ r l T v r l cos ⁡ 2 δ r ] u ( k ) = A X ( k ) + B u ( k ) \begin{aligned}\mathbf{X}(\mathrm{k}+1) & \left.=\left[\begin{array}{ccc}1 & 0 & -\mathrm{Tv}_{\mathrm{r}}\sin\psi_{\mathrm{r}}\\ 0 & 1 & \mathrm{Tv}_{\mathrm{r}}\cos\psi_{\mathrm{r}}\\ 0 & 0 & 1\end{array}\right.\right]\mathbf{X}(\mathrm{k})+\left[\begin{array}{ccc}\mathrm{T}\cos\psi_{\mathrm{r}} & 0\\ \mathrm{T}\sin\psi_{\mathrm{r}} & 0\\ \frac{\mathrm{T}\tan\delta_{\mathrm{r}}}{l} & \frac{T\mathrm{v}_{\mathrm{r}}}{l\cos^2\delta_{\mathrm{r}}}\end{array}\right]\mathbf{u}(\mathrm{k})\\ & \mathrm{=AX(k)+Bu(k)}\end{aligned} X(k+1)= 100010TvrsinψrTvrcosψr1 X(k)+ TcosψrTsinψrlTtanδr00lcos2δrTvr u(k)=AX(k)+Bu(k)

   有了上面的离散化线性运动学模型的状态空间方程,就可以基于此来使用LQR、MPC等控制算法进行轨迹跟踪控制或完成其他控制任务了


   二、使用LQR求解控制输入u

   有了第一部分得到的离散的状态空间方程,轨迹跟踪问题就是如何选取合适的输入量u来使得状态量X趋于0,即使得当前状态与期望状态趋于相等(即差值为0),在LQR中目标函数被设定为下式

   J = ∑ k = 1 n ( X T Q X + u T R u ) J = \sum_{k=1}^n (X^T QX + u^T Ru) J=k=1n(XTQX+uTRu)

   其中,Q为半正定的状态加权矩阵, R为正定的控制加权矩阵,且两者通常取为对角阵;这两个权重矩阵也就是LQR算法需要调节的参数,如果增大Q矩阵中某个元素值,意味着我们期望状态量X中对应的状态能够快速趋近于零,加强跟踪效果,如果增大R矩阵中某个元素值,意味着我们期望输入量u中对应的输入能够快速趋近于零,减少能量损耗。

【补充说明:正定矩阵】给定一个大小为n × n 的实对称矩阵A ,若对于任意长度为n 的非零向量x ,有 x T A x > 0 x^TA x > 0 xTAx>0,则为矩阵A为正定矩阵,有 x T A x > = 0 x^TA x >= 0 xTAx>0,则为矩阵A为半正定矩阵。

   因为,上述LQR目标函数的表达式本质上是一个典型的多目标优化最优控制问题,某一项的权重越大,那么在优化时,减少该项的值,对目标函数值的减少就越明显,自然会优先减少该项的值,从而达到使得该项更快的趋于0。在轨迹跟踪中,前一项优化目标表示跟踪过程路径偏差的累积大小,第二项优化目标表示跟踪过程控制能量的损耗,这样就将轨迹跟踪控制问题转化为一个最优控制问题,两个权重矩阵Q和R就用来平衡这两项的权重。

   对于上述目标函数的优化求解,使用线性二次型调节器 ( Linear-Quadratic Regulator)进行求解,解出的最优控制规律u是关于状态变量X的线性函数,如下所示:

   u = − ( R + B T P B ) − 1 B T P A x = K x u = - (R + B^T P B)^{-1} B^T P A x = Kx u=(R+BTPB)1BTPAx=Kx

   其中,

   P = A T P A − A T P B ( R + B T P B ) − 1 B T P A + Q P = A^T P A - A^T P B (R + B^T P B)^{-1} B^T P A + Q P=ATPAATPB(R+BTPB)1BTPA+Q

   可以发现解出的u的表达式中仅有P是未知的,P可通过上式进行求解,然而可以发现,上式直接求不好求,所以,我们可以选择求其近似解,

【补充说明:黎卡提方程】
形如 y ′ = P ( x ) y 2 + Q ( x ) y + R ( x ) \mathrm{y^{\prime}=P(x)y^2+Q(x)y+R(x)} y=P(x)y2+Q(x)y+R(x)的非线性微分方程称为黎卡提方程。

   针对黎卡提方程,可以采用循环迭代的思想求解P,具体方法如下:

   ①将等式右边的P初始化为P_old=Q;

   ② 然后用P_old取代等式右边的P来计算等式右边的值,计算结果为P_new

   ③ 比较P_old和P_new,若两者的差值小于预设值, 则认为等式两边相等,我们已经得到了P的近似解,结束循环;否则再令P_old=P_new,继续循环②~③步,直至得到P的近似解或者达到最大迭代次数。

   所以在求解控制输入u时,先通过上面的循环迭代,求出近似的P,然后代入u的表达式,即可得到当前的控制输入u,这里需要注意的是,我们定义的输入u,实际上是当前值与期望值的差值,需要再加上期望值 u r u_r ur才是当前的控制输出指令给机器人执行,进行轨迹跟踪控制。


   三、ros_motion_planning运动规划库中实现LQR算法的函数源码解析

   1、源码如下:

/**
 * @brief Execute LQR control process
 * @param s   current state
 * @param s_d desired state
 * @param u_r refered control
 * @return u  control vector
 */
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 error
  Eigen::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 equation
  Eigen::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_;
  }

  // feedback
  Eigen::MatrixXd K = -(R_ + B.transpose() * P_ * B).inverse() * B.transpose() * P_ * A;

  u = u_r + K * e;

  return u;
}

   2、主要作用:

   函数_lqrControl执行LQR(线性二次调节器)控制过程,其主要作用是根据当前状态、期望状态和参考控制输入来计算控制向量。这个控制向量旨在最小化路径追踪误差和控制输入的成本,是实现精确路径跟踪的关键。下面将逐段详细介绍这个函数的代码和其功能。

   3、结合第二部分的流程进行详细介绍

Eigen::Vector2d u;
Eigen::Vector3d e(s - s_d);
e[2] = regularizeAngle(e[2]);

   这段代码首先定义控制向量u,然后计算当前状态s和期望状态s_d之间的误差向量e。为了确保角度误差在合理的范围内(-π 到 π),使用regularizeAngle函数处理e[2],即角度误差部分。

   注:这里的s和s_d分别就是本文第一和第二部分中介绍中当前状态 X c X_c Xc和期望状态 X r X_r Xr,这里的e就是当前状态与期望状态的差值,也就本文介绍的 X X X

// state equation on error
Eigen::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_;

   这一部分定义了基于误差的状态方程的动态矩阵A和控制矩阵B。这些矩阵描述了当前状态误差如何随时间演变,以及控制输入如何影响状态误差的演变。A矩阵初始化为单位矩阵,然后通过调整以包含对状态误差动态的描述。B矩阵初始化为零矩阵,然后根据期望状态和控制间隔时间d_t_进行填充。

   注: 这里的A矩阵,跟本文前面给出的A矩阵完全相同,B矩阵的前两行相同,但第三行不同,是因为本文给出的矩阵A和B是按照单车模型进行建模得到的,而该程序是对差速模型进行建模得到的,所以这里不一样

// discrete iteration Ricatti equation
Eigen::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_;
}

   这一段代码通过使用前文介绍的离散迭代黎卡提(Riccati)方程来计算反馈增益矩阵K的中间变量PP是一个对称正定矩阵,用于评估状态误差的二次成本。这个过程反复迭代,直到P的变化小于设定的阈值eps_iter_或达到最大迭代次数max_iter_

   注:该过程与前文介绍完全吻合

// feedback
Eigen::MatrixXd K = -(R_ + B.transpose() * P_ * B).inverse() * B.transpose() * P_ * A;

u = u_r + K * e;

   最后,根据计算出的P来,计算反馈增益矩阵K,并使用它来调整参考控制u_r,以最小化误差。这个反馈机制是LQR控制的核心,它确保了即使在存在状态误差的情况下,控制系统也能产生接近最优的控制动作。

   注:该过程与前文介绍完全吻合

   综上所述,_lqrControl函数通过计算基于当前和期望状态的控制动作,实现了对机器人的精确控制,以跟踪给定的路径。这一过程包括误差的量化、系统动态的建模、反馈控制增益的计算,以及最终控制向量的生成。

在这里插入图片描述
在这里插入图片描述


   参考资料:

   1、基础算法 - LQR - 离散时间有限边界

   2、路径规划与轨迹跟踪系列算法学习_第12讲_线性二次型调节器(LQR)法

   3、【自动驾驶】LQR控制实现轨迹跟踪 | python实现 | c++实现

   4、ai-winter/ros_motion_planning


在这里插入图片描述
在这里插入图片描述


本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/2041290.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

js中的getElementById的使用方法

在JavaScript中&#xff0c;document.getElementById()是一种用于通过元素的id属性获取DOM元素的方法。它的作用是返回与指定id匹配的HTML元素。 使用document.getElementById()可以通过元素的id属性直接获取该元素的引用&#xff0c;然后可以使用该引用对元素进行各种操作。例…

LLMOps — 使用 BentoML 为 Llama-3 模型提供服务

使用 BentoML 和 Runpod 快速设置 LLM API 经常看到数据科学家对 LLM 的开发感兴趣&#xff0c;包括模型架构、训练技术或数据收集。然而&#xff0c;我注意到&#xff0c;很多时候&#xff0c;除了理论方面&#xff0c;许多人在以用户实际使用的方式提供这些模型时遇到了问题…

单元训练07:矩阵键盘的基本操作-sbit写法

蓝桥杯 小蜜蜂 单元训练07&#xff1a;矩阵键盘的基本操作 sbit写法中加入了定时器使用。 #include "stc15f2k60s2.h"typedef unsigned char uint8_t;uint8_t timerCounter 0; uint8_t timerEnable 0;#define LED(X) \{ …

数据结构之排序(下)

片头 嗨&#xff01;小伙伴们&#xff0c;咱们又见面啦&#xff0c;在上一篇数据结构之排序(上)中&#xff0c;我们学习了直接插入排序、冒泡排序和希尔排序&#xff0c;今天我们继续学习排序这一块&#xff0c;准备好了吗&#xff1f;Ready Go ! ! ! 一、选择排序 1.1 基本思…

测评各主流大模型对复杂文档处理的精确度,司马阅领先

司马阅一直在升级&#xff0c;这次升级后&#xff0c;我们将司马阅和主流的AI大模型再做一次测评。这次极端测评&#xff0c;主要pk各大模型对复杂文档处理的精确度。 我们选取的依然是这份专业的行业报告&#xff1a;《中国生成式AI开发者洞察》&#xff0c;共58页&#xff0…

js实现图片以鼠标为中心滚轮缩放-vue

功能背景 实现以鼠标在图中的位置为中心进行图片的滚轮缩放&#xff0c;现在是无论鼠标位置在哪都以图片中心进行缩放&#xff0c;这不符合预期&#xff1b; 关键点 缩放前鼠标在的位置是 A&#xff08;clinetX,clientY&#xff09; 点&#xff0c;缩放后鼠标的位置是 A’&a…

遇到 aspects 依赖引入失败问题

在引入 aspects 的相关依赖时&#xff0c;没有找到这个版本 <dependency><groupId>org.springframework</groupId><artifactId>spring-aspects</artifactId><version>6.0.0-M2</version> </dependency> 第一次尝试&#xff…

中国云计算技术(三)

目录 四、云视频监控技术&#xff08;一&#xff09;cVideo云视频监控系统&#xff08;二&#xff09;cVideo智能分析系统&#xff08;三&#xff09;cVideo云转码系统 四、云视频监控技术 随着云计算技术的飞速发展&#xff0c;许多传统行业纷纷向“云”上靠拢&#xff0c;视频…

【日记】朋友和他女朋友领证了(368 字)

正文 一定程度上感受到了驻场运维的水深火热&#xff0c;感觉成天到晚都在救火。今天下午就给人修了四五台机器…… 回想了一下&#xff0c;今天貌似还真没干什么。毕竟早上睁眼就是 8:35 了&#xff0c;给人吓得半死。 &#xff08;感觉 AI 也很智障&#xff0c;当初就是发现音…

0603定时器的输入捕获

定时器的输入捕获 最终程序现象&#xff1a; 1.输入捕获模式测频率 2.PWMI模式&#xff08;PWM输入模式&#xff09;测频率和占空比 输入捕获电路&#xff1a;左边这一部分。 右边的就是输出比较部分。 4个输入捕获和输出比较通道&#xff0c;共用4个CCR寄存器&#xff0c;另外…

uniapp 自定义全局弹窗

自定义全局弹窗可在js和.vue文件中调用&#xff0c;unipop样式不满足&#xff0c;需自定义样式。 效果图 目录结构 index.vue <template><view class"uni-popup" v-if"isShow"><view class"uni-popup__mask uni-center ani uni-cust…

10款企业网络准入控制系统排行榜|网络准入控制系统推荐

在当今数字化时代&#xff0c;企业网络的安全性对于维护业务连续性和保护敏感数据至关重要。网络准入控制系统&#xff08;NAC&#xff09;作为企业安全架构的核心组成部分&#xff0c;负责管理和控制所有试图接入企业网络的设备。我们列出了2024年企业网络准入控制系统的排行榜…

别急着买新手机:OPPO Reno13系列配置全解析,性价比爆表

在智能手机市场&#xff0c;OPPO Reno系列凭借其高性价比和出色的影像实力&#xff0c;一直是消费者关注的焦点。 随着科技的不断进步&#xff0c;OPPO也在不断推陈出新&#xff0c;满足用户对高性能手机的需求。最近&#xff0c;OPPO Reno13系列的曝光&#xff0c;预示着OPPO…

【高性能高易用】物联网AI开发套件----Qualcomm® RB3 Gen 2 开发套件

Qualcomm RB3 Gen 2 开发套件 专为高性能计算、高易用性而设计的物联网开发套件 Qualcomm RB3 Gen 2 开发套件拥有先进的功能和强大的性能&#xff0c;包括强大的AI运算&#xff0c;12 TOPS 算力和计算机图形处理能力&#xff0c;可轻松创造涵盖机器人、企业、工业和自动化等…

谷歌账号登录的时候提示被停用,原因是什么,账号还有救吗?该如何处理?

今日早上&#xff0c;有个久违的朋友找到我说&#xff0c;要恢复账号。 他的情况是这样的&#xff1a;7月21日的时候&#xff0c;他发现自己的谷歌账号登录的时候提示活动异常先&#xff0c;需要输入手机号码验证才能恢复账号。但是输入了自己和亲友们的多个手机号码都无法验证…

Astro + Cloudflare Pages 快速搭建个人博客

目录 1 选择 Astro 模板2 使用代码3 修改代码4 上传 Github5 部署 Cloudflare Pages6 后续修改 最近我搭建完了我的个人网站&#xff0c;很多人问是怎么做的&#xff0c;今天就来写一篇教程吧。 全部干货&#xff0c;看完绝对能成功搭建自己的网站&#xff01;&#xff08;还不…

8月12号笔记

工作组 工作组对计算机进行分层&#xff0c;通过创建不同的工作组&#xff0c;不同的计算机可以按照功能或部门归属到不同的组内&#xff0c;整个组织的网络就会变得具有层次性。在默认情况下&#xff0c;局域网内的计算机都是采用工作组方式进行资源管理的&#xff0c;即处在…

S71200 - 编程 - 笔记

1 DEMO 1.1气阀控制 1.2 红绿灯 基于PLC红绿灯控制_哔哩哔哩_bilibili 2 介绍变量DB&#xff0c;M&#xff0c;I&#xff0c;Q的使用 在PLC编程中&#xff0c;通常会使用多种类型的变量来实现逻辑控制、数据存储和输入输出操作。以下是常见的PLC变量类型及其用途&#xff…

C++笔记3•类和对象2•

1.类的6个默认成员函数 概念: 默认成员函数是用户没有显式实现,编译器会生成的成员函数称为默认成员函数。其中包括 构造函数、析构函数、拷贝构造、赋值重载、普通对象取地址重载、const对象取地址重载。也就是说类在空的情况下,空类中也不是什么也没有,会包含这六个默认成…

Linux shell脚本实战案例

文章目录 1. 基础案例&#xff1a;显示系统信息2. 文件备份案例3. 自动安装软件案例4. 批量重命名文件案例5. 监控磁盘空间案例6. 定时任务案例&#xff1a;定期清理日志文件7. 错误处理和日志记录案例&#xff1a;安全地运行命令8. 备份数据库案例&#xff1a;定期备份MySQL数…