传感器IMU

fengziyihang

已于 2023-08-24 13:00:51 修改

阅读量149

点赞数

文章标签：人工智能

于 2023-08-24 10:07:06 首次发布

本文链接：https://blog.csdn.net/fengziyihang/article/details/132447477

版权

IMU根据其属性，可以知道其主要测量是线加速度和角加速度：

$\hat{a_{t}} = a_{t}+b_{a_{t}}+R_{w}^{t}*g^{w}+n_{a}$ (1)

$\hat{w{_{t}}} = w{_{t}}+b{_{w{_{t}}}} + n_{w}$ (2)

其中 $b_{a_{t}}$ 和 $b_{w_{t}}$ 分别是线加速度偏差和角加速度偏差,可以利用随机游走模型建模， $\dot{b_{a_{t}}}=n_{a_{t}}$ , $\dot{b_{w_{t}}}=n_{w_{t}}$ ,且符合高斯分布 $N(0,\sigma ^{2})$ ; $n_{a}$ 和 $n_{w}$ 是噪声，一般认为符合高斯分布 $N(0,\sigma ^{2})$ ; $g^{w}$ 表示全球坐标下的重力加速度; $R_{w}^{t}$ 表示旋转变量，将 $g^{w}$ 变换到t时刻坐标系下。我们可以得到从k到k+1时刻IMU的位置、速度和旋转为：

$p_{b_{k+1}}^{w}=p_{b_{k}}^{w}+\int_{k}^{k+1}v_{b_{k}}^{w}dt+\iint_{k}^{k+1}R_{t}^{w}a_{t}dt^{2}$ (3)

$v_{b_{k+1}}^{w}=v_{b_{k}}^{w}+\int_{k}^{k+1}R_{t}^{w}a_{t}dt$ (4)

$q_{b_{k+1}}^{w}=q_{b_{k}}^{w}\int_{k}^{k+1}1/2q_{t}^{b_{k+1}}\bigotimes w_{t}dt =q_{b_{k}}^{w}\int_{k}^{k+1}1/2\Omega (w_{t})q_{t}^{b_{k}}dt$ (5)

$\Omega (w)=[-[w]_{x},w;-w^{t},0]$ (6)

其中 $p_{b_{k+1}}^{w},v_{b_{k+1}}^{w},q_{b_{k+1}}^{w},p_{b_{k}}^{w},v_{b_{k}}^{w},q_{b_{k}}^{w}$ 分别为第k时刻和第k+1时刻body在世界坐标系下的位置，速度和旋转; $q_{t}^{b_{k+1}}$ 代表t时刻body在 $b_{k+1}$ 坐标系下的旋转四元数。

由于IMU状态量发生变化，就需要重新计算积分，且积分计算量大，所以提出了预积分，缓解计算量问题。参考系由世界坐标系转到 $b_{k}$ 坐标系,即(3)、(4)、(5)左乘 $q_{w}^{b_{k}}$ 如下：

$q_{w}^{b_{k}}p_{b_{k}}^{w}=q_{w}^{b_{k}}(p_{b_{k}}^{w}+v_{b_{k}}^{w}\Delta t_{k}-1/2 g^{^{w}}\Delta t^{2}) +\alpha _{b_{k}}^{b_{k+1}}$

$q_{w}^{b_{k}}v_{b_{k+1}}^{w}=q_{w}^{b_{k}}(v_{b_{k}}^{w}-g^{w}\Delta t_{k})+\beta _{b_{k}}^{b_{k+1}}$

$q_{w}^{b_{k}}*q_{b_{k+1}}^{w}=\gamma_{b_{k}}^{b_{k+1}}$

其中预计分为：

$\alpha _{b_{k}}^{b_{k+1}}=\iint_{b_{k}}^{b_{k+1}}R_{t}^{b_{k}}(a\hat{}-b_{a_{t}}-n_{t})\delta t^{2}$

$\beta _{b_{k}}^{b_{k+1}}=\int_{k}^{k+1}R_{t}^{b_{k}}(a\hat{}-b_{a_{t}}-n_{a})\delta t$

$\gamma _{b_{k}}^{b_{k+1}}=\int_{k}^{k+1}1/2\Omega (\hat{w_{t}}-b_{w_{t}}-n_{w})\gamma _{t}^{b_{k}}\delta t$

由上面可以得知预计分与k和k+1时刻的状态量无关，而与两个时刻之间的IMU偏差有关。

fengziyihang

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
传感器IMU

变换到t时刻坐标系下。分别为第k时刻和第k+1时刻body在世界坐标系下的位置，速度和旋转;分别是线加速度偏差和角加速度偏差;是噪声，一般认为符合高斯分布。表示全球坐标下的重力加速度;代表t时刻body坐标系在。坐标系下的旋转四元数;
复制链接

扫一扫