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
参考算法
改自概率机器人
