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

文章详细推导了2D平面上机器人运动的误差状态卡尔曼滤波过程,包括真实状态、名义状态和误差状态的传播方程,以及误差观测方程。通过对非线性传播和观测方程的线性化,得到了误差状态的线性传播和观测表达,为后续的卡尔曼滤波计算奠定了基础。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

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

机器人(Robot)状态:世界系下位置(Positon)记为PR=∣xRyR∣P_R=\begin{vmatrix} x_R \\ y_R \end{vmatrix}PR=xRyR,世界系下航向(orientation)记为Φ\PhiΦ;
路标点(Landmark)状态:世界系下位置(Postion)记为PL=∣xLyL∣P_L=\begin{vmatrix} x_L\\ y_L \end{vmatrix}PL=xLyL;
约定:
真实状态(true state)表示为:PR,ΦR,PLP_R, \Phi _R, P_LPR,ΦR,PL;
名义状态(nominal state)表示为:P^R,Φ^R,P^L\hat{P}_R, \hat{\Phi} _R, \hat{P}_LP^R,Φ^R,P^L;
误差状态(error state)表示为:P~R,Φ~R,P~L\widetilde{P}_R, \widetilde{\Phi} _R, \widetilde{P}_LPR,ΦR,PL;
二维旋转矩阵:记为C(⋅)C(\cdot)C(),则对于yaw角Φ\PhiΦ,有:
C(Φ)=[cosΦ−sinΦsinΦcosΦ](1)C(\Phi) = \begin{bmatrix} cos\Phi & -sin\Phi\\ sin\Phi& cos\Phi \end{bmatrix}\tag{1}C(Φ)=[cosΦsinΦsinΦcosΦ](1)
旋转矩阵的转置:
CT(Φ)=[cosΦ−sinΦsinΦcosΦ](2)C^T(\Phi) = \begin{bmatrix} cos\Phi & -sin\Phi\\ sin\Phi& cos\Phi \end{bmatrix}\tag{2}CT(Φ)=[cosΦsinΦsinΦcosΦ](2)
记系数矩阵:
J=[0−110](3)J = \begin{bmatrix} 0 & -1 \\ 1& 0 \end{bmatrix}\tag{3}J=[0110](3)
根据求导法则,有:
旋转矩阵的导数:
C′(Φ)=JC(Φ)(4)C'(\Phi) = JC(\Phi)\tag{4}C(Φ)=JC(Φ)(4)
旋转矩阵转置的导数:
CT′(Φ)=−JCT(Φ)(5){C^T}'(\Phi) = -JC^T(\Phi)\tag{5}CT(Φ)=JCT(Φ)(5)

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

假设有kkk时刻状态量,由非线性传播函数g(⋅)g(\cdot)g()可以递推k+1k+1k+1时刻的状态量,传播函数所需输入记为uuu,则有:
Xk+1=g(Xk,u)(6)X_{k+1} = g(X_k, u)\tag{6}Xk+1=g(Xk,u)(6)
由于g(⋅)g(\cdot)g()是非线性的,而卡尔曼滤波器要求在线性系统中递推,因此,需要对g(Xk,u)g(X_k, u)g(Xk,u)进行线性化展开,使得:
Xk+1=FXk+Au(7)X_{k+1} = FX_k + Au\tag{7}Xk+1=FXk+Au(7)
这里的FFFAAA均为与状态量XXXuuu无关的矩阵,此外卡尔曼滤波器假设传感器的噪声是服从高斯分布的,这里假设Xk∼(0,Pk)X_k\sim(0, P_k)Xk(0,Pk)u∼(0,R)u\sim(0, R)u(0,R),根据高斯分布的线性递推方程:
Xk+1∼(0,FPkFT+R)(8)X_{k+1}\sim(0, FP_kF^T+R)\tag{8}Xk+1(0,FPkFT+R)(8)
对于非线性观测方程h(⋅)h(\cdot)h()也需要进行类似地线性展开,使得k+1k+1k+1时刻的状态量zk+1z_{k+1}zk+1kkk时刻状态量zkz_kzk呈线性关系即:
zk+1=HXk+1∣k+v(9)z_{k+1}=HX_{k+1|k}+v\tag{9}zk+1=HXk+1∣k+v(9)
其中vvv表示测量噪声。

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

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

误差传播状态方程推导

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

true state对应的传播方程:

