二、惯性导航解算的方法

前面一节主要是在讲解旋转的数学表达形式，而这一节主要讲解的是怎么通过原始的角速度和加速度得到我们想要的位置、速度、姿态，而具体点就是“角速度->姿态”、“加速度->速度”、“速度->位置”，而这三种情况就很明显是一个积分问题。

1. 姿态更新

1.1 基于旋转矩阵的姿态更新

1）连续时间处理

旋转矩阵的微分方程为
R˙wb=Rwb[ω]×\dot{\boldsymbol{R}}_{w b}=\boldsymbol{R}_{w b}[\boldsymbol{\omega}]_{\times}\\ R˙wb=Rwb[ω]×
其中$ \boldsymbol{\omega}$ 为IMU三个轴测量得到的角速度。

根据微分方程可以很容易写出它的积分形式为（此处用到的是高数中讲的求一阶微分方程通解的方法，基础知识不展开介绍了）
Rwbk=Rwbk−1e∫tk−1tk[ω]×dτ\boldsymbol{R}_{w b_{k}}=\boldsymbol{R}_{w b_{k-1}} e^{\int_{t_{k-1}}^{t_{k}}[\boldsymbol{\omega}]_{\times} d \tau}\\ Rwbk=Rwbk−1e∫tk−1tk[ω]×dτ
这里给出的是由 k-1 时刻的姿态积分得到 k 时刻姿态的积分形式。仔细观察指数部分，它表示的是角速度矢量的积分，而角速度矢量的积分其实不就是 k-1 时刻到 k 时刻对应的旋转向量嘛（这里也可以这样理解，即两个旋转矩阵之间的变化可以由一个旋转向量来表示），因此，我们可以直接写成
Rwbk=Rwbk−1eϕ×\boldsymbol{R}_{w b_{k}}=\boldsymbol{R}_{w b_{k-1}} e^{\phi \times}\\ Rwbk=Rwbk−1eϕ×
由于反对称矩阵的指数函数前面已经推导完毕(罗德里格斯公式)，因此上式又可以写为
Rwbk=Rwbk−1Rbk−1bk\boldsymbol{R}_{w b_{k}}=\boldsymbol{R}_{w b_{k-1}} \boldsymbol{R}_{b_{k-1} b_{k}}\\ Rwbk=Rwbk−1Rbk−1bk
其中
Rbk−1bk=I+sin⁡ϕϕ(ϕ×)+1−cos⁡ϕϕ2(ϕ×)2\boldsymbol{R}_{b_{k-1} b_{k}}=I+\frac{\sin \phi}{\phi}(\boldsymbol{\phi} \times)+\frac{1-\cos \phi}{\phi^{2}}(\boldsymbol{\phi} \times)^{2}\\ Rbk−1bk=I+ϕsinϕ(ϕ×)+ϕ21−cosϕ(ϕ×)2
可见，求旋转矩阵的问题，最终转化成了旋转向量的问题。咋求？接着用微分方程求呗。旋转向量的微分方程为
ϕ˙≈ω+12ϕ×ω\dot{\phi} \approx \omega+\frac{1}{2} \phi \times \omega\\ ϕ˙≈ω+21ϕ×ω
这个方程看起来还是比较复杂的，可以也应当化简。在方程右边第二项，可以看到是两个矢量的叉乘，根据叉乘的定义，当两个矢量方向重合时，叉乘的结果为0。一般 imu 的采样频率不会低于 100hz，即 k-1 时刻到 k 时刻的时间间隔不会大于10ms，而载体一般自动驾驶车或者机器人也不会发生过于高频的运动，因此可以近似认为满足叉乘为0的条件，因此可以直接把上式简化为
ϕ˙≈ω\dot{\phi} \approx \omega\\ ϕ˙≈ω
它对应的积分形式就很简单了
ϕ≈∫tk−1tkω(τ)dτ\phi \approx \int_{t_{k-1}}^{t_{k}} \boldsymbol{\omega}(\tau) d \tau\\ ϕ≈∫tk−1tkω(τ)dτ
有了旋转向量的积分形式，那么该旋转矩阵的形式就了然了，直接带进去就行。

