kyliancc
浅色深色
< Back
文章 - 基于 LQR 的 Cart-Pole 小角度最优控制
基于 LQR 的 Cart-Pole 小角度最优控制
记录第一次 MuJoCo 仿真实战
控制工程
发布于：2026-07-08

背景

Cart-Pole 是一个典型的物理模型。如图，小车在水平面上沿直线移动，小车上有一个铰链，连接一个末端有摆球的轻杆。我们需要控制力 F，改变小车的运动状态，让小车上的摆保持倒立状态。为此，我们需要根据系统状态，求解出最优的输入 F，让系统趋于稳定。这是一个典型的最优控制问题。 Cart-Pole

MJCF 建模

<mujoco>
    <default>
        <geom rgba=".5 .5 .5 1"/>
    </default>

    <asset>
        <texture type="skybox" builtin="gradient" rgb1="1 1 1" rgb2=".6 .8 1" width="256" height="256"/>
    </asset>

    <worldbody>
        <light pos="0 1 1" dir="0 -1 -1" diffuse="1 1 1"/>
        <geom type="capsule" size="0.02" fromto="-1.0 0 0  1.0 0 0" contype="0" conaffinity="0" rgba="0.9 0.9 0.9 1"/>
        <body pos="0 0 0">
            <joint name="cart-trail" type="slide" pos="0 0 0" axis="1 0 0"/>
            <geom type="box" pos="0 0 0" size=".08 .04 .04" mass="1"/>
            <body pos="0 0 0">
                <joint name="pole-joint" type="hinge" pos="0 0 0" axis="0 1 0"/>
                <geom type="capsule" size="0.02" fromto="0 0 0  0 0 0.5" mass="0"/>
                <body pos="0 0 0.5">
                    <geom type="sphere" pos="0 0 0" size=".05" mass="0.1"/>
                </body>
            </body>
        </body>
    </worldbody>

    <actuator>
        <motor name="cart-motor" joint="cart-trail" gear="1"/>
    </actuator>
</mujoco>

在我的模型中，m=0.1kgm = 0.1 \mathrm{kg}，M=1.0kgM = 1.0 \mathrm{kg}，l=0.5ml = 0.5 \mathrm{m}，g=9.81m/s2g = 9.81 \mathrm{m/s^2}。

力学分析

对 Cart-Pole 进行力学建模。容易知道，系统有两个自由度，分别是小车水平运动与杆的旋转。不妨设小车水平位移为 xx，杆与竖直向上方向的偏角为 θ\theta。接下来，我们将使用拉格朗日方程进行动力学建模，求出两个广义坐标满足的方程。

求取拉氏量

先求取拉氏量。拉氏量通过 L=T−VL = T - V 计算。其中 T 是系统的动能，V 是系统的势能。对于小车，容易知道：

T1=12Mx˙2T_1 = \frac{1}{2} M \dot{x}^2

而倒立摆受到小车运动的牵连运动，因此我们需要考虑在水平方向的额外速度。对小球速度进行正交分解：

vv=−lθ˙sin⁡θvh=lθ˙cos⁡θ+x˙\begin{aligned} v_v &= - l \dot{\theta} \sin\theta \\ v_h &= l \dot{\theta} \cos\theta + \dot{x} \end{aligned}

因此小球动能为：

T2=12m(vh2+vv2)=12m(l2θ˙2sin⁡2θ+l2θ˙2cos⁡2θ+x˙2+2lx˙θ˙cos⁡θ)=12m(l2θ˙2+x˙2+2lx˙θ˙cos⁡θ)\begin{aligned} T_2 &= \frac{1}{2} m (v_h^2 + v_v^2) \\ &= \frac{1}{2} m (l^2 \dot{\theta}^2 \sin^2\theta + l^2 \dot{\theta}^2 \cos^2\theta + \dot{x}^2 + 2 l \dot{x} \dot{\theta} \cos\theta) \\ &= \frac{1}{2} m (l^2 \dot{\theta}^2 + \dot{x}^2 + 2 l \dot{x} \dot{\theta} \cos\theta) \end{aligned}

总动能为：

T=12Mx˙2+12m(l2θ˙2+x˙2+2lx˙θ˙cos⁡θ)T = \frac{1}{2} M \dot{x}^2 + \frac{1}{2} m (l^2 \dot{\theta}^2 + \dot{x}^2 + 2 l \dot{x} \dot{\theta} \cos\theta)

