基于捷联惯导的悬臂式掘进机位姿解算算法研究

郑伟雄，沈 阳，马浚清，胡立同，符世琛，吴 淼

(1.中国矿业大学(北京) 机电与信息工程学院，北京 100083；2.枣庄矿业(集团)付村煤业有限公司，山东 济宁 277605)

随着我国煤炭开采的不断深入，煤炭开采的深度和难度越来越大，煤炭生产安全形势日益严峻，实现无人采掘技术、智能采掘技术有重要意义，而掘进机位姿的自动检测又是实现采掘无人化的基础[1]。目前已经有许多掘进机位姿检测的技术，如基于激光指向器、全站仪、iGPS、UWB的检测技术等，这些检测方法在某些方面仍有不足[2，3]。激光指向器法只能达到定向功能，不能精确检测掘进机的位姿；全站仪在大量粉尘并且照明差的井下环境中，测量结果可能不准确；iGPS技术由于技术限制了测量距离只能短距离测量；基于UWB的位姿检测技术在Z轴上的误差较大，不能满足使用要求。

捷联惯导技术是一种不受外界干扰、结构简单、测量范围广、精度高的导航技术，并能够实现实时测量、自主测量。捷联惯导技术的位姿解算算法决定着计算的复杂性和解算结果的精度，而捷联惯导系统的位姿解算算法包括姿态和位置解算算法两个部分。国内外研究学者对位姿解算算法进行了深入的推导和研究，1971年Bortz[4]提出了等效旋转矢量法，之后，Robin[5]对等效旋转矢量法的单子样、双子样和三子样算法进行了分析比较，对该领域做出了巨大的贡献。吴简彤等从计算精度上对欧拉角法、四元数法、方向余弦法和等效旋转矢量法进行了分析比较，四元数法和等效旋转矢量法精度更高且姿态更新周期较小，优于其它两种算法[6]。陶云飞[7]分析了基于捷联惯导的掘进机位姿检测的可行性，但并未进行深入研究。

本文对四元数法和等效旋转矢量法进行了分析和对比，在正弦运动和模拟掘进机行走两种情况下进行了仿真，并搭建了模拟实验平台，利用小车循迹代替掘进机掘进进行了实验。

1 捷联惯导位姿检测原理和解算算法

1.1 捷联惯导位姿检测原理

捷联惯导系统需要计算机定时获取惯性传感器的加速度和角速度信息，姿态解算算法根据陀螺仪测出的离散角速度信息解算出姿态角，位置解算算法根据加速度传感器测得的离散加速度信息解算出位置信息。陀螺仪测得的原始数据中包含地球自转角速度，加速度计测得的加速度包含载体当地的重力加速度，捷联惯性导航的自校准就是根据这两者进行的。位姿解算算法的作用一是将测得的离散运动信息进行平滑的、更接近实际的积分来获得位姿信息，二是将由于惯性传感器的离散性导致的不可测得的运动信息通过增加积分项进行补偿。

捷联惯导算法的简要流程如图1所示。

图1 捷联惯导算法简要解算流程

1.2 捷联惯导姿态解算算法

坐标系定义如下：

1)悬臂式掘进机坐标系(简称B系-ObXbYbZb)：与悬臂式掘进机固连，原点在悬臂式掘进机的中心，Xb指向悬臂式掘进机的右侧，Yb指向悬臂式掘进机的前进方向，Zb指向悬臂式掘进机的竖轴方向。

2)导航坐标系(简称N系-OnXnYnZn)：根据惯导系统的实际工作需要选取定义的坐标系，用来作为掘进机导航的基准。在接下来的仿真解算中直接选用地理坐标系作为此坐标系，两个坐标系之间没有角度差。

3)地理坐标系(简称T系-OtXtYtZt)：是在悬臂式掘进机上用来表示掘进机在东北天三个方向位置的坐标系。原点Ot选在悬臂式掘进机重心处，东向为Xt轴，北向为Yt轴，Zt沿重力方向指向天。

4)地球坐标系(简称E系-OeXeYeZe)：此坐标系Oe在地球中心点处，它Ze轴轴沿地轴北极方向，与I 系之间存在一个地球自转角速率ωe，Xe轴指向本初子午线(0°经线)，Ye轴垂直于XeOeZe平面指向东经 90°，固定在地球上。

5)地心惯性坐标系(简称I系-OiXiYiZi)：此坐标系的原点Oi同样在地球中心点处，Zi轴重合于Ze轴，而Xi轴由原点指向春分点，与惯性空间相固连。

要解算的位置和姿态角即掘进机坐标系与地理坐标系之间的关系。

1.2.1 四元数法

四元数由一个实数部分和三个虚数部分组成，它将载体的三维姿态表示为载体绕固定点的旋转，四元数的形式为：

Q=q0+q1i+q2j+q3k

(1)

如果一个四元数Q1中的实数q0为零，则称作纯四元数，它可以用来表示向量，掘进机的姿态可以由一个三维向量表示。如果另一个四元数Q2中实数q0不为0，则可以来表示绕一个等效轴的旋转。如果Q1中的向量转换Q2中的角度，则转换之后的向量可以用四元数表示为：

