python实现全向轮EKF_SLAM

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​)0​001​ ​ ​Δμ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​+Δμ1​cos(θt−1​)−Δμ2​sin(θt−1​)yt−1​+μ1​sin(θt−1​)+μ2​cos(θ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+(Δμ1​cos(θ)−Δμ2​sin(θ))y+(Δμ1​sin(θ)+Δμ2​cos(θ))θ+Δθ​ ​= ​100​010​−Δμ1​sin(θt−1​)−Δμ2​cos(θt−1​)Δμ1​cos(θt−1​)−Δμ2​sin(θ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−1​GtT​+Fx​Rt​FxT​

其中 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​+rti​cos(ϕti​+μt,θ​)=μt,y​+rti​sin(ϕ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​−δx​​0−q​q ​δx​−δy​​q ​δ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​=Σt​HtiT​(Hti​Σt​HtiT​+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−Kti​Hti​)Σt​

参考算法

改自概率机器人