无人船 | 图解基于LQR控制的路径跟踪算法(以欠驱动无人艇Otter为例)

1 离散时间线性二次型问题

设状态变量$\mathbf{x}(k):\mathbb{N}\to {\mathbb{R}}^n$，控制变量$\mathbf{u}(k):\mathbb{N}\to {\mathbb{R}}^m$，状态方程为

$${\mathbf{x}}_{k+1}={\mathbf{A}}_k{\mathbf{x}}_k+{\mathbf{B}}_k{\mathbf{u}}_k,k=0,1,\cdots ,N-1$$

最小化性能指标

$$J(\mathbf{u})=\frac{1}{2}{\mathbf{x}}_N^{\mathrm{T}}{\mathbf{Q}}_f{\mathbf{x}}_N+\frac{1}{2}\sum _{k=0}^{N-1}\left({\mathbf{x}}_k^{\mathrm{T}}{\mathbf{Q}}_k{\mathbf{x}}_k+{\mathbf{u}}_k^{\mathrm{T}}{\mathbf{R}}_k{\mathbf{u}}_k\right)$$

其中 ${\mathbf{Q}}_f$ 和 $\mathbf{Q}$ 实对称半正定，$\mathbf{R}$ 实对称正定。$\mathbf{Q}$ 衡量了系统状态向期望轨迹的收敛速度，$\mathbf{R}$ 衡量了系统能量消耗的相对大小，二者互相制约——希望系统快速收敛往往需要加强控制力度，导致能量耗散。$\mathbf{Q}$ 与 $\mathbf{R}$ 需要结合具体场景进行调节。假设终端时刻固定且终端状态自由。

设值函数满足终端边界条件

$$V\left({\mathbf{x}}_N,N\right)=\frac{1}{2}{\mathbf{x}}_N^{\mathrm{T}}{\mathbf{P}}_N{\mathbf{x}}_N,{\mathbf{P}}_N={\mathbf{Q}}_f$$

根据贝尔曼方程

$$V\left({\mathbf{x}}_{N-1},N-1\right)=\underset{{\mathbf{u}}_{N-1}}{\min }\left\{\frac{1}{2}\left({\mathbf{x}}_{N-1}^{\mathrm{T}}{\mathbf{Q}}_{N-1}{\mathbf{x}}_{N-1}+{\mathbf{u}}_{N-1}^{\mathrm{T}}{\mathbf{R}}_{N-1}{\mathbf{u}}_{N-1}\right)+V\left({\mathbf{x}}_N,N\right)\right\}$$

其中

$$V\left({\mathbf{x}}_N,N\right)=\frac{1}{2}{\left({\mathbf{A}}_{N-1}{\mathbf{x}}_{N-1}+{\mathbf{B}}_{N-1}{\mathbf{u}}_{N-1}\right)}^{\mathrm{T}}{\mathbf{P}}_N\left({\mathbf{A}}_{N-1}{\mathbf{x}}_{N-1}+{\mathbf{B}}_{N-1}{\mathbf{u}}_{N-1}\right)$$

为了求得上面等式右侧的最小值，令其对${\mathbf{u}}_{N-1}$的导数为零可得

$${\mathbf{u}}_{N-1}=-{\left({\mathbf{R}}_{N-1}+{\mathbf{B}}_{N-1}^{\mathrm{T}}{\mathbf{P}}_N{\mathbf{B}}_{N-1}\right)}^{-1}{\mathbf{B}}_{N-1}^{\mathrm{T}}{\mathbf{P}}_N{\mathbf{A}}_{N-1}{\mathbf{x}}_{N-1}$$

令

$${\mathbf{K}}_{N-1}=-{\left({\mathbf{R}}_{N-1}+{\mathbf{B}}_{N-1}^{\mathrm{T}}{\mathbf{P}}_N{\mathbf{B}}_{N-1}\right)}^{-1}{\mathbf{B}}_{N-1}^{\mathrm{T}}{\mathbf{P}}_N{\mathbf{A}}_{N-1}$$

则此时值函数为

$$V\left({\mathbf{x}}_{N-1},N-1\right)=\frac{1}{2}{\mathbf{x}}_{N-1}^{\mathrm{T}}{\mathbf{P}}_{N-1}{\mathbf{x}}_{N-1}$$

其中$P_{N-1}=Q_{N-1}+K_{N-1}^T{R}_{N-1}{K}_{N-1}+(A_{N-1}+B_{N-1}{K}_{N-1}{)}^T{P}_N(A_{N-1}+B_{N-1}{K}_{N-1})$，容易验证当 $P_N$实对称半正定时， $P_{N-1}$ 依然实对称半正定。根据数学归纳法，离散时间线性二次型最优控制问题的值函数总可以表示为半正定二次型

$$V(x_k,k)=\frac{1}{2}{x}_k^T{P}_k{x}_k$$

其中半正定矩阵$P_k$满足迭代公式

