惯性测量单元和地磁传感器整体标定装置及标定方法与流程

本发明涉及导航技术领域，具体涉及一种惯性器件和地磁传感器整体标定装置及其标定方法。

背景技术：

惯性测量单元(IMU)和地磁传感器广泛应用于智能手机等电子消费品，以及无人机和无人驾驶车等机器人系统中。IMU是惯性导航的基础测量装置，由三轴陀螺和三轴加速度计组成，固联于载体之上，实现对载体角速度和比力的感知，通过力学编排的微分方程，能够实时计算出载体的位置、速度和姿态角。但是，民用惯性器件的精度差，难以实现方位角的测量和对准。因此，采用三轴地磁传感器(例如磁通门传感器和磁阻传感器等)，感知当地磁场强度，实现方位角的计算。但是惯性导航的精度严重依赖惯性器件的精度，即使与GNSS(全球导航卫星系统，例如GPS和北斗等)接收机构成组合导航系统，也存在室内或城市峡谷等无GNSS信号环境。地磁传感器的测量精度受载体的电磁环境的影响明显，因此方位角的精度难以保证。为了提高惯导和方位角的解算精度，对惯性器件实现精确的标定和补偿是最普遍的方法。

传统IMU的标定通过三轴惯导测试转台和精密离心机实现。在陀螺标定过程中，IMU的坐标系与转台坐标系重合，建立标定模型计算陀螺的零偏，通过陀螺对多个基准角速度输入的响应，计算刻度因数，安装耦合系统和非线性度等标定系数。同样地，通过精密离心机能够标定出加速度计的零偏、刻度因数和耦合误差等标定系数。但是，三轴惯性转台和精密离心机的造价高，需要特定的隔离地基，标定过程复杂。地磁传感器的标定，目前无相关标准，通常采用圆周运动补偿方法，使标定对象的航向角在0～360°范围内变化，对地磁传感器进行连续采样，根据采样点最大和最小值，推导刻度系数和零位偏置。这种方法只能对地磁传感器进行定性的补偿，属于经验方法，不能保证补偿的系数的准确性。中国专利公开号CN105180968A，公开日2015年12月23日，发明创造的名称为《一种IMU/磁强计安装失准角在线滤波标定方法》，公开了一种IMU/磁强计安装失准角在线滤波标定方法，应用卡尔曼滤波方法得到捷联惯导系统IMU相对磁强计安装失准角全部误差参数；利用六面体或其它相似的可翻转装置即可完成现场标定试验，克服了传统实验室标定的不足，提高了系统实际使用精度。但是这种方法无法避免标定装置的软磁效应和硬磁效应对地磁传感器标定误差的影响，无法获得较高的精度和足够的标定参数。

技术实现要素：

本发明为解决现有惯性测量单元标定设备成本高，标定方法复杂，并且现有地磁传感器的补偿技术由于存在误差导致测量精度低等问题，提供一种惯性测量单元和地磁传感器整体标定装置及标定方法。

惯性测量单元和地磁传感器整体标定装置，包括双天线GNSS/SINS组合导航系统、标定处理系统、工业相机和六面体工装，标定对象安装在六面体工装中，作为标定六面体工装，所述标定六面体工装的六个表面上分别贴有不同ID的增强现实合作目标；所述双天线GNSS/SINS组合导航系统作为基准组合导航系统与工业相机固连，所述基准组合导航系统中IMU和工业相机安装于两个GNSS接收机天线的中点位置；

所述双天线GNSS/SINS组合导航系统测量当地地理位置和基准坐标系相对于当地地理坐标系的姿态角；所述标定处理系统采集双天线GNSS/SINS组合导航系统的测量值、标定对象中加速度、陀螺以及地磁传感器的测量值以及工业相机采集标定六面体工装表面的增强现实合作目标图像信息，所述标定处理系统计算当前增强现实合作目标相对相机坐标系的姿态角；

