井下无人驾驶列车惯性导航定位系统.pdf

第44卷 第 “ 期 2018年9月 工矿自动化 Industry and Mine Automation Vol. 44 No. 9 Sep. 2018 文章编号671-251X 201809-0005-05 DOI 10. 13272/j. issn. 1671-251x. 2018040022 并 下 无 八 驾 驶 列 车 惯 性 导 航 定 位 系 统 杜京义S 郭金宝S 张渤2 1.西安科技大学电气与控制工程学院，陝 西 西 安 710054 2. College of Communication and Ination Engineering, Xian University of Science and Technology, Xian 710054, China Abstract Current underground driverless train positioning technology calculates the train displacement based on optical sensor installed on the wheels. When the underground driverless train skids on the wet track, the positioning error will be generated. In view of the above problem, an inertial navigation and positioning system for underground driverless train was proposed. Inertial navigation module was introduced in the system based on current underground driverless train positioning technology, and double threshold algorithm was used to judge abnormal driving condition of the train combined with photoelectric sensor data and inertial navigation data, and safety factor of driverless train was increased. The inertial navigation module uses LPMS-NAV2 to obtain acceleration and heading angle of target, and the position coordinates of the target is calculated. In view of the problem that acceleration measurement of the target is affected by gravity acceleration, 2-axis acceleration compensation is used to eliminate the error 收稿日期2018-04-04;修回日期2018-07-16;责任编辑 胡娴。 基金项目 国家自然科学基金项目“1703329陕西省教育厅科研计划项目（17JK0504。 作者筒介 杜京义（1965 男 ， 陕西西安人， 教 授 ， 研究方向为复杂系统的辨识、 建 模 、 故 障 诊 断 等 ，E-mail17571853qq. com。通 信 作 者 郭金宝“991 一男 ， 河北张家口人， 硕士研究生， 研究方向为控制理论与控制工程，E-mail711211886qq. com。 引用格式 杜京义， 郭金宝， 张渤.井 下 无 人 驾 驶 列 车 惯 性 导 航 定 位 系 统 工 矿 自 动 化 ，2018,449-9. DU Jingyi, GUO Jinbao, ZHANG Bo. Inertial navigation and positioning system for underground driverless train.[J]. Industry and Mine Automation ,2018,449 5-9. 工 矿 自 动 化2 0 1 8 年 第 4 4 卷 brought by gravity. In view of the problem of cumulative error of positioning of the target, weight feedback constraint algorithm is introduced, and square difference loss function is constructed for weighted constraint of the positioning point, so as to reduce the cumulative error. A position beacon is set at each fork of the underground roadway to per secondary calibration on the positioning ination to further improve the positioning accuracy. The indoor test results show that the average positioning error of the inertial navigation and positioning system for the underground driverless train is 0.52 m. Key words underground driverless; inertial navigation; weight feedback constraint algorithm; double threshold algorithm; -axis acceleration compensation ; beacon calibration 〇 引言 为了提高铁矿井下作业效率及保障人员安全， 井 人驾 车实现矿 [12]。当前 井 人驾驶定位技术主要 发射器绘制车 3D 地形图， 在车轮上安装 传感器来 测 速 ， 算列车的位移， 结合自动电控匝 道 当 车 方向， 实现对无人驾 车 位置 [35]。 井 轨道 ， 列车 轨道上时经常发生 ， 此时根 据 传感器推算的位移会产生 ） ]。因此， 本 文 有井 人驾驶定位技术 上 ， 添加惯性 导航模块， 结合光电传感器数据和惯导数据来识别 列车启动 、 制动 状态， 提 人驾 丨 车 安全 数 。 惯 导航 模块 曼滤波（ Extended KalmanFilter，EKF算法[ 7 9]，存 累 ， 不适合长时间的参数统计。所以， 本文 传 E K F 融合算法 上 ， 引入权值反馈约束 算法， 避免 化导致的累 [10]。 时 ， 在 井下巷道 口设置位置信标， 对定位信息进 校准， 提高定位 [1112]。 1井下无人驾驶系统 井 人驾 车 惯 导航 定位 井 人驾 分 。井下无人驾 整体结构 如 图 1 所示。其中， 惯性导航定位 包括惯性导 航模块、PNP 传感器模块、数据 模块、 无线 模块和主控 器 ， 实现对列车 速度、 角度、 姿 态 数 集和解算， 并 车当 丨 运 行状态。后台控制 主要根据惯性导航 集 数据实现位置信息显示、 路况决策 及任务分 ， 从而实现对井下列车 及任务分配。 0惯性导航定位系统设计 2.1 系统硬件设计 惯性导航模块 LPMS- NAV2 航向传感 图1井 下 无 人 驾 驶 系 统 整 体 结 构 Fig. 1 Whole structure of underground driverless system 器 ， 其内 仪和三轴加速 ， 是一款基于微机电系统（ Micro-Electro-Mechanical Sy stem， MEMS传感器的低成本小型航向传感器。 LPMS- NAV2 通过数据流模式实现与主控 器 的通信， 将三轴加速度、 三轴角速度及航向角信息以 设定好的频率“00 Hz发送到主控服务器。 主 控 服 务 器 采 用 基 于 A R M 架 构 的 32 bit Cortex- M3 内核的处理芯片。为了识别列车启动 、 制 动 状态 ， 惯 导航 定位 中 PNP 传 器 ， 结 合 车 速 惯 导 数 据 ， 用双阈值算法实现对列车行驶状态的准确判断。 2.2 系统软件设计 惯 导 航 定 位 包 括 1 中 、 2 个定时器中断和2 个 U A R T 通信串口。首先初 始化各个模块并配置相关参数， 通 过 2 个定时器检 测 号的频率、 幅值， 实现对车 速 数;通过串口 UART2 实现主控服 务 器 与 LPMS- NAV2 的数据通信； 利 用 UART1 将计算合成的位 置信息通过井下监控 发 台主控 器 ， 实现惯性导航模块与主控服务器的双向 。 通信流程如图2 所示。 针对轨道有雨水时列车易发生打滑或抱死现象 的问题， 采用双阈值算法判断列车行驶异常状况， 双 阈值算法流程如图3 所示。 3惯性导航定位技术实现 惯性导航定位系统利用目标载体的加速度数 2 0 1 8 年 第 9 期杜 京 义 等 井 下 无 人 驾 驶 列 车 惯 性 导 航 定 位 系 统 图2惯性导航定位系统通信流程 Fig. 2 Communication flow of inertial navigation and positioning system 图3双阈值算法流程 Fig. 3 Flow of double threshold algorithm 据 ， 通过积分计算出目标载体的速度、 位移， 再结合 惯性导航传感器的航向角求出目标载体的位置坐 标 ； 在此基础上， 通过权值反馈约束算法来提高系统 的测量精度， 同时利用井下位置信标（ 间 隔 100 m 进行二次校准， 减小累积误差。 3. 1 惯性导航定位 在实际运动过程中， 用惯性导航模块测量加速 度时受到重力加速度的影响， 本 文 采 用 z 轴加速度 补偿的方法来消除重力加速度引入的误差。惯性导 航模块加速度分布如图所示。目标载体在初始原 点 S。 ， 当 Cx 0. 5 且小于上一时 刻 Cx值时， W x増 大 1 个步进。同理可得y 轴的权 值 Wy。经过反复训练， 直至将损失函数值均控制 在 0. 5 以内， 输出当前权值作为测试轨道上的最优 工 矿 自 动 化2 0 1 8 年 第 4 4 卷 权值参数。通常 井 下 巷 道 有 多 条 机 车 运 行 轨 道 ， 每条运行轨道都应该使用不同的权值参数来约束惯 性导航模块输出的坐标值。 利用最优权值来修正目标载体的坐标值 为 系 统 融 合 函 数 ; 为 正 则 项 常 数 。 3.3 信标二次校准 利用已知坐标的信标， 对系统引入权值后得到 的坐标值进行二次校准。无人驾驶系统控制终端根 据信标发送过来的实际位置坐标， 以不超过500 ms 的延迟通过地下专网发送至惯性导航定位系统， 并 将该位置坐标作为当前时刻的起始坐标值， 实现 二次校准。 4测试分析 在带有减震功能的全向移动实验车上进行了系 统性能测试， 测试环境如图5 所示。测试过程中实 验车以不同的速度进行直线运动、 S 型运动及任意 角度运动， 每次测试总位移约为500 m， 记 录 1 0 组 测试数据。 图5惯性导 航 定 位系统测 试环境 Fig. 5 Test environment of inertial navigation and positioning system 采集实验车的加速度、 角速度、 航向角度、 脉冲 数等惯性原始数据， 分 别 用 E K F 融合算法、 权值反 馈约束算法、 权值反馈约束十信标二次校准算法得 到其运动轨迹， 如图 所示。 由图 可看出， 传 统 E K F 融合算法因具有累积 误差， 位置坐标推算不稳定。权值反馈约束算法的 误 差 比 传 统 E K F 融 合 算 法 有 所 减 小 。添加信标 二次校准后， 实验车的轨迹基本接近实际运动轨迹， 在旋转、 加速等波动性比较大的情况下， 最大误差也 不超过088 m。对 1 0 组数据进行统计分析， 计算 采用不同算法时的最大误差及平均误差， 结果见 表 1 。 图6实 验 车 运 动 轨 迹 Fig. 6 Trajectory of experimental vehicle 表1不 同 算 法 定 位 误 差 Table 1 Positioning error of different algorithms 算法大 平 传 统EKF融合算法42. 3616.04 权值反馈约束算法5. 183.21 权值反馈约束十信标 二次校准算法 0. 980.52 由表 1 可知， 权值反馈修正算法有效抑制了惯 性导航模块自身的累积误差， 添加信标二次校准后， 系统平均误差减小到0. 52 m， 满足井下巷道无人驾 驶列车的定位精度要求。 3结论 1 井 人驾 车惯 导航定位 LPMS- NAV2 获取目标载体的加速度、 航向角， 根 据航向角计算目标载体的位置坐标， 采 用 z 轴加速 度补偿方法来消除重力加速度引入的误差。 “ 结合光电传感器数据和惯导数据， 采用双 阈值算法判断列车行驶异常状况， 提高了无人驾驶 列车的安全系数。 3通过权值反馈约束算法对定位信息进行修 正 ， 以避免定位累积误差。通过信标二次校准进一 步提高定位精度。 “ 该系统充分利用了当前地理信息、 运动载 体固有信息、 异常情况辅助信息， 实现了对井下目标 载体位置的准确估算。室内测试结果表明， 井下无 人驾 车惯 导航定位 平 定位 2 0 1 8 年 第 9 期杜 京 义 等 井 下 无 人 驾 驶 列 车 惯 性 导 航 定 位 系 统 0.52 m，满足井下巷道无人驾驶列车的定位精度 要求。 参考文献（ References 1 *端 木 庆 玲 ， 阮 界 望 ， 马 钧.无 人 驾 驶 汽 车 的 先 进 技 术 与 发 展[J].农 业 装 备 与 车 辆 工 程，2014,52330-33. DUANMU Qingling， RUAN Jiewang， MA Jun. Development and advanced technology of driverless car [ J ]. Agricultural Equipment j Vehicle Engineering, 2014,523 30-33. [2 ]李 论.基 于R SSI的 煤 矿 巷 道 高 精 度 定 位 算 法 研 究 [D ].徐 州 中 国 矿 业 大 学 ，2015. [ 3 ] 乔 钢 柱 ， 曾 建 潮 .信 标 节 点 链 式 部 署 的 井 下 无 线 传 感 器 网 络 定 位 算 法[ J ] .煤 炭 学 报 ，2010, 3 5 “ 1229-12338 QIAO Gangzhu， ZENG Jianchao. Localization algorithm of beacon nodes chain deployment based on coal mine underground wireless sensor networks [J]. Journal of China Coal Society， 2010， 35 7 1229-1233. [ 4 ] 裴 忠 民 ， 邓 志 东 ， 巫 天 华 ， 等.矿 井 无 线 传 感 器 网 络 三 阶 段 定 位 方 法[ J ] .中 国 矿 业 大 学 学 报 ，2010,39 “ 87-92. PEI Zhongmin, DENG Zhidong,WU Tianhua,et al. A novel three-phase localization in coal mine wireless sensor networks [ J ]. Journal of China University of Mining j Technology, 2010,39 1 87-92. [5 ]孙 继 平 ， 王 帅 . 基 于 信 号 强 度 的 改 进 型 井 下 测 距 算 法 的 研 究[J ].煤 炭 学 报 ，2013,38112072-2076. SUN Jiping, WANG Shuai. Study on the improved ranging algorithm based on signal strength in the mine[J]. Journal of China Coal Society, 2013,3811 2072-2076. [ 6 ]吴 静 然 ， 崔 冉 ， 赵 志 凯 ， 等.矿 井 人 员 融 合 定 位 系 统 [J ].工 矿 自 动 化 ，2018，444 74-79. WU Jingran, CUI Ran, ZHAO Zhikai, et al. Mine personnel fusion location system [J]. Industry and Mine Automation,2018,444 74-79. [ 7 ] 王 铨.基 于M EM S惯 性 传 感 器 的 井 下 人 员 定 位 系 统 [D]. 中 国 矿 大 学2017. [ 8 ] 尚 俊 娜 ， 刘 参 ， 施 浒 立 ， 等.IM U辅 助 的 室 内 单 星 定 位 方 法[J ].中 国 惯 性 技 术 学 报 ，2017，255 636-642. SH A N G Junna，L IU C an ,S H IH u li，etal.Indoor single-star positioning based on IMU assisting [J]. Journal of Chinese Inertial Technology，2017， 255636-642. [9 ]原 野.室 内W iFi指 纹 定 位 系 统 中 的 半 监 督 训 练 方 法 研 究[D ].上 海 上 海 交 通 大 学 ，2015. [10] YANG Z , LIU Y. Quality of trilateration confidence- based iterative localization[J]. IEEE Transactions on Parallel j Distributed Systems , 2009,21631-640. [ 1 ]李 博 文 ， 姚 丹 亚.低 成 本 车 载M EM S惯 导 导 航 定 位 方 法[J ].中 国 惯 性 技 术 学 报 ，2014，226719-723. LI Bowen， YAO Danya. Low-cost MEMS IMU navigation positioning for land vehicle [J]. Journal of Chinese Inertial Technology , 2014,226 719-7238 [12] ZHONG X ， RABIEE R ， YAN Y ， et l A particle filter for vehicle tracking with lane level accuracy under GNSS-denied environments [C ]//IE E E 20th International Conference on Intelligent Transportation Systems, 2017. [ 1 3 ] 路 永 乐 ， 刘 洪 志 ， 郭 俊 启 ， 等.基 于 腰 部MEMS-IMU 的 室 内 行 人 导 航 航 向 反 馈 修 正 算 法[ J ] .中 国 惯 性 技 术 学 报 ，2017，256725-730. LU Yongle，LIU Hongzhi，GUOJunqi，et al. Heading feedback correction algorithm for indoor pedestrian navigation based on waist MEMS-IMU[J]. Journal of Chinese Inertial Technology , 2017,256 725-730. [ 4 ]邓 子 豪.无 人 驾 驶 智 能 车 导 航 系 统 的 研 究 与 实 现 [D ].西 安 西 安 工 业 大 学 ，2014.