您现在的位置是：首页 >其他 >2D平面机器人运动ESKF过程相关方程推导网站首页其他

2D平面运动相关状态量及符号约定

机器人(Robot)状态：世界系下位置(Positon)记为$P_R=\left|\begin{array}{c}{x}_R\\ y_R\end{array}\right|$，世界系下航向(orientation)记为$\Phi$;
路标点(Landmark)状态：世界系下位置(Postion)记为$P_L=\left|\begin{array}{c}{x}_L\\ y_L\end{array}\right|$;
约定：
真实状态(true state)表示为：$P_R,{\Phi }_R,P_L$;
名义状态(nominal state)表示为：${\hat{P}}_R,{\hat{\Phi }}_R,{\hat{P}}_L$;
误差状态(error state)表示为：${\tilde{P}}_R,{\tilde{\Phi }}_R,{\tilde{P}}_L$;
二维旋转矩阵：记为$C(\cdot )$，则对于yaw角$\Phi$，有:
$$C(\Phi )=\left[\begin{array}{cc}cos\Phi & -sin\Phi \\ sin\Phi & cos\Phi \end{array}\right]\text{\hspace{1em}(1)}$$
旋转矩阵的转置:
$$C^T(\Phi )=\left[\begin{array}{cc}cos\Phi & -sin\Phi \\ sin\Phi & cos\Phi \end{array}\right]\text{\hspace{1em}(2)}$$
记系数矩阵:
$$J=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]\text{\hspace{1em}(3)}$$
根据求导法则，有：
旋转矩阵的导数：
$$C^{\prime }(\Phi )=JC(\Phi )\text{\hspace{1em}(4)}$$
旋转矩阵转置的导数：
$${C^T}^{\prime }(\Phi )=-J{C}^T(\Phi )\text{\hspace{1em}(5)}$$

卡尔曼滤波器(Kalman Filter)方程的一般形式

假设有$k$时刻状态量，由非线性传播函数$g(\cdot )$可以递推$k+1$时刻的状态量，传播函数所需输入记为$u$，则有：
$$X_{k+1}=g(X_k,u)\text{\hspace{1em}(6)}$$
由于$g(\cdot )$是非线性的，而卡尔曼滤波器要求在线性系统中递推，因此，需要对$g(X_k,u)$进行线性化展开，使得：
$$X_{k+1}=F{X}_k+Au\text{\hspace{1em}(7)}$$
这里的$F$和$A$均为与状态量$X$和$u$无关的矩阵，此外卡尔曼滤波器假设传感器的噪声是服从高斯分布的，这里假设$X_k\sim (0,P_k)$，$u\sim (0,R)$，根据高斯分布的线性递推方程：
$$X_{k+1}\sim (0,F{P}_k{F}^T+R)\text{\hspace{1em}(8)}$$
对于非线性观测方程$h(\cdot )$也需要进行类似地线性展开，使得$k+1$时刻的状态量$z_{k+1}$与$k$时刻状态量$z_k$呈线性关系即：
$$z_{k+1}=H{X}_{k+1|k}+v\text{\hspace{1em}(9)}$$
其中$v$表示测量噪声。

对于误差状态的卡尔曼滤波器(Error State Kalman Filter)其状态量为$\tilde{X}$，因此，需要推导与${\tilde{X}}_k$到${\tilde{X}}_{k+1}$相关的线性化传播方程以及测量方程。

2D平面机器人运动ESKF过程推导

误差传播状态方程推导

设，从$k$时刻到$k+1$时刻，机器人的真实状态(true state)变化量记为$(P_{k+1}^k,{\Phi }_{k+1}^k)$，路标点位置是在世界系下的位置是固定的，则有：

true state对应的传播方程：

$$\begin{array}{rl}{P}_{R_k+1}& =P_{R_k}+C({\Phi }_{R_k})P_{k+1}^k\\ {\Phi }_{R_k+1}& ={\Phi }_{R_k}+{\Phi }_{k+1}^k\\ P_{L_k+1}& =P_{L_k}\end{array}\text{\hspace{1em}(10)(11)(12)}$$

