| RMVL 2.3.0 Robotic Manipulation and Vision Library

载入中...
搜索中...
未找到
k 步前向预估能量机关预测

作者
  黄昊睿
  赵曦

日期
  2023/07/16

版本
  2.1

上一篇教程：在整车状态估计中涉及到的预测量
下一篇教程：如何使用/开发 decider 模块

相关类 rm::SpiRunePredictor

## 使用此方法的前提

### 前提 1

能量机关角度位置因为不受其他输入影响，所以可以视为一个输入信号为 \(1\)的系统对应的输出信号，输入信号固定，系统固定，因此输出信号固定。

### 前提 2

连续信号可以通过 \(z\)变换转变为离散信号，此时能量机关系统输出信号的关系可用一个 差分方程 来表示，根据 \(k\)步前向预估的原理，迭代差分方程即可求得任意 \(k\)步之后的能量机关系统输出信号，其中 \(k\)步代表 \(kT\)， \(T\)为采样周期，如果要预测非整数步可通过 \(k\)步和 \(k+1\)步线性插值求得。

### 前提 3

由于 \(k\)步前向预估公式迭代可求解，通过数学归纳法可证得任意步可求，且此公式可以被拆分成任意个数的项。

### 前提 4

数据有误差，不同采样中的误差假设成立， \(k\)步前向预估用到的项数越多，数据方差越小，估算越准确。

根据以上 4 个前提，可以用当前时刻之前任意长度的数据来估计任意时间之后的能量机关位置，此位置进度由前向预估步数和用到的数据长度决定，考虑到子弹射速相对固定，预估步数基本固定，若精度不够可以考虑增加数据长度来提高精度。

## 差分方程

k 步前向预估

由 前提 2 可以得到如下公式

\[x(k)=c_0+\sum_{i=n_f}^{n+n_f-1}c_ix(k-i)\quad n_f\in{N^*}\tag{1-1} \]

不难发现，其公式与自回归模型 (AR) 一致，因此，以下公式推导则间接证明了在能量机关预测中 AR 模型的正确性。

\(n_f\)可以为任意大小的正整数值，上述公式表明 \(x(k)\)的结果可以通过计算 \(x(k-n_f-n+1)\)， \(x(k-n_f-n+2)\)， \(\dots\)， \(x(k-n_f)\)共 \(n\)个数的线性组合来得到，换句话说，可以根据前 \(n\)帧的采样结果计算出 \(n_f\)帧之后的结果。

在能量机关预测中， \(n_f\)表示为子弹飞行时间所需要的帧数，若子弹飞行时间不为整数帧，那么可以使用线性插值的方式计算出 \(y(k)\)。设子弹飞行时间为 \(t_f\)，帧间隔为 \(T\)，可以使用以下公式近似计算。

计算帧数帧的 \(n_f\)，其中 \(floor\)表示向下取整函数

\[n_f=floor\left(\frac{t_f}T\right)\tag{1-2} \]

计算插值系数 \(k_1\)和 \(k_2\)