但是，到这里会遇到一个问题，我们一直讨论的都是连续时间的，即 k-1 时刻到 k 时刻之间的所有值都是连续可测的，而实际数据都是离散的，只能得到 k-1 和 k 这两个时刻的值，那么必须把上面的连续时间形式高程离散形式，才能继续走下去了。

2）离散时间处理

这就牵扯到常见的三种方法：欧拉法、中值法、龙哥库塔法，三者按复杂程度逐渐递增，按精度也是逐渐递增。我们先从简单的开始说。

a.欧拉法

欧拉法很好理解，虽然我不知道 k-1 到 k 时刻的所有值，但是如果我假设 imu 在这个时间段内是匀速旋转的，那不就解决了吗，此时，对应的旋转向量为
ϕ=ωk−1(tk−tk−1)\boldsymbol{\phi}=\boldsymbol{\omega}_{k-1}\left(t_{k}-t_{k-1}\right)\\ ϕ=ωk−1(tk−tk−1)
b.中值法

欧拉法固然简单，但是匀速的假设还是有些太强了，实际使用中会发现精度不够，于是就有了中值法。中值法假设 imu 是匀加速运动的，这就比匀速更高级了一些，此时离散时间形式为
ϕ=ωk−1+ωk2(tk−tk−1)\boldsymbol{\phi}=\frac{\boldsymbol{\omega}_{k-1}+\boldsymbol{\omega}_{k}}{2}\left(t_{k}-t_{k-1}\right)\\ ϕ=2ωk−1+ωk(tk−tk−1)
c.龙哥库塔法

中值法假设的匀加速运动，在imu运动更加剧烈的情况下仍然不够，那就继续把模型复杂化呗，龙哥库塔法就是这种，把运动模型搞成一个高阶曲线。由于这部分略显复杂，而且实际使用中，中值法多数情况下已经够用，因此在此处就不对这种方法做过多介绍了。

1.2 基于四元数的姿态更新

1）连续时间处理

