矿用智能传感器自主定位研究

柏思忠

（1.中煤科工集团重庆研究院有限公司，重庆 400039；2.瓦斯灾害监控与应急技术国家重点实验室，重庆 400039）

随着工业4.0、工业互联网、人工智能系统、物联网等相关技术的发展，对于各类传感器的应用需求愈发增大，例如我国提出的《中国制造2025））和德国政府推出的《Hightech-Strategie 2020 für Deutschland》都将智能传感器系统（Smart Sensors System）列入战略发展领域[1]。GB/T 34069—2017《物联网总体技术 智能传感器特性与分类》[2]明确了智能传感器定义和特性，其中智能传感器的物联网特性要求能自动向外部发出信息描述自己的位置，在物联网中能自动识别自身在网络中的位置。GB/T 33900—2017《工业物联网仪表应用属性协议》[3]规定工业物联网仪表位置属性是指工业物联网仪表的安装位置，是经纬度确定的绝对位置、海拔高度以及相对位置信息。

矿用智能传感器工作在井下巷道，无法接收地面卫星信号，位置信息必须依靠室内定位技术。张胜利等[4]在室内定位方法和技术综述中阐述了WiFi、地磁、惯导、Zigbee、蓝牙、RFID、红外、超声波、UWB等主流室内定位技术的优缺点；胡青松等[5]在矿井动目标定位：挑战、现状与趋势中重点从原理、方法和算法上阐述了WiFi、Zigbee、蓝牙、RFID、红外、超声波、UWB 等主流无线定位技术需要外围预装设备配合才能实现定位，预装设备增加了成本和不确定性；另外采用地磁数据库比对方式的地磁定位易受设备布局和环境因素变化影响；最后单一惯导存在累积误差的影响，随着时间和距离的增加会增大。因此，现有的单一定位技术无法满足智能传感器定位的要求。为此，针矿用智能传感器地面和井下定位的问题，采用地磁辅助惯导/北斗（BDS）的组合定位模式[6]，全部国产化芯片方案，研制出低功耗的自主定位模块，并将自主定位模块带载到现有高低体积分数甲烷传感器实现传感器自主定位功能。

1 地磁辅助惯导/北斗组合定位

1.1 地磁辅助的惯导/北斗组合定位原理

地磁辅助惯导/北斗组合定位原理框图如图1。

图1 组合定位原理框图Fig.1 Block diagram of combined positioning principle

组合定位在地面以北斗定位为主，没有卫星信号的地方惯导[7]辅助定位，煤矿井下以惯导为主，地磁辅助定位[8]。惯导系统是利用MEMS 惯性测量单元（MEMS IMU）测量载体的加速度，并在给定运动初始条件下，由MCU 算出传感器的速度、距离和相对位置，由陀螺仪测量传感器的角速率，并经转换、处理，输出传感器的姿态。由于惯性器件存在着累计误差，解算出来的姿态信息并不准确，所以利用地磁对其进行辅助，在特定地点进行标校消除系统累计误差[9]。

1.2 惯导姿态更新算法

根据惯性器件的输出实时计算出传感器的姿态矩阵，姿态计算采用四元数法[10-11]，根据一段时间（k时刻到k+1 时刻）内的角度变化量来解算出最新时刻的四元数，如式（1）：

式中：Q 为姿态四元数，由1 个实部 和3 个虚部构成，Q＝q0＋q1i q1＋q2i q2＋q3i q3；tk为k 时刻；tk+1为（k＋1）时刻；ωx为陀螺仪x 轴角速率；ωy为陀螺仪y轴角速率；ωz为陀螺仪z 轴角速率；t 为时间。

对于固定输出频率的陀螺仪而言，计算精度受到陀螺仪输出频率的影响，输出频率越高，计算精度越高，考虑实际硬件计算能力和需求情况，陀螺仪数据更新频率选取0.01 s。当得知姿态四元数时，根据四元数与姿态角之间的关系计算出姿态角[12]，根据陀螺仪实时输出不断更新姿态。

式中：θ 为俯仰角；γ 为横滚角；ψ 为航向角。

1.3 惯导速度更新算法

取当地地理坐标系（东北天-ENU）作为导航坐标系，在从kT 时刻到（k+1）T 时刻的1 个周期T 内，由东北天方向的速度微分方程[1]可以得到东向、北向和天向速度的更新方程：

式中：VE为东向速度；VN为北向速度；VU为天向速度；fE为东向加速度；fN为北向加速度；fU为天向加速度；ωie为位置速率；L 为经度；RN为卯酉圈的曲率半径；RM为子午圈的曲率半径；H 为高度；g0为重力加速度；T 为更新时间周期。

1.4 惯导位置更新算法

在从kT 时刻到（k+1）T 时刻的1 个周期T 内，传感器的经度L 递推更新方程、纬度B 递推更新方程、高度H 递推更新方程如式（6）～式（8）：

1.5 地磁辅助定位算法

在载体静态或者匀速运动的状态下，单独利用加速度计只能得到俯仰角和横滚角，航向角信息需要通过三轴磁强计辅助定位计算[13]，通过预测矿井定位区域内巷道分叉和转折点的磁场强度作为惯导信息的标定点，分段调整方向余弦阵，消除惯导累计误差。

式中：ψ 为航向角；θ 为俯仰角；M 为本地磁场强度；η 为磁倾角；mx为磁强强度x 轴分量。

1.6 北斗定位算法

2 自主定位模块构成及电路

