IMU姿态惯性推导

本文是我上学期间写得，之前已经在另一个博客发布过，如今转至此发布。

最近从事行人惯性导航的研究，本人也是一个小白，其中看了很多文献，有很多个人思考很费时间的地方，撰写此随笔的目的不仅是给自己做一个笔记，也是给各位有需要的仁兄一点个人理解。
本文只关于使用IMU传感器为主的行人导航算法。
本文为一篇行人惯性导航的入门，主要针对其中重要的涉及的知识点之间的解释、串联，包括航向更新、速度更新、位置更新、坐标变换原理即代码，不深究其推导。要是有什么讲得不对的地方，请给予指正，谢谢。

引言

以IMU为主，其他传感器（多为磁力计、高度计、气压计、压力传感器、肌肉电传感器等）为辅的行人导航研究中，主要有两种方案（这里忽略SLAM、激光雷达之类的，）。一个是基于航迹推移的方案（SHS，step-and-heading systems），另一个是基于惯性积分的方案（INS，inertial navigation systems）。个人研究的技术方向属于后者（INS），所以后面将会着重介绍后者的技术细节。

SHS

也就是常见的航迹推移算法（PDR，Pedestrian Dead Reckoning），根据计算出来的步长和航向，通过三角函数关系递推下一步的位置。所以该方案的核心就是求步长和航向，同时也是该算法的两个主要误差来源。求步长有诸多步长估计模型，比如Weinberg模型[1]

l=k×(amax−amin)1/4l=k \times (a_{max}-a_{min})^{1/4} l=k×(amax−amin)1/4

或者Scarlet模型[2]

l=k×∑i=1NaiN−aminamax−aminl=k \times \cfrac{\cfrac{\sum_{i=1}^Na_i}{N}-a_{min}}{a_{max}-a_{min}} l=k×amax−aminN∑i=1Nai−amin

等等，详细细节可自行了解。但是无论使用哪一个步长模型，这里面的参数（比如k）都跟个体参数（步频、步长、腿长等）的不同而不同，即需要对人体进行运动学建模，这也是这种模型最大的一个弊端（个人觉得）[3]。

关于航向修正放在后面（INS）一起总结，因为无论是SHS，还是INS，航向漂移都是两者的痛点。

INS

基于惯性递推的技术方案，给出常见的递推公式[4]便一目了然。

pkn=pn−1n+vk−1nΔtvkn=vk−1n+{R(qk−1)fb+gn}Δtqb,kn=Ω(ωkΔt)qb,k−1np_k^n=p_{n-1}^n+v_{k-1}^n\Delta t \\ v_k^n = v_{k-1}^n+\{R(q_{k-1})f^b + g^n\}\Delta t \\ q_{b,k}^n = \Omega(\omega_k \Delta t)q_{b,k-1}^n pkn=pn−1n+vk−1nΔtvkn=vk−1n+{R(qk−1)fb+gn}Δtqb,kn=Ω(ωkΔt)qb,k−1n
其中nnn、bbb分别表示导航坐标系和载体坐标系，ppp、vvv、qqq分别表示位置、速度、姿态四元数（对应欧拉角，俯仰角pitch、滚转角raw、航向角yaw）。这里是一个简化后的模型，忽略了地球自转、经纬度等因素的影响，因为这些因素对低精度的惯性器件影响可以忽略不计，具体细节可以参考文献[7]，这里不再展开。

INS特点：

基于惯性递推的方案相比于航迹推移算法，其本身的姿态推导不需要为人体运动学建模，适用性更好；
基于惯性递推的方案误差随时间的立方根次增长，这是其最大的弊端；
基于惯性递推的方案一般结合零速修正技术，使用卡尔曼滤波器将两者进行融合，限制误差的增长。

关于INS方法的技术点

零速修正
- 使用零速修正技术，重点是使用什么检测器检测零速条件！
- 当前有很多零速检测器，比SHOE、ARED、MBGTD等，其中SHOE公认精度是最好的[6]。
航向修正
- 无论是SHS还是INS，在航向修正方面所遇到的问题都基本相同，及决方案大致一样，要么借助磁力计，要么是HDR/iHDR，或者其他的方案。

干货

下面所讲的内容是个人所经历的，也是本文的重点。如果看惯性导航参考书[5]的话，你一般都会看到复杂的推导，容易搞蒙，尤其是涉及到坐标系的转换关系，除非死磕硬骨头才能拿下。其实有一本不错的讲义[8]，本人的大多数困惑都能从上边找到答案。