四元数的微分方程为
q˙wb=qwb⊗12[0ω]\dot{\boldsymbol{q}}_{w b}=\boldsymbol{q}_{w b} \otimes \frac{1}{2}\left[\begin{array}{l} 0 \\ \boldsymbol{\omega} \end{array}\right]\\ q˙wb=qwb⊗21[0ω]
其矩阵形式为(可回顾四元数乘法那一小节的推导)
q˙wb=12[0ω]Rqwb\dot{\boldsymbol{q}}_{w b}=\frac{1}{2}\left[\begin{array}{l} 0 \\ \boldsymbol{\omega} \end{array}\right]_{R} \boldsymbol{q}_{w b}\\ q˙wb=21[0ω]Rqwb
同样按照微分方程的解法，得到其积分形式为
q˙wb=e12Θqwb\dot{\boldsymbol{q}}_{w b}=\mathrm{e}^{\frac{1}{2} \boldsymbol{\Theta}} \boldsymbol{q}_{w b}\\ q˙wb=e21Θqwb
其中
Θ=[0−ϕx−ϕy−ϕzϕx0ϕz−ϕyϕy−ϕz0ϕxϕzϕy−ϕx0]\boldsymbol{\Theta}=\left[\begin{array}{cccc} 0 & -\phi_{x} & -\phi_{y} & -\phi_{z} \\ \phi_{x} & 0 & \phi_{z} & -\phi_{y} \\ \phi_{y} & -\phi_{z} & 0 & \phi_{x} \\ \phi_{z} & \phi_{y} & -\phi_{x} & 0 \end{array}\right]\\ Θ=⎣⎡0ϕxϕyϕz−ϕx0−ϕzϕy−ϕyϕz0−ϕx−ϕz−ϕyϕx0⎦⎤
为了求解该方程，要对指数函数进行泰勒展开，同样包含高次幂，展开方法与前面旋转向量指数形式展开的方法类似，可以自行推导，此处直接给出结论
e12Θ(T)=Icos⁡ϕ2+Θϕsin⁡ϕ2\mathrm{e}^{\frac{1}{2} \Theta(T)}=I \cos \frac{\phi}{2}+\frac{\Theta}{\phi} \sin \frac{\phi}{2}\\ e21Θ(T)=Icos2ϕ+ϕΘsin2ϕ
因此有
qwbk=[Icos⁡ϕ2+Θϕsin⁡ϕ2]qwbk−1\boldsymbol{q}_{w b_{k}}=\left[I \cos \frac{\phi}{2}+\frac{\boldsymbol{\Theta}}{\phi} \sin \frac{\phi}{2}\right] \boldsymbol{q}_{w b_{k-1}}\\ qwbk=[Icos2ϕ+ϕΘsin2ϕ]qwbk−1
由于
[cos⁡ϕ2ϕϕsin⁡ϕ2]R=[Icos⁡ϕ2+Θϕsin⁡ϕ2]\left[\begin{array}{c} \cos \frac{\phi}{2} \\ \frac{\phi}{\phi} \sin \frac{\phi}{2} \end{array}\right]_{R}=\left[I \cos \frac{\phi}{2}+\frac{\Theta}{\phi} \sin \frac{\phi}{2}\right]\\ [cos2ϕϕϕsin2ϕ]R=[Icos2ϕ+ϕΘsin2ϕ]
另
qbk−1bk=[cos⁡ϕ2ϕϕsin⁡ϕ2]\boldsymbol{q}_{b_{k-1} b_{k}}=\left[\begin{array}{c} \cos \frac{\phi}{2} \\ \frac{\phi}{\phi} \sin \frac{\phi}{2} \end{array}\right]\\ qbk−1bk=[cos2ϕϕϕsin2ϕ]
则
qwbk=qwbk−1⊗qbk−1bk\boldsymbol{q}_{w b_{k}}=\boldsymbol{q}_{w b_{k-1}} \otimes \boldsymbol{q}_{b_{k-1} b_{k}}\\ qwbk=qwbk−1⊗qbk−1bk

2) 离散时间处理

此处也是同样用中值法算出旋转矢量即可得到 qbk−1bk\boldsymbol{q}_{b_{k-1} b_{k}}qbk−1bk ，从而得到更新后的四元数 qwbk\boldsymbol{q}_{w b_{k}}qwbk ，与前述方法相同，不再重复。

1.3 总结

通过旋转矩阵去更新姿态需要去求一个增量选装矩阵Rbk−1bk\boldsymbol{R}_{b_{k-1} b_{k}}Rbk−1bk，而Rbk−1bk\boldsymbol{R}_{b_{k-1} b_{k}}Rbk−1bk则需要去求旋转向量，如果器件直接输出的是角增量（角速度在时间上的积分）那就皆大欢喜，如果是输出的角速度则需要通过中值积分求得选装向量。通过四元数更新也是类似的。
角增量和旋转向量的区别

如果是在匀速模型下，也就是前后两个时刻的旋转方向没有发生改变，那么认为角增量和旋转向量是相等的，如果是在线性或在高阶情况下，则不相等。

2. 速度更新

相比来讲，速度更新就显得很简单，速度的微分方程为
v˙=Rwba−g\dot{\boldsymbol{v}}=\boldsymbol{R}_{w b} \boldsymbol{a}-\boldsymbol{g}\\ v˙=Rwba−g
其中a=[axayaz]\boldsymbol{a}=\left[\begin{array}{lll} a_{x} & a_{y} & a_{z} \end{array}\right]a=[axayaz] 为测量加速度， g=[00g0]\boldsymbol{g}=\left[\begin{array}{lll} 0 & 0 & g_{0} \end{array}\right]g=[00g0]为重力加速度。

