差速轮轮速计建模和优化量求导

• author:
  by yuhongsong(hongsong.yu2010@gmail.com 18311028576)

  2020-06-05

  转载和基于此文档编辑，请注明原作者和链接

• 标签 <tag> 里程计建模 ,<tag> 预积分, <tag> 残差

• 说明

  本文内容部分是基于参考链接进行的摘抄和编辑,且已注明参考，为的是加强自己的阅读效果，留下阅读的笔记，内容做过深度阅读和改动，参照部分版权属于原作者所有。

1. update log

版本	时间	作者	info
V1.0	2020-06-05	余红松	创建

2. 两帧之间建模

内建型的递推传感器
根据传感器参数,获取 $x_{t_k}$到$x_{t_{k+1}}$时刻的递推关系公式

$ x = \left[ \begin{array}{c}p
\ v
\ q
\ f_l
\ f_r
\ \eta
\end{array} \right]$

已知内参
$f$:编码器系数,表一个编码器计数对应移动距离的系数 可优化参数
$d$:车轮到轴中间距离,可优化参数

$\eta \triangleq \frac{1}{d}$ 轴距的逆轴距,避免后面雅克比求导中的除法问题

编码器数据上传帧率是一个固定值,每两帧之间,编码器计数增加 $\delta c=c_{t_{k+1}}-c_{t_k}$,这个编码器计数数值,两帧增加的量 视移动速度而定,帧间时间间隔是固定的,$c$为 $cnt$的缩写

两帧之间根据两轮的编码器读数的不同,会是一个弧形的运动轨迹,现在将弧形的轨迹假定为直线处理

2.1. 挡位因子

挡位方向说明
$\tau =1$前进挡
$\tau =-1$后退挡
$\tau =0$停车挡

$ \displaystyle \varepsilon \triangleq \left { \begin{array} {ll} \varepsilon=1 & | \tau=1
\ \varepsilon=-1 & | \tau=3
\ \varepsilon=0 & | \tau=4
\end{array} \right. $

2.2. 位置 P

两帧之间运动时间假定为 $\delta t$,轴中心点$M$运动距离为$\delta S$
时刻$t_k$到$t_{k+1}$的位置变化分析

左侧轮的移动距离: $\delta S_{L_k}=\epsilon \cdot \left(c_{L_{k+1}}-c_{L_k}\right)\cdot f_L=\epsilon \cdot \Delta c_{L_k}\cdot f_L$

右侧轮的移动距离: $\delta S_{R_k}=\epsilon \cdot \left(c_{R_{k+1}}-c_{R_k}\right)\cdot f_R=\epsilon \cdot \Delta c_{R_k}\cdot f_R$

轴中心的移动距离:
$\delta S_{k} =\varepsilon \cdot v_{k}^{o} \cdot \delta t = \varepsilon \cdot \frac{v_{R_{k}}+v_{L_{k}}}{2} \cdot \delta t $

$=\epsilon \cdot \frac{\delta S_L+\delta S_R}{2}=\epsilon \cdot \frac{\left(c_{L_{k+1}}-c_{L_k}\right)\cdot f_L+\left(c_{R_{k+1}}-c_{R_k}\right)\cdot f_R}{2}$

$ = \varepsilon \cdot \frac{\delta S_L + \delta S_R}{2} = \varepsilon \cdot \frac{ \Delta c_{L_{k}} \cdot f_{L} + \Delta c_{R_{k}} \cdot f_{R}}{2} $

$p_{w{o}_k}=\epsilon \cdot \left[\begin{array}{c}{x}_{t_k}\\ y_{t_k}\\ z_{t_k}\end{array}\right]$

$p_{wo_{k+1}} = p_{wo_{k}} + \varepsilon \cdot v_{o_k}^{w} \cdot \delta t $

$= p_{wo_{k}} + \varepsilon \cdot \mathbf{q}{wo_k} \cdot v{k}^{o} \cdot \delta t $

$= p_{wo_{k}} + \varepsilon \cdot \mathbf{q}{wo_k} \cdot \left[ \begin{array} {c} \delta S{k}
\ 0
\ 0
\end{array} \right] $

$= p_{wo_{k}} + \varepsilon \cdot \mathbf{q}{wo_k} \cdot \left[ \begin{array} {c} \frac{\Delta c{L_{k}} \cdot f_{L} + \Delta c_{R_{k}} \cdot f_{R}}{2}
\ 0
\ 0
\end{array} \right]
$

$p_{w{o}_{}}$