FEJ-黄国权

Catalogue
  1. 1. 1. First Estimates Jacobian (FEJ)-EKF
  2. 2. 2. 标准EKF-SLAM
    1. 2.1. 2.1. EKF传播
    2. 2.2. 2.2. EKF更新
  3. 3. 3. SLAM的非线性可观测性分析
    1. 3.1. 3.1. 先导内容(引自_崔老师_连续非线性系统的可观性)
      1. 3.1.1. 3.1.1. 非线性系统的可观测性矩阵
    2. 3.2. 3.2. 基于小车实例的可观测性分析
      1. 3.2.1. 3.2.1. 补充:引自_崔老师_可观性矩阵的计算
        1. 3.2.1.1. 3.2.1.1. 观测z对状态X的雅克比\(\frac{\partial z}{\partial X}\)
        2. 3.2.1.2. 3.2.1.2. {观测值z对时间的导数\(\dot{z}\)}对状态X的雅克比\(\frac{\partial \dot{z}}{\partial X}\)
        3. 3.2.1.3. 3.2.1.3. {观测值z对时间的二次偏导\(\ddot{z}\)}对状态X的雅克比\(\frac{\partial \ddot{x}}{\partial X}\)
        4. 3.2.1.4. 3.2.1.4. 最终得到可观性矩阵
    3. 3.3. 3.3. 不可观测的意义
      1. 3.3.1. 3.3.1. (1) 零空间内平移
      2. 3.3.2. 3.3.2. (2) 零空间内旋转
      3. 3.3.3. 3.3.3. 结论

1. First Estimates Jacobian (FEJ)-EKF

2. 标准EKF-SLAM

(引用崔老师的论文阅读配图)

如图所示,是以 2D 平面上的小车为例,且假设运动输入\(u\)为始终沿车头方向的速度\(v\),和角速度\(\omega\),世界坐标系为W(orld),小车坐标系为R(obot)

取状态向量,由小车的位姿以及路标点的在全局坐标系的位置组成

在时间步k,状态向量如下:

其中,\(x_{Rk}=[p_{Rk}^T,\phi_{Rk}]^T\),表示了小车的位姿(位置和旋转),\(p_L\)则是路标的坐标(世界坐标系)

2.1. EKF传播

在这一步中,得到两个连续时间步之间的姿态变化估计值,然后利用EKF来传播机器人状态估计。

另一方面,由于路标是静态的,它的估计值不会随着新的里程测量的加入而改变

于是,可以得到EKF传播步骤:

其中,

  • \(C(\cdot)\)表示了2x2的旋转矩阵(基于k时刻更新之后的从机器人坐标系到世界坐标系的旋转变换)
  • \(^{R_k}\hat{x}_{R_{k+1}}=[^{R_k}\hat{p}_{R_{k+1}}^T,^{R_k}\hat{\phi}_{R_{k+1}}]\)是机器人在k到k+1时刻的运动估计,这个估计受到零均值高斯白噪声的影响,白噪声为\(w_k=^{R_k}x_{R_{k+1}}-^{R_k}\hat{x}_{R_{k+1}}\),其协方差矩阵为Q

除了状态传播,线性化的误差状态传播也是需要的,即:

其中,\(\Phi_{R_k}\)\({3\times 3}\)矩阵,是上面的预测方程对状态量的雅克比,

于是,\(\Phi_{R_k}\)\(G_{R_k}\)形式如下:

上下标说明 - 下标\(l|j\),是指在时间步长为j的所有测量值处理完毕后,在时间步l处对一个状态量的估计 - \(\hat{x}\)用来表示随机变量x的估计值 - \(\tilde{x}=x-\hat{x}\)表示估计值的与真值的误差 - \(s\phi,c\phi\)分别表示\(\sin \phi\)\(\cos \phi\)

需要指出的是,上述传播方程的形式是通用的,适用于任何机器人运动学模型(如独轮车、自行车或Ackerman模型).

2.2. EKF更新

EKF中用于更新的观测是路标相对于机器人的相对位置的函数:

其中,\(v_k\)是协方差为\(R_k\)的零均值高斯测量噪声,在这里,允许观测函数h为任意的2维观测函数,只要它是可逆的(只要我们能从\(z_k\)完全确定机器人相对地标位置的估计值)。

通常,测量函数是非线性的,因此它被线性化,用于EKF,线性化的测量误差方程如下:

其中,\(H_{Rk}\)\(H_{Lk}\)是h分别对机器人位姿和路标位置的雅克比,在线性化点\(\hat{x}_{k|k-1}\)处计算得到。使用链式法则,有:

其中,

  • \(\bigtriangledown h_k\)表示观测函数h()对于机器人-路标的相对位置的雅克比
  • 如对\(^{R_k}p_L=C^T(\phi_{Rk})(p_L-P_{RK})\)的雅克比,也就是观测函数h()对路标在机器人坐标系的坐标的雅克比
  • 线性化点在\(\hat{x}_{k|k-1}\)