所述标定处理系统计算基准坐标系和当地地理坐标系的方向余弦矩阵，目标坐标系和相机坐标系的方向余弦矩阵；获得载体坐标系相对于当地地理坐标系的方向余弦矩阵；计算所述载体坐标系三轴传感器的标称值；将标定对象中加速度计、陀螺和地磁传感器标称值和标定对象中加速度计、陀螺和地磁传感器的测量值建立方程组，实现对惯性测量单元和地磁传感器的整体标定。

惯性测量单元和地磁传感器整体标定方法，该方法由以下步骤实现：

步骤一、将标定对象安装于标定六面体工装内，所述标定六面体工装、双天线GNSS/SINS组合导航系统以及工业相机处于同一平面，并且所述增强现实合作目标位于工业相机的视场中心位置；

步骤二、建立标定对象中加速度计、陀螺以及磁场传感器的标定模型；

步骤三、在所述工业相机视场内以任意姿态放置标定六面体工装，保证至少有一个面上的增强现实合作目标落在工业相机的视场中；

步骤四、所述标定处理系统采集基准组合导航系统输出的当地地理位置和基准坐标系相对于当地地理坐标系的姿态角，并采集标定对象中加速度、陀螺以及地磁传感器的测量值；所述工业相机采集所述标定六面体工装表面的增强现实合作目标的图像，并将所述合作目标的图像传送至标定处理系统，所述标定处理系统计算当前增强现实合作目标相对相机坐标系的姿态角；

步骤五、标定处理系统计算基准坐标系和当地地理坐标系的方向余弦矩阵，目标坐标系和相机坐标系的方向余弦矩阵；获得载体坐标系相对于当地地理坐标系的方向余弦矩阵；

步骤六、计算所述载体坐标系三轴传感器的标称值；将标定对象中加速度计、陀螺和地磁传感器标称值和步骤四获得的标定对象中加速度计、陀螺和地磁传感器的测量值带入步骤二中的标定模型；

步骤六、判断所述测量值是否满足最小测量次数限制，如果是，则执行步骤七，如果否，返回执行步骤三；

步骤七、将标定对象中加速度计、陀螺和地磁传感器的标称值和测量值建立方程组，实现对惯性测量单元和地磁传感器的整体标定。

本发明的有益效果：本发明中将高分辨率的长焦工业相机与双天线GNSS/SINS组合导航系统固连在一起，基准组合导航系统测量地球系位置和当地地理系姿态角，查表获得理论地磁场强度值，计算理论比力和角速度，经过光学基准传递，计算标定对象载体系上标称值，采集被测IMU和地磁传感器的测量值，任意放置六面体工装，得到多组被标定对象标称值和测量值，建立方程组，根据最小二乘法，求取标定参数，完成标定。具体优点如下：

一、本发明采用双天线GNSS/SINS组合导航系统作为标定基准，克服了传统IMU标定手段采用三轴转台和高精度离心机设备昂贵，场地受限，标定流程和数据处理复杂的缺点。双天线GNSS/SINS组合导航系统测量精确的地球系位置，基准系相对于当地地理系的姿态角，获得精确计算比力、角速度和地磁场强度理论值。

二、本发明采用高分辨率相机对增强现实合作目标所呈图像进行测量，计算标定对象与基准装置的相对位姿，采用光学方式实现了基准传递，避免了传统机械方式基准传递对空间的局限。除了有效地降低了成本，还避免了直接的机械安装，以及标定装置对地磁传感器严重的电磁干扰，提高了地磁传感器标定的准确性和可信度。

三、本发明提出了能够通过光学与机器视觉方式实现精密测量的标定六面体。六面体外涂敷高精度加工的二维正方形增强现实合作目标，每一个平面上的合作目标ID各不相同，将标定对象安装于标定六面体中，确定二者几何关系，同时采集标定对象和基准标定装置输出，将高分辨率相机采集的增强现实图像进行解码和测量作为基准传递媒介，采用最小二乘法求解标定模型方程组，计算标定参数和标定噪声协方差。

附图说明

图1为本发明所述的惯性测量单元和地磁传感器整体标定装置的机械结构示意图；

图2为本发明的基准组合导航系统的基准坐标系与当地地理坐标系定义示意图；