势能的求取比较简单，即

V=mgh+φ0=mglcos⁡θV = mgh + \varphi_0 = mgl\cos\theta

其中当 ∣θ∣=π2|\theta| = \frac{\pi}{2} 时势能为 0。

故拉氏量为：

L=T−V=12Mx˙2+12m(l2θ˙2+x˙2+2lx˙θ˙cos⁡θ)−mglcos⁡θL = T - V = \frac{1}{2} M \dot{x}^2 + \frac{1}{2} m (l^2 \dot{\theta}^2 + \dot{x}^2 + 2 l \dot{x} \dot{\theta} \cos\theta) - mgl\cos\theta

导出广义坐标方程

先对 xx 建立拉格朗日方程：

ddt∂L∂x˙−∂L∂x=F\frac{d}{dt} \frac{\partial L}{\partial \dot{x}} - \frac{\partial L}{\partial x} = F

其中

ddt∂L∂x˙−∂L∂x=ddt(Mx˙+mx˙+mlθ˙cos⁡θ)=(M+m)x¨+mlθ¨cos⁡θ−mlθ˙2sin⁡θ=F\begin{aligned} &\frac{d}{dt} \frac{\partial L}{\partial \dot{x}} - \frac{\partial L}{\partial x} \\ = &\frac{d}{dt} (M \dot{x} + m \dot{x} + ml\dot{\theta} \cos\theta) \\ = &(M+m) \ddot{x} + ml \ddot{\theta} \cos\theta - ml \dot{\theta}^2 \sin\theta = F \end{aligned}

同样地，对 θ\theta 建立拉格朗日方程：

ddt∂L∂θ˙−∂L∂θ=ddt(ml2θ˙+mlx˙cos⁡θ)+mlx˙θ˙sin⁡θ−mglsin⁡θ=ml2θ¨+mlx¨cos⁡θ−mlx˙θ˙sin⁡θ+mlx˙θ˙sin⁡θ−mglsin⁡θ=ml2θ¨+mlx¨cos⁡θ−mglsin⁡θ=0\begin{aligned} &\frac{d}{dt} \frac{\partial L}{\partial \dot{\theta}} - \frac{\partial L}{\partial \theta} \\ = &\frac{d}{dt} (ml^2 \dot{\theta} + ml \dot{x} \cos\theta) + ml\dot{x}\dot{\theta} \sin{\theta} - mgl \sin{\theta} \\ = &ml^2 \ddot{\theta} + ml \ddot{x} \cos\theta - ml\dot{x}\dot{\theta}\sin\theta + ml\dot{x}\dot{\theta} \sin{\theta} - mgl \sin{\theta} \\ = &ml^2 \ddot{\theta} + ml \ddot{x} \cos\theta - mgl \sin{\theta} = 0 \end{aligned}

消去公因式 mlml，则有

lθ¨+x¨cos⁡θ−gsin⁡θ=0l \ddot{\theta} + \ddot{x} \cos\theta - g\sin{\theta} = 0

至此，我们就求解出了 xx 和 θ\theta 满足的方程组：

(M+m)x¨+mlθ¨cos⁡θ−mlθ˙2sin⁡θ=Flθ¨+x¨cos⁡θ−gsin⁡θ=0\begin{aligned} (M+m) \ddot{x} + ml \ddot{\theta} \cos\theta - ml \dot{\theta}^2 \sin\theta &= F \\ l \ddot{\theta} + \ddot{x} \cos\theta - g\sin{\theta} &= 0 \end{aligned}

当然，我们也可以求出 x¨\ddot{x} 与 θ¨\ddot{\theta} 的显式解。将方程表示成线性方程组形式：

[M+mmlcos⁡θcos⁡θl][x¨θ¨]=[F+mlθ˙2sin⁡θgsin⁡θ]\begin{bmatrix} M+m & ml\cos\theta \\ \cos\theta & l \end{bmatrix} \begin{bmatrix} \ddot{x} \\ \ddot{\theta} \end{bmatrix} = \begin{bmatrix} F + ml\dot{\theta}^2 \sin{\theta} \\ g \sin\theta \end{bmatrix}

由 Cramer 法则，

