python实现全向轮EKF_SLAM
代码地址及效果
运动预测
简化控制量 u t u_t ut 分别定义为 v x Δ t v_x \Delta t vxΔt， v y Δ t v_y \Delta t vyΔt，和 ω z Δ t \omega_z \Delta t ωzΔt。这样，我们将离散时间控制向量 u k u_k uk 表示为：
u k = [ Δ μ 1 Δ μ 2 Δ θ ] = [ v x Δ t v y Δ t ω z Δ t ] u_k = \begin{bmatrix} \Delta \mu_1 \\ \Delta \mu_2 \\ \Delta \theta \end{bmatrix} = \begin{bmatrix} v_x \Delta t \\ v_y \Delta t \\ \omega_z \Delta t \end{bmatrix} uk= Δμ1Δμ2Δθ = vxΔtvyΔtωzΔt
这里， Δ μ 1 \Delta \mu_1 Δμ1 和 Δ μ 2 \Delta \mu_2 Δμ2 表示在 Δ t \Delta t Δt 时间内沿X和Y轴的速度增量，而 Δ θ \Delta \theta Δθ 是在 Δ t \Delta t Δt 时间内绕Z轴的角速度增量。状态更新方程如下：
[ x y θ ] t ∣ t − 1 = [ x y θ ] t − 1 + [ cos ( θ t − 1 ) − sin ( θ t − 1 ) 0 sin ( θ t − 1 ) cos ( θ t − 1 ) 0 0 0 1 ] [ Δ μ 1 Δ μ 2 Δ θ ] \begin{bmatrix} x \\ y \\ \theta \end{bmatrix}{t|t-1}=\begin{bmatrix} x \\ y \\ \theta \end{bmatrix}{t-1}+\begin{bmatrix} \cos(\theta_{t-1}) & -\sin(\theta_{t-1}) & 0 \\ \sin(\theta_{t-1}) & \cos(\theta_{t-1}) & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} \Delta\mu_1 \\ \Delta\mu_2 \\ \Delta \theta \end{bmatrix} xyθ t∣t−1= xyθ t−1+ cos(θt−1)sin(θt−1)0−sin(θt−1)cos(θt−1)0001 Δμ1Δμ2Δθ
简化上述方程得到：
[ x y θ ] t ∣ t − 1 = [ x t − 1 + Δ μ 1 cos ( θ t − 1 ) − Δ μ 2 sin ( θ t − 1 ) y t − 1 + μ 1 sin ( θ t − 1 ) + μ 2 cos ( θ t − 1 ) θ t − 1 + Δ θ ] \begin{bmatrix} x \\ y \\ \theta \end{bmatrix}{t|t-1} =\begin{bmatrix} x{t-1} + \Delta\mu_1 \cos(\theta_{t-1}) - \Delta\mu_2 \sin(\theta_{t-1}) \\ y_{t-1} + \mu_1 \sin(\theta_{t-1}) + \mu_2 \cos(\theta_{t-1}) \\ \theta_{t-1} + \Delta \theta \end{bmatrix} xyθ t∣t−1= xt−1+Δμ1cos(θt−1)−Δμ2sin(θt−1)yt−1+μ1sin(θt−1)+μ2cos(θt−1)θt−1+Δθ
考虑到 Δ μ 1 \Delta\mu_1 Δμ1 和 Δ μ 2 \Delta\mu_2 Δμ2 是控制输入的变化量，并不直接依赖于状态 x t − 1 x_{t-1} xt−1，雅可比矩阵 G t G_t Gt 可以表示为：
G t = ∂ ∂ x [ x + ( Δ μ 1 cos ( θ ) − Δ μ 2 sin ( θ ) ) y + ( Δ μ 1 sin ( θ ) + Δ μ 2 cos ( θ ) ) θ + Δ θ ] = [ 1 0 − Δ μ 1 sin ( θ t − 1 ) − Δ μ 2 cos ( θ t − 1 ) 0 1 Δ μ 1 cos ( θ t − 1 ) − Δ μ 2 sin ( θ t − 1 ) 0 0 1 ] G_t = \frac{\partial}{\partial x} \begin{bmatrix} x + (\Delta\mu_1 \cos(\theta) - \Delta\mu_2 \sin(\theta)) \\ y + (\Delta\mu_1 \sin(\theta) + \Delta\mu_2 \cos(\theta)) \\ \theta + \Delta \theta \end{bmatrix} =\begin{bmatrix} 1 & 0 & -\Delta\mu_1 \sin(\theta_{t-1}) - \Delta\mu_2 \cos(\theta_{t-1}) \\ 0 & 1 & \Delta\mu_1 \cos(\theta_{t-1}) - \Delta\mu_2 \sin(\theta_{t-1}) \\ 0 & 0 & 1 \end{bmatrix} Gt=∂x∂ x+(Δμ1cos(θ)−Δμ2sin(θ))y+(Δμ1sin(θ)+Δμ2cos(θ))θ+Δθ = 100010−Δμ1sin(θt−1)−Δμ2cos(θt−1)Δμ1cos(θt−1)−Δμ2sin(θt−1)1
此雅可比矩阵 G t G_t Gt 反映了当前状态对下一时刻状态预测的依赖性，将被用于状态协方差矩阵 Σ t \Sigma_t Σt的更新：
Σ t ∣ t − 1 = G t Σ t − 1 ∣ t − 1 G t T + F x R t F x T \Sigma_{t|t-1} = G_t \Sigma_{t-1|t-1} G_t^T + F_x R_t F_x^T Σt∣t−1=GtΣt−1∣t−1GtT+FxRtFxT
其中 R t R_t Rt 是过程噪声的协方差矩阵，代表了预测模型中的不确定性。 F t F_t Ft是一个空间映射矩阵，除了左上角对应机器人位姿的3乘3矩阵为单位阵，其余都置0。虽然运动过程仅对机器人位姿进行更新，但是机器人和每个地标位置之间的相关性同样会被更新。
观测修正
对于所有观测到的特征 z t i z_t^i zti，我们首先检查地标 j j j 是否是新观测到的。如果是，则初始化该地标的状态，并对应扩展状态矩阵 μ t \mu_t μt 和协方差矩阵 Σ t \Sigma_t Σt。状态向量 μ t \mu_t μt 增加地标的横纵坐标值，而对应的协方差矩阵初始化为无穷。状态向量初始化时将地标距离 r t i r_t^i rti 和地表方向 ϕ t i \phi_t^i ϕti 转为全局坐标：
μ j , x = μ t , x + r t i cos ( ϕ t i + μ t , θ ) μ j , y = μ t , y + r t i sin ( ϕ t i + μ t , θ ) \begin{align*} \mu_{j,x} &= \mu_{t,x} + r_t^i \cos(\phi_t^i + \mu_{t,\theta}) \\ \mu_{j,y} &= \mu_{t,y} + r_t^i \sin(\phi_t^i + \mu_{t,\theta}) \end{align*} μj,xμj,y=μt,x+rticos(ϕti+μt,θ)=μt,y+rtisin(ϕti+μt,θ)
计算预测和测量之间的差值：
δ = [ δ x δ y ] = [ μ j , x − μ t , x μ j , y − μ t , y ] q = δ T δ \begin{align*} \delta &= \begin{bmatrix} \delta_x \\ \delta_y \end{bmatrix} = \begin{bmatrix} \mu_{j,x} - \mu_{t,x} \\ \mu_{j,y} - \mu_{t,y} \end{bmatrix} \\ q &= \delta^T \delta \end{align*} δq=[δxδy]=[μj,x−μt,xμj,y−μt,y]=δTδ
计算预测的观测值：
z ^ t i = [ q atan2 ( δ y , δ x ) − μ t , θ ] \hat{z}t^i = \begin{bmatrix} \sqrt{q} \\ \text{atan2}(\delta_y, \delta_x) - \mu{t,\theta} \end{bmatrix} z^ti=[q atan2(δy,δx)−μt,θ]
构建雅克比矩阵 H t i H_t^i Hti， F x , j F_{x,j} Fx,j 是空间映射矩阵，除了机器人位姿和地标 j 的位姿对应位置为单位矩阵，其余为 0：
H t i = 1 q [ − q δ x − q δ y 0 q δ x q δ y δ y − δ x − q − δ y δ x ] F x , j H_t^i = \frac{1}{q} \begin{bmatrix} -\sqrt{q}\delta_x & -\sqrt{q}\delta_y & 0 & \sqrt{q}\delta_x & \sqrt{q}\delta_y \\ \delta_y & -\delta_x & -q & -\delta_y & \delta_x \end{bmatrix} F_{x,j} Hti=q1[−q δxδy−q δy−δx0−qq δx−δyq δyδx]Fx,j
计算卡尔曼增益 K t i K_t^i Kti，其中 Q t i Q_t^i Qti 是观测误差协方差矩阵：
K t i = Σ t H t i T ( H t i Σ t H t i T + Q t i ) − 1 K_t^i = \Sigma_t H_t^{i^T} \left( H_t^i \Sigma_t H_t^{i^T} + Q_t^i \right)^{-1} Kti=ΣtHtiT(HtiΣtHtiT+Qti)−1
最后，更新状态向量 μ t \mu_t μt 和协方差矩阵 Σ t \Sigma_t Σt（这里的 μ t \mu_t μt 和 Σ t \Sigma_t Σt 是由机器人状态和路标状态组成的）：
μ t = μ t + K t i ( z t i − z ^ t i ) \mu_t = \mu_t + K_t^i \left( z_t^i - \hat{z}_t^i \right) μt=μt+Kti(zti−z^ti)
Σ t = ( I − K t i H t i ) Σ t \Sigma_t = \left( I - K_t^i H_t^i \right) \Sigma_t Σt=(I−KtiHti)Σt
参考算法
改自概率机器人
