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

继续EKF建图公式推导。

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

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

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

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

建图EKF公式推导

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

状态空间

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

X=[xyθmx1my1...mxNmyN]TX=\begin{bmatrix} x & y &\theta & m_x^1 & m_y^1 &...& m_x^N & m_y^N\end{bmatrix}^TX=[xyθmx1my1...mxNmyN]T

运动模型-预测

（1）位姿预测：

初始位姿X0=[x0y0θ0]TX_0=\begin{bmatrix}x_0 & y_0 &\theta_0\end{bmatrix}^TX0=[x0y0θ0]T，一般假设为单位阵，也可以指定初始位置。

运动方程可以写为：

[xtytθtmx1my1...mxNmyN]2∗N+3=[xt−1yt−1θt−1mx1my1...mxNmyN]2∗N+3+[vtΔtcos⁡(θt−1+ωtΔt/2)vtΔtsin⁡(θt−1+ωtΔt/2)ωtΔt00...00]2∗N+3\begin{bmatrix}x_t \\ y_t \\ \theta_t \\ m_x^1 \\ m_y^1 \\...\\ m_x^N \\ m_y^N\end{bmatrix}_{2*N+3} = \begin{bmatrix}x_{t-1} \\ y_{t-1} \\ \theta_{t-1 }\\ m_x^1 \\ m_y^1 \\...\\ m_x^N \\ m_y^N\end{bmatrix}_{2*N+3} + \begin{bmatrix}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{bmatrix}_{2*N+3}⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡xtytθtmx1my1...mxNmyN⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤2∗N+3=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡xt−1yt−1θt−1mx1my1...mxNmyN⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤2∗N+3+⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡vtΔtcos(θt−1+ωtΔt/2)vtΔtsin(θt−1+ωtΔt/2)ωtΔt00...00⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤2∗N+3

假设轮速计的线速度和角速度服从均值为0的高斯分布，即控制的协方差为Σu\Sigma_uΣu

Σu=[σv200σω2]2∗2\Sigma_u=\begin{bmatrix}\sigma_{v}^2 & 0 \\ 0 & \sigma_{\omega}^2\end{bmatrix}_{2*2}Σu=[σv200σω2]2∗2

（2）协方差预测

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

Σξ,0=[Σx000Σy000Σθ]3∗3\Sigma_{\xi,0} =\begin{bmatrix}\Sigma_{x} & 0 & 0 \\ 0 & \Sigma_{y} & 0 \\ 0 & 0 & \Sigma_{\theta}\end{bmatrix}_{3*3}Σξ,0=⎣⎡Σx000Σy000Σθ⎦⎤3∗3

状态量在ttt时刻的协方差预测方程为：Σt=Gξ,tΣξ,t−1Gξ,tT+GuΣuGuT\Sigma_{t} = G_{\xi,t}\Sigma_{\xi,t-1}G_{\xi,t}^T+G_u\Sigma_uG_u^TΣt=Gξ,tΣξ,t−1Gξ,tT+GuΣuGuT

其中：

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

Gξ,t=∂Xt∂ξt−1=[10−vtΔtsin⁡(θt−1+ωtΔt/2)01vtΔtcos⁡(θt−1+ωtΔt/2)001000000.........000000](2∗N+3)∗3G_{\xi,t} = \frac{\partial X_t}{\partial\xi_{t-1}}=\begin{bmatrix}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{bmatrix}_{(2*N+3)*3}Gξ,t=∂ξt−1∂Xt=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡10000...0001000...00−vtΔtsin(θt−1+ωtΔt/2)vtΔtcos(θt−1+ωtΔt/2)100...00⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤(2∗N+3)∗3

GuG_uGu是运动模型关于控制（轮速计线速度和角速度）u=[vtωt]Tu = \begin{bmatrix}v_t & \omega_t\end{bmatrix}^Tu=[vtωt]T的雅克比：

Gu=∂Xt∂u=[Δtcos⁡(θt−1+ωtΔt/2)−vtΔt2sin⁡(θt−1+ωtΔt/2)2Δtsin⁡(θt−1+ωtΔt/2)vtΔt2cos⁡(θt−1+ωtΔt/2)20Δt0000......0000](2∗N+3)∗2G_u = \frac{\partial X_t}{\partial u}=\begin{bmatrix}\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{bmatrix}_{(2*N+3)*2}Gu=∂u∂Xt=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡Δtcos(θt−1+ωtΔt/2)Δtsin(θt−1+ωtΔt/2)000...002−vtΔt2sin(θt−1+ωtΔt/2)2vtΔt2cos(θt−1+ωtΔt/2)Δt00...00⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤(2∗N+3)∗2

观测模型

假设当前激光帧中检测到KKK个反光板[rx1ry1...rxiryi...rxKryK]2K∗1\begin{bmatrix}r_x^1 & r_y^1 & ...&r_x^i& r_y^i&... &r_x^K & r_y^K\end{bmatrix}_{2K*1}[rx1ry1...rxiryi...rxKryK]2K∗1，需要分为两种情况讨论：

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

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

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