姿态矩阵可以用四元数表示为：

姿态矩阵由姿态角表示为：

式中γ、θ、φ分别为悬臂式掘进机的滚转角、俯仰角和航向角。利用式(3)和式(4)中对应元素相等，可得姿态角与四元数的关系如下：

θ=asin(2q0q1+2q2q3)

(5)

四元数更新微分方程如下：

式中，t为时间，T更为姿态角更新周期，不难看出四元数法的求解过程较为简单，计算量较小，所以适合对时间要求苛刻的场合。

1.2.2 等效旋转矢量法

旋转矢量的微分方程近似为：

本文中对旋转矢量法的解算采用三子样算法，公式为：

姿态变化的四元数为：

姿态四元数的更新为：

Q(t+T)=Q(t)⊗q(T)

(13)

1.3 捷联惯导位置解算算法

由于目前基于捷联惯导的位置解算算法尚未发展成熟，位置解算误差随时间发散较快，因此本次只选取了改进的特定力积分算法[8]，对误差发散速度进行仿真和实验。

位置解算需要求出悬臂式掘进机在导航坐标系的x、y、z三个方向的值，捷联惯导解算位置需要先解算出速度。当T=tm-tm-1时，捷联惯导系统的速度更新方程为：

上式右边的第三项是重力速度增量，它很小并且在较小的时间周期内变化缓慢，可以用线性插值法计算。上式右边第二项为比力积分项，Vn的精度取决于此项：

利用特定力积分法，上式可以近似为：

其中：

式中，ΔVrotm是旋转运动的补偿项，ΔVsculm是划船运动误差的补偿项。划船运动误差是刚体同时承受同一频率的线振动和角振动产生的一种校正误差。

在计算得到速度之后，则可以开始位置的更新。位移可以用两时刻速度的平均值近似表示为：

2 仿真与比较

2.1 姿态解算算法误差分析

由于目前单基于捷联惯导的位置解算误差较大而且发散较快，与井下掘进机位姿检测的要求相差甚远，所以这里不再分析位置的解算误差。捷联惯导的解算算法误差是指数值求解姿态矩阵和比力方程时产生的原理性误差，它包含不可交换性误差、量化误差、舍入误差、积分误差、正交化误差和漂移误差[6]。在这些误差中，正交性误差可以通过正交化来很好的解决，计算误差、漂移误差、量化误差和舍入误差在高性能计算机上对结果影响较小，因此对结果影响较大的主要有截断误差和不可交换性误差。

等效旋转矢量算法把刚体在一段时间内的转动等效为绕某个定轴矢量的旋转，对不可交换性误差抑制的效果较好，因此对测量物体的震动有较强的抗干扰性，但对运动信息的测量速度有较高要求，采样频率在很大程度上决定着这种算法的解算精度，增加了此算法的计算量，导致解算速度和精度不能兼得。等效旋转矢量三子样算法的截断误差是运动角速度与更新周期乘积wT的七阶，是姿态角φ的二阶[8]，而四元数法的截断误差是w的五阶，与姿态角无太大关系[9]，从这一点来看四元数法优于等效旋转矢量法，并且四元数法只需要求解四个方程，计算量小，计算精度较高，微分方程的解算可以应用数值微分解法，变成简单的方程，比如四阶龙格库塔算法[10-13]。四元数法不足的是因解算过程是直接对角速度矢量进行积分，因此不可交换性误差较大。

2.2 姿态角解算算法仿真

对姿态角的解算算法在有干扰正弦运动情况下进行仿真比较，并对位置的解算算法在模拟掘进机行走时的误差发散情况进行仿真[14，15]。

设定姿态角的运动函数为：

φ=φ0sinw0t

θ=θ0sinw1t

γ=γ0sinw2t

(18)

式中，φ、θ、γ分别为悬臂式掘进机的航向角、俯仰角和滚转角；φ0、θ0、γ0分别为悬臂式掘进机三个姿态角的运动幅度，都设为5°，w0、w1、w2为三个姿态角正弦运动的周期，分别设为2π/100、3π/100、4π/100，采样周期设为T=0.01s。

设白噪声e1、e2、e3随均值为0，方差为0.01的正态分布，将白噪声加入模拟得到的测量数据之中，此时可得陀螺仪的实时输出函数为：

对误差求解，四元数法姿态角误差如图2所示，等效旋转矢量法姿态角误差如图3所示。

图2 四元数法姿态角误差

图3 等效旋转矢量法姿态角误差

从仿真结果中可以看出两种算法解算精度都在0.1°以内，小于国家煤矿井巷工程验收标准中的7.13°，两种算法均能满足要求。等效旋转矢量法的误差与运动状态有较大关系，四元数法的误差没有明显的规律。四元数法的误差略小于等效旋转矢量法，所以认为四元数法略好一些。

2.3 位置解算算法仿真