PRk+1=PRk+C(ΦRk)Pk+1kΦRk+1=ΦRk+Φk+1kPLk+1=PLk \begin{align*} P_{R_k+1} &= P_{R_k} + C(\Phi_{R_k})P^k_{k+1} \tag{10}\\ \Phi _{R_k+1} &= \Phi_{R_k} + \Phi^k_{k+1}\tag{11}\\ P_{L_k+1} &= P_{L_k}\tag{12} \end{align*} PRk+1ΦRk+1PLk+1=PRk+C(ΦRk)Pk+1k=ΦRk+Φk+1k=PLk(10)(11)(12)

nominal state对应的传播方程:

P^Rk+1=P^Rk+C(Φ^Rk)P^k+1kΦ^Rk+1=Φ^Rk+Φ^k+1kP^Lk+1=P^Lk \begin{align*} \hat{P}_{R_k+1} &= \hat{P}_{R_k} + C(\hat{\Phi}_{R_k})\hat{P}^k_{k+1} \tag{13}\\ \hat{\Phi}_{R_k+1} &= \hat{\Phi}_{R_k} + \hat{\Phi}^k_{k+1}\tag{14}\\ \hat{P}_{L_k+1} &= \hat{P}_{L_k}\tag{15} \end{align*} P^Rk+1Φ^Rk+1P^Lk+1=P^Rk+C(Φ^Rk)P^k+1k=Φ^Rk+Φ^k+1k=P^Lk(13)(14)(15)
由于:
X~=X−X^(16)\widetilde{X} = X - \hat{X}\tag{16}X=XX^(16)
则:

error state对应的传播方程:

P~Rk+1=P~Rk+C(ΦRk)Pk+1k−C(Φ^Rk)P^k+1kΦ~Rk+1=Φ~Rk+Φ~k+1kP~Lk+1=P~Lk \begin{align*} \widetilde{P}_{R_k+1} &= \widetilde{P}_{R_k} + C(\Phi_{R_k})P^k_{k+1} - C(\hat{\Phi}_{R_k})\hat{P}^k_{k+1} \tag{17}\\ \widetilde{\Phi} _{R_k+1} &= \widetilde{\Phi}_{R_k} + \widetilde{\Phi}^k_{k+1}\tag{18}\\ \widetilde{P}_{L_k+1} &= \widetilde{P}_{L_k}\tag{19} \end{align*} PRk+1ΦRk+1PLk+1=PRk+C(ΦRk)Pk+1kC(Φ^Rk)P^k+1k=ΦRk+Φk+1k=PLk(17)(18)(19)
由于Robot状态量XR=∣xRyRΦ∣X_R = \begin{vmatrix} x_R \\ y_R \\ \Phi\end{vmatrix}XR=xRyRΦ,所以公式(17)(17)(17)是非线性的,需要对其进行线性化展开,展开点在X^R\hat{X}_RX^R
P~Rk+1=P~Rk+C(ΦRk)Pk+1k−C(Φ^Rk)P^k+1k=P~Rk+C(ΦRk)Pk+1k−C(Φ^Rk)(Pk+1k−P~k+1k)=P~Rk+(C(ΦRk)−C(Φ^Rk))Pk+1k+C(Φ^)P~k+1k≈P~Rk+JC(Φ^Rk)Φ~RkPk+1k+C(Φ^)P~k+1k≈P~Rk+C(Φ^)P~k+1k+JC(Φ^Rk)P^k+1kΦ~Rk \begin{align*} \widetilde{P}_{R_k+1} &= \widetilde{P}_{R_k} + C(\Phi_{R_k})P^k_{k+1} - C(\hat{\Phi}_{R_k})\hat{P}^k_{k+1}\\ &=\widetilde{P}_{R_k}+C(\Phi_{R_k})P^k_{k+1}-C(\hat{\Phi}_{R_k})(P^k_{k+1}-\widetilde{P}^k_{k+1})\\ &=\widetilde{P}_{R_k}+(C(\Phi_{R_k})-C(\hat{\Phi}_{R_k}))P^k_{k+1}+C(\hat{\Phi})\widetilde{P}^k_{k+1}\\ &\approx\widetilde{P}_{R_k}+JC( \hat{\Phi}_{R_k}) \widetilde{\Phi}_{R_k}P^k_{k+1}+C(\hat{\Phi})\widetilde{P}^k_{k+1}\\ &\approx\widetilde{P}_{R_k}+C(\hat{\Phi})\widetilde{P}^k_{k+1}+JC( \hat{\Phi}_{R_k}) \hat{P}^k_{k+1}\widetilde{\Phi}_{R_k} \tag{20} \end{align*} PRk+1=PRk+C(ΦRk)Pk+1kC(Φ^Rk)P^k+1k=PRk+C(ΦRk)Pk+1kC(Φ^Rk)(Pk+1kPk+1k)=PRk+(C(ΦRk)C(Φ^Rk))Pk+1k+C(Φ^)Pk+1kPRk+JC(Φ^Rk)ΦRkPk+1k+C(Φ^)Pk+1kPRk+C(Φ^)Pk+1k+JC(Φ^Rk)P^k+1kΦRk(20)
误差状态传播函数经过线性化展开后,有:
X~k+1=∣I2JC(Φ^Rk)P^k+1k01×21∣×∣P~RkP~Lk∣+∣C(Φ^)001×21∣×∣P~k+1kΦ~k+1k∣=∣I2JC(Φ^Rk)P^k+1k01×21∣X~k+∣C(Φ^)001×21∣X~k+1k \begin{align*} \widetilde{X}_{k+1} &= \begin{vmatrix} I_2& JC( \hat{\Phi}_{R_k}) \hat{P}^k_{k+1}\\ \textbf{0}_{1\times2}&1 \end{vmatrix}\times\begin{vmatrix} \widetilde{P}_{R_k} \\ \widetilde{P}_{L_k} \end{vmatrix}+\begin{vmatrix} C(\hat{\Phi}) & \textbf{0} \\ \textbf{0}_{1\times2} & 1 \end{vmatrix} \times\begin{vmatrix} \widetilde{P}^k_{k+1}\\ \widetilde{\Phi}^k_{k+1} \end{vmatrix} \\ &= \begin{vmatrix} I_2& JC( \hat{\Phi}_{R_k}) \hat{P}^k_{k+1}\\ \textbf{0}_{1\times2}&1 \end{vmatrix}\widetilde{X}_k+\begin{vmatrix} C(\hat{\Phi}) & \textbf{0} \\ \textbf{0}_{1\times2} & 1 \end{vmatrix}\widetilde{X}^k_{k+1} \tag {21} \end{align*} Xk+1=I201×2JC(Φ^Rk)P^k+1k1×PRkPLk+C(Φ^)01×201×Pk+1kΦk+1k=I201×2JC(Φ^Rk)P^k+1k1Xk+C(Φ^)01×201Xk+1k(21)