3. SLAM的非线性可观测性分析

3.1. 先导内容(引自_崔老师_连续非线性系统的可观性)

3.1.1. 非线性系统的可观测性矩阵

3.2. 基于小车实例的可观测性分析

在研究EKF系统模型的可观测特性之前,先对连续时间非线性SLAM系统进行可观测性分析,将实际非线性系统的性质与EKF系统模型的性质进行比较,我们可以确定标准EKF系统中的一个基本缺点,这将会导致滤波器的不一致性。

下面证明SLAM系统不满足可观测性秩条件,因此它既不是局部弱可观测的,也不是局部可观测的

对于连续时间的分析,我们使用了一个上面所述的2D平面小车运动学模型,以及相对位置测量模型。连续时间下的系统模型为:

其中,\(u\triangleq [v,w]^T\)是控制输入量,包含线速度和角速度。

连续时间的相对位置测量模型为(即路标点在机器人坐标系的坐标):

\[ \begin{aligned} C_{wr} p_{L,inR}=P_{L,in w}-P_{R}(t) \\ \Longrightarrow C_{wr} p_{L,inR}+P_{R}(t)=P_{L,in w} \end{aligned} \]

上式就是将机器人坐标系下的路标点转换到世界坐标系的变换,反过来,就是(式16),可以得到机器人坐标系下的路标点的坐标

对式(15)和式(16)中描述的系统模型进行可观测性分析,首先计算所有k阶Lie 微分\(L_{f_j}^k h_i (for k \in \N,j=1,2,i=1,2)\),用\(\mathcal{G}\)来表示。

\(\mathcal{G}\)各元素的梯度张成的空间\(d\mathcal{G}\)为:

上述矩阵就是非线性SLAM系统的可观测矩阵,显然,这个矩阵的秩是2,因此这个系统是不可观测的。

3.2.1. 补充:引自_崔老师_可观性矩阵的计算

为什么只取到2阶? 意味着n=3? 3个自由度(平移xy+旋转)?

3.2.1.1. 观测z对状态X的雅克比\(\frac{\partial z}{\partial X}\)

3.2.1.2. {观测值z对时间的导数\(\dot{z}\)}对状态X的雅克比\(\frac{\partial \dot{z}}{\partial X}\)

3.2.1.3. {观测值z对时间的二次偏导\(\ddot{z}\)}对状态X的雅克比\(\frac{\partial \ddot{x}}{\partial X}\)

3.2.1.4. 最终得到可观性矩阵

3.3. 不可观测的意义

直觉上,就是我们无法获得绝对的结果,只是从有效测量中获得的相对的状态信息。

尽管不可观测的子空间的概念不能在这个系统中严格定义,通过对\(d\mathcal{G}\)的正交子空间\(d\mathcal{G}\perp\)的基进行物理解释,我们可以获得有用的信息。

通过观察,我们可以看到\(d\mathcal{G}\perp\)空间一种可能的基具有如下形式:

3.3.1. (1) 零空间内平移

从向量n1和n2的结构中我们可以看到状态的变化,即

\[ \Delta x=\alpha n_1 +\beta n_2 \]

这对应于在x轴和y轴上的漂移,x轴上漂移\(\alpha\)个单位,y轴上漂移\(\beta\)单位

因此,如果机器人和地标位置移动相等,则根据测量结果,\(x\)\(x+\Delta x\)的状态是相等的,即满足\(Hx=0\)\(H(x+\Delta x)=0\)

3.3.2. (2) 零空间内旋转

为了更好地理解\(n_3\)的物理意义,我们考虑一个情况:

即'x-y'平面通过一个小的旋转量\(\delta \phi\)进行旋转,即受到小扰动。对于平面内所有的点都进行旋转变换,从\(\boldsymbol{x}=[x,y]^T\)变换为\(\boldsymbol{x'}=[x' , y']\),变换等式为:

其中,旋转矩阵如下:

\[ \begin{aligned} \begin{bmatrix} \cos \delta \phi & -\sin \delta \phi \\ \sin \delta \phi & \cos \delta \phi \end{bmatrix} \sim \begin{bmatrix} 1 & -\delta \phi \\ \delta \phi & 1 \end{bmatrix} \end{aligned} \]

利用上面这个结果,我们可以看到,如果机器人和路标点所在平面受到小扰动\(\delta \phi\)进行旋转,SLAM系统的状态向量变化如下:

这表明,向量\(n_3\)x-y平面的旋转相关,因为\(n_3 \in d \mathcal{G} \perp\),这个结果展示了对于旋转量也是不可观测的,因为旋转不会导致观测量的改变。

3.3.3. 结论

在上面的分析中,对\(d \mathcal{G} \perp\)空间的基向量意义的分析,与我们的直觉是一致的,即全局坐标下的状态向量不可观测。