文章目录
位置、速度的连续积分
P：位置
V：速度
Q：姿态
位置和速度的连续时间积分公式如下：
- 位置P的连续积分：表示在世界坐标系 w w w 下 b k + 1 b_{k+1} bk+1和 b k b_{k} bk 时刻的位置的变化 ，上式可以简单理解为 Δ x = p + v 0 ⋅ t + 1 2 a t 2 Δx = p + v_{0}·t + \frac{1}{2}at^{2} Δx=p+v0⋅t+21at2，加速度两次积分=位移，加速度一次积分=速度，实际上的a会随时间 t t t 变化，每一时刻的值都可能不同的，不会是一个固定值，所以需要写成积分的形式，里面的加速度根据建模方式要减去偏置、噪声和重力，其中 a ⃗ t \vec{a}_{t} at 为观测值 ， b a t b_{at} bat 为零偏 ， n a n_{a} na 为高斯噪声， R t w R^{w}_{t} Rtw 为当前时刻的姿态，将当前时刻的加速度计的值转换到世界系下面去，这里面的 R t w R^{w}_{t} Rtw是一直在维护的全局的旋转矩阵，且其必须是和重力对齐的，这样重力方向才和z轴平行，这样才可以去除重力g的影响，这个在系统开始时会进行初始化标定，因为IMU的起始姿态必然与g方向不平行。
括号里表示的就是加速度去除重力去除零偏和噪声后在世界坐标系下面的值
- 速度V的连续积分：可以简单理解为 v t + Δ t = v t + a Δ t v_{t+Δt} = v_{t}+aΔt vt+Δt=vt+aΔt
四元数矩阵乘法
先给出四元数自己的乘法规则
我们更希望可以采用矩阵相乘的形式，上面的乘法需要自定义乘法法则，变成矩阵法则如下：
其中 [ q 1 ] L ∈ 4 × 4 ， q 2 属于 4 × 1 [q_{1}]_{L}∈4×4，q_{2}属于4×1 [q1]L∈4×4，q2属于4×1，具体矩阵构造如下
这样做的好处就是，四元数的乘法通过这个矩阵变换变成矩阵的乘法，且这样可以有两个表示形式，两种形式的结果都是一样的
这两个矩阵构造都是有相似的形式，可以整理成比较一致的表达
描述： 两个矩阵的对角线都是 q w q_{w} qw ，第一行都是 − q x , − q y , − q z -q_{x},-q_{y},-q_{z} −qx,−qy,−qz, 第一列都是 q x , q y , q z q_{x},q_{y},q_{z} qx,qy,q</
最低0.47元/天 解锁文章
2750
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