nominal state对应的传播方程：

$$\begin{array}{rl}{\hat{P}}_{R_k+1}& ={\hat{P}}_{R_k}+C({\hat{\Phi }}_{R_k}){\hat{P}}_{k+1}^k\\ {\hat{\Phi }}_{R_k+1}& ={\hat{\Phi }}_{R_k}+{\hat{\Phi }}_{k+1}^k\\ {\hat{P}}_{L_k+1}& ={\hat{P}}_{L_k}\end{array}\text{\hspace{1em}(13)(14)(15)}$$
由于：
$$\tilde{X}=X-\hat{X}\text{\hspace{1em}(16)}$$
则：

error state对应的传播方程：

$$\begin{array}{rl}{\tilde{P}}_{R_k+1}& ={\tilde{P}}_{R_k}+C({\Phi }_{R_k})P_{k+1}^k-C({\hat{\Phi }}_{R_k}){\hat{P}}_{k+1}^k\\ {\tilde{\Phi }}_{R_k+1}& ={\tilde{\Phi }}_{R_k}+{\tilde{\Phi }}_{k+1}^k\\ {\tilde{P}}_{L_k+1}& ={\tilde{P}}_{L_k}\end{array}\text{\hspace{1em}(17)(18)(19)}$$
由于Robot状态量$X_R=\left|\begin{array}{c}{x}_R\\ y_R\\ \Phi \end{array}\right|$，所以公式$(17)$是非线性的，需要对其进行线性化展开，展开点在${\hat{X}}_R$：
$$\begin{array}{rl}{\tilde{P}}_{R_k+1}& ={\tilde{P}}_{R_k}+C({\Phi }_{R_k})P_{k+1}^k-C({\hat{\Phi }}_{R_k}){\hat{P}}_{k+1}^k\\ & ={\tilde{P}}_{R_k}+C({\Phi }_{R_k})P_{k+1}^k-C({\hat{\Phi }}_{R_k})(P_{k+1}^k-{\tilde{P}}_{k+1}^k)\\ & ={\tilde{P}}_{R_k}+(C({\Phi }_{R_k})-C({\hat{\Phi }}_{R_k}))P_{k+1}^k+C(\hat{\Phi }){\tilde{P}}_{k+1}^k\\ & \approx {\tilde{P}}_{R_k}+JC({\hat{\Phi }}_{R_k}){\tilde{\Phi }}_{R_k}{P}_{k+1}^k+C(\hat{\Phi }){\tilde{P}}_{k+1}^k\\ & \approx {\tilde{P}}_{R_k}+C(\hat{\Phi }){\tilde{P}}_{k+1}^k+JC({\hat{\Phi }}_{R_k}){\hat{P}}_{k+1}^k{\tilde{\Phi }}_{R_k}\end{array}\text{\hspace{1em}(20)}$$
误差状态传播函数经过线性化展开后，有：
$$\begin{array}{rl}{\tilde{X}}_{k+1}& =\left|\begin{array}{cc}{I}_2& JC({\hat{\Phi }}_{R_k}){\hat{P}}_{k+1}^k\\ {\text{0}}_{1\times 2}& 1\end{array}\right|\times \left|\begin{array}{c}{\tilde{P}}_{R_k}\\ {\tilde{P}}_{L_k}\end{array}\right|+\left|\begin{array}{cc}C(\hat{\Phi })& \text{0}\\ {\text{0}}_{1\times 2}& 1\end{array}\right|\times \left|\begin{array}{c}{\tilde{P}}_{k+1}^k\\ {\tilde{\Phi }}_{k+1}^k\end{array}\right|\\ & =\left|\begin{array}{cc}{I}_2& JC({\hat{\Phi }}_{R_k}){\hat{P}}_{k+1}^k\\ {\text{0}}_{1\times 2}& 1\end{array}\right|{\tilde{X}}_k+\left|\begin{array}{cc}C(\hat{\Phi })& \text{0}\\ {\text{0}}_{1\times 2}& 1\end{array}\right|{\tilde{X}}_{k+1}^k\end{array}\text{\hspace{1em}(21)}$$