姿态更新
- 首先要明白IMU测的是相对于惯性坐标系 iii 的加速度和角速度，不是相对于导航系 nnn ，这之间存在转换。这也是为什么我早期一直很困惑的原因。具体的推导比较复杂，本文只给出关键结果。姿态的更新一般用四元数、旋转矩阵或旋转向量表示。本文以旋转矩阵进行说明。不同表达形式，都有各自的优缺点。旋转矩阵存在万向锁问题，四元数存在交换不可逆误差等。一般在使用的时候结合使用。
  Cb(k)n(k)=Cn(k−1)n(k)Ci(k−1)n(k−1)Cb(k−1)i(k−1)Cb(k)b(k−1)=Cn(k−1)n(k)Cb(k−1)n(k−1)Cb(k)b(k−1)C_{b(k)}^{n(k)}=C_{n(k-1)}^{n(k)}C_{i(k-1)}^{n(k-1)}C_{b(k-1)}^{i(k-1)}C_{b(k)}^{b(k-1)}=C_{n(k-1)}^{n(k)}C_{b(k-1)}^{n(k-1)}C_{b(k)}^{b(k-1)} Cb(k)n(k)=Cn(k−1)n(k)Ci(k−1)n(k−1)Cb(k−1)i(k−1)Cb(k)b(k−1)=Cn(k−1)n(k)Cb(k−1)n(k−1)Cb(k)b(k−1)
  其中，Cb(k)b(k−1)C_{b(k)}^{b(k-1)}Cb(k)b(k−1) 表示以i系作为参考基准，b系从 tk−1t_{k-1}tk−1 时刻到 tkt_{k}tk 时刻的旋转变化，Cb(k)b(k−1)C_{b(k)}^{b(k-1)}Cb(k)b(k−1) 由角速度 wibbw_{ib}^bwibb 确定；Cn(k)n(k−1)C_{n(k)}^{n(k-1)}Cn(k)n(k−1) 表示以i系作为参考基准，n系从 tk−1t_{k-1}tk−1 时刻到 tkt_{k}tk 时刻的旋转变化，Cn(k−1)n(k)C_{n(k-1)}^{n(k)}Cn(k−1)n(k) 由计算角速度 −winn-w_{in}^n−winn 确定；Cn(k−1)n(k−1)C_{n(k-1)}^{n(k-1)}Cn(k−1)n(k−1) 和 Cb(k)n(k)C_{b(k)}^{n(k)}Cb(k)n(k) 分别表示 tk−1t_{k-1}tk−1 和 tkt_{k}tk 时刻的捷联姿态矩阵。通常在导航更新周期 [tk−1,tk][t_{k-1},t_k][tk−1,tk] 内，可认为速度和位置引起的 winnw_{in}^nwinn 计算角速度很小，即可视为常值[8]；通常在更新周期 [tk−1,tk][t_{k-1},t_k][tk−1,tk] 内，导航系的变化很缓慢，所以 Cn(k−1)n(k)≈I3×3C_{n(k-1)}^{n(k)} \approx I_{3 \times 3}Cn(k−1)n(k)≈I3×3[5]。从这个角度应该能更好的理解坐标系之间的变化关系。

用四元数表示为：
Q(tk)=Q(tk−1)⨂q(ωkΔt)q(ωkΔt)=cosϕ2+ϕ→ϕsinϕ2Q(t_k) = Q(t_{k-1})\bigotimes q(\omega_k \Delta t) \\ q(\omega_k \Delta t) = cos\cfrac{\phi}{2} + \cfrac{\overrightarrow{\phi}}{\phi}sin\cfrac{\phi}{2} Q(tk)=Q(tk−1)⨂q(ωkΔt)q(ωkΔt)=cos2ϕ+ϕϕsin2ϕ
其中 QQQ 为四元数，表示载体的姿态，ϕ\phiϕ 是旋转矢量 ϕ→\overrightarrow{\phi}ϕ 的模长，四元数、旋转矢量、旋转矩阵可以互相转换。这里所写的形式和之前的递推公式没有什么区别，只是一个是四元数的左乘，一个是四元数的右乘。为什么四元数需要结合旋转向量使用，就是弥补各自的缺点（建议看书，这里面存在一个理论推导）。

速度更新
- 速度的更新也存在一个坐标系的转换；
- 其实只要理解的旋转矩阵的更新，后面的推导都是在旋转矩阵更新的基础上的得到；
- 关于速度更新，更注意的一点是剔除重力因素的影响。
- 上面的递推公式 vkn=vk−1n+{R(qk−1)fb+gn}ΔTv_k^n = v_{k-1}^n+\{R(q_{k-1})f^b + g^n\}\Delta Tvkn=vk−1n+{R(qk−1)fb+gn}ΔT，其中 R(qk−1)fbR(q_{k-1})f^bR(qk−1)fb 对所测到的加速度 fbf^bfb（一般称为比力）进行坐标变换，R(qk−1)R(q_{k-1})R(qk−1) 表示对四元数转换为旋转矩阵。