图3为工业相机和相机坐标系的定义示意图；

图4为标定六面体工装和载体坐标系定义示意图；

图5为增强现实合作目标和目标坐标系定义示意图；

图6为本发明所述的惯性测量单元和地磁传感器整体标定装置的电路结构示意图；

图7为本发明所述的惯性测量单元和地磁传感器整体标定方法的流程图。

具体实施方式

具体实施方式一、结合图1至图6说明本实施方式，惯性测量单元和地磁传感器整体标定装置，结合图1说明本实施方式，包括一套双天线GNSS/SINS高精度组合导航系统，一个长焦高分辨率工业相机2和一个标定六面体工装4。六面体工装的各面上涂敷有不同ID的高精度的二维正方形增强现实合作目标。高精度组合导航系统与高分辨率工业相机通过金属板杆5固定，天线之间的基线长度保证1.5m以上，以保证基准航向角的噪声保持在较小水平。所述双天线GNSS/SINS高精度组合导航系统作为地球标称物理量与当地地理系变换的基准测试设备，包括两个GNSS接收机天线(天线1和天线3)，GNSS接收机，基准IMU和导航计算机。高精度组合导航系统作为标定基准，要求其采用的基准IMU的零偏和噪声水平至少优于被标定IMU的精度一个量级。组合导航系统IMU和工业相机2安装于两个天线的中点位置附近，二者质心位置应尽量集中，且要求较高的安装精度，以保证基准坐标系与相机坐标系的方向余弦矩阵准确。工业相机采集标定六面体工装的合作目标图像，确定标定对象在相机坐标系中的位姿，结合机械结构确定的基准坐标系与相机坐标系的关系，标定对象IMU与六面体的关系，完成由地球坐标系到载体坐标系的基准传递。

结合图2说明本实施方式，基准坐标系定义O_BX_BY_BZ_B指向基准IMU的三个敏感轴方向，它与当地地理坐标系NED的关系通过滚转角，俯仰角和航向角三个欧拉角表示(φ,θ,ψ)^T，当地地理坐标系三个坐标轴分别指向北向，东向和地向。获得基准坐标坐标系与当地地理坐标系的方向余弦矩阵为，

结合图3说明本实施方式，图3为相机坐标系定义O_CX_CY_CZ_C。O_C为光心，O_CX_C和O_CY_C分别平行于成像平面的两条边，O_CZ_C为深度方向。结合图1、图2和图3可确定相机坐标系与基准坐标系的转换方向余弦矩阵：

图4为载体坐标系。假设IMU/地磁传感器测量模块的三个敏感轴方向一致，载体坐标系O_bX_bY_bZ_b指向标定对象中IMU的三个敏感轴方向，标定对象安装于标定六面体工装中，因此可通过六面体定义载体坐标系。

结合图5，图5为增强现实合作目标的目标坐标系O_TX_TY_TZ_T，增强现实合作目标通过内部二维正方形表征唯一的ID，且可以表示矢量方向，因此增强现实合作目标能够定义唯一的坐标系。所述的增强现实合作目标为AprilTagTag.36h11系列中ID为16的合作目标，将此合作目标定义的坐标系表示成六面体的其它五个平面分别采用不同ID的图形，表示成这样能够确定六面体工装每一个平面与载体坐标系的关系由图5和图6可知，目标坐标系与载体坐标系的方向余弦矩阵为：

所述的增强现实合作目标的图形可以为其他形式，例如，ARTag或QR Code等。除此之外，标定板必须严格保证为正方形，并且边长须精密测量。标定装置的高分辨率工业相机，通过对六面体工装上增强现实合作目标的图像进行预处理，阈值处理，边缘检测，图像分割，四边形提取，识别出唯一确定ID的增强现实图案，根据目标、图像和焦距三者之间的关系计算标定对象中IMU/地磁传感器相对于基准坐标系的位置和姿态。