\[\left\{\begin{align} k_1&=\frac{t_f}T-n_f\\ k_2&=1-k_1 \end{align}\right.\tag{1-3} \]

修正后的预测值 \(x_p\)的计算公式为

\[x_p=k_2y(n_f)+k_1y(n_f+1)\tag{1-4} \]

## 最小二乘递推算法

### 1. 最小二乘

注解
  此部分详细介绍请参考 最小二乘法 一文。

对于一个超定方程组，写为矩阵表达即为

\[A\boldsymbol{x}=\boldsymbol{b}\tag{2-1} \]

有如下最小二乘解

\[\hat{x}=\left(A^TA\right)^{-1}A^T\boldsymbol{b}\tag{2-2} \]

### 2. 递推公式

对上式的 \(\left(A^TA\right)^{-1}\)部分，现设 \(P_m^{-1}=A_m^TA_m\)，其中 \(A_m=\left[\boldsymbol{a_1}\quad \boldsymbol{a_2}\quad \dots\quad \boldsymbol{a_m}\right]^T\)， \(\boldsymbol{a_i}\)为上文出现过的行向量： \(\boldsymbol{a_i}=\left[a_{i1}\quad a_{i2}\quad \dots\quad a_{in}\right]\)，则有

\[\begin{align} P_m^{-1}&=A_m^TA_m=\sum_{i=1}^{m-1}\boldsymbol{a_i}^T\boldsymbol{a_i}+\boldsymbol{a_m}^T\boldsymbol{a_m}\\ &=P_{m-1}^{-1}+\boldsymbol{a_m}^T\boldsymbol{a_m} \end{align}\tag{2-3} \]

同样，对于 \(A^Tb\)的部分也能用相同的方法，现设 \(Q_m=A_m^T\boldsymbol{b}\)，其中 \(\boldsymbol{b}=\left[b_1\quad b_2\quad \dots\quad b_m\right]^T\)，则有

\[\begin{align} Q_m&=A_m^T\boldsymbol{b}=\sum_{i=1}^{m-1}\boldsymbol{a_i}^Tb_i+\boldsymbol{a_m}^Tb_m\\ &=Q_{m-1}+\boldsymbol{a_m}^Tb_m \end{align}\tag{2-4} \]

因此，第 \(m\)轮迭代的估计值 \(\hat{x}\)可表示为

\[\begin{align} \hat{x}_m&=P_mQ_m\\ &=P_m(Q_{m-1}+\boldsymbol{a_m}^Tb_m)\\ &=P_m(P_{m-1}^{-1}\hat{x}_{m-1}+\boldsymbol{a_m}^Tb_m)\\ &=P_m(P_m^{-1}\hat{x}_{m-1}-\boldsymbol{a_m}^T\boldsymbol{a_m}\hat{x}_{m-1}+\boldsymbol{a_m}^Tb_m)\\ &=\hat{x}_{m-1}+P_m\boldsymbol{a_m}^T(b_m-\boldsymbol{a_m}\hat{x}_{m-1}) \end{align}\tag{2-5} \]

上式即为递推最小二乘法的公式，但在其中， \(P_m=\left(P_{m-1}^{-1}+\boldsymbol{a_m}^T\boldsymbol{a_m}\right)^{-1}\)，求解 \(P_m\)的递推形式需要进行两次求逆操作，会显著增加时间复杂度，需要对这一部分进行修改，对于形如 \(\left(A^{-1}+BC\right)^{-1}\)的式子，不妨令

\[\left(A^{-1}+BC\right)^{-1}=A+X\tag{2-6a}\]

因此满足

\[\left(A^{-1}+BC\right)(A+X)=I\tag{2-6b}\]

这是个求解 \(X\)的过程，详细步骤如下

\[\begin{align}\left(A^{-1}+BC\right)(A+X)&=I\\ BCA+A^{-1}X+BCX&=\boldsymbol 0\\\left(A^{-1}+BC\right)X&=-BCA\\ X&=-\left(A^{-1}+BC\right)^{-1}BCA\\ &=-\left(BB^{-1}A^{-1}+BC\right)^{-1}BCA\\ &=-\left[B\left(B^{-1}A^{-1}+C\right)\right]^{-1}BCA\\ &=-\left(B^{-1}A^{-1}+C\right)^{-1}CA\\ &=-\left(B^{-1}A^{-1}+CABB^{-1}A^{-1}\right)^{-1}CA\\ &=-\left[\left(I+CAB\right)B^{-1}A^{-1}\right]^{-1}CA\\ &=-AB\left(I+CAB\right)^{-1}CA \end{align}\]

因此，最终得到了矩阵求逆过程中有以下的变换公式

\[\boxed{\left(A^{-1}+BC\right)^{-1}=A-AB\left(I+CAB\right)^{-1}CA}\tag{2-6c}\]

因此， \(P_m\)可以表示为

\[P_m=P_{m-1}-P_{m-1}\boldsymbol{a_m}^T\left(I+\boldsymbol{a_m}P_{m-1}\boldsymbol{a_m}^T\right)^{-1}\boldsymbol{a_m}P_{m-1}\tag{2-7} \]

注解
  由于 \(\boldsymbol{a_m}\)为行向量，因此 \(I\)为 \(1\times1\)矩阵，即数字 \(1\)。

汇总以上结果，最后可以得到

\[\boxed{\left\{\begin{align} P_m&=P_{m-1}-\frac{P_{m-1}\boldsymbol{a_m}^T\boldsymbol{a_m}P_{m-1}}{1-\boldsymbol{a_m}P_{m-1}\boldsymbol{a_m}^T}\\ \hat{x}_m&=\hat{x}_{m-1}+P_m\boldsymbol{a_m}^T\left(b_m-\boldsymbol{a_m}\hat{x}_{m-1}\right) \end{align}\right.}\tag{2-8} \]

## 预测

首先能够直接观测到能量机关的角度信息，因此我们直接辨识角度曲线的各系数即可。对每一次角度的观测结果 \(\theta(k)\)得到的 \(\text{(1-1)}\)式写成方程 \(\text{(2-1)}\)的形式

\[c_0+\theta(k-n_f)c_{n_f}+\theta(k-n_f-1)c_{n_f+1}+\dots+\theta(k-n_f-n+1)c_{n_f+n-1}=\theta(k)\tag{3-1} \]

结合公式 \(\text{(2-8)}\)，得到

\[\boxed{\left\{\begin{align} \boldsymbol{a_m}&=\left[1\quad \theta(k-n_f)\quad \theta(k-n_f-1)\quad \dots\quad \theta(k-n_f-n+1)\right]\\ b_m&=\theta(k)\\ \hat{x}_m&=\left[c_0\quad c_{n_f}\quad c_{n_f+1}\quad \dots\quad c_{n_f+n-1}\right]^T \end{align}\right.}\tag{3-2} \]

\(\theta(k)\)表示当前帧采样得到的能量机关数据数据， \(\theta(k-n_f)\)表示 \(n_f\)个采样周期（帧）之前采集到的能量机关角度信息， \(\theta(k-n_f-n+1)\)表示从 \(n_f\)个采样周期之前再往前 \(n\)个周期的能量机关角度信息。若要预测当前帧后 \(n_f\)个采样周期的能量机关角度，运用公式 \(\text{(1-1)}\)，使用前 \(n\)帧到当前帧的 \(\theta\)角度，代入迭代公式，即可计算出 \(n_f\)个采样周期后的角度值。最后使用 \(\text{(1-3)}\)和 \(\text{(1-4)}\)即可得到最终的预测结果 \(y_p\)。