位置更新
- 这就不用说了，一眼明了。

总结

关于IMU的行人导航技术，还有很多研究的热点，比如初始校准、高程问题、航向问题、多运动模型的问题等等。以上只是个人的理解，如果有什么不对的地方，请大佬尽管指出，虚心请教。欢迎各位同学留言，互相学习、探讨。

参考文献

[1] Weinberg H. Using the ADXL202 in pedometer and personal navigation applications[J].
[2] Scarlett J. Enhancing the performance of pedometers using a single accelerometer[J].
[3] 潘献飞, 穆华, 胡小平. 单兵自主导航技术发展综述[J]. 导航定位与授时, 2018, 5(1):11.
[4] Wagstaff B , Peretroukhin V , Kelly J . Robust Data-Driven Zero-Velocity Detection for Foot-Mounted Inertial Navigation[J]. IEEE Sensors Journal, 2019, PP(99):1-1.
[5] 秦永元. 惯性导航.第2版[M]. 科学出版社, 2014.
[6] Wahlstrm J , Skog I . Fifteen Years of Progress at Zero Velocity: A Review[J]. 2020.
[7] Muhammad I , Kuk C , Seung-Ho B , et al. Drift Reduction in Pedestrian Navigation System by Exploiting Motion Constraints and Magnetic Field[J]. Sensors (Basel, Switzerland), 2016, 16(9).
[8] 捷联惯导算法与组合导航原理讲义. 严恭敏，翁浚编著.

【行人惯性导航】关于行人导航中IMU位姿推导的知识点及相关代码相关推荐

通过非递归贝叶斯滤波实现室内环境下建筑平面图的行人惯性导航
文章目录一.介绍二.方法 A.零速更新 B.递归贝叶斯滤波器(RBF) C.提出的方法三.实验设置和结果四.结论 REFERENCES 摘要在这篇论文中,使用一种新颖的非递归贝叶斯滤波 ...
【行人检测】检测视频中的行人
[行人检测]检测视频中的行人在上一节检测图片中的行人的基础上,实现检测视频中的行人. 检测行人的视频可戳:https://download.csdn.net/download/u012679707/ ...
vins中imu融合_VINS-Mono代码分析与总结(最终版)
VINS-Mono代码分析总结参考文献前言 ??视觉与IMU融合的分类: 松耦合和紧耦合:按照是否把图像的Feature加入到状态向量区分,换句话说就是松耦合是在视觉和IMU各自求出的位姿的基础上 ...
车道线检测在AR导航中的应用与挑战
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 1. 导读现代社会中,随着车辆的普及,人的活动范围在逐步扩大,单单依靠人类记忆引导行驶到达目的地已经 ...
基于视觉的车道线识别技术在智能车导航中的应用研究
密级:公开摘要摘要室外移动机器人的研究是机器人研究领域的重要分支,同时也是备受关注的热点领域.面向高速公路等结构化道路的室外移动机器人研究已成为现阶段民用交通运输领域移动机器人研究的主流 ...
惯性导航在石油测井中的应用
惯性导航在石油测井中的应用民用领域目前我国惯性技术民用市场主要集中于导航.测绘.石油勘探.应急通讯.智能交通等领域.民用市场已进入市场化竞争阶段,各企业面向市场自主经营与竞争.在民用领域,除惯性航设 ...
行人重识别技术（Re-ID）概述、研究现状以及相关数据集下载
ReID技术概述什么是ReID 一般流程 Re-ID特点研究现状难点数据集 reid常用数据集以及下载链接评价指标实现思路研究方式基于表征学习基于度量学习基于局部特征全局特征切 ...
【行人检测】行人检测综述
1. pipeline Proposal generation:滑窗,粒子窗(particle-window),对象性方法(objectness method),区域建议网络(region propo ...
代码开源！！行人检测与行人重识别结合 person search
0 前言最近在做自己课题相关的小项目,行人检测与行人重识别进行结合进行场景图片进行特定行人的检索由于也比较着急,先利用现有的模型搭建了简单的demo,简单效果展示如下,速度大概在20FPS: 先是给 ...

【行人惯性导航】关于行人导航中IMU位姿推导的知识点及相关代码

IMU姿态惯性推导

引言

SHS

INS

INS特点：

关于INS方法的技术点

干货

相关代码（Python）

总结

参考文献

【行人惯性导航】关于行人导航中IMU位姿推导的知识点及相关代码相关推荐

最新文章

热门文章