一起做激光反光板(一)-EKF定位公式推导

前提:本文只考虑平面运动。

EKF的公式及原理不再细述:EKF原理

观测:观测数据为反光板。反光板的检测暂时也不考虑。(一般来说,反光板的检测都是基于反射强度来做的,需要自己手写,如果有疑问留言)。

预测:以轮速计为例,我们只使用轮速计的线速度和角速度,轮速计的模型,我们使用轮速计模型博客中提到的模型。

定位EKF公式推导

假如我们已经事先构建了一个地图(后续再考虑),地图中包含 M M M个反光板:
反光板地图

每个反光板还会有一个协方差矩阵,该矩阵的形式为:
反光板协方差矩阵
假设我们只维护位姿,不考虑新增的反光板,且不更新老地图中反光板的位置和协方差矩阵,所以有:

状态空间

状态空间

差速运动模型-预测

(1)位姿预测:
初始位姿:
初始位姿
初始位姿的确定,涉及到重定位,重定位有很多手段(比如3点定位法、其他算法给出等),在这里我们假定初始位置是已知的。

运动方程可以写为:
运动方程假设轮速计的线速度和角速度服从均值为0的高斯分布,即控制的协方差为 Σ u \Sigma_u Σu
控制协方差
(2)协方差预测

初始时刻的协方差需要给定初值:
初始协方差

添加协方差公式的推导-2023.06.15

请添加图片描述

状态量在 t t t时刻的协方差预测方程为: Σ ξ , t = G ξ Σ ξ , t − 1 G ξ T + G u Σ u G u T \Sigma_{\xi,t} = G_{\xi}\Sigma_{\xi,t-1}G_{\xi}^T+G_u\Sigma_uG_u^T Σξ,t=GξΣξ,t1GξT+GuΣuGuT

其中:

G ξ G_{\xi} Gξ是运动模型关于机器人位姿 ξ t − 1 \xi_{t-1} ξt1的雅克比:
雅克比矩阵
G u G_u Gu是运动模型关于控制(轮速计线速度和角速度) u u u的雅克比:
控制
雅克比
根据协方差预测可知,假如没有观测数据,预测位姿的协方差矩阵会越来越大,导致位姿不可靠性增大。

全向运动模型-预测-2021.4.11更新

考虑到有些机器人是全向的,所以我们扩展代码,支持差速和全向模型。差速和全向的差别不大,不过是多了 y y y轴速度,所以需要把运动模型进行扩充。为了方便,我们这里仅为了展示推导过程,后面不再针对每一篇博客继续推导 。

(1)位姿预测:
为了方便,我们可以将运动方程可以写为:
[ x t y t θ t ] = [ x t − 1 y t − 1 θ t − 1 ] + [ v x Δ t cos ⁡ ( θ t − 1 ) − v y Δ t sin ⁡ ( θ t − 1 ) v x Δ t sin ⁡ ( θ t − 1 ) + v y Δ t cos ⁡ ( θ t − 1 ) ω Δ t ] \begin{bmatrix}x_t \\ y_t \\ \theta_t\end{bmatrix} =\begin{bmatrix}x_{t-1} \\ y_{t-1} \\ \theta_{t-1}\end{bmatrix} + \begin{bmatrix}v_x\Delta t\cos(\theta_{t-1}) - v_y \Delta t\sin(\theta_{t-1}) \\ v_x\Delta t\sin(\theta_{t-1}) + v_y \Delta t\cos(\theta_{t-1}) \\ \omega\Delta t \end{bmatrix} xtytθt = xt1yt1θt1 + vxΔtcos(θt1)vyΔtsin(θt1)vxΔtsin(θt1)+vyΔtcos(θt1)ωΔt

(2)协方差预测:

Σ t = G t Σ t − 1 G t T + G u Σ u G u T \Sigma_{t} = G_{t}\Sigma_{t-1}G_{t}^T+G_u\Sigma_uG_u^T Σt=GtΣt1GtT+GuΣuGuT

控制的协方差为 Σ u \Sigma_u Σu

Σ u = [ σ v x 2 0 0 0 σ v y 2 0 0 0 σ ω 2 ] 3 ∗ 3 \Sigma_u=\begin{bmatrix}\sigma_{v_x}^2 & 0 & 0\\ 0 & \sigma_{v_y}^2& 0 \\ 0&0&\sigma_{\omega}^2\end{bmatrix}_{3*3} Σu= σvx2000σvy2000σω2 33

其中:

G t G_{t} Gt是运动模型关于状态量 X t − 1 X_{t-1} Xt1的雅克比:

G t = ∂ X t ∂ X t − 1 = [ 1 0 − v x Δ t sin ⁡ ( θ t − 1 ) − v y Δ t cos ⁡ ( θ t − 1 ) 0 1 v x Δ t cos ⁡ ( θ t − 1 ) − v y Δ t sin ⁡ ( θ t − 1 ) 0 0 1 ] 3 ∗ 3 G_{t} = \frac{\partial X_t}{\partial X_{t-1}}=\begin{bmatrix}1 & 0 & -v_x\Delta t\sin(\theta_{t-1}) - v_y \Delta t\cos(\theta_{t-1}) \\ 0 & 1 & v_x\Delta t\cos(\theta_{t-1}) - v_y \Delta t\sin(\theta_{t-1}) \\ 0 & 0 & 1\end{bmatrix}_{3*3} Gt=Xt1Xt= 100010vxΔtsin(θt1)vyΔtcos(θt1)vxΔtcos(θt1)vyΔtsin(θt1)1 33