自主定位模块包括中央微处理器（MCU）、惯性导航、北斗导航、数据存储、数据接口、电平转换、电源转化和外部接口等电路。在地面北斗卫星接收信号正常时以北斗定位为主，没有卫星信号的地方以惯导辅助定位；煤矿井下以惯导为主，地磁辅助定位，以此实现智能传感器地磁辅助惯导/北斗组合定位功能。自主定位模块构成框图如图2，自主定位模块电路图如图3。

图2 自主定位模块构成框图Fig.2 Principle block diagram of autonomous positioning module

图3 自主定位模块电路图Fig.3 Circuit diagram of autonomous positioning module

3 自主定位模块关键技术

整个模块硬件采用低功耗和国产化设计，软件采用了零速修正算法、四元数法、地磁辅助修正算法，实现了智能传感器移动轨迹存储和反演。

3.1 模块硬件低功耗和国产化

MCU 选择超低功耗HC32L130，0.9 μA @3 V深度休眠模式+ RTC 工作；130 μA/MHz@ 3V@24 MHz 工作模式：CPU 和外设运行，从Flash 运行程序；4 μs 超低功耗唤醒时间，使模式切换更加灵活高效。

MEMS IMU 采用9 轴芯片ICM-20948，3 轴加速度、3 轴陀螺仪和3 轴磁强计，1.8 V 供电，仅需2.5 mW，是目前世界上功耗最低的9 轴惯导芯片。

北斗定位采用低功耗一体化模组TAU1103，工作时3.3 V 供电35 mA，待机模式电流仅需14 μA。

数据存储Flash 选用低功耗MX25L128，3.3 V供电最大电流25 mA，休眠状态电流不超过20 μA。

自主定位模块3.3 V 供电时，工作电流小于80 mA，休眠状态小于2 mA；外接USB 采用5 V 供电时，工作电流小于100 mA，休眠状态小于3 mA。

所有芯片选择都遵循国产化原则，全部国产化，避免“卡脖子”问题出现。自主定位模块所有芯片和厂家见表1。

表1 模块芯片生产厂家统计表Table 1 Total table of module chip manufacturers

3.2 关键算法及软件设计

关键算法包括开机零速修正、四元数法移动过程姿态角和位置更新、地磁辅助修正转弯点航向角3 部分。

零速修正是指智能传感器上电开机后，静止一段时间后将速度计算结果清零，达到修正速度误差的目的。为充分利用开机静止检测结果，要求智能传感器在移动前的起始点静止30 s 以上，并且将该点建立为平面内相对移动的坐标原点O（0，0）。

四元数法时陀螺仪输出的3 轴角速度数据经过卡尔曼滤波后代入式（1）不断更新四元数，更新后的四元数代入式（2）得到实时姿态角；惯性导航输出的3 轴加速度数据经过式（3）～式（8）转换为速度更新再到位置更新。

地磁辅助修正是根据实时输出的3 轴地磁数据，当智能传感器在直线运动或有一定弧度的巷道移动时，单位长度内航向角变化不超过阈值（＜15°/m）时，地磁数据不参与导航参数航向角修正，智能传感器移动到矿井定位区域内巷道分叉和转折点等这些地磁标定点时，航向角变化超过阈值，该点的航向角如式（9）以地磁数据为准进行修正，实现地磁辅助定位功能。

结合3 部分算法，智能传感器自主定位软件流程图如图4。

图4 软件流程图Fig.4 Software flow chart

当系统开机时，首先将煤矿井下巷道标定点地磁位置数据下载到存储单元中，再配置惯性导航参数，读取3 轴陀螺仪、3 轴加速度和3 轴地磁数据，完成初始化；然后将传感器上电开机，在起始点静止30s 以上，完成零速修正。实时判断卫星信号有效性，能有效接收北斗卫星信号，直接将北斗定位数据作为当前坐标和位置更新数据；不能有效接收卫星数据时，以惯性导航为主、地磁校准点比对辅助，更新姿态角和位置信息，更新后的姿态角和位置信息一边输出一边存储，单次处理完毕后重复上述主循环操作，实现智能传感器实时更新自主定位信息。

4 试验验证

将自主定位模块搭载到高低体积分数甲烷传感器内部，并备有蓄电池可供持续工作8 h 以上。在地面全长376 m 某环形道路上，将环形道的地图对应弯道处设置标定点A（0，43）、B（－115，46）、C（－116，－24）、D（0，－25）。生成的地磁矩阵数据如式（11），将其下载到传感器。

提着传感器开机后静止30 s，出发移动1 圈，回到起点后静止5 s，测试结果移动轨迹和实际环道吻合程度较高，终点和起点相差约2 m，航向偏差约2°，整个移动距离375.01 m，与参考距离378 m 相差2.99 m，相对误差0.8%。

5 结 语

针对智能传感器物联网特性要求，采用地磁辅助的惯导/北斗组合定位算法，研制了智能传感器自主定位模块。智能传感器搭载自主定位模块可完成移动轨迹存储和反演，在网传感器可实时更新位置、速度和姿态信息，离线传感器可在恢复网络后补传定位信息反演移动轨迹，实现了智能传感器物联网特性要求能自动向外部发出或自动识别自己的位置信息，智能传感器自主定位具有以下4 个方面优点：①依靠自身测量信息进行连续定位；②不需接收外部信息，不受外界干扰；③不向外部辐射能量，具有隐蔽性；④同时确定传感器位置、速度和姿态信息。但在实际应用中，智能传感器还需要解决移动中供电和通信问题，才能进一步实用。