该微分方程的通解形式为
Δv=(Rwba−g)Δt\Delta \boldsymbol{v}=\left(\boldsymbol{R}_{w b} \boldsymbol{a}-\boldsymbol{g}\right) \Delta t\\ Δv=(Rwba−g)Δt
对应的基于中值法的速度更新形式为
vk=vk−1+(Rwbkak+Rwbkak−12−g)(tk−tk−1)\boldsymbol{v}_{k}=\boldsymbol{v}_{k-1}+\left(\frac{\boldsymbol{R}_{w b_{k}} \boldsymbol{a}_{k}+\boldsymbol{R}_{w b_{k}} \boldsymbol{a}_{k-1}}{2}-\boldsymbol{g}\right)\left(t_{k}-t_{k-1}\right)\\ vk=vk−1+(2Rwbkak+Rwbkak−1−g)(tk−tk−1)

3. 位置更新

位置更新就更加简单，微分形式为
p˙=v\dot{\boldsymbol{p}}=\boldsymbol{v}\\ p˙=v
其通解形式为
Δp=vΔt\Delta \boldsymbol{p}=\boldsymbol{v} \Delta t\\ Δp=vΔt
基于中值法的离散形式为
pk=pk−1+vk+vk−12(tk−tk−1)\boldsymbol{p}_{k}=\boldsymbol{p}_{k-1}+\frac{\boldsymbol{v}_{k}+\boldsymbol{v}_{k-1}}{2}\left(t_{k}-t_{k-1}\right)\\ pk=pk−1+2vk+vk−1(tk−tk−1)
另外，如果把速度的通解代入，则位置的通解形式也可以写为
Δp=vΔt+12aΔt2\Delta \boldsymbol{p}=\boldsymbol{v} \Delta t+\frac{1}{2} \boldsymbol{a} \Delta t^{2}\\ Δp=vΔt+21aΔt2
相应的基于中值法的离散形式为
pk=pk−1+vk−1(tk−tk−1)+12(Rwbkak+Rwbk−1ak−12−g)(tk−tk−1)2\begin{aligned} \boldsymbol{p}_{k}= \boldsymbol{p}_{k-1}+\boldsymbol{v}_{k-1}\left(t_{k}-t_{k-1}\right) +\frac{1}{2}\left(\frac{\boldsymbol{R}_{w b_{k}} \boldsymbol{a}_{k}+\boldsymbol{R}_{w b_{k-1}} \boldsymbol{a}_{k-1}}{2}-\boldsymbol{g}\right)\left(t_{k}-t_{k-1}\right)^{2} \end{aligned}\\ pk=pk−1+vk−1(tk−tk−1)+21(2Rwbkak+Rwbk−1ak−1−g)(tk−tk−1)2

4. 补充知识

4.1 导航系与惯性系

在实际导航模型中,不动系(i系)是地心惯性系,而我们要的导航结果是相对于导航系(n系)的,这两
个坐标系之间有相对旋转,他们之间的旋转角速度可以表示为
ωinn=ωien+ωenn\boldsymbol{\omega}^{n}_{i n} = \boldsymbol{\omega}^{n}_{i e}+\boldsymbol{\omega}^{n}_{e n} ωinn=ωien+ωenn
其中ωien\boldsymbol{\omega}^{n}_{i e}ωien表示地球自转引起的角速度;ωenn\boldsymbol{\omega}^{n}_{e n}ωenn表示载体在地球表面运动时,绕地球旋转形成的角速度。其表达式如下:
ωien=[0ωiencos⁡Lωiensin⁡L]Tωenn=[−vNrM+hvErN+hvErN+htan⁡L]T\boldsymbol{\omega}^{n}_{i e} = \left[\begin{array}{c} 0 & \boldsymbol{\omega}^{n}_{i e}\cos{L} & \boldsymbol{\omega}^{n}_{i e}\sin{L} \end{array}\right]^T\\ \boldsymbol{\omega}^{n}_{e n} = \left[\begin{array}{c} -\frac{v_N}{r_M+h} & \frac{v_E}{r_N+h}& \frac{v_E}{r_N+h}\tan{L} \end{array}\right]^T ωien=[0ωiencosLωiensinL]Tωenn=[−rM+hvNrN+hvErN+hvEtanL]T
后者的理解需要借助地球模型,此处不引入地球模型的推导,其推导过程同样可参考《捷联惯导算法与组合导航
原理》(严恭敏等编著)。
原文链接：多传感器融合定位理论基础（五）：惯性导航解算