x¨=F+mlθ˙2sin⁡θ−mgsin⁡θcos⁡θM+msin⁡2θθ¨=(M+m)gsin⁡θ−Fcos⁡θ−mlθ˙2sin⁡θcos⁡θl(M+msin⁡2θ)\begin{aligned} \ddot{x} &= \frac{F + ml\dot{\theta}^2\sin\theta - mg\sin\theta\cos\theta}{M + m\sin^2\theta} \\ \ddot{\theta} &= \frac{(M+m)g\sin\theta - F\cos\theta - ml\dot{\theta}^2 \sin\theta\cos\theta}{l(M + m\sin^2\theta)} \end{aligned}

这就是系统的动力学方程。

线性化

可以看到，系统的动力学方程是很复杂的，不适合直接用来计算最优控制。因此，我们需要对系统进行线性化。这里我们假设 θ\theta 很小，使用一阶近似 sin⁡θ≈θ\sin\theta \approx \theta，cos⁡θ≈1\cos\theta \approx 1，且只保留 θ\theta 的一次项：

x¨=F+mlθ˙2sin⁡θ−mgsin⁡θcos⁡θM+msin⁡2θ≈F+mlθ˙2θ−mgθM+mθ2≈F−mgθMθ¨=(M+m)gsin⁡θ−Fcos⁡θ−mlθ˙2sin⁡θcos⁡θl(M+msin⁡2θ)≈(M+m)gθ−F−mlθ˙2θl(M+mθ2)≈(M+m)gθ−FMl\begin{aligned} \ddot{x} &= \frac{F + ml\dot{\theta}^2\sin\theta - mg\sin\theta\cos\theta}{M + m\sin^2\theta} \\ &\approx \frac{F + ml\dot{\theta}^2 \theta - mg\theta}{M + m\theta^2} \\ &\approx \frac{F - mg\theta}{M} \\ \ddot{\theta} &= \frac{(M+m)g\sin\theta - F\cos\theta - ml\dot{\theta}^2 \sin\theta\cos\theta}{l(M + m\sin^2\theta)} \\ &\approx \frac{(M+m)g\theta - F -ml\dot{\theta}^2 \theta}{l(M + m\theta^2)} \\ &\approx \frac{(M+m)g\theta - F}{Ml} \\ \end{aligned}

设状态空间变量 x=[xvθω]\boldsymbol{x} = \begin{bmatrix} x \\ v \\ \theta \\ \omega \end{bmatrix}，输入 u=Fu = F，则状态方程为

x˙=vv˙=−mgMθ+1MFθ˙=ωω˙=(M+m)gMlθ−1MlF\begin{aligned} \dot{x} &= v \\ \dot{v} &= -\frac{mg}{M} \theta + \frac{1}{M} F \\ \dot{\theta} &= \omega \\ \dot{\omega} &= \frac{(M+m)g}{Ml} \theta - \frac{1}{Ml} F \end{aligned}

写成矩阵形式：

x˙=[010000−mgM0000100(M+m)gMl0]x+[01M0−1Ml]u\dot{\boldsymbol{x}} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & -\frac{mg}{M} & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \frac{(M+m)g}{Ml} & 0 \end{bmatrix} \boldsymbol{x} + \begin{bmatrix} 0 \\ \frac{1}{M} \\ 0 \\ - \frac{1}{Ml} \end{bmatrix} u

至此，我们就完成了系统的线性化。当然，这个系统仅当 θ\theta 很小时才能保证近似。

系统分析

能控性矩阵：

Uc=[bAbA2bA3b]=[01M0mgM2l1M0mgM2l00−1Ml0−(M+m)gM2l2−1Ml0−(M+m)gM2l20]\begin{aligned} \boldsymbol{U}_c &= \begin{bmatrix} \boldsymbol{b} & \boldsymbol{Ab} & \boldsymbol{A}^2\boldsymbol{b} & \boldsymbol{A}^3\boldsymbol{b} \end{bmatrix} \\ &= \begin{bmatrix} 0 & \frac{1}{M} & 0 & \frac{mg}{M^2 l} \\ \frac{1}{M} & 0 & \frac{mg}{M^2 l} & 0 \\ 0 & -\frac{1}{Ml} & 0 & -\frac{(M+m)g}{M^2 l^2} \\ -\frac{1}{Ml} & 0 & -\frac{(M+m)g}{M^2 l^2} & 0 \end{bmatrix} \end{aligned}

作初等行变换

