目录
- 1 最优控制线性二次型问题
- 2 LQR的价值迭代推导
- 3 基于全驱动无人船动力学的LQR
- 4 跟踪效果分析
1 最优控制线性二次型问题
最优控制理论是一种数学和工程领域的理论，旨在寻找如何使系统在给定约束条件下达到最佳性能的方法。它的基本思想是通过选择合适的控制输入，以最小化或最大化某个性能指标来优化系统的行为。其中，系统的动态行为通常用状态方程描述，状态表示系统在某一时刻的内部状态。状态空间表示将系统的状态和控制输入表示为向量，通常用微分方程或差分方程来描述系统的演化。在最优控制理论中，会设置代价函数或者目标函数，用来衡量系统行为的好坏的函数。性能指标可以是各种形式，如最小化路径长度、最小化能量消耗、最大化系统稳定性等。最优控制理论在许多领域都有广泛的应用，包括航空航天、机器人学、经济学、生态学等。
若系统动力学特性可以用一组线性微分方程表示，且性能指标为状态变量和控制变量的二次型函数 ，则此类最优控制问题称为线性二次型问题。线性二次调节器(Linear Quadratic Regulator, LQR)是求解线性二次型问题常用的求解方法之一，其假设系统零输入且期望状态为零。
如图所示的全状态反馈控制系统。设控制误差 x k = z k − z k ∗ \boldsymbol{x}_k=\boldsymbol{z}k-\boldsymbol{z}{k}^{*} xk=zk−zk∗，其中 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=vk∗−Kxku=v−v∗ 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=0∑N(xkTQxk+ukTRuk)
其中 Q \boldsymbol{Q} Q与 R \boldsymbol{R} R是用户设定的半正定矩阵，前者衡量了系统状态向期望轨迹的收敛速度，后者衡量了系统能量消耗的相对大小，二者互相制约------希望系统快速收敛往往需要加强控制力度，导致能量耗散。因此， 与 需要结合具体场景进行调节。
2 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)=ukminxkTQxk+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} ∂uk∂Jk(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+(A−BKk)Pk+1(A−BKk))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+1A−ATPk+1B(R+BTPk+1B)−1BTPk+1A
称为离散迭代黎卡提方程。根据贝尔曼最优原理，在迭代过程中 P k \boldsymbol{P}_k Pk会逐步收敛。
3 基于全驱动无人船动力学的LQR
针对WAMV非线性动力学模型，首先将非线性问题线性化，得到系统线性误差状态方程。具体而言，选择状态量 x = l x y ψ u v r T \boldsymbol{x}=\left \\begin{matrix}{l} x\& y\& \\psi\& u\& v\& r\\\\\\end{matrix} \\right ^T x=lxyψuvrT和控制量 u = τ l τ m τ r T \boldsymbol{u}=\left \\begin{matrix} \\tau _l\& \\tau _m\& \\tau _r\\\\ \\end{matrix} \\right ^T u=τlτmτrT，系统状态方程为 x ˙ = f ( x , u ) \boldsymbol{\dot{x}}=\boldsymbol{f}\left( \boldsymbol{x},\boldsymbol{u} \right) x˙=f(x,u)，则令 f \boldsymbol{f} f在参考点 x r \boldsymbol{x}_r xr、 u r \boldsymbol{u}_r ur泰勒级数展开，忽略非线性项可得
x ˙ = f ( x r , u r ) + ∂ f ( x , u ) ∂ x ∣ x r , u r ( x − x r ) + ∂ f ( x , u ) ∂ u ∣ x r , u r ( u − u r ) \boldsymbol{\dot{x}}=\boldsymbol{f}\left( \boldsymbol{x}_r, \boldsymbol{u}r \right) +\frac{\partial \boldsymbol{f}\left( \boldsymbol{x}, \boldsymbol{u} \right)}{\partial \boldsymbol{x}}\mid{\boldsymbol{x}_r,\boldsymbol{u}_r}^{}\left( \boldsymbol{x}-\boldsymbol{x}r \right) +\frac{\partial \boldsymbol{f}\left( \boldsymbol{x}, \boldsymbol{u} \right)}{\partial \boldsymbol{u}}\mid{\boldsymbol{x}_r,\boldsymbol{u}_r}^{}\left( \boldsymbol{u}-\boldsymbol{u}_r \right) x˙=f(xr,ur)+∂x∂f(x,u)∣xr,ur(x−xr)+∂u∂f(x,u)∣xr,ur(u−ur)
其中雅克比矩阵
{ A = ∂ f ( x , u ) ∂ x ∣ x r , u r = 0 0 − u r sin ψ r − v r cos ψ r cos ψ r − sin ψ r 0 0 0 u r cos ψ r − v r sin ψ r sin ψ r cos ψ r 0 0 0 0 0 0 1 0 0 0 X u + 2 X ∣ u ∣ u ∣ u r ∣ m r r v r 0 0 0 − r r Y v / m − u r 0 0 0 0 0 N r / I z B = ∂ f ( x , u ) ∂ u ∣ x r , u r = 0 0 0 0 0 0 0 0 0 1 / m 0 1 / m 0 1 / m 0 − D / I z 0 D / I z \begin{cases} \boldsymbol{A}=\frac{\partial \boldsymbol{f}\left( \boldsymbol{x}, \boldsymbol{u} \right)}{\partial \boldsymbol{x}}\mid_{\boldsymbol{x}_r,\boldsymbol{u}r}^{}=\left \\begin{matrix} 0\& 0\& -u_r\\sin \\psi _r-v_r\\cos \\psi _r\& \\cos \\psi _r\& -\\sin \\psi _r\& 0\\\\ 0\& 0\& u_r\\cos \\psi _r-v_r\\sin \\psi _r\& \\sin \\psi _r\& \\cos \\psi _r\& 0\\\\ 0\& 0\& 0\& 0\& 0\& 1\\\\ 0\& 0\& 0\& \\frac{X_u+2X_{\\left\| u \\right\|u}\\left\| u_r \\right\|}{m}\& r_r\& v_r\\\\ 0\& 0\& 0\& -r_r\& {{Y_v}/{m}}\& -u_r\\\\ 0\& 0\& 0\& 0\& 0\& {{N_r}/{I_z}}\\\\\\end{matrix} \\right\\ \boldsymbol{B}=\frac{\partial \boldsymbol{f}\left( \boldsymbol{x}, \boldsymbol{u} \right)}{\partial \boldsymbol{u}}\mid{\boldsymbol{x}_r,\boldsymbol{u}_r}^{}=\left \\begin{matrix} 0\& 0\& 0\\\\ 0\& 0\& 0\\\\ 0\& 0\& 0\\\\ {{1}/{m}}\& 0\& {{1}/{m}}\\\\ 0\& {{1}/{m}}\& 0\\\\ {{-D}/{I_z}}\& 0\& {{D}/{I_z}}\\\\\\end{matrix} \\right\\\end{cases} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧A=∂x∂f(x,u)∣xr,ur=⎣⎢⎢⎢⎢⎢⎢⎡000000000000−ursinψr−vrcosψrurcosψr−vrsinψr0000cosψrsinψr0mXu+2X∣u∣u∣ur∣−rr0−sinψrcosψr0rrYv/m0001vr−urNr/Iz⎦⎥⎥⎥⎥⎥⎥⎤B=∂u∂f(x,u)∣xr,ur=⎣⎢⎢⎢⎢⎢⎢⎡0001/m0−D/Iz00001/m00001/m0D/Iz⎦⎥⎥⎥⎥⎥⎥⎤
选择状态误差量 x ~ = l x − x r y − y r ψ − ψ r u − u r v − v r r − r r T \boldsymbol{\tilde{x}}=\left \\begin{matrix}{l} x-x_r\& y-y_r\& \\psi -\\psi _r\& u-u_r\& v-v_r\& r-r_r\\\\\\end{matrix} \\right ^T x~=lx−xry−yrψ−ψru−urv−vrr−rrT得到线性误差状态方程 x ~ ˙ = A x ~ + B u + C \boldsymbol{\dot{\tilde{x}}}=\boldsymbol{A\tilde{x}}+\boldsymbol{Bu}+\boldsymbol{C} x~˙=Ax~+Bu+C，其中 C = B u r \boldsymbol{C}=\boldsymbol{Bu}_r C=Bur是常数矩阵。为了应用标准LQR过程，暂时忽略常数 C \boldsymbol{C} C，采用采样步长为 T T T的前向欧拉法离散化上述状态方程可得
x ~ ( k + 1 ) = ( T A + I ) x ~ ( k ) + T B u ( k ) \boldsymbol{\tilde{x}}\left( k+1 \right) =\left( T\boldsymbol{A}+\boldsymbol{I} \right) \boldsymbol{\tilde{x}}\left( k \right) +T\boldsymbol{Bu}\left( k \right) x~(k+1)=(TA+I)x~(k)+TBu(k)
接着进行LQR过程即可
4 跟踪效果分析
定性测试结果：
在考虑水动力、风力等真实干扰的情况下，跟踪轨迹如下所示，定量测试结果为：
- 平均跟踪误差：0.188978 m
- 最大跟踪误差：0.626185 m
- 最小跟踪误差：0.006301 m
🔥 更多精彩专栏：
👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