误差观测方程推导

对于世界坐标系下一点PLP_LPL,将其变换到机器人坐标系下一点PRP_RPR的观测方程为:
PR=CT(ΦRk+1∣k)(PL−PRk+1∣k)(22) P_R = C^T(\Phi_{R_{k+1|k}})(P_L-P_{R_{k+1|k}})\tag{22} PR=CT(ΦRk+1∣k)(PLPRk+1∣k)(22)
那么,机器人在真实状态(true state)下时,有观测方程:
z=CT(ΦRk+1∣k)(PL−PRk+1∣k)+v(23) z = C^T(\Phi_{R_{k+1|k}})(P_L-P_{R_{k+1|k}})+v\tag{23} z=CT(ΦRk+1∣k)(PLPRk+1∣k)+v(23)
机器人在名义状态(nominal state)下时,有观测方程:
z^=CT(Φ^Rk+1∣k)(PL^−P^Rk+1∣k)(24) \hat{z} = C^T(\hat{\Phi}_{R_{k+1|k}})(\hat{P_L}-\hat{P}_{R_{k+1|k}})\tag{24} z^=CT(Φ^Rk+1∣k)(PL^P^Rk+1∣k)(24)
那么误差状态(error state)方程由式(23)−(24)(23)-(24)(23)(24)得到:
z~=CT(ΦRk+1∣k)(PL−PRk+1∣k)−CT(Φ^Rk+1∣k)(PL^−P^Rk+1∣k)+v=CT(ΦRk+1∣k)(PL−PRk+1∣k)−CT(Φ^Rk+1∣k)(PL−P~L−PRk+1∣k+P~Rk+1∣k)+v=(CT(ΦRk+1∣k)−CT(Φ^Rk+1∣k))(PL−PRk+1∣k)+CT(Φ^Rk+1∣k)(P~L−P~Rk+1∣k)+v≈−JCT(Φ^Rk+1∣k)(PL−PRk+1∣k)Φ~Rk+1∣k+CT(Φ^Rk+1∣k)P~L−CT(Φ^Rk+1∣k)P~Rk+1∣k+v≈−JCT(Φ^Rk+1∣k)(P^L−P^Rk+1∣k)Φ~Rk+1∣k+CT(Φ^Rk+1∣k)P~L−CT(Φ^Rk+1∣k)P~Rk+1∣k+v \begin{align*} \widetilde{z} &= C^T(\Phi_{R_{k+1|k}})(P_L-P_{R_{k+1|k}})-C^T(\hat{\Phi}_{R_{k+1|k}})(\hat{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- \widetilde{P}_L-P_{R_{k+1|k}}+\widetilde{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}})(\widetilde{P}_L-\widetilde{P}_{R_{k+1|k}})+v\\ &\approx-JC^T(\hat{\Phi}_{R_{k+1|k}})(P_L-P_{R_{k+1|k}})\widetilde{\Phi}_{R_{k+1|k}}+C^T(\hat{\Phi}_{R_{k+1|k}})\widetilde{P}_L-C^T(\hat{\Phi}_{R_{k+1|k}})\widetilde{P}_{R_{k+1|k}}+v\\ &\approx-JC^T(\hat{\Phi}_{R_{k+1|k}})(\hat{P}_L-\hat{P}_{R_{k+1|k}})\widetilde{\Phi}_{R_{k+1|k}}+C^T(\hat{\Phi}_{R_{k+1|k}})\widetilde{P}_L-C^T(\hat{\Phi}_{R_{k+1|k}})\widetilde{P}_{R_{k+1|k}}+v \tag{25} \end{align*} z=CT(ΦRk+1∣k)(PLPRk+1∣k)CT(Φ^Rk+1∣k)(PL^P^Rk+1∣k)+v=CT(ΦRk+1∣k)(PLPRk+1∣k)CT(Φ^Rk+1∣k)(PLPLPRk+1∣k+PRk+1∣k)+v=(CT(ΦRk+1∣k)CT(Φ^Rk+1∣k))(PLPRk+1∣k)+CT(Φ^Rk+1∣k)(PLPRk+1∣k)+vJCT(Φ^Rk+1∣k)(PLPRk+1∣k)ΦRk+1∣k+CT(Φ^Rk+1∣k)PLCT(Φ^Rk+1∣k)PRk+1∣k+vJCT(Φ^Rk+1∣k)(P^LP^Rk+1∣k)ΦRk+1∣k+CT(Φ^Rk+1∣k)PLCT(Φ^Rk+1∣k)PRk+1∣k+v(25)
252525写成线性矩阵的形式有:
z~=∣−CT(Φ^Rk+1∣k)−JCT(Φ^Rk+1∣k)(P^L−P^Rk+1∣k)CT(Φ^Rk+1∣k)∣×∣P~Rk+1∣kΦ~Rk+1∣kP~L∣+v=CT(Φ^Rk+1∣k)∣−I2−J(P^L−P^Rk+1∣k)I2∣×∣X~k∣k+1P~L∣+v \begin{align*} \widetilde{z} &= \begin{vmatrix} -C^T(\hat{\Phi}_{R_{k+1|k}}) & -JC^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{vmatrix}\times\begin{vmatrix} \widetilde{P}_{R_{k+1|k}}\\ \widetilde{\Phi}_{R_{k+1|k}}\\ \widetilde{P}_L\\ \end{vmatrix}+v\\ &=C^T(\hat{\Phi}_{R_{k+1|k}})\begin{vmatrix} -\textbf{I}_2 & -J(\hat{P}_L-\hat{P}_{R_{k+1|k}}) & \textbf{I}_2 \end{vmatrix}\times\begin{vmatrix} \widetilde{X}_{k|k+1}\\ \widetilde{P}_L \end{vmatrix}+v \tag{26} \end{align*} z=CT(Φ^Rk+1∣k)JCT(Φ^Rk+1∣k)(P^LP^Rk+1∣k)CT(Φ^Rk+1∣k)×PRk+1∣kΦRk+1∣kPL+v=CT(Φ^Rk+1∣k)I2J(P^LP^Rk+1∣k)I2×Xkk+1PL+v(26)
至此我们推导了误差状态传播方程和观测方程的线性表达式,后续按照卡尔曼滤波器的一般过程即可。

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值