Uc∼[1M0mgM2l001M0mgM2l00−gMl20000−gMl2]\boldsymbol{U}_c \sim \begin{bmatrix} \frac{1}{M} & 0 & \frac{mg}{M^2 l} & 0 \\ 0 & \frac{1}{M} & 0 & \frac{mg}{M^2 l} \\ 0 & 0 & -\frac{g}{Ml^2} & 0 \\ 0 & 0 & 0 & -\frac{g}{Ml^2} \end{bmatrix}

故 rank⁡Uc=4\operatorname{rank} \boldsymbol{U}_c = 4，系统是能控的。

线性二次型性能指标的最优控制

问题的提法

在设计最优控制之前，这里先阐述一下线性二次型性能指标最优控制的原理。

设系统的动态方程为：

x˙=A(t)x(t)+B(t)u(t)y=C(t)x(t)\begin{aligned} \dot{\boldsymbol{x}} &= \boldsymbol{A}(t) \boldsymbol{x}(t) + \boldsymbol{B}(t) \boldsymbol{u}(t) \\ \boldsymbol{y} &= \boldsymbol{C}(t) \boldsymbol{x}(t) \end{aligned}

指标泛函的标准形式如下：

J=12xT(T)Sx(T)+12∫t0T[xT(t)Q(t)x(t)+uT(t)R(t)u(t)]dtJ = \frac{1}{2} \boldsymbol{x}^T(T) \boldsymbol{S} \boldsymbol{x}(T) + \frac{1}{2} \int^T_{t_0} [\boldsymbol{x}^T(t) \boldsymbol{Q}(t) \boldsymbol{x}(t) + \boldsymbol{u}^T(t) \boldsymbol{R}(t) \boldsymbol{u}(t)] dt

其中 S 为半正定对称常数阵，Q(t) 为半正定对称时变矩阵，R(t) 为正定对称时变矩阵。我们的任务是求解 u(t) 使得泛函 J 最小。可以看到，泛函指标的第一项是对末端状态的要求，而第二个积分项是对过程的要求。在积分项中又有两项，分别对状态 x 以及输入 u 作要求。可以理解为，泛函同时规定了末端状态不能太偏、系统状态不能太极端、系统输入不能太大。而三个要求各自的重要性，即权重，由矩阵 S、Q、R 决定。

末端自由问题

末端自由问题是最经典的最优控制场景，可以得出最普适的结论。因此我们先对末端自由问题进行讨论。

构造哈密顿函数

H=12xTQx+12uTRu+λTAx+λTBuH = \frac{1}{2} \boldsymbol{x}^T \boldsymbol{Q} \boldsymbol{x} + \frac{1}{2} \boldsymbol{u}^T \boldsymbol{R} \boldsymbol{u} + \boldsymbol{\lambda}^T \boldsymbol{A} \boldsymbol{x} + \boldsymbol{\lambda}^T \boldsymbol{B} \boldsymbol{u}

哈密顿正则方程及其边界条件为：

x˙(t)=∂H∂λ,x(t0)=x0λ˙(t)=−∂H∂x,λ(T)=∂Φ(x(T))∂x(T)=Sx(T)\begin{aligned} \dot{\boldsymbol{x}}(t) &= \frac{\partial H}{\partial \boldsymbol{\lambda}}, \qquad \boldsymbol{x}(t_0) = \boldsymbol{x}_0 \\ \dot{\boldsymbol{\lambda}}(t) &= -\frac{\partial H}{\partial \boldsymbol{x}}, \qquad \boldsymbol{\lambda}(T) = \frac{\partial \Phi(\boldsymbol{x}(T))}{\partial \boldsymbol{x}(T)} = \boldsymbol{S} \boldsymbol{x}(T) \end{aligned}

求解正则方程有：

x˙(t)=∂H∂λ=Ax+Buλ˙(t)=−∂H∂x=−Qx−ATλ\begin{aligned} \dot{\boldsymbol{x}}(t) &= \frac{\partial H}{\partial \boldsymbol{\lambda}} = \boldsymbol{A} \boldsymbol{x} + \boldsymbol{B} \boldsymbol{u} \\ \dot{\boldsymbol{\lambda}}(t) &= -\frac{\partial H}{\partial \boldsymbol{x}} = - \boldsymbol{Q} \boldsymbol{x} - \boldsymbol{A}^T \boldsymbol{\lambda} \end{aligned}

为了让 δH=0\delta H = 0，输入应该满足：

∂H∂u=Ru+BTλ=0\frac{\partial H}{\partial \boldsymbol{u}} = \boldsymbol{R} \boldsymbol{u} + \boldsymbol{B}^T \boldsymbol{\lambda} = \boldsymbol{0}