观测方程直接写为：

[rx1ry1...rxiryi...rxk2ryk2]2∗k2∗1=[(mx1−x)cos⁡θ+(my1−y)sin⁡θ−(mx1−x)sin⁡θ+(my1−y)cos⁡θ...(mxi−x)cos⁡θ+(myi−y)sin⁡θ−(mxi−x)sin⁡θ+(myi−y)cos⁡θ...(mxN−x)cos⁡θ+(myN−y)sin⁡θ−(mxN−x)sin⁡θ+(myN−y)cos⁡θ]2∗k2∗1\begin{bmatrix}r_x^1 \\ r_y^1 \\ ...\\r_x^i \\ r_y^i \\... \\r_x^{k2} \\ r_y^{k2}\end{bmatrix}_{2*k2*1} =\begin{bmatrix} (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{bmatrix}_{2*k2*1}⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡rx1ry1...rxiryi...rxk2ryk2⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤2∗k2∗1=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡(mx1−x)cosθ+(my1−y)sinθ−(mx1−x)sinθ+(my1−y)cosθ...(mxi−x)cosθ+(myi−y)sinθ−(mxi−x)sinθ+(myi−y)cosθ...(mxN−x)cosθ+(myN−y)sinθ−(mxN−x)sinθ+(myN−y)cosθ⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤2∗k2∗1

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

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

Hti=∂hi∂Xt=[−cos⁡θ−sin⁡θ−(mx1−x)sin⁡θ+(my1−y)cos⁡θ02∗(j−1)cos⁡θsin⁡θ02(N−j)sin⁡θ−cos⁡θ−(mx1−x)cos⁡θ−(my1−y)sin⁡θ02∗(j−1)−sin⁡θcos⁡θ02(N−j)]2∗(2∗N+3)H_t^i = \frac{\partial h^i }{ \partial X_t} =\begin{bmatrix}-\cos\theta & -\sin\theta & -(m_x^1-x)\sin\theta+(m_y^1-y)\cos\theta &0_{2*(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*(j-1)} &-\sin\theta&\cos\theta&0_{2(N-j)}\end{bmatrix}_{2*(2*N+3)}Hti=∂Xt∂hi=[−cosθsinθ−sinθ−cosθ−(mx1−x)sinθ+(my1−y)cosθ−(mx1−x)cosθ−(my1−y)sinθ02∗(j−1)02∗(j−1)cosθ−sinθsinθcosθ02(N−j)02(N−j)]2∗(2∗N+3)

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

Ht=∂h∂Xt=[Ht1Ht2...Htk2]2∗k2∗(2∗N+3)H_t = \frac{\partial h }{ \partial X_t} = \begin{bmatrix}H_t^1\\H_t^2\\...\\H_t^{k2}\end{bmatrix}_{2*k2*(2*N+3)}Ht=∂Xt∂h=⎣⎢⎢⎡Ht1Ht2...Htk2⎦⎥⎥⎤2∗k2∗(2∗N+3)

匹配上的反光板更新

计算增益

Kt=ΣtHtT(HtΣtHtT+Q)−1K_t = \Sigma_{t}H_{t}^{T}(H_t\Sigma_{t}H_t^{T}+Q)^{-1}Kt=ΣtHtT(HtΣtHtT+Q)−1

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

更新状态空间

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

Xt=Xt+Kt(zt−z^)X_t = X_t + K_t(z_t-\hat{z})Xt=Xt+Kt(zt−z^)

更新协方差

Σt=(I−KtHt)Σt\Sigma_{t} = (I-K_tH_t)\Sigma_{t}Σt=(I−KtHt)Σt

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

状态空间的扩展

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

Xt′=[xyθmx1my1mx2my2...mxNmyNmxN+1myN+1...mxN+k3myN+k3]TX_t^{'}=\begin{bmatrix} 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{bmatrix}^TXt′=[xyθmx1my1mx2my2...mxNmyNmxN+1myN+1...mxN+k3myN+k3]T

其中，新观测到的反光板的为z=[rx1ry1...rxk3ryk3]Tz=\begin{bmatrix}r_x^{1}&r_y^{1} & ... & r_x^{k3} & r_y^{k3}\end{bmatrix}^Tz=[rx1ry1...rxk3ryk3]T，新增的反光板地图点坐标为[mxN+1myN+1...mxN+k3myN+k3]T\begin{bmatrix}m_x^{N+1}&m_y^{N+1} & ...&m_x^{N+k3}&m_{y}^{N+k3}\end{bmatrix}^T[mxN+1myN+1...mxN+k3myN+k3]T，根据当前机器人的位姿，有：

h=[mxN+1myN+1...mxN+k3myN+k3]2N∗1=[rx1cos⁡θ−ry1sin⁡θ+xrx1sin⁡θ+ry1cos⁡θ+y...rxk3cos⁡θ−ryk3sin⁡θ+xrxk3sin⁡θ+ryk3cos⁡θ+y]2∗k3∗1h=\begin{bmatrix}m_x^{N+1} \\ m_y^{N+1} \\ ...\\ m_{x}^{N+k3}\\m_{y}^{N+k3}\end{bmatrix}_{2N*1}= \begin{bmatrix}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{bmatrix}_{2*k3*1}h=⎣⎢⎢⎢⎢⎡mxN+1myN+1...mxN+k3myN+k3⎦⎥⎥⎥⎥⎤2N∗1=⎣⎢⎢⎢⎢⎡rx1cosθ−ry1sinθ+xrx1sinθ+ry1cosθ+y...rxk3cosθ−ryk3sinθ+xrxk3sinθ+ryk3cosθ+y⎦⎥⎥⎥⎥⎤2∗k3∗1

协方差矩阵的扩展

新的协方差矩阵为：

Σt′=[ΣtΣmxTΣmxΣmm](2N+3+2∗k3)∗(2N+3+2∗k3)\Sigma_t^{'} = \begin{bmatrix}\Sigma_t & \Sigma_{mx}^T\\ \Sigma_{mx} & \Sigma_{mm}\end{bmatrix}_{(2N+3+2*k3)*(2N+3+2*k3)}Σt′=[ΣtΣmxΣmxTΣmm](2N+3+2∗k3)∗(2N+3+2∗k3)

其中：Σt\Sigma_tΣt为扩展前的协方差矩阵，大小为（2N+3）∗（2N+3）（2N+3）*（2N+3）（2N+3）∗（2N+3）

新反光板的协方差为：Σmm=GpΣξGpT+GzQtGzT\Sigma_{mm} = G_p \Sigma_{\xi}G_p^T+G_zQ_tG_z^TΣmm=GpΣξGpT+GzQtGzT，是一个(2∗k3)∗(2∗k3)(2*k3)*(2*k3)(2∗k3)∗(2∗k3)的矩阵。其中：

GpG_pGp为反光板hhh对位姿ξ\xiξ的雅克比：

Gp=∂h∂ξ=[10−rx1sin⁡θ−ry1cos⁡θ01rx1cos⁡θ−ry1sin⁡θ.........10−rxk3sin⁡θ−ryk3cos⁡θ01rxk3cos⁡θ−ryk3sin⁡θ](2∗k3)∗3G_p = \frac{\partial h}{\partial\xi} = \begin{bmatrix}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{bmatrix}_{(2*k3)*3}Gp=∂ξ∂h=⎣⎢⎢⎢⎢⎡10...1001...01−rx1sinθ−ry1cosθrx1cosθ−ry1sinθ...−rxk3sinθ−ryk3cosθrxk3cosθ−ryk3sinθ⎦⎥⎥⎥⎥⎤(2∗k3)∗3

Σξ\Sigma_{\xi}Σξ为当前协方差矩阵的前三行三列。

GzG_zGz为反光板hhh对观测zzz的雅克比：

Gz=∂h∂z=[cos⁡θ−sin⁡θsin⁡θcos⁡θ......cos⁡θ−sin⁡θsin⁡θcos⁡θ](2∗k3)∗2G_z = \frac{\partial h}{\partial z} =\begin{bmatrix}\cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \\ ... & ... \\ \cos\theta & -\sin\theta \\ \sin\theta& \cos\theta\end{bmatrix}_{(2*k3)*2}Gz=∂z∂h=⎣⎢⎢⎢⎢⎡cosθsinθ...cosθsinθ−sinθcosθ...−sinθcosθ⎦⎥⎥⎥⎥⎤(2∗k3)∗2

QtQ_tQt为观测协的方差:

Qt=[σx200σy2]2∗2Q_t = \begin{bmatrix}\sigma _x^2 & 0 \\ 0 & \sigma_y^2\end{bmatrix}_{2*2}Qt=[σx200σy2]2∗2

Σmx\Sigma_{mx}Σmx是新加的反光板和原状态空间之间的联合协方差，且Σmx=GfxΣt\Sigma_{mx}=G_{fx}\Sigma_tΣmx=GfxΣt

GfxG_{fx}Gfx为反光板hhh对原状态量XXX的雅克比：

Gfx=∂h∂Xt=[10−rx1sin⁡θ−ry1cos⁡θ00...0001rx1cos⁡θ−ry1sin⁡θ00...00........................10−rxk3sin⁡θ−ryk3cos⁡θ00...0001rxk3cos⁡θ−ryk3sin⁡θ00...00](2∗k3)∗(2N+3)G_{fx} = \frac{\partial h}{\partial X_t} =\begin{bmatrix}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{bmatrix}_{(2*k3)*(2N+3)}Gfx=∂Xt∂h=⎣⎢⎢⎢⎢⎡10...1001...01−rx1sinθ−ry1cosθrx1cosθ−ry1sinθ...−rxk3sinθ−ryk3cosθrxk3cosθ−ryk3sinθ00...0000...00...............00...0000...00⎦⎥⎥⎥⎥⎤(2∗k3)∗(2N+3)

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