一起做激光反光板（一）-EKF定位公式推导

前提：本文只考虑平面运动。

EKF的公式及原理不再细述：EKF原理

观测：观测数据为反光板。反光板的检测暂时也不考虑。（一般来说，反光板的检测都是基于反射强度来做的，需要自己手写，如果有疑问留言）。

预测：以轮速计为例，我们只使用轮速计的线速度和角速度，轮速计的模型，我们使用轮速计模型博客中提到的模型。

定位EKF公式推导

假如我们已经事先构建了一个地图（后续再考虑），地图中包含$M$个反光板：

每个反光板还会有一个协方差矩阵，该矩阵的形式为：

假设我们只维护位姿，不考虑新增的反光板，且不更新老地图中反光板的位置和协方差矩阵，所以有：

状态空间

差速运动模型-预测

（1）位姿预测：
初始位姿：

初始位姿的确定，涉及到重定位，重定位有很多手段（比如3点定位法、其他算法给出等），在这里我们假定初始位置是已知的。

运动方程可以写为：
假设轮速计的线速度和角速度服从均值为0的高斯分布，即控制的协方差为${\Sigma }_u$

（2）协方差预测

初始时刻的协方差需要给定初值：

添加协方差公式的推导-2023.06.15

状态量在$t$时刻的协方差预测方程为：${\Sigma }_{\xi ,t}=G_{\xi }{\Sigma }_{\xi ,t-1}{G}_{\xi }^T+G_u{\Sigma }_u{G}_u^T$

其中：

$G_{\xi }$是运动模型关于机器人位姿${\xi }_{t-1}$的雅克比：

$G_u$是运动模型关于控制（轮速计线速度和角速度）$u$的雅克比：


根据协方差预测可知，假如没有观测数据，预测位姿的协方差矩阵会越来越大，导致位姿不可靠性增大。

全向运动模型-预测-2021.4.11更新

考虑到有些机器人是全向的，所以我们扩展代码，支持差速和全向模型。差速和全向的差别不大，不过是多了$y$轴速度，所以需要把运动模型进行扩充。为了方便，我们这里仅为了展示推导过程，后面不再针对每一篇博客继续推导 。

（1）位姿预测：
为了方便，我们可以将运动方程可以写为：
$\left[\begin{array}{c}{x}_t\\ y_t\\ {\theta }_t\end{array}\right]=\left[\begin{array}{c}{x}_{t-1}\\ y_{t-1}\\ {\theta }_{t-1}\end{array}\right]+\left[\begin{array}{c}{v}_x\Delta t\cos ({\theta }_{t-1})-v_y\Delta t\sin ({\theta }_{t-1})\\ v_x\Delta t\sin ({\theta }_{t-1})+v_y\Delta t\cos ({\theta }_{t-1})\\ \omega \Delta t\end{array}\right]$

（2）协方差预测：

${\Sigma }_t=G_t{\Sigma }_{t-1}{G}_t^T+G_u{\Sigma }_u{G}_u^T$

控制的协方差为${\Sigma }_u$

${\Sigma }_u={\left[\begin{array}{ccc}{\sigma }_{v_x}^2& 0& 0\\ 0& {\sigma }_{v_y}^2& 0\\ 0& 0& {\sigma }_{\omega }^2\end{array}\right]}_{3\ast 3}$

其中：

$G_t$是运动模型关于状态量$X_{t-1}$的雅克比：

$G_t=\frac{\partial X_t}{\partial X_{t-1}}={\left[\begin{array}{ccc}1& 0& -v_x\Delta t\sin ({\theta }_{t-1})-v_y\Delta t\cos ({\theta }_{t-1})\\ 0& 1& v_x\Delta t\cos ({\theta }_{t-1})-v_y\Delta t\sin ({\theta }_{t-1})\\ 0& 0& 1\end{array}\right]}_{3\ast 3}$

$G_u$是运动模型关于控制$u={\left[\begin{array}{ccc}{v}_x& v_y& \omega \end{array}\right]}^T$的雅克比：

$G_u=\frac{\partial X_t}{\partial u}={\left[\begin{array}{ccc}\Delta t\cos ({\theta }_{t-1})& -\Delta t\sin ({\theta }_{t-1})& 0\\ \Delta t\sin ({\theta }_{t-1})& \Delta t\cos ({\theta }_{t-1})& 0\\ 0& 0& \Delta t\end{array}\right]}_{3\ast 3}$

观测模型

假设当前激光帧中检测到$N$个反光板：
通过数据关联（欧式距离或马氏距离等手段），我们找到地图中对应的$N$个反光板。对于关联不上的反光板，我们不放进观测方程中。观测方程可以写为：

观测对状态空间$X_t$的雅克比为：

更新

计算增益

$K_t={\Sigma }_{\xi ,t}{H}_t^T(H_t{\Sigma }_{\xi ,t}{H}_t^T+Q{)}^{-1}$

$K_t$为$3\ast 2N$矩阵。其中，$Q$为观测的协方差矩阵。

该矩阵的取值可以为：

（1）采用观测误差，都取一个固定的值；

（2）采用地图中每个反光板的协方差矩阵；

（3）采用观测误差和地图中反光板协方差矩阵的加权，或者和等方式；

这三种方式，理论上都能得到结果，不过，具体到底哪个好，需要根据实际情况来选择。

更新状态空间

$X_t=X_t+K_t(z_t-\hat{z})$

更新协方差

${\Sigma }_{\xi ,t}=(I-K_t{H}_t){\Sigma }_{\xi ,t}$

以上，完成了一个最简单的基于反光板的EKF定位方案，不过还差一些工作需要补充：

（1）反光板的检测；

（2）反光板数据关联方法；

（3）状态空间的扩展：假如在定位过程中出现了一些新的反光板，则需要把该反光板加入状态空间；

（4）反光板地图的构建；

（5）基于反光板的重定位。