本次位置解算算法仿真主要是通过模拟掘进机行走的路线，将模拟得到的数据进行位姿解算。这样不仅可以排除测量元件误差的影响，还可以验证算法的准确性和优越性。在进行位置解算时，需要实时参考掘进机的姿态矩阵，以便将方向变化之后的加速度计测得的数据进行坐标变换，在此对姿态角的求解算法采用四元数法中的四阶龙格塔库法。

掘进机工作时的基本运动形式有静止、加速、匀速、转弯、减速、上坡下坡等。模拟的掘进机行走轨迹如图4和图5所示。

图4 模拟的位置变化轨迹

图5 模拟的姿态角变化轨迹

轨迹模拟中的中的横滚角未发生变化。对姿态角运动函数求一阶导数得角速度函数，可作为陀螺仪的输入数据；对位置运动函数求求二阶导数可得加速度函数，但加速度计测得的为比力，与实际的求导得到加速度含义不同，根据比力与角速度、速度和加速度三者之间的关系：

由上式可得加速度计的理想测量数据即比力信息。

姿态角和位置的求解误差分别如图6、图7所示。

图6 姿态角解算误差

图7 位置解算误差

从仿真中可以看出，姿态角误差在0.5°以内，小于国家煤矿井巷工程验收标准中的7.13°，满足要求。从图中还可以看出姿态角运动的幅度越大，解算误差越大，所以当运动角度较大时，四元数法的误差与运动形势也有一定关系。位置误差较大且呈发散状态，400s时z轴误差达到20.0586m，远远不能满足国家煤矿井巷工程验收标准中的250mm的要求[10]，需其他装置辅助校准。

3 捷联惯导位姿检测模拟实验

本章通过搭建模拟实验台，利用小车代替掘进机行走，通过小车上的单片机和惯性传感器将运动数据传送到上位机进行位姿解算，验证选用的算法的可行性。

3.1 利用小车代替掘进机模拟实验设计

由于条件限制，利用小车模拟悬臂式掘进机，规划出一条比较符合实际的路线，用小车的循迹功能代替掘进机的掘进行走，利用小车上的惯性传感器测量运动数据，设置传感器的采样频率为100Hz。数据传送到计算机并用MATLAB进行解算。将解算得到的位姿轨迹与实际的轨迹相比较，计算出误差。

规划的轨迹如图8所示。

图8 小车轨迹路线图(mm)

小车的运动为三维运动，上图所示轨迹中，左边第一个坐标系为XOY水平面直角坐标系，即从O1到O2，所有的运动都为水平面上的运动；第二个坐标系为竖直面直角坐标系，即O2之后为爬坡运动。

本次实验选择的惯性传感器的是MPU6050，它可以同时测量角速度和加速度，小巧轻便，单片机选择Stm32f407GT，利用单片机定时器的定时功能，每10ms产生一次中断，在中断程序里读取惯性传感器的测量结果。

3.2 传感器数据采集

将集成有惯性测量元件的开发板安装到小车上部，利用小车的循迹功能产生测量元件的运动数据，将开发板通过数据线与上位机相连，波特率设置为500kHz，单片机通过定时中断程序采集惯性原件的测量数据并发送到上位机。实验总体布置如图9所示。

图9 实验总体布置

照片为俯视拍得，小车上面固定有开发板，开发板与计算机有一条白色的数据线连接。

3.3 实验数据解算

姿态角解算结果如图10所示，位置解算结果如图11所示。

图10 姿态角解算

图11 位置解算

由解算结果可以看出，姿态角的解算存在较大的波动，这大部分是寻迹小车循迹过程中的车身波动导致的；位置解算结果呈发散状态。对姿态角和位置计算误差分别如图12、图13所示。

图12 姿态角解算误差

图13 位置解算误差

横滚角和俯仰角的误差都在5°以内，满足要求。航向角误差存在较大的波动，但大致趋与0°误差线，误差波动可归因于小车循迹过程中前进方向的调整导致的航向角方向的波动。而且由于选择的惯性传感器本身误差较大(高精度的惯性传感器成本非常高昂，由于实验室条件限制，本文采用了低成本的惯性传感器)，是模拟实验误差的一个重要原因。位置误差呈发散状态，16s时y轴位置误差达到了18m，远远不能满足国家煤矿井巷工程验收标准中的250mm的要求[9]，且发散速度较快。

4 结 论

1)说明了应用捷联惯导对悬臂式掘进机进行位姿解算的优点，不受外界环境干扰、结构简单，并能够实现实时测量、自主测量。

2)通过仿真对比，表明两种解算方法解算精度都在0.1°以内，四元数解算方法中的姿态角解算误差小于0.05°，略好于等效旋转矢量法，小于国家煤矿井巷工程验收标准中的7.13°，适合进行位姿解算。位置误差较大且呈发散状态，400s时z轴误差达到20.06m，且在模拟实验中y轴位置误差在16s时达到了18m，远远不能满足国家煤矿井巷工程验收标准中的250mm的要求，且发散速度较快，需其他装置辅助校准。

3)根据实验结果姿态角的解算误差小于5°，小于国家煤矿井巷工程验收标准中的7.13°，满足要求，验证了选用的姿态角解算算法的可行性和有效性。