一起做激光反光板（三）-EKF建图公式推导

继续EKF建图公式推导。

构建反光板地图和定位篇有很大部分的重复。因为上一篇其实也包含了建图。

如果已经有初始的反光板地图，且初始的反光板地图不允许优化，且允许添加新的反光板，则该公式和上一篇反光板定位EKF公式推导所使用的公式一模一样；

如果已经有初始的反光板地图，且初始的反光板地图允许被优化，且允许添加新的反光板，该种相当于反光板地图的更新，暂时不考虑反光板地图的更新；

本部分只考虑没有初始反光板地图的情况，从零开始添加反光板。

建图EKF公式推导

初始的状态空间为$x,y,\theta$，在建图过程中，该状态空间在增长。

状态空间

假如当前的状态空间中有$N$个新增的反光板（$N>=0$），此时，状态空间为：

$X={\left[\begin{array}{cccccccc}x& y& \theta & m_x^1& m_y^1& ...& m_x^N& m_y^N\end{array}\right]}^T$

运动模型-预测

（1）位姿预测：

初始位姿$X_0={\left[\begin{array}{ccc}{x}_0& y_0& {\theta }_0\end{array}\right]}^T$，一般假设为单位阵，也可以指定初始位置。

运动方程可以写为：

${\left[\begin{array}{c}{x}_t\\ y_t\\ {\theta }_t\\ m_x^1\\ m_y^1\\ ...\\ m_x^N\\ m_y^N\end{array}\right]}_{2\ast N+3}={\left[\begin{array}{c}{x}_{t-1}\\ y_{t-1}\\ {\theta }_{t-1}\\ m_x^1\\ m_y^1\\ ...\\ m_x^N\\ m_y^N\end{array}\right]}_{2\ast N+3}+{\left[\begin{array}{c}{v}_t\Delta t\cos ({\theta }_{t-1}+{\omega }_t\Delta t/2)\\ v_t\Delta t\sin ({\theta }_{t-1}+{\omega }_t\Delta t/2)\\ {\omega }_t\Delta t\\ 0\\ 0\\ ...\\ 0\\ 0\end{array}\right]}_{2\ast N+3}$

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

${\Sigma }_u={\left[\begin{array}{cc}{\sigma }_v^2& 0\\ 0& {\sigma }_{\omega }^2\end{array}\right]}_{2\ast 2}$

（2）协方差预测

初始时刻位姿的协方差需要给定初值，手动给定：

${\Sigma }_{\xi ,0}={\left[\begin{array}{ccc}{\Sigma }_x& 0& 0\\ 0& {\Sigma }_y& 0\\ 0& 0& {\Sigma }_{\theta }\end{array}\right]}_{3\ast 3}$

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

其中：

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

$G_{\xi ,t}=\frac{\partial X_t}{\partial {\xi }_{t-1}}={\left[\begin{array}{ccc}1& 0& -v_t\Delta t\sin ({\theta }_{t-1}+{\omega }_t\Delta t/2)\\ 0& 1& v_t\Delta t\cos ({\theta }_{t-1}+{\omega }_t\Delta t/2)\\ 0& 0& 1\\ 0& 0& 0\\ 0& 0& 0\\ ...& ...& ...\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]}_{(2\ast N+3)\ast 3}$

$G_u$是运动模型关于控制（轮速计线速度和角速度）$u={\left[\begin{array}{cc}{v}_t& {\omega }_t\end{array}\right]}^T$的雅克比：

$G_u=\frac{\partial X_t}{\partial u}={\left[\begin{array}{cc}\Delta t\cos ({\theta }_{t-1}+{\omega }_t\Delta t/2)& \frac{-v_t\Delta t^2\sin ({\theta }_{t-1}+{\omega }_t\Delta t/2)}{2}\\ \Delta t\sin ({\theta }_{t-1}+{\omega }_t\Delta t/2)& \frac{v_t\Delta t^2\cos ({\theta }_{t-1}+{\omega }_t\Delta t/2)}{2}\\ 0& \Delta t\\ 0& 0\\ 0& 0\\ ...& ...\\ 0& 0\\ 0& 0\end{array}\right]}_{(2\ast N+3)\ast 2}$

观测模型

假设当前激光帧中检测到$K$个反光板${\left[\begin{array}{cccccccc}{r}_x^1& r_y^1& ...& r_x^i& r_y^i& ...& r_x^K& r_y^K\end{array}\right]}_{2K\ast 1}$，需要分为两种情况讨论：

（1）通过数据关联，我们找到状态空间中对应的$k2$个反光板；

（2）无法关联上的反光板有$k3=K-k2$个。

对于关联不上的反光板，我们不放进观测方程中，在后续进行状态空间的扩展。

观测方程直接写为：

${\left[\begin{array}{c}{r}_x^1\\ r_y^1\\ ...\\ r_x^i\\ r_y^i\\ ...\\ r_x^{k2}\\ r_y^{k2}\end{array}\right]}_{2\ast k2\ast 1}={\left[\begin{array}{c}(m_x^1-x)\cos \theta +(m_y^1-y)\sin \theta \\ -(m_x^1-x)\sin \theta +(m_y^1-y)\cos \theta \\ ...\\ (m_x^i-x)\cos \theta +(m_y^i-y)\sin \theta \\ -(m_x^i-x)\sin \theta +(m_y^i-y)\cos \theta \\ ...\\ (m_x^N-x)\cos \theta +(m_y^N-y)\sin \theta \\ -(m_x^N-x)\sin \theta +(m_y^N-y)\cos \theta \end{array}\right]}_{2\ast k2\ast 1}$

需要注意的是，上述中所用的$m_x^i$和$m_y^i$是通过数据关联，匹配上的状态空间内的反光板位置。