结合图6说明本实施方式，标定处理系统用于采集基准组合导航系统输出的位置和姿态角；采集高分率工业相机输出的增强现实合作目标的图像，并计算合作目标相对于相机坐标系的位姿；采集被测IMU输出的比力和角速度以及地磁传感器输出的地磁场强度；电源根据各用电单元的电压供电；显示器用于与用户交互并提示标定进程；存储单元记录存储结果。

所述的标定处理系统可为DSP，ARM等嵌入式计算机，也可为工控机或PC机。各采样接口根据选择器件的实际接口，对标定处理系统进行电路接口设计或选择对应的工业采集卡。

具体实施方式二、结合图7说明本实施方式，本实施方式为具体实施方式一所述的惯性测量单元和地磁传感器整体标定装置的标定方法，其具体标定过程如下：

一、将基准导航系统与标定六面体放置在同一水平面之上，为保证测量精度，尽量减小相机与六面体间的距离；

二、建立被测对象传感器的标定模型，

加速度计的模型为

其中，为标定后比力；为加速度计原始输出的比力；b_a为加速度计零偏，w_a表示加速度计噪声水平；K_a为加速度计刻度因数与安装耦合系数矩阵。

陀螺的标定模型为

其中，为标定后陀螺角速度；为陀螺原始输出的角速度；b_g为陀螺零偏，w_g表示陀螺噪声水平；K_g为陀螺刻度因数与安装耦合系数矩阵。

地磁传感器的标定模型为

其中，为标定后地磁场强度，根据当地地理位置查表计算求取；为地磁传感器原始输出的地磁场强度；b_h为当前条件下地磁传感器偏置，ω_h表示地磁传感器噪声水平；K_h为地磁传感器刻度因数与安装耦合系数矩阵。

三、以任意姿态放置标定对象的六面体工装，但必须保证至少一个平面的合作目标完整的出现在工业相机的视场中；

四、采集基准组合导航系统输出的当地地理位置，记作(L₀ λ₀ h₀)，根据WMM(World Magnetic Model)能够计算当地磁场强度和当地地理坐标系三轴磁场强度分量，记作分别表示北向，东向和地向的磁场强度。采集高分辨率工业相机测量的六面体工装上第i个平面合作目标的姿态角，记为其中i表示工装六个面中的第i个平面，并计算相机坐标系的方向余弦矩阵计算载体坐标系相对当地地理坐标系的方向余弦矩阵；

因此，载体坐标系三轴传感器的标称值为

五、计算载体坐标系中标定对象的比力和角速度的理论标称值。在当地地理坐标系中，加速度计输出的比力fⁿ和陀螺输出的加速度的理论标称值分别为

fⁿ＝(0 0 1)^T

ωⁿ＝(0 0 7.292115×10^-5)^T

其中，fⁿ无量纲，ωⁿ的单位为rad/s。因此，根据当地地理坐标系与载体坐标系的转换关系则可确定载体坐标系内加速度计和陀螺的输出的理论标称值，记作和

六、采集标定对象中加速度计原始输出的比力陀螺原始输出的角速度和地磁传感器原始输出的地磁场强度的测量值；

七、将标定对象的标称值和测量值带入步骤二中的标定模型，即具体如下：加速度计标定方程：

陀螺标定方程：

地磁传感器标定方程：

上述方程共有九个方程，36个未知数，因此至少需要四组测量值才能求解；

八、为了使获得较高的标定精度，至少保证六面体工装的每一个面至少有三个姿态角位置被工业相机采样。这样至少有18组量测值，充分保证标定参数的精度。根据最小二乘法，求解各传感器标定参数。

加速度计标定方程可以写成：

陀螺标定方程可以写成：

地磁传感器标定方程可以写成：

其中，j∈[1,N]，N表示对特定的增强现实合作目标的角位置变换测量的次数。令

根据最小二乘法分别计算加速度计、陀螺和地磁场传感器标定系数矩阵的估计值，

加速度计、陀螺和地磁场传感器噪声的协方差矩阵分别为：

本实施方式实现了惯性测量单元与地磁传感器整体快速标定，以光学手段实现标定对象与基准装置的基准传递，避免了地磁传感器与标定设备直接接触引起的电磁效应，本发明操作简单，无须专业实验室。