惯性导航解算（三）续相关推荐

多传感器融合定位七-惯性导航解算及误差分析其一
多传感器融合定位七-惯性导航解算及误差分析其一 1. 三维运动描述基础知识 1.1 概述 1.2 姿态描述方法 1.2.1 欧拉角 1.2.2 旋转矩阵 1.2.3 四元数 1.2.4 等效旋转矢量 ...
【惯性导航姿态仪】 04 -Mini AHRS 姿态解算说明
[惯性导航姿态仪] 04 -Mini AHRS 姿态解算说明引言加速度计陀螺仪测量什么? 从ADC值到 dps 参考链接:( MiniIMU AHRS 姿态板销售网址:) http://chip ...
空间三点定圆的解算过程
记得去年在上海船厂期间一次员工要我们检测一个圆形构件,用全站仪在一圆形构件的同一高度上测得三个点,然后算出构件的圆心坐标和半径,数学模型如下: 已知空间三点的坐标为(x1,y1,z1),(x2,y2, ...
姿态解算知识（三）-陀螺仪加速度计6轴数据融合
这么久的惯导总算是没白看,加上一篇博客的指点,这两天把Mahony的九轴数据融合算法看懂了.可惜第二版硬件还没到,磁力计用不了,没法验证效果~今天先总结下陀螺仪和加速度计的六轴数据融合. 版权声明原 ...
ansible自动化运维详解（三）ansible常用模块续
文章目录 ansible自动化运维详解(三)ansible常用模块续四.ansible常用模块(2) 4.10.yum_repository 4.11.dnf 4.12.service 及 fire ...
GNSS说第(七)讲---自适应动态导航定位(三)---序贯导航定位解算原理
GNSS说第(七)讲-自适应动态导航定位(三)-序贯导航定位解算原理序贯导航定位解算原理序贯平差法属于逐步平差法,即递推平差.其基本思想是:在对线性模型的统计性质作某些合适的假设后,基于不同的平差 ...
三轴磁力计解算姿态（四元数）
原理根据地磁场向量在水平面上的投影来计算载体的偏航角,类似于加速度计解算姿态,不同在于磁场易受干扰,且只能得到偏航角. 方法假设导航坐标系为东北天,载体坐标系为右前上. 初始载体坐标系和导航坐标系 ...
三轴加速度计解算姿态（四元数）
原理当传感器载体静止时,加速度计只会输出重力加速度,可以凭此来计算载体的俯仰角和滚转角. 方法假设导航坐标系为东北天,载体坐标系为右前上. 初始载体坐标系和导航坐标系重合,对应的四元数为q=[1, ...
STM32实现四驱小车（三）传感任务——姿态角解算
目录一. 绪论二. 惯性传感器测量原理 1. 三轴加速度计 2. 三轴陀螺仪 3. 三轴磁力计三. 状态估计 1. 姿态估计 (1)线性互补滤波器 (2)非线性互补滤波器 (3)卡尔曼滤波器 2 ...

惯性导航解算（三）续

二、惯性导航解算的方法

1. 姿态更新

1.1 基于旋转矩阵的姿态更新

1）连续时间处理

2）离散时间处理

1.2 基于四元数的姿态更新

1）连续时间处理

2) 离散时间处理

1.3 总结

2. 速度更新

3. 位置更新

4. 补充知识

4.1 导航系与惯性系

惯性导航解算（三）续相关推荐

最新文章

热门文章