故最优输入 u∗\boldsymbol{u}^* 满足：

u∗(t)=−R−1BTλ\boldsymbol{u}^*(t) = - \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{\lambda}

又 ∂2H∂u2=R>0\frac{\partial^2 H}{\partial \boldsymbol{u}^2} = \boldsymbol{R} > 0，故此时 H 取极小值。

代回到正则方程，则有

x˙(t)=Ax−BR−1BTλ,x(t0)=x0λ˙(t)=−Qx−ATλ,λ(T)=Sx(T)\begin{aligned} \dot{\boldsymbol{x}}(t) &= \boldsymbol{A} \boldsymbol{x} - \boldsymbol{B} \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{\lambda}, \qquad \boldsymbol{x}(t_0) = \boldsymbol{x}_0 \\ \dot{\boldsymbol{\lambda}}(t) &= - \boldsymbol{Q} \boldsymbol{x} - \boldsymbol{A}^T \boldsymbol{\lambda}, \qquad \boldsymbol{\lambda}(T) = \boldsymbol{S} \boldsymbol{x}(T) \end{aligned}

为了找出 λ\boldsymbol{\lambda} 与 x\boldsymbol{x} 间的关系，设 λ(t)=P(t)x(t)\boldsymbol{\lambda}(t) = \boldsymbol{P}(t) \boldsymbol{x}(t)，且由 λ\boldsymbol{\lambda} 的边界条件可知 P(T)=S\boldsymbol{P}(T) = \boldsymbol{S}。对该式求导有

λ˙(t)=P˙x+Px˙=P˙x+P(Ax−BR−1BTλ)=P˙x+P(Ax−BR−1BTPx)=(P˙+PA−PBR−1BTP)x\begin{aligned} \dot{\boldsymbol{\lambda}}(t) &= \dot{\boldsymbol{P}}\boldsymbol{x} + \boldsymbol{P} \dot{\boldsymbol{x}} \\ &= \dot{\boldsymbol{P}}\boldsymbol{x} + \boldsymbol{P} (\boldsymbol{A} \boldsymbol{x} - \boldsymbol{B} \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{\lambda}) \\ &= \dot{\boldsymbol{P}}\boldsymbol{x} + \boldsymbol{P} (\boldsymbol{A} \boldsymbol{x} - \boldsymbol{B} \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{P} \boldsymbol{x}) \\ &= (\dot{\boldsymbol{P}} + \boldsymbol{P}\boldsymbol{A} - \boldsymbol{P} \boldsymbol{B} \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{P}) \boldsymbol{x} \end{aligned}

又由正则方程 λ˙(t)=−Qx−ATλ=−Qx−ATPx=(−Q−ATP)x\dot{\boldsymbol{\lambda}}(t) = - \boldsymbol{Q} \boldsymbol{x} - \boldsymbol{A}^T \boldsymbol{\lambda} = - \boldsymbol{Q} \boldsymbol{x} - \boldsymbol{A}^T \boldsymbol{P} \boldsymbol{x} = (- \boldsymbol{Q} - \boldsymbol{A}^T \boldsymbol{P}) \boldsymbol{x}，

(P˙+PA−PBR−1BTP)x=(−Q−ATP)x(\dot{\boldsymbol{P}} + \boldsymbol{P}\boldsymbol{A} - \boldsymbol{P} \boldsymbol{B} \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{P}) \boldsymbol{x} = (- \boldsymbol{Q} - \boldsymbol{A}^T \boldsymbol{P}) \boldsymbol{x}

此式对任意 x 均成立，因此

P˙+PA−PBR−1BTP=−Q−ATP\dot{\boldsymbol{P}} + \boldsymbol{P}\boldsymbol{A} - \boldsymbol{P} \boldsymbol{B} \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{P} = - \boldsymbol{Q} - \boldsymbol{A}^T \boldsymbol{P}

即

P˙+PA+ATP−PBR−1BTP+Q=0\dot{\boldsymbol{P}} + \boldsymbol{P}\boldsymbol{A} + \boldsymbol{A}^T \boldsymbol{P} - \boldsymbol{P} \boldsymbol{B} \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{P} + \boldsymbol{Q} = \boldsymbol{0}

这就是矩阵 Riccati 微分方程，其边界条件为 P(T)=S\boldsymbol{P}(T) = \boldsymbol{S}。