假如观测的第$i$个反光板和状态空间的第$j$个反光板匹配上，该反光板对应的子雅克比矩阵为：

$H_t^i=\frac{\partial h^i}{\partial X_t}={\left[\begin{array}{ccccccc}-\cos \theta & -\sin \theta & -(m_x^1-x)\sin \theta +(m_y^1-y)\cos \theta & 0_{2\ast (j-1)}& \cos \theta & \sin \theta & 0_{2(N-j)}\\ \sin \theta & -\cos \theta & -(m_x^1-x)\cos \theta -(m_y^1-y)\sin \theta & 0_{2\ast (j-1)}& -\sin \theta & \cos \theta & 0_{2(N-j)}\end{array}\right]}_{2\ast (2\ast N+3)}$

观测对状态空间总的雅克比矩阵为：

$H_t=\frac{\partial h}{\partial X_t}={\left[\begin{array}{c}{H}_t^1\\ H_t^2\\ ...\\ H_t^{k2}\end{array}\right]}_{2\ast k2\ast (2\ast N+3)}$

匹配上的反光板更新

计算增益

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

其中，$Q$为观测的协方差矩阵。

更新状态空间

$z_t-\hat{z} $的写法基本一样。

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

更新协方差

${\Sigma }_t=(I-K_t{H}_t){\Sigma }_t$

状态空间与协方差矩阵的扩展

状态空间的扩展

对于新增的反光板，状态空间扩展为：

$X_t^{{}^{\prime }}={\left[\begin{array}{ccccccccccccccc}x& y& \theta & m_x^1& m_y^1& m_x^2& m_y^2& ...& m_x^N& m_y^N& m_x^{N+1}& m_y^{N+1}& ...& m_x^{N+k3}& m_y^{N+k3}\end{array}\right]}^T$

其中，新观测到的反光板的为$z={\left[\begin{array}{ccccc}{r}_x^1& r_y^1& ...& r_x^{k3}& r_y^{k3}\end{array}\right]}^T$，新增的反光板地图点坐标为${\left[\begin{array}{ccccc}{m}_x^{N+1}& m_y^{N+1}& ...& m_x^{N+k3}& m_y^{N+k3}\end{array}\right]}^T$，根据当前机器人的位姿，有：

$h={\left[\begin{array}{c}{m}_x^{N+1}\\ m_y^{N+1}\\ ...\\ m_x^{N+k3}\\ m_y^{N+k3}\end{array}\right]}_{2N\ast 1}={\left[\begin{array}{c}{r}_x^1\cos \theta -r_y^1\sin \theta +x\\ r_x^1\sin \theta +r_y^1\cos \theta +y\\ ...\\ r_x^{k3}\cos \theta -r_y^{k3}\sin \theta +x\\ r_x^{k3}\sin \theta +r_y^{k3}\cos \theta +y\end{array}\right]}_{2\ast k3\ast 1}$

协方差矩阵的扩展

新的协方差矩阵为：

${\Sigma }_t^{{}^{\prime }}={\left[\begin{array}{cc}{\Sigma }_t& {\Sigma }_{mx}^T\\ {\Sigma }_{mx}& {\Sigma }_{mm}\end{array}\right]}_{(2N+3+2\ast k3)\ast (2N+3+2\ast k3)}$

其中：${\Sigma }_t$为扩展前的协方差矩阵，大小为$（2N+3）\ast （2N+3）$

新反光板的协方差为：${\Sigma }_{mm}=G_p{\Sigma }_{\xi }{G}_p^T+G_z{Q}_t{G}_z^T$，是一个$(2\ast k3)\ast (2\ast k3)$的矩阵。其中：

$G_p$为反光板$h$对位姿$\xi$的雅克比：

$G_p=\frac{\partial h}{\partial \xi }={\left[\begin{array}{ccc}1& 0& -r_x^1\sin \theta -r_y^1\cos \theta \\ 0& 1& r_x^1\cos \theta -r_y^1\sin \theta \\ ...& ...& ...\\ 1& 0& -r_x^{k3}\sin \theta -r_y^{k3}\cos \theta \\ 0& 1& r_x^{k3}\cos \theta -r_y^{k3}\sin \theta \end{array}\right]}_{(2\ast k3)\ast 3}$

${\Sigma }_{\xi }$为当前协方差矩阵的前三行三列。

$G_z$为反光板$h$对观测$z$的雅克比：

$G_z=\frac{\partial h}{\partial z}={\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \\ ...& ...\\ \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]}_{(2\ast k3)\ast 2}$

$Q_t$为观测协的方差:

$Q_t={\left[\begin{array}{cc}{\sigma }_x^2& 0\\ 0& {\sigma }_y^2\end{array}\right]}_{2\ast 2}$

${\Sigma }_{mx}$是新加的反光板和原状态空间之间的联合协方差，且${\Sigma }_{mx}=G_{fx}{\Sigma }_t$

$G_{fx}$为反光板$h$对原状态量$X$的雅克比：

$G_{fx}=\frac{\partial h}{\partial X_t}={\left[\begin{array}{cccccccc}1& 0& -r_x^1\sin \theta -r_y^1\cos \theta & 0& 0& ...& 0& 0\\ 0& 1& r_x^1\cos \theta -r_y^1\sin \theta & 0& 0& ...& 0& 0\\ ...& ...& ...& ...& ...& ...& ...& ...\\ 1& 0& -r_x^{k3}\sin \theta -r_y^{k3}\cos \theta & 0& 0& ...& 0& 0\\ 0& 1& r_x^{k3}\cos \theta -r_y^{k3}\sin \theta & 0& 0& ...& 0& 0\end{array}\right]}_{(2\ast k3)\ast (2N+3)}$

至此，反光板建图方案的EKF公式推导完成。