场数据进行高频采集。 [0043]陀螺仪测量的是载体的角速度，也就是姿态角的变化量，有动态精度高的特点，而 磁力计和加速度计测量的是环境磁场数据和加速度，具有静态精度好的优点，因此采取三 者结合、相互补偿的方法对姿态角进行解算，以期达到最优解算精度。
[0044] 2)在高频采集的同时，对采集到的实时数据进行预处理。
[0045] 3)根据预处理后的实时数据获得载体的初始姿态，并采用梯度下降法解算载体姿 ??τ 〇
[0046] 初始姿态为测量开始时静止状态的载体姿态，包括初始四元数、初始姿态矩阵和 初始姿态角。
[0047]设四元数矩阵9=[90^1^2，(13]，那么得以表示出旋转矩阵。在处理动态的姿态 计算之前，首先得到此时的初始姿态角，也就是初始四元数。通过测量静止状态下的重力加 速度，可以确定姿态角中与地理系绕两个水平轴旋转的俯仰角和横滚角，再以磁场的读数 确定绕垂直轴旋转的指向角，最后将欧拉角转换成初始的四元数。
[0048]步骤3)中，解算载体姿态具体为：
[0049]采用梯度下降法，将载体坐标系中重力造成的加速度理论值和预处理后的实时数 据中的加速度测量值间的误差作为目标函数，循环求解使误差最小的姿态四元数，即求得 的四元数收敛。
[0050] 结合实际需求，以陀螺仪的解算为主，采取梯度下降法融合加速度计进行补偿。
[0051] 误差标量函数ef(q)，以求解ef(q)=0。在一维情况下，标量函数对向量求导，将问 题就转化为求多元向量函数极值问题。考虑到计算量和精度需求之间的平衡，我们选用一 阶线性收敛的梯度下降法来求解这一问题。
[0052]在^处的梯度（导数）的反方向，是当前最快的下降收敛速度，按照这个方向走步 长γ，就从χη走到了χη+1。在多元向量误差函数可以表示重力加速度g在地理坐标系中的值， 通过四元数法旋转到载体坐标系中的值，和当前加速度计测得的值的差，也就是通过加速 度计表征的旋转矩阵的误差。当这个函数等于0时，认为得到了旋转矩阵没有误差，也就是 姿态四元数是精确的。由于只与重力加速度作差，而欧拉角中航向角与重力分量无关，这里 实际上只能保证横滚角和俯仰角的精确度。
[0053]为了求解方程ef(q)=0,我们对误差函数fg求导，并用它来表示梯度计算公式，可 求得用四元数表示的梯度公式。假设我们经过梯度下降法计算的目标姿态为qv,t，那么有：
即为根据梯度下降算法求得的四元数。
[0054]对梯度归一化是为了得到梯度的方向，其梯度大小由步长决定。此一阶线性收 敛通过迭代运算，可以从初始时刻的姿态，经由梯度的相反方向，达到多元向量误差函数的 一个局部极值点，这个点就是消除了旋转矩阵误差的点，也就是精确的四元数。
[0055]而步长μ*和实际物体运动的角速度及采样时间相关，物体运动的角速度越快，采 样时间越长，那么就需要越长的收敛步长，具体关系为实际使用中根 据运动速率的不同动态的选择步长，以获得最佳的解算效果。
[0056]因采用了较高精度的传感器，实测的结果显示只需对陀螺仪校正零偏。
[0057] 4)通过解算得到了与时刻对应的摄影机姿态，由此可以推出该姿态相对应的旋转 矩阵。传感器测得的加速度向量，经过旋转矩阵转换，可得惯性系下的加速度向量。该向量 相对于时间积分即得三轴轨迹。
[0058] 根据解算得的载体姿态通过积分方式得到位置坐标，从而获得时刻与对应轨迹坐 标的矩阵，将该矩阵导入MAYA软件，可得到一条空间轨迹，并以此为MAYA软件中摄像机拍摄 轨迹，从而t旲拟真空拍摄中的摄像机角度和运动状态。
[0059] 积分的方法，从积分精度和时间、内存消耗等角度，可作多种选择;不同场景可匹 配不同积分方法，包括但不限于矩形积分、梯形积分等。本实施例为达到一定精度，主要采 用了等距节点下使用拉格朗日插值多项式建立的数值积分公式。
【主权项】
1. 一种基于微惯性传感器的轨迹还原方法，其特征在于，包括以下步骤： 1) 由多种微惯性传感器对载体的加速度、角速度和环境磁场数据进行高频采集； 2) 在高频采集的同时，对采集到的实时数据进行预处理； 3) 根据预处理后的实时数据获得载体的初始姿态，并采用梯度下降法解算载体姿态； 4) 根据解算得的载体姿态通过积分方式求得三维空间内的轨迹。2. 根据权利要求1所述的基于微惯性传感器的轨迹还原方法，其特征在于，步骤2)中， 所述预处理包括确定量程和单位归一化处理。3. 根据权利要求1所述的基于微惯性传感器的轨迹还原方法，其特征在于，步骤3)中， 所述初始姿态为测量开始时静止状态的载体姿态，包括初始四元数、初始姿态矩阵和初始 姿态角。4. 根据权利要求1所述的基于微惯性传感器的轨迹还原方法，其特征在于，步骤3)中， 解算载体姿态具体为： 采用梯度下降法，将载体坐标系中重力造成的加速度理论值和预处理后的实时数据中 的加速度测量值间的误差作为目标函数，求解使所述误差最小的姿态四元数。5. 根据权利要求1所述的基于微惯性传感器的轨迹还原方法，其特征在于，步骤4)中， 所述积分方式包括牛顿-柯斯特积分、数值积分、矩形积分或梯形积分。6. -种基于微惯性传感器的轨迹还原系统，其特征在于，包括： 轨迹记录装置，安装于载体上，用于高频采集载体的加速度、角速度和环境磁场数据， 并对采集得的实时数据进行预处理，所述轨迹记录装置包括多种微惯性传感器； 后台处理器，通过无线网络与轨迹记录装置连接，用于根据预处理后的实时数据采用 梯度下降法还原载体在三维空间内的轨迹； 显示器，与后台处理器连接，用于显示后台处理器获得的轨迹。7. 根据权利要求6所述的一种基于微惯性传感器的轨迹还原系统，其特征在于，所述后 台处理器包括： 初始姿态获取模块，用于在预处理后的实时数据中分离出测量开始时静止状态阶段的 数据，获得初始姿态，所述初始姿态包括初始四元数、初始姿态矩阵和初始姿态角； 载体姿态解算模块，用于采用梯度下降法将载体坐标系中重力造成的加速度理论值和 预处理后的实时数据中的加速度测量值间的误差作为目标函数，求解使所述误差最小的姿 态四元数； 积分模块，用于根据解算得的载体姿态通过积分方式求得三维空间内的轨迹。8. 根据权利要求6所述的一种基于微惯性传感器的轨迹还原系统，其特征在于，所述多 种微惯性传感器包括加速度计、磁力计和陀螺仪，其中，所述加速度计和磁力计集成于同一 模块中，共用一套寄存器地址。9. 根据权利要求6所述的一种基于微惯性传感器的轨迹还原系统，其特征在于，所述轨 迹记录装置内还包括与多种微惯性传感器分别连接的数据处理芯片，所述数据处理芯片通 过无线传输模块与后台处理器连接。10. 根据权利要求6所述的一种基于微惯性传感器的轨迹还原系统，其特征在于，所述 无线传输模块包括无线网卡或蓝牙芯片。
【专利摘要】本发明涉及一种基于微惯性传感器的轨迹还原方法及系统，所述轨迹还原方法包括以下步骤：1)由多种微惯性传感器对载体的加速度、角速度和环境磁场数据进行高频采集；2)在高频采集的同时，对采集到的实时数据进行预处理；3)根据预处理后的实时数据获得载体的初始姿态，并采用梯度下降法解算载体姿态；4)根据解算得的载体姿态通过积分方式求得三维空间内的轨迹。与现有技术相比，本发明具有准确性高、成本低等优点。
【IPC分类】G06F3/0346
【公开号】CN105607760
【申请号】CN201510957912
【发明人】周亦迪, 王雨辰, 张峥, 冯耀
【申请人】上海开圣影视文化传媒股份有限公司
【公开日】2016年5月25日
【申请日】2015年12月18日