一种鲁棒的矿井接近探测定位算法研究.pdf

第44卷 第“期 2018年6月 工矿自动化 Industry and Mine Automation Vol. 44 No. 6 Jun. 2018 ; 分析研究言 文章编号671-251X201806-0011-05 DOI 10. 13272/j. issn. 1671-251x. 17321 一 神 鲁 棒 的 矿 并 接 近 探 测 定 位 算 法 研 究 陈康1， 包建军1， 王伟1 1.中煤科工集团常州研究院有限公司， 江 苏 常 州 213015 2.Tiandi Changzhou Automation Co.， Ltd.， Changzhou 213015, China Abstract In view of problems of existing positioning algorithms for mine proximity detection that ranging error is large due to non-line-〇 --sight and other factors， and solution of ranging equation set does not converge under complex conditions， a robust positioning algorithm for mine proximity detection was proposed， namely weighted LM algorithm， which combines Gauss-Newton algorithm， weighted least square algorithm and Levenberg-Marquardt algorithm. The algorithm adds ranging error ination to nonlinear iterative solution by adding weight， and adds damping coefficient in iterative process， which greatly improves positioning stability and robustness under precondition of ensuring convergence speed of iteration. Test results show that the weighted LM algorithm has high positioning efficiency and accuracy. Key words mine accurate positioning； proximity detection； weighted LM algorithm； Gauss-Newton algorithm ； nonlinear least square algorithm ； weighted least square algorithm ； Levenberg- M / rqu/ rdt/lgorithm 收稿日期 2018-03-12;修回日期 2018-04-25;责任编辑胡娴。 基金项目中国煤炭科工集团青年项目（2018QN028;天地（ 常州） 自动化股份有限公司技术研究项目（2018GY105。 作者筒介陈康（1987 ， 男， 江苏常州人， 助理研究员， 硕士， 主要研究方向为井下精确定位技术，E-mailono_backyeah. net。 引用格式陈康，包建军，王伟.一种鲁棒的矿井接近探测定位算法研究工矿自动化，2018，446 1-15. CHEN KangBAO Jian.jun WANG Wei. Research on a robust positioning algorithm for mine proximity detection[J]. Industry and Mine Automation，018，46 11-15. -12 -工 矿 自 动 化2 0 1 8 年 第 4 4 卷 〇 引言〇 引言 基于到达时间（ T im eofA m valJO A 的精确 定位方法具有方便部署、 定位解算计算量小等优势， 被广泛用于井下精确定位中。文献[1]提出一种基 于对称双边双向到达时间（ Symmetric-Double-Sides Two-WayTime-of-Arrival， SDS-TW-TOA的无线 测距方法， 并用于煤矿井下接近探测中。该方法结 合各传感器的位置坐标及测距结果得到测距方程 组 ， 求解测距方程组得到待定标签坐标。但其先将 非线性定位方程平方后相减， 再利用最小二乘法来 求解， 得到的标签坐标会随测距误差的变化而抖动， 定位 定 。 煤矿井下接近探测定位 定位 有 以下难点[ 2 A]①传感器固定在矿车上， 矿车作业会 影响测距精度及稳定性。②矿车多为钢铁外壳， 对 电磁波的透射能力很差， 测量标签到某些传感器的 距离时非视距（ N onLineofSight， NLOS误差严 重 。③矿车形状复杂， 矿井内地形复杂且会随时间 变化， 使 得 NLOS误 差 存 在 不 确 定 性 ， 难 以 消 除 。 最常用的测距方程组直接求解法是Caffery提出的 相交线模型[ 4 ]， 该算法运算速度快， 适用于受限环 境 ， 但 当 测 距 存 在 NLOS误 差 时 ， 定 位 误 差 较 大 。 两边定位法[;]可 以 减 小 NLOS误 差 ， 但当所取的 两测距圆不相交时或准确测距的传感器小于一半 时， 定位误差较大。最小中值法[6’ 8c]有很好的定位 准确性及鲁棒性， 但计算量较大， 同时也需要多于一 半的传感器测距准确， 否则定位误差也较大。 在复杂环境中， 用基于泰勒展开的高斯牛顿法 求解非线性方程组通常有更好的精度。本文在高斯 牛顿法的基础上， 结合加权法求解非线性测距方程 组 ， 使 用 Levenberg-Marquardt法 （ 以 下 简 称 LM 法解决能准确测距的传感器很少时存在的迭代不 收敛问题， 使测距方程组的迭代收敛过程更快、 更稳 定 ， 鲁棒性更强。 1高斯牛顿法1高斯牛顿法 高斯牛顿法是一种利用泰勒展开并使用迭代求 解非线性方程组的方法， 这种迭代求解法具有二阶 收敛性[10]。 1.1 非线性方程组 设 待 定 位 标 签 到 第 个 定 位 传 感 器 的 距 离 为 4 ，二维 平 面 中 各 定 位 传 感 器 的 位 置 坐 标 为 T,， 3〇 ,待定位标签的位置坐标为（ 心 ,定位传感 器数量为 ， 则测距方程组可表示为 槡 X -- X02 3-- 3〇 2 “0 槡 X --X12 3-- 312 “ 1 ⑴ V X -- Xn 3-- 32“ 式 1 写成 函 数 式 ，即 / P 槡x Xi 3 3i2p “X， 3， 则方程组中每个 方程可写为/ , 0 4 。可以看出， /,4为非线性 函数， 要 求 解 心 应 当 求 解 由 /,4构 成 的 非 线 性方程组。 1.2 方程组求解 将 /々） 在 化 处 泰 勒 展 开 ， 忽略二阶及二阶以 上展开项[10 11]， 得 /, W / , 九） f /;九） ％ 心其 中 / * , / ， *, / /]， 为雅可 比坐标变换矩阵J *,的一行， 每次迭代中的待求 未知量 ，y 3。 力（ *在 处 一 阶 泰 勒 展 开 后 ， 测距方程组可 重新表示为 J pkP s 2 式 中 为残差向量， d / * , ， d 为测得的距离 向。 井下定位过程中传感器通常都有冗余， 因此， 本 文只讨论测距方程组为超定方程的情况。若测距方 程组为超定方程， 则 ％ p 可用最小二乘法求解， 即 P T i / s 。求得 ％ p 后 ， 用 ％ p 更新 pk1， Pk 1 Pk P ， 并 将 Pk1作为下次迭代计算的泰勒 展开点， 通过迭代逐步逼近标签实际位置。 1. 3 迭代初值选取 若标签为第1 次初始化， 可以用直接求解法快 速求得准确度较低的坐标， 并将其作为迭代初值;若 标签已初始化， 则可用上次计算得到的坐标作为本 次迭代计算的初值。当距离向量的采样间隔较小 时， 这种初值选取方法可以提高迭代效率， 加快迭代 收敛 。 1.4 迭代终止条件 迭代终止条件 ①残差范数S 小于残差最小值 S1， 即 | k || S 1当 ||上升时不断增 大 A， 直到残差下降时再逐步减小A。 4测试分析4测试分析 4. 1 测 试 1 在采煤机上部署个测距传感器， 测量各自到 标签的距离， 然后计算标签坐标。计算参数设置 S1100， s2 4 ， 々 m a x 15， A 初 值 为 0 3。使用加权 LM 法计算标签坐标， 结果如图1 所示， 其带数字的 黑点处为测距传感器在采煤机上的位置， 圆形的半 径为对应传感器测距， 采煤机下方的小圆点为算得 14 工 矿 自 动 化2 0 1 8 年 第 4 4 卷 的标签坐标。标签的实际位置与标签计算位置一 致 。从 图 1 可看出， 1 号传感器测距值比真实距离 要大很多， 同 时 1 号 标签的直线连接需要 穿过矿车车身， 可以认为这是由于机车遮挡造成 NLOS所 的。0 号 、 2 号 、 3 号传感器的测距结 果与传感器到标签的真实距 ， 可以认为 是 没 有 受 到 NLOS影响的正常测距值。从测试结 果可以看出， 使用加权LM 法可以有效减弱1 号传 感器测量数据的影响， 同 时 对 2 号 及 3 号传感器测 距结 加强， 使计算结果更精确。 图1采煤机与传感器的位置关系 Fig. 1 Positional relationship between shearer and sensors 分 析 标 签 连 续 运 动 时 ， 取 同 一 标 签 连 续 1 “ 7 6 次传感器测距向量作为样本， 用不同的定位 算法 标签的位置坐标， 结 果 见 表 1。均方根的 计算公式为*ef||2*e 为标签的真实 坐标，n 1 676;计 算 总 时 间 指 在 I5- 6200U 平台 上 ， 单 标 签 1 676次定位计算的用时。最小中值法 的样本选取总数为C3， 即选取4 个样本。 表1不同定位算法计算结果 Table 1 Calculation results of different positioning algorithms 定位算法方 算 时 间 /ms 直接最小二乘法70. 3 11 最小中值法 11. 2 82 非线性高斯牛顿法 57. 8 17 加权最小二乘法16. 120 加 权 LM法10. 4 20 从 表 1 可以看出， 最小中值法与加权L M 法有 着相近的高计算精度， 加 权 L M 法比最小中值法计 算效率高3 倍以上。直接最小二乘法虽然计算速度 ， 但是精度较差。非线性高斯牛顿法、 加权最小 二乘法及加权LM 法虽然计算复杂度提升， 计算用 时上升， 但计算精度却有大幅提高。单独使用加权 最小二乘法求得解的稳定性不如加权L M 法好， 求 得的坐标 收敛的 ， 使其均方根 加权 LM 法 。 用 加 权 L M 法计算标签坐标并将求得的坐标 依次首尾相连， 得到标签运动轨迹， 如 图 2 所示， 标 签运动轨迹平滑， 与真实轨迹一致。 图2定位效果及标签运动轨迹 Fig. 2 Positioning effect and label movement trajectory 4.2 测 试 2 大型采煤机上需要安装6 个测距传感器， 由于 设 ， 受 NLOS影响的 数量更多。使用 加 权 LM 法计算标签坐标（ 参数设置与测试1 中相 同），结果如图3 所示，图 中 0 号、1 号、3 号传感器受 到了 N LO S的影响， 测距精度很低。从 图 3 可 知 ， 加 权 LM 法通过权值的选择有效减小了 N LO S的 标 的 算 标 实 标 。 Fig. 3 Positioning effect with 6 sensors 用 表 1 中的5 种算法分别计算标签坐标， 并记 录计算坐标与真实坐标的距离误差， 计算结果见 表 2， 其中距离误差的计算式为||*-*ef ||2。 表 2 同 定 位 算 法 的 Table 2 Error of different positioning algorithms 定位算法 /cm 直接最小二乘法- 最小中值法31.2 非线性高斯牛顿法 - 加 权 法5.0 加 权 LM法 4.1 直接最小二乘法与非线性高斯牛顿法的误差很 2 0 1 8 年 第 6 期陈 康 等 一 种 鲁 棒 的 矿 井 接 近 探 测 定 位 算 法 研 究-15 大 ， 其解不能作为正确的定位结果使用； 最小中值法 由于正确测量的传感器数量小于测距传感器总数的 一半， 所以也没有取得较好的精度； 加权最小二乘法 及 加 权 LM 法由于选取的权值恰当， 计算精度都比 较高。 5结语5结语 将高斯牛顿法、 加权最小■乘法及L M 法相结 合 ， 介绍了一种鲁棒的矿井接近探测定位算法 加 权 L M 法 。该 算 法 有 较 强 的 鲁 棒 性 ， 在具有 NLOS误差的复杂场景， 甚 至 在只有2 个传感器进 行准确测距的情况下， 都能得到较准确的标签坐标。 同时该算法的时间及空间复杂度可满足在单片机上 运行的条件。测试结果表明， 加 权 LM 法的定位效 率和精度较高。在实际工作中， 即使测距过程中某 些传感器工作异常， 不能返回测距结果， 该算法也能 正常运行。 参 考 文 献 （References 1 * ALARIFIA， ALSALMAN A M ， ALSALEH M， et al. Ultra wideband indoor positioning technologies analysis and recent advances [J*. Sensors， 2016, 1651-36. [2 * WYLIE M P， HOLTZMAN Thenon-line-ofsight problem in mobile location estimation [ C*// IEEE International Conference on Universal Personal Communications 9 96. [3 *包建军.煤矿井下装备接近探测方法研究[ *.工矿自 动 化 ，2017,4311-4. BAO Jianjun. Research on proximity detection for machines in coal mine [ J *. Industry and Mine Automation,2017,431 1-4. [4 * CAFFERY J J. A new approach to the geometry of TOA location [ C *//Vehicular Technology Conference Boston 20008 [5 * CHENG G. Accurate TOA-based UWB localization system in coal mine based on WSN [ J *. Physics Procedia，2012,2416 534-540. [6 * LI X，HUA B，SHANG Y，et al. Arobust localization algorithm in wireless sensor networks[J*. Frontiers of Computer Science in China,2008,24 438-450. [7 * NORRDINE A. An algebraic solution to the multilateration problem[C*//International Conference on Indoor Positioning and Indoor Navigation,2012. [8 * LI Z， TRAPPE W ， ZHANG Y， et al. Robust statistical s for securing wireless localization in sensor networks[C*//IEEE International Symposium on Ination Processing in Sensor Networks, 2005. [9 * ROUSSEEUW P J. Least median of squares regression[J*. Publications of the American Statistical Association, 1984,79388 871-880. [10* MADSEN U， NIELSEN H B， TINGLEFF O. s for nonlinear least squares problems [ J*. Society for Industrial k Applied Mathematics，2004, 201211409-1415. [ 1 1 *王 磊 ， 李 鹏 涛 ， 贾 宗 璞.基于全质心-Taylor的UWB 室 内 定 位 算 法[ J * .传 感 器 与 微 系 统 ，2017,36 6 146-1498 WANG Lei, LI Pengtao, JIA Zongpu. UWB indoor localization algorithm based on full centroid and Taylor [ J *. Transducer and M1crosystem Technologies,2017,366 146-149. [ 2 *李春华， 蔡 成 林 ， 邓 克 群， 等.一种北斗伪距单点定位 的加权最小二乘（WLS快速算法[J*.重庆邮电大学 学报（ 自然科学版） ，2014,264 466-472. LI Chunhua， CAI Chenglin， DENG Kequn， et al. A fast pseudo-range single point positioning algorithm for the Beidou navigation system based on weighted least squares [J*. Journal of Chongqing University of Posts and TelecommunicationsCNatural Science Edition，2014，264 466-472. [ 3 *潘旦光， 高莉莉， 李小翠.求解Rayleigh阻尼系数的加 权 法 [J * 8 算 学 学 报 2015 32 1 70-768 PAN Danguang , GAO Lili , LI Xiaocui. Weighted least squares for solving Rayleigh damping coefficients [ J *. Chinese Journal of Computational Mechanics，2015，321 70-76.