回顾最优输入方程，

u∗(t)=−R−1BTλ=−R−1BTPx\boldsymbol{u}^*(t) = - \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{\lambda} = - \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{P} \boldsymbol{x}

可以得到状态到最优输入的负反馈增益为

K(t)=R−1BTP\boldsymbol{K}(t) = \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{P}

此外，将矩阵 Riccati 方程转置不难得到，P\boldsymbol{P} 也是一个对称矩阵。故方程包含 n(n+1)2\frac{n(n+1)}{2} 个未知数。

固定端问题

在固定端问题中，我们严格限制了控制的终端状态。此时指标泛函的形式为

J=∫t0T[xT(t)Q(t)x(t)+uT(t)R(t)u(t)]dtJ = \int^T_{t_0} [\boldsymbol{x}^T(t) \boldsymbol{Q}(t) \boldsymbol{x}(t) + \boldsymbol{u}^T(t) \boldsymbol{R}(t) \boldsymbol{u}(t)] dt

为了使用前面的结论，我们依然还是从标准形式出发，求解最优控制。

标准指标泛函为

J=12xT(T)Sx(T)+12∫t0T[xT(t)Q(t)x(t)+uT(t)R(t)u(t)]dtJ = \frac{1}{2} \boldsymbol{x}^T(T) \boldsymbol{S} \boldsymbol{x}(T) + \frac{1}{2} \int^T_{t_0} [\boldsymbol{x}^T(t) \boldsymbol{Q}(t) \boldsymbol{x}(t) + \boldsymbol{u}^T(t) \boldsymbol{R}(t) \boldsymbol{u}(t)] dt

在这里不妨设 x(T)=0\boldsymbol{x}(T) = \boldsymbol{0}。为了让终端状态在任何过程状态与输入下严格趋于 0，S 矩阵必须要趋于 ∞\infty。不妨定义 S=σI\boldsymbol{S}=\sigma \boldsymbol{I}，此时令 σ→∞\sigma \rightarrow \infty，则 S→∞\boldsymbol{S} \rightarrow \infty，x(T)→0\boldsymbol{x}(T) \rightarrow \boldsymbol{0}。

由于 P(T)=S→∞\boldsymbol{P}(T) = \boldsymbol{S} \rightarrow \infty 难以计算，我们不妨转化为求解 P 的逆，此时 P−1(T)=S−1→0\boldsymbol{P}^{-1}(T) = \boldsymbol{S}^{-1} \rightarrow \boldsymbol{0}。

观察 Riccati 方程，为了将 P\boldsymbol{P} 转化为 P−1\boldsymbol{P}^{-1}，将方程左右两边同乘 P−1\boldsymbol{P}^{-1}：

P˙+PA+ATP−PBR−1BTP+Q=P−1P˙P−1+AP−1+P−1AT−BR−1BT+P−1QP−1=0\begin{aligned} & \dot{\boldsymbol{P}} + \boldsymbol{P}\boldsymbol{A} + \boldsymbol{A}^T \boldsymbol{P} - \boldsymbol{P} \boldsymbol{B} \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{P} + \boldsymbol{Q} \\ =& \boldsymbol{P}^{-1} \dot{\boldsymbol{P}} \boldsymbol{P}^{-1} + \boldsymbol{A} \boldsymbol{P}^{-1} + \boldsymbol{P}^{-1} \boldsymbol{A}^T - \boldsymbol{B} \boldsymbol{R}^{-1} \boldsymbol{B}^T + \boldsymbol{P}^{-1} \boldsymbol{Q} \boldsymbol{P}^{-1} = \boldsymbol{0} \end{aligned}

为了将 P˙\dot{\boldsymbol{P}} 转化为 P˙−1\dot{\boldsymbol{P}}^{-1}，将 PP−1=I\boldsymbol{P} \boldsymbol{P}^{-1} = \boldsymbol{I} 对 t 求导得 P˙P−1+PP˙−1=0\dot{\boldsymbol{P}} \boldsymbol{P}^{-1} + \boldsymbol{P} \dot{\boldsymbol{P}}^{-1} = \boldsymbol{0}，即 P−1P˙P−1=−P˙−1\boldsymbol{P}^{-1} \dot{\boldsymbol{P}} \boldsymbol{P}^{-1} = - \dot{\boldsymbol{P}}^{-1}。因此上式可以转化为

