LQR原理及其在路径跟踪的应用

我搞slam

于 2024-05-03 11:51:04 发布

阅读量1.3k

点赞数 22

分类专栏：路径规划 & slam 文章标签：算法

本文链接：https://blog.csdn.net/qq_37705385/article/details/138388634

版权

路径规划 & slam 专栏收录该内容

14 篇文章 2 订阅

订阅专栏

一、原理及推导

LQR ，全称Linear Quadratic Regulator，翻译为线性二次型调节器。

1、它要做什么事情呢？

我这样理解：给定一个线性系统（控制量与状态量呈现线性关系），然后为这个系统建立一个代价函数（cost function，评价控制好坏的指标），在一定的约束范围内，求出一个控制序列，使得这个系统能够稳定（稳定可以理解为维持某个值，也可以是跟踪某个曲线）。这样看来是不是有点像高中学过的线性规划问题？

2、它能做什么事情呢？

顾名思义，首先它能调节的是一个线性系统（非线性系统首先要尽心线性化），代价函数是以二次型的方式构建的。通过二次型代价函数找到一组控制量 $\left \{ u_{0},u_{1},u_{2}... \right \}$ ，使得状态量 $\left \{ x_{0},x_{1},x_{2}... \right \}$ 尽可能稳定。

什么是线性系统？
对于一个函数 y=f(x) 来说，线性性必须要满足两个准则：比例性和叠加性。
1、比例性：对于任意的a，有ay = f(ax)成立；
2、叠加行：若 y1=f(x1)， y2=f(x2)，则 y1 +y2 =f(x1 + x2)。

什么是二次型？
参考博客：http://t.csdnimg.cn/2GB63
在这里插入图片描述

3、举例说明LQR研究的问题

假设我们要控制的是一个热水器的保温功能，怎么用LQR理解呢？热水器的预设保温温度就是这个系统需要调节的稳定值；每一时刻水的温度就是状态量，用 $x_{k}$ 表示；加热棒的输出功率就是控制量，用 $u_{k}$ 表示，很容易用函数表征这个线性系统的状态转移方程（暂时认为它是一个离散型的系统，后面会讨论连续性和离散型）：
$x_{k+1} =Ax_{k} + Bu_{k}$
$x_{k+1}$ 代表下一时刻的水温状态， $x_{k}$ 代表当前时刻的水温状态， $u_{k}$ 代表当前时刻的控制量，A、B为系数常量，这个方程也是一个非常经典的状态转移方程。当前时刻和下一时刻要怎么理解？其实不用纠结，这是对真实物理连续化场景的一种离散化表征。为了使这个系统稳定，我们研究的对象肯定不是一个时刻，而是一段时间区间，所以我们要找的是一组控制量 $\left \{ u_{0},u_{1},u_{2}... \right \}$ 。
那么二次型的代价函数表示为：

$\frac{1}{2} [x_{N} -x_{d[N]} ]S[x_{N} -x_{d[N]} ] +\frac{1}{2}\sum_{k=0}^{N-1} \left [ [x_{k} -x_{d[k]} ]Q[x_{k} -x_{d[k]} ] + u_{k}Ru_{k} \right ]$

$x_{N}$ 代表末时刻的状态量， $x_{d[N]}$ 代表末时刻的控制目标， $\frac{1}{2} [x_{N} -x_{d[N]} ]S[x_{N} -x_{d[N]} ]$ 整体代表了末端代价； $x_{k}$ 代表中间过程第k步的状态量， $x_{d[k]}$ 代表中间过程第k步的控制目标， $x_{k} -x_{d[k]} ]Q[x_{k} -x_{d[k]} ]$ 整体代表了过程代价； $u_{k}$ 代表第k步的控制量， $u_{k}Ru_{k}$ 整体代表了控制代价，越小越好。S,Q,R表示代价系数。那么就是要求出一系列的 $\left \{ u_{0},u_{1},u_{2}... \right \}$ ，使得J最小即可。

思维发散
实际调节对象的状态量难道只是一维的吗？实际的控制量难道也只是一维的吗？显而易见，他们都有可能是多维的，比如说跟踪一条路径，对于地上的小车来说，至少是一个三维的 $\theta$ ，控制量也不太可能只有一个，我能想到最简单的也包括油门和方向角。对于无人机来说，状态量就更多了。那么这个线性系统的状态转移方程和代价函数应该怎么表征呢？so esay! 用向量表示就行了嘛
状态转移方程：
$\overrightarrow{x_{k+1}} =A\overrightarrow{{x_{k}}} + B\overrightarrow{u_{k}}$
代价函数：
$\frac{1}{2} {[\vec{x_{N}} -\vec{x_{d[N]}}]^T} S[\vec{x_{N}} -\vec{x_{d[N]}} ] +\frac{1}{2}\sum_{k=0}^{N-1} \left [ [\vec{x_{k}} -\vec{x_{d[k]}} ]^TQ[\vec{x_{k}} -\vec{x_{d[k]} } ] + \vec{u_{k}} ^TR\vec{u_{k}} \right ]$

如果 $x_{d[N]}$ 是一个常量，则研究对象是一个稳定调节问题；
如果 $x_{d[N]}$ 变成一条跟踪的路径点集，则研究的是一个路径跟踪问题；

4、纯理论推导过程（离散型系统）

在这里插入图片描述

到这里为止，推导就完成了，这里其实是一个反向递推的过程，当k=1时，可以求得 $F_{N-1}$ …当k=n时，可以求得 $F_{0}$ ,在使用时先推导求出 $u_{(0)}$ ，然后根据状态转移求得 $x_{(1)}$ ,再继续求 $u_{(1)}$ …这里大部分运算都是矩阵运算，使用程序计算就非常简单了。