G u G_u Gu是运动模型关于控制 u = [ v x v y ω ] T u =\begin{bmatrix}v_x & v_y & \omega \end{bmatrix} ^T u=[vxvyω]T的雅克比:

G u = ∂ X t ∂ u = [ Δ t cos ⁡ ( θ t − 1 ) − Δ t sin ⁡ ( θ t − 1 ) 0 Δ t sin ⁡ ( θ t − 1 ) Δ t cos ⁡ ( θ t − 1 ) 0 0 0 Δ t ] 3 ∗ 3 G_u = \frac{\partial X_t}{\partial u}=\begin{bmatrix}\Delta t\cos(\theta_{t-1}) & -\Delta t\sin(\theta_{t-1}) &0 \\ \Delta t\sin(\theta_{t-1}) & \Delta t\cos(\theta_{t-1}) & 0 \\ 0&0 & \Delta t\end{bmatrix}_{3*3} Gu=uXt= Δtcos(θt1)Δtsin(θt1)0Δtsin(θt1)Δtcos(θt1)000Δt 33

观测模型

假设当前激光帧中检测到 N N N个反光板:
反光板通过数据关联(欧式距离或马氏距离等手段),我们找到地图中对应的 N N N个反光板。对于关联不上的反光板,我们不放进观测方程中。观测方程可以写为:
在这里插入图片描述
观测对状态空间 X t X_t Xt的雅克比为:
在这里插入图片描述

更新

计算增益

K t = Σ ξ , t H t T ( H t Σ ξ , t H t T + Q ) − 1 K_t = \Sigma_{\xi,t}H_{t}^{T}(H_t\Sigma_{\xi,t}H_t^{T}+Q)^{-1} Kt=Σξ,tHtT(HtΣξ,tHtT+Q)1

K t K_t Kt 3 ∗ 2 N 3*2N 32N矩阵。其中, Q Q Q为观测的协方差矩阵。
在这里插入图片描述

该矩阵的取值可以为:

(1)采用观测误差,都取一个固定的值;

(2)采用地图中每个反光板的协方差矩阵;

(3)采用观测误差和地图中反光板协方差矩阵的加权,或者和等方式;

这三种方式,理论上都能得到结果,不过,具体到底哪个好,需要根据实际情况来选择。

更新状态空间

在这里插入图片描述
X t = X t + K t ( z t − z ^ ) X_t = X_t + K_t(z_t-\hat{z}) Xt=Xt+Kt(ztz^)

更新协方差

Σ ξ , t = ( I − K t H t ) Σ ξ , t \Sigma_{\xi,t} = (I-K_tH_t)\Sigma_{\xi,t} Σξ,t=(IKtHt)Σξ,t

以上,完成了一个最简单的基于反光板的EKF定位方案,不过还差一些工作需要补充:

(1)反光板的检测;

(2)反光板数据关联方法;

(3)状态空间的扩展:假如在定位过程中出现了一些新的反光板,则需要把该反光板加入状态空间;

(4)反光板地图的构建;

(5)基于反光板的重定位。

  • 11
    点赞
  • 42
    收藏
    觉得还不错? 一键收藏
  • 28
    评论
RTKLIB使用了一个EKF(扩展卡尔曼滤波器)来对状态进行简单的滤波,实现了RTK定位的相对定位流程。在RTKLIB中,有几个关键的函数用于更新状态和参数。 其中,函数initx用于初始化状态向量rtk->x\[i\]和协方差矩阵rtk->P\[i\]。在该函数中,通过赋值操作将rtk->x\[i\]赋值为xi,将rtk->P\[i,i\]和rtk->P\[i\]上的非对角线元素赋值为0。这样可以初始化状态向量和协方差矩阵的初始值。 函数udrcvbias用于更新接收机硬件偏移参数。在该函数中,通过遍历GLONASS的频率,对接收机硬件偏移参数进行更新。如果接收机硬件偏移参数为0,则将其初始化为1E-6,并用VAR_HWBIAS进行初始化。如果已经获得了固定解(rtk->nfix>=rtk->opt.minfix)并且解的比率(rtk->sol.ratio)大于阈值(rtk->opt.thresar\[0\]),则将接收机硬件偏移参数初始化为rtk->xa\[j\],并用rtk->Pa\[j+j*rtk->na\]进行初始化。否则,将协方差矩阵rtk->P\[j+j*rtk->nx\]加上过程噪声。 函数udbias用于更新单差相位偏移参数。在该函数中,通过单差模糊度更新原理,对单差相位偏移参数进行更新。 总结来说,RTKLIB的相对定位流程使用了EKF滤波器对状态进行简单的滤波,通过初始化状态向量和协方差矩阵,更新接收机硬件偏移参数和单差相位偏移参数,以实现RTK定位的相对定位过程。\[1\]\[2\]\[3\] #### 引用[.reference_title] - *1* *2* [RTKLIB学习总结(九)相对定位算法](https://blog.csdn.net/daoge2666/article/details/130674816)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v91^control_2,239^v3^insert_chatgpt"}} ] [.reference_item] - *3* [MATLAB算法实战应用案例精讲-【自动驾驶】精准定位RTK](https://blog.csdn.net/qq_36130719/article/details/130973888)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v91^control_2,239^v3^insert_chatgpt"}} ] [.reference_item] [ .reference_list ]

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 28
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值