P˙−1−AP−1−P−1AT+BR−1BT−P−1QP−1=0\dot{\boldsymbol{P}}^{-1} - \boldsymbol{A} \boldsymbol{P}^{-1} - \boldsymbol{P}^{-1} \boldsymbol{A}^T + \boldsymbol{B} \boldsymbol{R}^{-1} \boldsymbol{B}^T - \boldsymbol{P}^{-1} \boldsymbol{Q} \boldsymbol{P}^{-1} = \boldsymbol{0}

这就是逆 Riccati 方程，边界条件为 P−1(T)=0\boldsymbol{P}^{-1}(T) = \boldsymbol{0}。解出 P−1\boldsymbol{P}^{-1} 后求逆即可得到 P\boldsymbol{P}。

定常系统

设系统为能控定常系统（A、B、C 为常阵），指标泛函中的 S、Q、R 矩阵也为常阵。在工程中，我们往往希望负反馈增益 K\boldsymbol{K} 也为常阵。为此，应该如何求解最优控制呢？

根据前面的讨论，我们知道

u∗(t)=−R−1BTP(t)x(t)\boldsymbol{u}^*(t) = - \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{P}(t) \boldsymbol{x}(t)

为了使负反馈增益 K=R−1BTP(t)\boldsymbol{K} = \boldsymbol{R}^{-1} \boldsymbol{B}^T \boldsymbol{P}(t) 为常阵，我们需要选取一个最具代表性的常阵 Pˉ\bar{\boldsymbol{P}} 代替时变矩阵 P(t)\boldsymbol{P}(t)。当然，Pˉ\bar{\boldsymbol{P}} 也需要满足 Riccati 方程。由于 Pˉ\bar{\boldsymbol{P}} 是常阵，此时 Pˉ\bar{\boldsymbol{P}} 的导数为零。因此 Riccati 微分方程退化为：

PˉA+ATPˉ−PˉBR−1BTPˉ+Q=0\bar{\boldsymbol{P}}\boldsymbol{A} + \boldsymbol{A}^T \bar{\boldsymbol{P}} - \bar{\boldsymbol{P}} \boldsymbol{B} \boldsymbol{R}^{-1} \boldsymbol{B}^T \bar{\boldsymbol{P}} + \boldsymbol{Q} = \boldsymbol{0}

这就是代数 Riccati 方程。这也是 LQR 方法所使用的方程。

实际上，对于渐近稳定系统，当 t→∞t \rightarrow \infty 时，系统趋于稳态，此时 P(t)\boldsymbol{P}(t) 也应当趋于稳态，即为常阵。容易知道，

lim⁡t→∞P(t)=Pˉ\lim_{t \rightarrow \infty} \boldsymbol{P}(t) = \bar{\boldsymbol{P}}

因此 Pˉ\bar{\boldsymbol{P}} 实际上就是 t→∞t \rightarrow \infty 时 P(t)\boldsymbol{P}(t) 的极限。

需要指出的是，当负反馈为定常矩阵，系统将不再能在时间 T 时到达稳定状态，而是只能在无限长时间下趋近于稳定状态。

求解 Riccati 方程

对于上面的系统，系统的稳定状态为：

x(∞)=0v(∞)=0θ(∞)=0ω(∞)=0\begin{aligned} x(\infty) = 0 \\ v(\infty) = 0 \\ \theta(\infty) = 0 \\ \omega(\infty) = 0 \end{aligned}

指标泛函为

J=∫t0+∞[xT(t)Qx(t)+ru2(t)]dtJ = \int^{+\infty}_{t_0} [\boldsymbol{x}^T(t) \boldsymbol{Q} \boldsymbol{x}(t) + r u^2(t)] dt

不妨设

Q=[q1q2q3q4]\boldsymbol{Q} = \begin{bmatrix} q_1 &&& \\ & q_2 && \\ && q_3 & \\ &&& q_4 \end{bmatrix}

接下来我们将在 Python 中求解出负反馈增益 K。不妨先将 Q、r 的各个权重先设为 1.0：

import numpy as np
from scipy.linalg import solve_continuous_are

m = 0.1
M = 1.0
l = 0.5
g = 9.81

Q = np.diag([1.0, 1.0, 1.0, 1.0])
r = np.array([[1.0,]])

A = np.array([
    [0, 1, 0, 0],
    [0, 0, - m * g / M, 0],
    [0, 0, 0, 1],
    [0, 0, (M + m) * g / (M * l), 0]
])

