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

doi 10. 11799/ ce202009035 收稿日期 2019-09-06 基金项目 国家自然科学基金面上资助项目51874308; 国家自然基金重点支持项目U1610251; 中国博士后科学基金 2019M660860 作者简介 郑伟雄1996, 男, 河北保定人, 博士研究生, 研究方向为煤矿装备智能化, E-mail zwx3337 163. com。 引用格式 郑伟雄, 沈 阳, 马浚清, 等. 基于捷联惯导的悬臂式掘进机位姿解算算法研究 [J]. 煤炭工程, 2020, 52 9 170-176. 基于捷联惯导的悬臂式掘进机位姿解算算法研究 郑伟雄1, 沈 阳1, 马浚清2, 胡立同1, 符世琛1, 吴 淼1 1. 中国矿业大学北京 机电与信息工程学院, 北京 100083; 2. 枣庄矿业集团付村煤业有限公司, 山东 济宁 277605 摘 要 针对煤矿井下高煤尘、 光线暗的恶劣环境中掘进机无法实现动态位姿检测的问题, 提 出了基于捷联惯导的悬臂式掘进机位姿检测方法并对其解算算法做了对比研究。 捷联惯导是通过三 轴陀螺仪和三轴加速度计实时测得掘进机的角速度和加速度, 根据位姿解算算法动态输出掘进机的 姿态和位置信息。 位姿解算算法很大程度决定着姿态和位置的精度, 因此结合掘进机实际掘进工艺 的特点, 围绕位姿解算算法的适用性和优劣性, 对比了四元数和等效旋转矢量两种位姿解算算法, 分析了姿态解算误差的影响因素。 在有噪声正弦运动和模拟掘进机行走运动两种情况下进行仿真并 搭建了利用小车循迹代替掘进机掘进的实验平台进行实验, 结果表明, 两种算法均能满足掘进机姿 态角的解算精度要求, 四元数法的姿态解算精度高于等效旋转矢量法; 位置的解算误差较大且呈发 散状态, 这有待于进一步研究。 关键词 捷联惯导; 掘进机位姿; 四元数法; 等效旋转矢量 中图分类号 TD421. 5 文献标识码 A 文章编号 1671-0959202009-0170-07 Research on position and pose calculation algorithm of boom-type roadheader based on strap-down inertial navigation ZHENG Wei-xiong1, SHEN Yang1, MA Jun-qing2, HU Li-tong1, FU Shi-chen1, WU Miao1 1. School of Mechanical Electronic and Ination Engineering, China University of Mining and TechnologyBeijing, Beijing 100083, China; 2. Zaozhuang Mining Group Fucun Coal Industry Co. , Ltd. , Jining 277605, China Abstract As it is hard to obtain accurate position and posture ination of boom-type roadheader in the high coal dust and dark environment of underground coal mines, a using SINS strap-down inertial navigation was proposed and its calculation algorithm were compared. SINS measured the angular velocity and acceleration of the roadheader in real time by triaxial gyroscope and triaxial accelerometer, and dynamically output the attitude and position ination of the roadheader based on the solution algorithm. The solution algorithm determines the accuracy of the attitude and position to a great extent. Therefore, we compared the algorithms of quaternion and equivalent rotation vector in the applicability and advantages, and analyzed the influencing factors of the pose solving error combining with the characteristics of the actual driving process of the roadheader. The simulation was carried out under the sinusoidal motion with noise condition and roadheader walking condition, and the experimental plat was set up using a mini-car track to simulate the tunneling trajectory. The results show that both algorithms can meet the accuracy requirements of the attitude angle of the roadheader, and quaternion algorithm is better than the equivalent rotation vector algorithm, but the position error is large and divergent which needs further study. Keywords strap-down inertial navigation; heading machine position; quaternion ; equivalent rotation vector 随着我国煤炭开采的不断深入, 煤炭开采的深 度和难度越来越大, 煤炭生产安全形势日益严峻, 实现无人采掘技术、 智能采掘技术有重要意义, 而 掘进机位姿的自动检测又是实现采掘无人化的基 071 第52卷第9期 煤 炭 工 程 COAL ENGINEERING Vol. 52, No. 9 万方数据 陀螺仪 悬臂式掘进机位姿初始值 悬臂式掘进机 姿态矩阵3 JL 卜A E r 十 姿态四元数更新H 姿态角计算 加速度传感器卜l 比力变换 卜l 速度位置计算 悬臂式掘进机 初始位置和速度 显示 悬臂 式掘 进机 次太 文儿尘 角和 位置 信息 础[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 系-OYeZe 此坐标 系 Oe在地球中心点处, 它 Ze轴轴沿地轴北极方向, 与 I 系之间存在一个地球自转角速率 ωe, Xe轴指向 本初子午线0经线, Ye轴垂直于 XeOeZe平面指 向东经 90, 固定在地球上。 5 地心惯性坐标系简称 I 系-OiXiYiZi 此 坐标系的原点 Oi同样在地球中心点处, Zi轴重合于 Ze轴, 而 Xi轴由原点指向春分点, 与惯性空间相 固连。 要解算的位置和姿态角即掘进机坐标系与地理 坐标系之间的关系。 1. 2. 1 四元数法 四元数由一个实数部分和三个虚数部分组成, 它将载体的三维姿态表示为载体绕固定点的旋转, 四元数的形式为 Q q0 q 1i q2j q3k 1 如果一个四元数 Q1中的实数 q0为零, 则称作 纯四元数, 它可以用来表示向量, 掘进机的姿态可 171 2020 年第 9 期 煤 炭 工 程 装备技术 万方数据 以由一个三维向量表示。 如果另一个四元数 Q2中实 数 q0不为 0, 则可以来表示绕一个等效轴的旋转。 如果 Q1中的向量转换 Q2中的角度, 则转换之后的 向量可以用四元数表示为 Q3 Q 1Q2Q -1 1 2 姿态矩阵可以用四元数表示为 T T11T12T13 T21T22T23 T31T32T33 q02 q 1 2 - q 2 2 - q 3 2 2q1q2- 2q0q32q0q2 2q1q3 2q0q3 2q1q2q02 - q 1 2 q 2 2 - q 3 2 2q2q3- 2q0q1 2q1q3- 2q0q22q0q1 2q2q3q02 - q 1 2 - q 2 2 q 3 2 3 姿态矩阵由姿态角表示为 T cosγcosφ - sinγsinθsinφcosθsinφcosφsinγ - cosγsinθsinφ cosφsinγsinθ - cosγsinφcosθcosφ- sinγsinφ - cosγcosφsinθ - cosθsinγsinθ cosγcosθ 4 式中 γ、 θ、 φ 分别为悬臂式掘进机的滚转角、 俯仰角和航向角。 利用式3和式4中对应元素相 等, 可得姿态角与四元数的关系如下 θ asin 2q0q1 2q2q35 γ atan 2q0q2- 2q1q3 q02 - q 1 2 - q 2 2 q 3 2 ′ 6 α - atan 2q0q3- 2q1q2 q02 - q 1 2 q 2 2 - q 3 2 7 四元数更新微分方程如下 Q 1 2 Mwb nbQ 8 Mwb nb为角速度构成的矩阵, 对式8微分方 程进行求解便可得到姿态四元数的更新, 进而求出 掘进机的姿态角。 式8微分方程可以由四阶龙格 库塔法进行求解, 计算公式如下 Qt T更 Qt 1 6 K1 2K2 2K3 K 49 K1 T更 2 M[wbt][Qt] K2 T更 2 M[wbt T更 2 ][Qt K1 2 ] K3 T更 2 M[wbt T更 2 ][Qt K2 2 ] K4 T更 2 M[wbt T更][Qt K3] 式中, t 为时间, T更为姿态角更新周期, 不难 看出四元数法的求解过程较为简单, 计算量较小, 所以适合对时间要求苛刻的场合。 1. 2. 2 等效旋转矢量法 旋转矢量法是另一种进行四元数更新的方法, 没有奇点和不确定性, 旋转矢量和四元数都能表示 一次转动, 即将一个更新周期的旋转表示为绕某一 瞬时轴转过一定的角度一次到达更新时刻之后的位 置, 因此他们两个之间可以相互转换。 如果用 α 和 n → 分别表示转过的角度和瞬时轴的单位矢量, 则这 一次的旋转可以表示为 φαn →, φ 简称为旋转矢量。 与四元数法不同的是, 旋转矢量法先计算姿态变化 的四元数, 然后再进行四元数更新[8], 它将旋转向 量解和更新四元数完全分开考虑。 旋转矢量的微分方程近似为 φ ⇀ w ⇀ 1 2 φ ⇀ w ⇀ 1 12φ ⇀ φ ⇀ w ⇀ 10 本文中对旋转矢量法的解算采用三子样算法, 公式为 φ ⇀ Δθ 9 20Δθ1 Δθ2 27 40Δθ2 Δθ3- Δθ1 11 其中, Δθ1、 Δθ2、 Δθ3分别为 t 1 3 T、 2 3 T、 T 时刻处陀螺的输出, ΔθΔθ1 Δθ 2 Δθ 3。 姿态变化的四元数为 qT cos φ 2 φ φ sin φ 2 12 姿态四元数的更新为 Qt T Qt 􀱌 qT13 1. 3 捷联惯导位置解算算法 由于目前基于捷联惯导的位置解算算法尚未发 展成熟, 位置解算误差随时间发散较快, 因此本次 只选取了改进的特定力积分算法[8], 对误差发散速 度进行仿真和实验。 位置解算需要求出悬臂式掘进机在导航坐标系 的 x、 y、 z 三个方向的值, 捷联惯导解算位置需要 271 装备技术 煤 炭 工 程 2020 年第 9 期 万方数据 先解算出速度。 当 Ttm -t m-1时, 捷联惯导系统的速 度更新方程为 vn m v n m-1 c nm-1bm-1 ∫ tm tm-1 cbm-1btfbtdt ∫ tm tm-1 [gn- 2wn ie w n en v n]dt 14 其中, vn m是在 m 时刻掘进机相对于地理坐标系 的速度在导航坐标系下的投影, fbt为载体坐标系 下的比力, gn为重力加速度在导航坐标系 n 下的投 影, 这三者都是三维列向量。 cnm-1bm-1为从掘进机 坐标系与地理坐标系之间的转换矩阵。 cbm-1bt为在 载体坐标系下从 t 时刻到 m-1 时刻的比力变换矩阵。 上式右边的第三项是重力速度增量, 它很小并 且在较小的时间周期内变化缓慢, 可以用线性插值 法计算。 上式右边第二项为比力积分项, Vn的精度 取决于此项 Δvb sfm ∫ tm tm-1 cbm-1btfbtdt15 利用特定力积分法, 上式可以近似为 Δvb sfm ΔVm ΔVsculm 1 2 Δθm ΔVm ΔVsculm 16 其中 ΔVrotm 1 2 Δθm ΔVm ΔVsculm 1 2 ∫ tm tm-1 [Δθt fbt ΔVt wbt]dt Δθt ∫ tm tm-1 wbtdt ΔVt ∫ tm tm-1 fbtdt 式中, ΔVrotm是旋转运动的补偿项, ΔVsculm是划 船运动误差的补偿项。 划船运动误差是刚体同时承 受同一频率的线振动和角振动产生的一种校正误差。 在计算得到速度之后, 则可以开始位置的更新。 位移可以用两时刻速度的平均值近似表示为 ΔRn m ΔT 2 Vn m V n m-1 17 2 仿真与比较 2. 1 姿态解算算法误差分析 由于目前单基于捷联惯导的位置解算误差较大而 且发散较快, 与井下掘进机位姿检测的要求相差甚 远, 所以这里不再分析位置的解算误差。 捷联惯导的 解算算法误差是指数值求解姿态矩阵和比力方程时产 生的原理性误差, 它包含不可交换性误差、 量化误 差、 舍入误差、 积分误差、 正交化误差和漂移误 差[6]。 在这些误差中, 正交性误差可以通过正交化来 很好的解决, 计算误差、 漂移误差、 量化误差和舍入 误差在高性能计算机上对结果影响较小, 因此对结果 影响较大的主要有截断误差和不可交换性误差。 等效旋转矢量算法把刚体在一段时间内的转动等 效为绕某个定轴矢量的旋转, 对不可交换性误差抑制 的效果较好, 因此对测量物体的震动有较强的抗干扰 性, 但对运动信息的测量速度有较高要求, 采样频率 在很大程度上决定着这种算法的解算精度, 增加了此 算法的计算量, 导致解算速度和精度不能兼得。 等效 旋转矢量三子样算法的截断误差是运动角速度与更新 周期乘积 wT 的七阶, 是姿态角 φ 的二阶[8], 而四元 数法的截断误差是 w 的五阶, 与姿态角无太大关 系[9], 从这一点来看四元数法优于等效旋转矢量法, 并且四元数法只需要求解四个方程, 计算量小, 计算 精度较高, 微分方程的解算可以应用数值微分解法, 变成简单的方程, 比如四阶龙格库塔算法[10-13]。 四元 数法不足的是因解算过程是直接对角速度矢量进行积 分, 因此不可交换性误差较大。 2. 2 姿态角解算算法仿真 对姿态角的解算算法在有干扰正弦运动情况下 进行仿真比较, 并对位置的解算算法在模拟掘进机 行走时的误差发散情况进行仿真[14,15]。 设定姿态角的运动函数为 φ φ0sinw0t θ θ0sinw1t γ γ0sinw2t18 式中, φ、 θ、 γ 分别为悬臂式掘进机的航向角、 俯仰角和滚转角; φ0、 θ0、 γ0分别为悬臂式掘进机 三个姿态角的运动幅度, 都设为 5, w0、 w1、 w2为 三个姿态角正弦运动的周期, 分别设为 2π/100、 3π/100、 4π/100, 采样周期设为 T0. 01s。 设白噪声 e1、 e2、 e3随均值为 0, 方差为 0. 01 的正态分布, 将白噪声加入模拟得到的测量数据之 中, 此时可得陀螺仪的实时输出函数为 wb tb - sinγcosθcosθ0 sinθ01 cosγsinγ0 φ θ γ e1 e2 e3 19 371 2020 年第 9 期 煤 炭 工 程 装备技术 万方数据 O ．0 5 0 0 ．0 5 仿真时间／s C＼梢蝼媸晕警C＼榭蝼册避靼C＼榭蝼册匠蟮 对误差求解, 四元数法姿态角误差如图 2 所示, 等效旋转矢量法姿态角误差如图 3 所示。 图 2 四元数法姿态角误差 图 3 等效旋转矢量法姿态角误差 从仿真结果中可以看出两种算法解算精度都在 0. 1以内, 小于国家煤矿井巷工程验收标准中的 7. 13, 两种算法均能满足要求。 等效旋转矢量法的 误差与运动状态有较大关系, 四元数法的误差没有 明显的规律。 四元数法的误差略小于等效旋转矢量 法, 所以认为四元数法略好一些。 2. 3 位置解算算法仿真 本次位置解算算法仿真主要是通过模拟掘进机 行走的路线, 将模拟得到的数据进行位姿解算。 这 样不仅可以排除测量元件误差的影响, 还可以验证 算法的准确性和优越性。 在进行位置解算时, 需要 实时参考掘进机的姿态矩阵, 以便将方向变化之后 的加速度计测得的数据进行坐标变换, 在此对姿态 角的求解算法采用四元数法中的四阶龙格塔库法。 掘进机工作时的基本运动形式有静止、 加速、 匀速、 转弯、 减速、 上坡下坡等。 模拟的掘进机行 走轨迹如图 4 和图 5 所示。 轨迹模拟中的中的横滚角未发生变化。 对姿态 图 4 模拟的位置变化轨迹 图 5 模拟的姿态角变化轨迹 角运动函数求一阶导数得角速度函数, 可作为陀螺 仪的输入数据; 对位置运动函数求求二阶导数可得 加速度函数, 但加速度计测得的为比力, 与实际的 求导得到加速度含义不同, 根据比力与角速度、 速 度和加速度三者之间的关系 f ⇀ dv ⇀ dt w ⇀ep v⇀ 2w →ie v→ - g → 11 由上式可得加速度计的理想测量数据即比力 信息。 姿态角和位置的求解误差分别如图 6、 图 7 所示。 图 6 姿态角解算误差 从仿真中可以看出, 姿态角误差在 0. 5以内, 471 装备技术 煤 炭 工 程 2020 年第 9 期 万方数据 仿真时间／s Ⅲ／椭蝼删迥暴吲Ⅲ／椭蝼删迥暴A III＼榭蝼删迥暴N 图 7 位置解算误差 小于国家煤矿井巷工程验收标准中的 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 姿态角解算 由解算结果可以看出, 姿态角的解算存在较大 的波动, 这大部分是寻迹小车循迹过程中的车身波 动导致的; 位置解算结果呈发散状态。 对姿态角和 位置计算误差分别如图 12、 图 13 所示。 横滚角和俯仰角的误差都在 5以内, 满足要求。 航向角误差存在较大的波动, 但大致趋与 0误差线, 误差波动可归因于小车循迹过程中前进方向的调整 导致的航向角方向的波动。 而且由于选择的惯性传 感器本身误差较大高精度的惯性传感器成本非常高 昂, 由于实验室条件限制, 本文采用了低成本的惯 571 2020 年第 9 期 煤 炭 工 程 装备技术 万方数据 仿真时间／s Ⅲ／榭蝼删迥暴吲Ⅲ／椭蝼删迥暴A III＼榭蝼删迥暴N 图 11 位置解算 图 12 姿态角解算误差 图 13 位置解算误差 性传感器, 是模拟实验误差的一个重要原因。 位置 误差呈发散状态, 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, 满足要 求, 验证了选用的姿态角解算算法的可行性和有 效性。 参考文献 [1] 赵开功, 李彦平.我国煤炭资源安全现状分析及发展研究 [J]. 煤炭工程, 2018, 5010 185-189. [2] 陈慎金, 成 龙, 王鹏江, 等. 基于激光测量技术的掘进机 航向角精度研究 [J]. 煤炭工程, 2018, 507 107-110. [3] 陈慎金, 成 龙, 王鹏江, 等. 基于掘进机位姿测量系统的 自主标定方法误差分析 [J].煤炭学报, 2018, 439 2647-2652. [4] Bortz, J. E. A New Mathematical ulation for Strapdown In- ertial Navigation [J].IEEE Transactions on Aerospace and E- lectronic Systems, 1971, AES-71 61-66. [5] RobinB.Mi1ler.A New Strapdown Attitude Algorithm [ J]. Journal of Guidanee. 1983, 64 287-291. [6] 吴简彤, 马文国, 王利存, 等. 船舶捷联式惯导系统中的姿 态矩阵算法研究 [J]. 导航, 19971 89-93. [7] 陶云飞, 杨健健, 李嘉赓, 等. 基于惯性导航技术的掘进机 位姿测量系统研究 [J]. 煤炭技术, 2017, 361 235-237. [8] Yurong Lin, Wei Zhang, Jianqiong Xiong.Specific force inte- gration algorithm with high accuracy for strapdown inertial naviga- tion system [ J].Aerospace Science and Technology, 2015, 42. 25-30. [9] 李荣华, 刘 播. 微分方程数值解法 [M].北京 高等教 育出版社, 2009. [10] GB 502132010, 煤矿井巷工程质量验收规范 [S]. [11] THRYBOM L, NEANDER J, HANSEN E, et al.Future chal- lenges of positioning in underground mines [J].IFAC-Papers On Line, 2015, 4810 222-226. [12] 邓国华 基于激光导向器的悬臂式掘进机位置姿态自动测定 方法 [J]. 工矿自动化, 20099 45-48. [13] 田 原.基于机器视觉的掘进机空间位姿检测技术研究 [J]. 矿山机械, 2013, 412 27-30. [14] DU Yuxin, TONG Minming, LIU Ting, et al.Visual measure- ment system for roadheaders pose detection in mines [J].Opti- cal Engineering, 2016, 5510 104-107. [15] 吴 淼, 贾文浩, 华 伟, 等. 基于空间交汇测量技术的悬 臂式掘进机位姿自主测量方法 [J].煤炭学报, 2015, 40 11 2596-2602.责任编辑 赵巧芝 671 装备技术 煤 炭 工 程 2020 年第 9 期 万方数据