误差观测方程推导

对于世界坐标系下一点$P_L$,将其变换到机器人坐标系下一点$P_R$的观测方程为：
$$P_R=C^T({\Phi }_{R_{k+1|k}})(P_L-P_{R_{k+1|k}})\text{\hspace{1em}(22)}$$
那么，机器人在真实状态(true state)下时，有观测方程：
$$z=C^T({\Phi }_{R_{k+1|k}})(P_L-P_{R_{k+1|k}})+v\text{\hspace{1em}(23)}$$
机器人在名义状态(nominal state)下时，有观测方程：
$$\hat{z}=C^T({\hat{\Phi }}_{R_{k+1|k}})(\widehat{P_L}-{\hat{P}}_{R_{k+1|k}})\text{\hspace{1em}(24)}$$
那么误差状态(error state)方程由式$(23)-(24)$得到：
$$\begin{array}{rl}\tilde{z}& =C^T({\Phi }_{R_{k+1|k}})(P_L-P_{R_{k+1|k}})-C^T({\hat{\Phi }}_{R_{k+1|k}})(\widehat{P_L}-{\hat{P}}_{R_{k+1|k}})+v\\ & =C^T({\Phi }_{R_{k+1|k}})(P_L-P_{R_{k+1|k}})-C^T({\hat{\Phi }}_{R_{k+1|k}})(P_L-{\tilde{P}}_L-P_{R_{k+1|k}}+{\tilde{P}}_{R_{k+1|k}})+v\\ & =(C^T({\Phi }_{R_{k+1|k}})-C^T({\hat{\Phi }}_{R_{k+1|k}}))(P_L-P_{R_{k+1|k}})+C^T({\hat{\Phi }}_{R_{k+1|k}})({\tilde{P}}_L-{\tilde{P}}_{R_{k+1|k}})+v\\ & \approx -J{C}^T({\hat{\Phi }}_{R_{k+1|k}})(P_L-P_{R_{k+1|k}}){\tilde{\Phi }}_{R_{k+1|k}}+C^T({\hat{\Phi }}_{R_{k+1|k}}){\tilde{P}}_L-C^T({\hat{\Phi }}_{R_{k+1|k}}){\tilde{P}}_{R_{k+1|k}}+v\\ & \approx -J{C}^T({\hat{\Phi }}_{R_{k+1|k}})({\hat{P}}_L-{\hat{P}}_{R_{k+1|k}}){\tilde{\Phi }}_{R_{k+1|k}}+C^T({\hat{\Phi }}_{R_{k+1|k}}){\tilde{P}}_L-C^T({\hat{\Phi }}_{R_{k+1|k}}){\tilde{P}}_{R_{k+1|k}}+v\end{array}\text{\hspace{1em}(25)}$$
式$25$写成线性矩阵的形式有：
$$\begin{array}{rl}\tilde{z}& =\left|\begin{array}{ccc}-C^T({\hat{\Phi }}_{R_{k+1|k}})& -J{C}^T({\hat{\Phi }}_{R_{k+1|k}})({\hat{P}}_L-{\hat{P}}_{R_{k+1|k}})& C^T({\hat{\Phi }}_{R_{k+1|k}})\end{array}\right|\times \left|\begin{array}{c}{\tilde{P}}_{R_{k+1|k}}\\ {\tilde{\Phi }}_{R_{k+1|k}}\\ {\tilde{P}}_L\end{array}\right|+v\\ & =C^T({\hat{\Phi }}_{R_{k+1|k}})\left|\begin{array}{ccc}-{\text{I}}_2& -J({\hat{P}}_L-{\hat{P}}_{R_{k+1|k}})& {\text{I}}_2\end{array}\right|\times \left|\begin{array}{c}{\tilde{X}}_{k|k+1}\\ {\tilde{P}}_L\end{array}\right|+v\end{array}\text{\hspace{1em}(26)}$$
至此我们推导了误差状态传播方程和观测方程的线性表达式，后续按照卡尔曼滤波器的一般过程即可。