$$\begin{array}{rl}{P}_k& =Q_k+K_k^T{R}_k{K}_k+(A_k-B_k{K}_k)P_{k+1}(A_k-B_k{K}_k)\\ & =Q_k+A_k^T{P}_{k+1}{A}_k-A_k^T{P}_{k+1}{B}_k{\left(R_k+B_k^T{P}_{k+1}{B}_k\right)}^{-1}{B}_k^T{P}_{k+1}{A}_k\end{array}$$

称为黎卡提差分方程。线性增益反馈矩阵为

$$K_k=-(R_k+B_k^T{P}_{k+1}{B}_k{)}^{-1}{B}_k^T{P}_{k+1}{A}_k$$

最优闭环反馈控制律为：

$$u_k^{\star }=K_k{x}_k$$

称为线性二次调节器(Linear Quadratic Regulator, LQR)。

2 基于欠驱动无人船动力学的LQR

针对Otter非线性动力学模型，首先将非线性问题线性化，得到系统线性误差状态方程。具体而言，选择状态量$\mathbf{x}={\left[\begin{array}{cccccc}lx& y& \psi & u& v& r\end{array}\right]}^T$和控制量$\mathbf{u}={\left[\begin{array}{ccc}{\tau }_l& {\tau }_m& {\tau }_r\end{array}\right]}^T$，系统状态方程为$\dot{\mathbf{x}}=\mathbf{f}\left(\mathbf{x},\mathbf{u}\right)$，则令$\mathbf{f}$在参考点${\mathbf{x}}_r$、${\mathbf{u}}_r$泰勒级数展开，忽略非线性项可得

$$\dot{\mathbf{x}}=\mathbf{f}\left({\mathbf{x}}_r,{\mathbf{u}}_r\right)+\frac{\partial \mathbf{f}\left(\mathbf{x},\mathbf{u}\right)}{\partial \mathbf{x}}{\mid }_{{\mathbf{x}}_r,{\mathbf{u}}_r}^{}\left(\mathbf{x}-{\mathbf{x}}_r\right)+\frac{\partial \mathbf{f}\left(\mathbf{x},\mathbf{u}\right)}{\partial \mathbf{u}}{\mid }_{{\mathbf{x}}_r,{\mathbf{u}}_r}^{}\left(\mathbf{u}-{\mathbf{u}}_r\right)$$

其中雅克比矩阵

$$\{\begin{array}{l}\mathbf{A}=\frac{\partial \mathbf{f}\left(\mathbf{x},\mathbf{u}\right)}{\partial \mathbf{x}}{\mid }_{{\mathbf{x}}_r,{\mathbf{u}}_r}^{}=\left[\begin{array}{cccccc}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}{m+X_{\dot{u}}}& 0& \frac{m-Y_{\dot{v}}}{m+X_{\dot{u}}}\\ 0& 0& 0& 0& \frac{Y_v}{m+Y_{\dot{v}}}& \frac{-\left(m-X_{\dot{u}}\right)}{m+Y_{\dot{v}}}\\ 0& 0& 0& \frac{Y_{\dot{v}}-X_{\dot{u}}}{I_z+N_{\dot{r}}}{v}_r& \frac{Y_{\dot{v}}-X_{\dot{u}}}{I_z+N_{\dot{r}}}{u}_r& \frac{N_r}{I_z+N_{\dot{r}}}\end{array}\right]\\ \mathbf{B}=\frac{\partial \mathbf{f}\left(\mathbf{x},\mathbf{u}\right)}{\partial \mathbf{u}}{\mid }_{{\mathbf{x}}_r,{\mathbf{u}}_r}^{}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ \frac{1}{m+X_{\dot{u}}}& 0& \frac{1}{m+X_{\dot{u}}}\\ 0& 0& 0\\ -\frac{D}{I_z+N_{\dot{r}}}& 0& \frac{D}{I_z+N_{\dot{r}}}\end{array}\right]\end{array}$$

必须指出，由于目标函数为最小化$x$和$u$，因此应用时$x$和$u$一般分别取为状态误差和控制误差，即跟踪期望输入的同时消除状态误差，也称LQR由误差驱动。如图所示对应了LQR算法的流程：从终端时刻$N$开始反向迭代计算每个时间步$k$的反馈矩阵$K_k$，称为反向过程；接着从初始时刻开始，结合系统方程计算每个时间步$k$的最优控制得到最优状态轨迹，称为前向过程。对于给定的时域长度$N$，LQR可以计算唯一的最优轨迹。例如，轨迹优化问题中输入一条初始轨迹{x0,x1,⋯,xN}\{x_{0},x_{1},\cdots,x_{N}\}{x0​,x1​,⋯,xN​}，LQR将优化出唯一的 $x_0^{\star },x_1^{\star },\cdots ,x_N^{\star }$

3 路径跟踪效果分析

定性测试结果：

在考虑水动力、风力等真实干扰的情况下，跟踪轨迹如下所示，定量测试结果为：

• 平均跟踪误差：0.212722 m
• 最大跟踪误差：0.852798 m
• 最小跟踪误差：0.003284 m