b = np.array([
    [0,],
    [1 / M,],
    [0,],
    [-1 / (M * l)]
])

P = solve_continuous_are(A, b, Q, r)

print("P =\r\n" + str(P))

K = b.T @ P / r

print("K =\r\n" + str(K))

eig_vals = np.linalg.eigvals(A - b @ K)
print("λ(A-BK) =\r\n" + str(eig_vals))
~~~
得到：
~~~
P =
[[  2.23141596   1.98960859   6.68877359   1.4948043 ]
 [  1.98960859   3.7578122   13.43063185   2.99461408]
 [  6.68877359  13.43063185 101.17472214  21.86984607]
 [  1.4948043    2.99461408  21.86984607   4.84169383]]
 
K =
[[ -1.          -2.23141596 -30.3090603   -6.68877359]]

λ(A-BK) =
[-5.75824641+0.j         -3.76659358+0.j         -0.81064561+0.49745432j
 -0.81064561-0.49745432j]

至此我们就得到了负反馈增益

K=[−1−2.23141596−30.3090603−6.68877359]\boldsymbol{K} = \begin{bmatrix} -1 & -2.23141596 & -30.3090603 & -6.68877359 \end{bmatrix}

由 λ(A−BK)\lambda(A-BK) 可见，闭环系统的极点实部均为负，系统是稳定的。

MuJoCo 仿真程序实现

接下来进行 MuJoCo 仿真：

#include <mujoco/mujoco.h>
#include <iostream>
#include <GLFW/glfw3.h>
#include <eigen3/Eigen/Dense>

using namespace Eigen;

char error[1024];
mjModel* m;
mjData* d;

int main() {
    // load model
    m = mj_loadXML("cart-pole.xml", nullptr, error, 1024);
    if (!m) {
        std::cout << "error: " << error << std::endl;
        return 1;
    }
    // make data
    d = mj_makeData(m);

    // struct
    mjvCamera cam;
    mjvPerturb pert;
    mjvOption opt;
    mjvScene scn;
    mjrContext con;

    // init glfw
    if (!glfwInit()) {
        return 1;
    }
    GLFWwindow* window = glfwCreateWindow(1200, 900, "LQR", nullptr, nullptr);
    if (!window) {
        return 1;
    }
    glfwMakeContextCurrent(window);
    glfwSwapInterval(1);

    // init struct, create scene and content
    mjv_defaultCamera(&cam);
    mjv_defaultPerturb(&pert);
    mjv_defaultOption(&opt);
    mjv_defaultScene(&scn);
    mjr_defaultContext(&con);

    cam.lookat[2] = 0.4;
    cam.distance = 3;
    cam.elevation = -5;

    mjv_makeScene(m, &scn, 1000);
    mjr_makeContext(m, &con, mjFONTSCALE_100);

    d->qpos[1] = M_PI / 12;
    mj_forward(m, d);

    auto K = Vector4d(-1.0, -2.23141596, -30.3090603, -6.68877359);

    while (!glfwWindowShouldClose(window)) {
        mjtNum simstart = d->time;

        while (d->time - simstart < 1.0/60.0) {
            // read state
            auto x = Vector4d(d->qpos[0], d->qvel[0], d->qpos[1], d->qvel[1]);
            // control
            auto u = - x.dot(K);
            d->ctrl[0] = u;
            // step
            mj_step(m, d);
        }

        // get viewport & framebuffer
        mjrRect viewport = {0, 0, 0, 0};
        glfwGetFramebufferSize(window, &viewport.width, &viewport.height);

        // update
        mjv_updateScene(m, d, &opt, nullptr, &cam, mjCAT_ALL, &scn);
        mjr_render(viewport, &scn, &con);

        // v-sync
        glfwSwapBuffers(window);

        glfwPollEvents();
    }

    // free glfw
    glfwTerminate();
    mjv_freeScene(&scn);
    mjr_freeContext(&con);

    // free
    mj_deleteData(d);
    mj_deleteModel(m);

    return 0;
}

运行程序，可以发现小车成功准确并迅捷地把倒立摆摆正并回到了起点。

文章目录
背景MJCF 建模力学分析求取拉氏量导出广义坐标方程线性化系统分析线性二次型性能指标的最优控制问题的提法末端自由问题固定端问题定常系统求解 Riccati 方程MuJoCo 仿真程序实现