LQR原理及其在路径跟踪的应用
LQR原理及其在路径跟踪的应用
LQR（线性二次型调节器）是一种在控制系统中广泛应用的优化方法，主要用于解决线性系统的稳定性和跟踪问题。本文将详细介绍LQR的基本原理、数学推导及其在路径跟踪中的应用，通过具体的例子帮助读者理解其核心思想和实际应用。
一、原理及推导
LQR，全称Linear Quadratic Regulator，翻译为线性二次型调节器。
1、它要做什么事情呢？
LQR的主要目标是：给定一个线性系统（控制量与状态量呈现线性关系），建立一个代价函数（cost function，评价控制好坏的指标），在一定的约束范围内，求出一个控制序列，使得这个系统能够稳定（稳定可以理解为维持某个值，也可以是跟踪某个曲线）。这与高中学过的线性规划问题有相似之处。
2、它能做什么事情呢？
LQR主要用于调节线性系统，代价函数是以二次型的方式构建的。通过二次型代价函数找到一组控制量{ u 0 , u 1 , u 2 . . . } \left { u_{0},u_{1},u_{2}... \right }{u0 ,u1 ,u2 ...}，使得状态量{ x 0 , x 1 , x 2 . . . } \left { x_{0},x_{1},x_{2}... \right }{x0 ,x1 ,x2 ...}尽可能稳定。
什么是线性系统？
对于一个函数 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 x_{k}xk 表示；加热棒的输出功率就是控制量，用u k u_{k}uk 表示，很容易用函数表征这个线性系统的状态转移方程（暂时认为它是一个离散型的系统，后面会讨论连续性和离散型）：
x k + 1 = A x k + B u k x_{k+1} =Ax_{k} + Bu_{k}xk+1 =Axk +Buk
x k + 1 x_{k+1}xk+1 代表下一时刻的水温状态，x k x_{k}xk 代表当前时刻的水温状态，u k u_{k}uk 代表当前时刻的控制量，A、B为系数常量，这个方程也是一个非常经典的状态转移方程。当前时刻和下一时刻要怎么理解？其实不用纠结，这是对真实物理连续化场景的一种离散化表征。为了使这个系统稳定，我们研究的对象肯定不是一个时刻，而是一段时间区间，所以我们要找的是一组控制量{ u 0 , u 1 , u 2 . . . } \left { u_{0},u_{1},u_{2}... \right }{u0 ,u1 ,u2 ...}。
那么二次型的代价函数表示为：
J = 1 2 [ x N − x d [ N ] ] S [ x N − x d [ N ] ] + 1 2 ∑ k = 0 N − 1 [ [ x k − x d [ k ] ] Q [ x k − x d [ k ] ] + u k R u k ] J = \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 ]J=21 [xN −xd[N] ]S[xN −xd[N] ]+21 k=0∑N−1 [[xk −xd[k] ]Q[xk −xd[k] ]+uk Ruk ]
x N x_{N}xN 代表末时刻的状态量，x d [ N ] x_{d[N]}xd[N] 代表末时刻的控制目标，1 2 [ x N − x d [ N ] ] S [ x N − x d [ N ] ] \frac{1}{2} [x_{N} -x_{d[N]} ]S[x_{N} -x_{d[N]} ]21 [xN −xd[N] ]S[xN −xd[N] ]整体代表了末端代价；x k x_{k}xk 代表中间过程第k步的状态量，x d [ k ] x_{d[k]}xd[k] 代表中间过程第k步的控制目标，[ x k − x d [ k ] ] Q [ x k − x d [ k ] ] [x_{k} -x_{d[k]} ]Q[x_{k} -x_{d[k]} ][xk −xd[k] ]Q[xk −xd[k] ]整体代表了过程代价；u k u_{k}uk 代表第k步的控制量，u k R u k u_{k}Ru_{k}uk Ruk 整体代表了控制代价，越小越好。S,Q,R表示代价系数。那么就是要求出一系列的{ u 0 , u 1 , u 2 . . . } \left { u_{0},u_{1},u_{2}... \right }{u0 ,u1 ,u2 ...}，使得J最小即可。
思维发散
实际调节对象的状态量难道只是一维的吗？实际的控制量难道也只是一维的吗？显而易见，他们都有可能是多维的，比如说跟踪一条路径，对于地上的小车来说，至少是一个三维的x , y , θ x,y, \thetax,y,θ，控制量也不太可能只有一个，我能想到最简单的也包括油门和方向角。对于无人机来说，状态量就更多了。那么这个线性系统的状态转移方程和代价函数应该怎么表征呢？so esay! 用向量表示就行了嘛
状态转移方程：
x k + 1 → = A x k → + B u k → \overrightarrow{x_{k+1}} =A\overrightarrow{{x_{k}}} + B\overrightarrow{u_{k}}xk+1 =Axk +Buk
代价函数：
J = 1 2 [ x N ⃗ − x d [ N ] ⃗ ] T S [ x N ⃗ − x d [ N ] ⃗ ] + 1 2 ∑ k = 0 N − 1 [ [ x k ⃗ − x d [ k ] ⃗ ] T Q [ x k ⃗ − x d [ k ] ⃗ ] + u k ⃗ T R u k ⃗ ] J = \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 ]J=21 [xN −xd[N] ]TS[xN −xd[N] ]+21 k=0∑N−1 [[xk −xd[k] ]TQ[xk −xd[k] ]+uk TRuk ]
如果x d [ N ] x_{d[N]}xd[N] 是一个常量，则研究对象是一个稳定调节问题；
如果x d [ N ] x_{d[N]}xd[N] 变成一条跟踪的路径点集，则研究的是一个路径跟踪问题；
4、纯理论推导过程（离散型系统）
到这里为止，推导就完成了，这里其实是一个反向递推的过程，当k=1时，可以求得F N − 1 F_{N-1}FN−1 …当k=n时，可以求得F 0 F_{0}F0 ,在使用时先推导求出u ( 0 ) u_{(0)}u(0) ，然后根据状态转移求得x ( 1 ) x_{(1)}x(1) ,再继续求u ( 1 ) u_{(1)}u(1) …这里大部分运算都是矩阵运算，使用程序计算就非常简单了。
以上过程要是没看懂也没关系，在实际的计算过程中，一般没有这么复杂，只需要记住下面公式能使用即可，总结如下：
是不是很简单了？？？
4、连续型系统的推导
很多时候系统状态方程是连续的，要进行离散化，下面会进行说明
二、实际举例说明
1、经典的弹簧阻尼系统（恢复稳态）
大家请注意，这里推导过程是将连续型系统转化为离散型的系统去计算的。
2、路径跟踪问题（以车辆运动学模型为例子）
LQR在机器人领域的控制已经非常成熟了，在搞懂理论的基础是就是套用公式， 核心点是建立车辆的误差模型、线性化、离散化、再去求增益矩阵K就行了，这里我也简单做个推导，欢迎查看。