面向掘进机的超宽带位姿协同检测方法.pdf

第4 3 卷第1 0 期 2 0 1 8 年1 0 月 煤炭学报 J O U R N A LO FC H I N AC O A LS O C I E T Y V 0 1 ．4 3 0 c t ． N o ．1 0 2 0 1 8 移动阅读 符世琛，成龙，陈慎金，等．面向掘进机的超宽带位姿协同检测方法[ J ] ．煤炭学报，2 0 1 8 ，4 3 1 0 2 9 1 8 2 9 2 5 ． d o i 1 0 ．1 3 2 2 5 ／j ．c nk i ．j c c s ．2 0 1 7 ．1 7 4 2 F US h i c h e n ，C H E N G 【力n g ，C H E NS h e n j i n ，e ta 1 ．U l t r a w i d e b a n dp o s ec o l l a b o m t i v ed e t e c t i o nm e t h o do fr o a d h e a d e r [ J ] ．J o u m a lo fc h i n ac o a ls o c i e t y ，2 0 1 8 ，4 3 1 0 2 9 1 8 2 9 2 5 ．d o i 1 0 ．1 3 2 2 5 ／j ．c n k i ．j c c s ．2 0 1 7 ．1 7 4 2 面向掘进机的超宽带位姿协同检测方法 符世琛，成龙，陈慎金，王鹏江，宗凯，吴淼 中国矿业大学 北京 机电与信息工程学院，北京1 0 0 0 8 3 摘要为符合安全高效生产对掘进机作业的要求，提出了一种面向掘进机的超宽带位姿协同检测 方法。依据T D O A 波达时间差 定位原理及P 4 4 0 模块的测距信息建立定位模型，利用协同定位算 法得到定位点坐标的最终估计值。对比C h a n 算法与协同算法的定位点仿真分布，并分析协同算法 的三轴定位精度。建立了空间全断面定位误差场，得出协同算法的空间定位精度变化规律。对机 身定位点进行布局设计，解算得到掘进机的姿态角。结果表明在9 0m 范围内，协同算法的l ，轴定 位误差基本上可以保持2m m ，而X 轴、Z 轴定位误差由0 ．4 至4c m 呈线性增加。随测量距离的增 大，在巷道X l ，断面上，定位误差呈线性增加；在巷道X z 断面上，定位误差的随机性增加。姿态 角误差均由o ．2 0 至1 ．5 0 呈线性增加，满足掘进机位姿检测精度的需求。 关键词掘进机；T D O A ；c h a n 算法；泰勒展开；精度分析；位姿检测 中图分类号T D 4 2 1 ．5文献标志码A文章编号0 2 5 3 9 9 9 3 2 0 1 8 1 0 2 9 1 8 0 8 U l t r a ．w i d e b a n dp o s ec o U a b o r a t i v ed e t e c t i o nm e t h o do fr o a d h e a d e r F US h i c h e n ，C H E N CI 由n g ，C H E NS h e n j i n ，W H A N GP e n 舀i a n g ，z O N GK a i ，W UM i a o 肘e c 地n 髓o fE f e c ￡r o n 斑n 蒯』，扣肌n 肋nE 愕i n e e 一昭，吼i 眦U 凡沁舟蚵∥肘i n i n go 凡d2 b 曲加z q g y B e 埘昭 ，曰e 彬增1 0 0 0 8 3 ，C i 眦 A b s t r a c t I no r d e rt om e e tt h er e q u i r e m e n t so fs 出t ya n dh i g he m c i e n c yp m d u c t i o nf o rt h eo p e r a t i o no fr o a d h e a d e r ，a m e t h o do fU W Bp o s ec o U a b o m t i v ed e t e c t i o nw a sp r o p o s e d ．B a s e do nT D O A t i m ed i f k r e n c eo fa n ’i v a l p o s i t i o n i n g p r i n c i p l ea n dP 4 4 0m o d u l e ’sr a n g i n gi n f o r m a t i o n ，t h ep o s i t i o n i n gm o d e lw a se s t a b l i s h e du s i n gt h ec o o p e r a t i V ea l g o r i t h mt oo b t a i nt h e6 n a le s t i m a t ec o o r d i n a t e so ft h ef h s e l a g ep o s i t i o n i n gp o i n t ．T h i sp a p e rc o m p a r e dt h ep o s i t i o n i n g p o i n ts i m u l a t i o nd i s t r i b u t i o no fC h a na l g o r i t h ma n dc o U a b o r a t i v ea l g o r i t h m ，t h e na n a l y s e dt h et h r e ea x i sp o s i t i o n i n g a c c u r a c yo ft h ec o o p e I a t i v ea l g o r i t h ma n do b t a i n e dt h el a wo fv a r i a t i o ni nt h ew a yo fe s t a b l i s h i n gt h es p a t i a l { ．u Ⅱ- s e c t i o n e Ⅱo rf i e l d ．T oo b t a i nt h ep o s ea n g l e s ，t h ea u t h o rc a r r i e do u tl a y o u td e s i g nf o rt h ep o s i t i o n i n gp o i n t ．7 I ’h er e s u l t ss h o w t h a tt h ey - a x i sp o s i t i o n i n ge r m ro ft h ec o o p e r a t i v ea l g o r i t h mc 肌b a s i c a l l ys t a ya t2m m ，w h i l et h eX a x i sa n dZ a x i s a r el i n e a d yi n c r e a s e df 南m0 ．4t o4c mw i t h i n9 0m ．I nt h eX ys e c t i o no ft h er o a d w a y ，t h ep o s i t i o n i n ge n ．o ri n c r e a s e s l i n e a r l ya n di nt h eX Zs e c t i o no ft h er o a d w a y ，t h er a n d o m n e s so fp o s i t i o n i n ge I T o ri n c r e a s e sw i t ht h ei n c r e a s eo f m e a s u I e m e n td i s t a n c e ．T h ee n - o ro ft h ep o s ea n 9 1 e si sl i n e a d yi n c I e a s e df b m0 ．2t o1 ．5d e g r e e s ，w h i c hs a t i s f i e st h e r e q u i r e m e n to ft h ep o s ed e t e c t i o na c c u r a c yo fr o a d h e a d e r ． K e yw o r d s I D a d h e a d e r ；T D O A ；C h a na L l g o r i t h m ；T a y l o re x p a n s i o n ；a c c u r a c ya n a l y s i s ；p o s ed e t e c t i o n 煤炭是我国的主导能源，虽然其在一次能源消费 结构中的比例逐步下降，但据专家预测，到2 0 2 0 年煤 炭在我国能源中比例仍将高达5 8 ％，到2 0 5 0 年的比 例仍可以达到5 0 ％J 。采掘工作面一直是煤矿灾害 收稿日期2 0 1 7 一1 2 1 0修回日期2 0 1 8 一0 4 0 4责任编辑韩晋平 基金项目国家重点基础研究发展计划 9 7 3 资助项目 2 0 1 4 c B 0 4 6 3 0 6 ；国家自然科学基金资助项目 5 1 8 7 4 3 0 8 作者简介符世琛 1 9 9 1 一 ，男，河北邢台人，博士。E m a i l f u s h i c h e n 0 5 2 6 1 6 3 ．c o m 通讯作者成龙 1 9 9 3 一 ，男，辽宁阜新人，硕士研究生。E m a i l c h e n d o n 9 9 3 1 1 1 6 3 ．c o m 万方数据 第1 0 期符世琛等面向掘进机的超宽带位姿协同检测方法 事故的高发区，近年来我国煤矿重大事故中，掘进事 故最多，实现综掘工作面的少人化乃至无人化迫在眉 睫。而检测掘进机的实时位姿对实现综掘工作面的 无人化至关重要。 近年来，有一些学者对位姿检测方法进行研究。 丁雅斌心。提出一种基于单目摄像机拍摄立体靶标序 列图像信息的位姿误差辨识方法。陶云飞旧1 提出了 一种基于i G P s 的掘进机单站多点分时机身位姿测量 方法。杨文辉等Ho 提出了一种组合式位姿测量系 统。何斐彦等一1 提出一种单目视觉与倾角仪组合优 化的位姿测量系统。杜雨馨等∞1 提出了一种基于机 器视觉的掘进机机身监测系统。上述研究对我国煤 矿掘进机位姿检测起着重要的指导作用，但由于矿井 环境复杂恶劣，粉尘很大，不利于红外线、激光及可见 光的传输，若将这些方法应用于掘进机作业过程中， 一定程度上会导致测量精度低，掘进效率不高。 本文基于T D O A 定位原理进行掘进机位姿检测 方法的研究，采用超宽带测距模块获得测距信息，提 出了一种融合c h a n 算法和泰勒级数展开法的掘进机 机身位姿协同检测方法。通过协同算法对掘进机3 个机身定位点进行定位后，将3 个定位点的坐标估计 值代人掘进机空间位姿参数模型进行计算，进而得到 位姿参数值。 1 超宽带位姿检测系统 1 ．1T D O A 定位原理 T D O A 定位。卜9 ‘，即波达时间差定位，又称为测距 差定位或双曲线定位，在无源和有源定位中均有应 用。在一个特定的空间内，保持和两个基准点A ，B 距离差相等的点的轨迹是以A ，B 两点为焦点的双曲 面，称为双曲线位置面。在测量无误差的情况下，如 果得到这个距离差参量，目标一定在由该距离差所确 定的双曲面位置面上。 1 ．2 超宽带定位技术 超宽带的名称来源于可在非常宽的带宽上传输 信号。超宽带的带宽是比中心频率高2 5 ％或者是大 于1 ．5G H z 的带宽。1 0 。1 1J 。超宽带技术能直接调制脉 冲信号，产生带宽高达几兆赫兹的窄脉冲波形，其带 宽远远大于商业无线通信技术所占用的带宽，具有提 高频谱利用率、短距离高速数据通信、对多径衰落具 有鲁棒性等特点。12 I 。 目前，掘进机位姿检测的主流技术是采用陀螺仪 和加速度计的惯性导航。惯性导航技术具有测量精 度高、测量范围广、结构简单、体积小等特点，可实现 实时检测、自主检测等功能。掘进机在工作过程中移 动缓慢、停顿时间长，经常出现侧滑、横摆、甩尾等情 况，使得该技术通常需要外部辅助设备进行阶段性标 定，导致技术复杂，且成本太高，位姿检测误差随测量 时间的不断增加而上升。而超宽带测量技术不但测 量范围广、具有良好的测量实时I 生，且系统组成简单、 没有位姿误差累积，局域范围内具有高精度测距功 能，在掘进机静止或者移动过程中均能提供较高的定 位精度。本系统的观测基站采用美国时域公司的 P 4 4 0 测距模块，带宽3 ．1 ～5 ．3G H z ，中心频率为 4 ．3G H z 。 1 ．3C h a n 定位算法 c h a n 算法4 一引，是针对T D O A 定位模型而提出 的一种视距传播环境下的最大似然估计的近似实现 方法，是一种非迭代的方法。算法计算量小，在噪声 服从高斯分布的环境下，定位精度高。对于平面定位 来说，当定位的有效观测基站数为3 时，它等效为线 交叉定位方法；当进行空间定位时，有效观测基站数 必须大于3 ，它通过引入一个中间变量将非线性方程 变为线性方程，然后利用中问变量与目标位置的确定 关系，并用加权最小二乘法估计出目标的位置。 假定共有Ⅳ个观测基站，距离定位点最近的观 测基站取为主观测基站，可得到Ⅳ一1 个测距差参量， 这Ⅳ一1 个独立的距离差参量均为与主观测基站测得 的距离参量比较的结果6 | 。这里取第1 个观测基站 为主观测基站。定位点的位置坐标为 戈，y ，彳 ，观测 基站的位置坐标分别为 z 。，y 。，盈 ，测量得到的测距 差参量为- ，，而对应的真实距离差为r 。n ，距离差测量 误差为△r i ，。则有 r n r o n △r ∽江2 ，3 ，⋯，Ⅳ 1 通过引入中间变量r ．，可将非线性方程变为线性 方程。中间变量r ，的数学表达式为 r ； z 1 一戈 2 y 1 一y 2 彳1 一彳 2 2 当没有测距差误差时，变换形式如下 r 。。l r 1 2 戈。一戈 2 y 。一y 2 z 。一彳 2 3 整理式 3 解得 r 。1 2 r 。n r l 一2 兕1 1 戈一2 y 。l y 一2 z 。l 三 后i 一庀l 4 当存在测距差误差时，将式 1 代人式 4 ，最终 解得 戈。1 z ，n y 盈1 三 庀。一七l r j △r ； ／2 △r 。1r l r 。lr I 5 式中，各参数的表达式分别为 f 石n 石f 一戈1 ；y i l y i y l ；三‘1 毛一彳l i 元。 z ； y ； z ；；尼， z ； y ； 彳； ‘6 万方数据 2 9 2 0 煤炭 学报 2 0 1 8 年第4 3 卷 1 ．4 超宽带测距实验 本文的观测基站及定位点均采用美国时域公司 的P 4 4 0 测距模块，其测距精度将直接影响定位精 度，进而对位姿检测精度产生影响。因此选择在石家 庄煤矿机械有限责任公司搭建的与井下巷道狭长封 闭空间相似的模拟巷道环境开展测距精度实验。 如图1 所示，为了均衡体现P 4 4 0 模块的测距性 能，实验采用多个模块并与位姿检测系统的布局方式 相同，即掘进机机身安装3 个模块，掘进机后方定位 观测基站设置4 个模块。选取模拟巷道坐标系原点， 固定掘进机后方4 个模块并进行三维坐标标定。每 隔一定距离向前开动掘进机，开展超宽带测距实验， 记录每两个模块间的10 0 0 次测距数据，同时利用激 光测距仪标定机身模块坐标值。选取典型标定距离， 进行数据处理，实验数据见表1 。 一 事。 I 叠日 1 L 二潮函■■■- 图1 测距实验 F i g ．1E x p e r i m e n tf o rm n g i n g 表1P 4 4 0 模块间实测数据 T a b l e1M e a s u r e dd i s t a n c ed a t ab e t w e e nP 4 4 0m o d u l e s 通过对大量实验数据的分析，可以得知P 4 4 0 测 距模块在掘进机巷道的测距误差及标准差一般都小 于2c m 。 1 ．5T D O A 模型的建立 T D O A 定位模型钊如图2 所示，在掘进机机身 选取P ，p ，R 三个定位点，掘进机后方放置4 个位置 已知的定位观测基站，每个基站及定位点均安装一个 P 4 4 0 测距模块。已经通过大量的测距实验验证模块 的测距误差和标准差一般小于2c m ，基本呈正态分 布。因此将所有的测距信息引入均值为0 ，标准差为 2c m 的正态分布原始随机测距误差参量进行计算。 根据模块误差性质和C h a n 算法解算原理进行T D O A 模型观测方程组的建立及线性变换。 图2T D O A 定位模型 F i g ．2 T D O Ap o s i t i o n i n gm o d e l 定位点P Q ，R 的位置坐标间的T D O A 观测方 程组 F 篙 ㈩ 【r 4 一r I r 4 l 式中，r 。 扣1 ，2 ，3 ，4 的表达式为 r ； 石。一z 2 y 。一y 2 毛一z 2 8 方程组 7 进行变换，移项整理，结果如式 9 所 示。 戈一z 。 戈 y 一y 。 y 孑一z ， 彳 士 [ 戈i y ； 三； 一 z y ； z ； 一r ；，] 一r 1r 。 z ，一戈。 x y ，一y ． y 彳，一三。 三 { 一x [ 戈； y ； z ； 一 戈； y ； 彳； 一r ；，] 一r ，。r 。 戈。一戈． 戈 y 4 一y 。 y 缸一彳。 彳 士x [ 石； y z ； 一 戈； y ； 三； 一r ；，] 一r 。。r ， 9 将式 9 写成矩阵形式 A x 6f1 0 1 6 士 恢I 士 №I 士 №I | I 戈，| | 2 一r ；1 一r 2 lr l №I I2 一r ；1 一r 3 lr l l 旧I I2 一r 。 一r 4 l r 。 万方数据 第1 0 期 符世琛等面向掘进机的超宽带位姿协同检测方法 2 9 2 l A ※蠹争2 ㈢ 2 协同定位算法 本文提出将C h a n 算法与泰勒级数展开法进行融 合得到一种协同定位算法。通过c h a n 算法计算得到 初始点坐标后，代人泰勒级数展开法进行循环迭代消 除误差。 泰勒级数定位算法列是在目标位置的初始估计 点利用泰勒级数展开，并忽略二次及以上项，将非线 性方程转换为线性方程，然后利用估计偏移值对初始 坐标进行修正，反复迭代，使得估计的目标位置逼近 真实位置，从而得到对目标位置的最优估计。泰勒级 数展开法虽然精度高，但是需要一个初始点才可以进 行泰勒展开，初始点的位置要靠近真实值，防止其收 敛到局部最优点。经由c h a n 算法得到的定位点坐标 估计值解决了这一问题，满足初始点要求。算法主要 步骤如下。 1 利用由式 1 1 中计算得出 ‰，％，知 作为对 掘进机机身定位点的初始坐标估计值，与真值 石，y ， z 的误差预设为6 。，6 ，，6 ，则有 d M A 岱 d 戈1 一x o y 1 一y o彳l z o 戈2 一z o y 2 一y oz 2 一％ 戈3 一戈oy 3 一y o彳3 一％ x 4 一x o，4 一 oz 4 一z o ，，6 石、 ；6 l6 y ㈦ 1 4 【 x 。一戈。 2 y 。一y 。 2 z 。一z 。 2 一r 2 】、 导[ z 一戈。 2 y 一y 0 2 石一z 。 2 一r 为 善[ 戈，一戈。 z y ，一y 。 z 彳，一知 2 一，羽 妻【 戈。一戈。 2 y 。一y 。 2 。。一z 。 2 一r 翔 - ／ 6 的最小二乘解为 艿 M 。M ‘1 M T df1 5 3 判断是否小于给定的门限s ，设定8 0 ．1 为 合理门限值。若s 小于门限，则停止迭代，目标位置 P 的最终估计值为 r z P 石o 6 。 { y P y 0 6 ， 1 6 【却 铂 6 4 若8 大于门限，则用式 1 7 代替对目标的上 次估计值，并重复算法步骤。 r z o 一戈o 6 。 { y o 卜y o 6 y 1 7 【彳。卜彳o 6 1 2 3 定位仿真分析 2 将式 8 中的 在 z ，y ，孑 点用泰勒级数展 开，并忽略二次及以上项，移项整理则有 戈。 戈。 6 。 y 。 y 。 6 ， 彳， 知 6 [ 戈l 一戈o 2 y l y o 2 彳l 一％ 2 一r ；] z 戈。 6 。 y y 0 6 ， z ％ 6 丢 [ z 2 一‰ 2 y 2 一y o 2 孑2 一知 2 一疙] 搿， 戈。 6 。 y ， y o 6 ， z 。 铂 6 ； [ 戈3 一‰ 2 y 3 一y o 2 彳3 一铂 2 一r ；] z 。 z 。 6 。 y 。 y 。 6 ， 彳。 z 。 6 } [ 石4 一‰ 2 y 4 一y o 2 缸一铴 2 一蠢] f1 31 同样写成矩阵形式 3 ．1 均方根误差 基于第2 节协同算法的数学推导流程进行定位 仿真分析。P 4 4 0 测距模块的有效可靠测距距离一般 在9 0m 左右，为了对比c h a n 算法和协同算法的定位 精度，分别利用两种算法开展9 0m 范围内定位精度 仿真分析。将A 设定为主观测基站，D 取为巷道坐标 系的原点。各基站坐标分别为A 0 ，4 ，O ，B 2 ，2 ， 4 ，C 一2 ，2 ，4 ，D 0 ，0 ，O ，选取待测定位点坐标 为 0 ，d ，0 ，从1 0m 处开始，每隔1 0m 进行10 0 0 次 定位实验，增大定位距离直至9 0m 处，即d 从1 0 依 次变化到9 0 。为了优化定位精度，每次定位实验定 位多次，并进行均值处理。 以每次测量的定位点位置为横坐标，以两种算法 定位的均方根误差为纵坐标绘制曲线，得到图3 所示 的均方根误差随定位点位置变化的关系。 由图3 分析得知，协同算法在9 0m 定位范围内 ≈ y 6 6 允 卜 ‰ ％ ％ 戈 y Z ，I__l-‘l___I 万方数据 煤炭 学报 2 叭8 年第4 3 卷 0 ．0 3 5 O0 3 0 宴0 ．0 2 5 型0 0 2 0 罂O ．0 1 5 橱 墨0 ．0 1 0 00 0 5 0 图3 均方根误差随距离变化 F i g ．3 R M S E d i s t a n c ed i s t r i b u t i o ncurve 的均方根误差均小于c h a n 算法，其定位精度较c h a n 算法有明显提高。由于定位方程组的非线性，两种定 位算法的均方根误差都随定位距离的增加大体上呈 现增加趋势。 3 ．2 三轴误差分布 为了研究协同算法的三轴误差分布规律，依次选 取1 0 ，5 0 和9 0m 定位实验中的1 0 0 次定位结果，绘 制协同算法的三轴误差分布图。 根据图4 分析可知，协同算法x 轴、Z 轴方向误 差基本上在误差零值上下呈对称分布，且随定位距离 的增加呈线性增大趋势，1 0m 定位点可达到4m m 以 下，5 0m 定位点可控制在2c m 左右，9 0m 定位点的 三轴误差可控制在4c m 左右。而l ，轴方向误差基本 保持稳定不变，9 0m 范围内都可控制在2m m 左右。 6 4 2 0 2 _ 4 6 2 04 06 08 0 1 0 0 定位次数 一 肿自】轴一Z 牟m 5 0 m 定位淡簪分币 ．f I ．．A 山． l l 肌】 L 五 I 。l l 西忆血．A ．』。．j I 烈 1 。Ⅵ～⋯‘一。1 ‘州。V ．。wj r O2 04 06 08 01 0 0 定位次数 篇[ 胤一啪一蒯9 0 m 定位误差分布 鹱脚愀一 图4 协同算法三轴误差分布 F i g ．4 T h r e ea x e se I T o rd i s t r i b u t i o nm a po fc o l l a b o r a “v e a l g o r i t h m 3 ．3 定位点分布 为了得出两种定位算法的定位点分布情况，开 展短距离、中距离、长距离的定位点分布仿真。在 上述仿真实验的基础上，选取1 0 ，5 0 和9 0m 定位 实验，绘制c h a n 算法和协同算法的定位点分布图 进行分析。 如图5 所示，两种算法除定位点分布区域变大， 其他定位分布规律相同。C h a n 算法和协同算法的定 位点分布都呈椭球形。 之所以产生椭球形区域，是因为由于测距差误差 的存在，造成了双曲线位置面的偏移，多个位置面偏 移形成位置面的空间交汇区域。随定位距离的增加， 定位精度下降，相应的位置面交汇区域变大，得到的 定位点坐标估计值越来越模糊。协同算法y 轴方向 误差分布位置不同是由于利用泰勒级数展开时，忽略 项对不同定位距离影响不同造成的。通过两种算法 定位点分布对比图明显看出，协同算法大大提高了y 轴方向的定位精度。 3 ．4 空间全断面定位误差场 为研究协同算法的空间定位规律，建立了巷道空 间的X l ，断面及X Z 断面的定位误差场。X y 断 面定位误差场建立过程如下首先在X D y 平面上的 】，轴方向1 0 ～9 0m 定位范围内每隔1m 取一组定位 点，每组定位点在X 轴方向一3 ～3m 范围内间隔 0 ．2m ，这样得到一个x y 待测断面。之后将此断面 在z 轴方向一2 ～4m 范围内每隔0 ．2m 向上或向下 平移，每平移一次，就产生新的待测断面，这样就得到 了大量的x y 待测断面。接着对每个x y 待测断面 上的定位点利用协同算法进行定位，计算得到每个定 位点的均方根误差。最后利用均方根误差值绘制等 高线图，即得到x l ，断面定位误差场。 以Z 1m 时的x y 断面定为例，如图6 所示。 在巷道空间限定的范围内，根据线条颜色和密集程度 分析协同算法X l ，断面定位误差。定位点沿X y 断 面的x 轴方向移动，定位误差变化很小，但随定位距 离的变大，曲线变得密集，定位误差的随机性随之增 大；定位点沿X y 断面的l ，轴方向向远处移动，定位 误差有线性增大趋势。同理，基于上面产生大量空问 定位点，建立x z 断面定位误差场进行分析。分别 选择l ， 1 0m ，l ， 5 0m ，y 9 0m 处的X Z 断面，绘制 断面误差场图。 同样在巷道空间限定的范围内根据图6 线条颜 色和密集程度分析协同算法x z 断面定位误差，定 位点在X Z 断面移动，定位误差具有一定的随机 性，在短距离时误差值相差很小，定位精度相当。 随定位距离的增加，误差曲线变得密集，误差的随 机性增大。 目．一 分心 一 箍丑n 一 诲 ～ 一 位H． 一 定●H O 一 一而 乃越r 一 一聊一 哞■佴， 一 一‰』译 ■ 一 。酗一一 M∞舵叭0叭眈∞∞ O O O O O O O O Ⅲ／珊嗤 万方数据 第l O 期 符世琛等面向掘进机的超宽带位姿协同检测方法 2 9 2 3 g 商 O c h a n 算法定位点分布 0 4 0 8 8 4 量 0 闩_ 4 1 0 齑 协| 司算法定位点分布 图5 定位点分布 F 培5P o s i t i o n i n gp o i n td i s t r i b u t i o n j ，_ 】1 断面误差场 z lm oo 堕 Q Q 幽三 O ．0 2 0O ，0 2 50 ，0 3 0 O ，0 3 5O ，0 4 0 ■■■■●■■■■■■■■■■■■■■●■霸丽雾厂f _ 2 彳 。_ _ ～ 图6x y ，X z 断面定位误差场 F 培．6 x ya n dx zs e c t i o np o s i t i o n i n ge r r o rf i e l d 4 掘进机机身位姿参数精度仿真 4 ．1 机身定位点选取 以E B z l 6 0 机型的悬臂式掘进机为例，选取3 个 机身定位点。为了方便进行位姿参数解算，将3 个定 位点选取在同一个水平面上且相互问构成直角三角 形的定位点布局方式，建立掘进机空间位姿参数解算 模型m 1 。4 个坐标已知的观测基站分别对3 个机身 定位点P ，Q ，R 进行超宽带测距，利用协同定位算法 解算出P ，Q ，R 三点的坐标，将坐标值依次代入姿态 角解算公式最终解算出航向角“、俯仰角J I B 、横滚角 7 。 万方数据 2 9 2 4 煤炭学报2 0 1 8 年第4 3 卷 4 ．2 位姿参数误差 根据E B z l 6 0 机型的悬臂式掘进机的尺寸，利用 协同定位算法进行掘进机姿态角误差的检测。选取 3 个定位点坐标分别为P 1 ，n ，0 ，Q 一1 ，n 2 ，0 ， R 一1 ，n ，0 ，各观测基站坐标仍为A O ，4 ，0 ，B 2 ， 2 ，4 ，C 一2 ，2 ，4 ，D O ，0 ，0 。每隔1m 开展位姿误 差仿真实验，直至9 0m 处，即n 从1 0 依次变化到9 0 。 以检测距离为横坐标，分别以航向角误差、俯仰 角误差、横滚角误差为纵坐标绘制点图，得到图7 所 示的掘进机机身各姿态角随检测距离变化的误差分 布图。可以明显看出，随检测距离的增加，各姿态角 误差在误差零值上下对称分布，大体上呈线性增大趋 势。航向角误差略大于俯仰角和横滚角，该位姿检测 方法在9 0m 范围内的航向角、俯仰角、横滚角误差基 本上可达到1 ．5 。以下。 检测距离／m 图7 姿态角误差随距离分布 F i g ．7 E I T o ro fp o s ea n g l e - d i s t a n c ed i s t r i b u t i o np a t t e r n 5结论 1 设计了一种面向掘进机的超宽带位姿协同 检测方法。通过建立基于波达时间差模型的定位观 测方程组，利用融合c h a n 算法和泰勒级数展开法的 协同算法得到定位点三维坐标估计值。 2 开展了定位仿真实验，通过与c h a n 算法的 均方根误差进行对比，得出协同算法的定位精度有明 显提高。协同算法的定位点分布呈椭球形，在9 0m 范围内随定位距离的增加，X 轴、z 轴定位误差由0 ．4 至4c m 呈线性增加，y 轴定位误差可稳定保持在 2m m 左右。建立了巷道空间全断面误差场，在巷道 X y 断面上，随测量距离增大，定位误差呈线性增 加。而在某一个巷道x z 断面上，定位误差有一定 的随机性。 3 对掘进机机身定位点进行布局，建立空间位 姿参数解算模型。根据协同算法得到的定位点坐标 值及位姿解算公式解算出的航向角、俯仰角、横滚角 误差均由O ．2 。至1 ．5 。呈线性增加，满足巷道成型过 程中掘进机机身位姿检测精度的需求。 参考文献 R e f e r e n c e s [ 1 ] 谢和平，高明忠，高峰，等．关停矿井转型升级战略构想与关键 技术[ J ] ．煤炭学报，2 叭7 ，4 2 6 1 3 5 5 1 3 6 5 ． x I EH e p i “g ，G A OM i n g z h o n g ，G A 0F e n g ，e ta 1 ．S t r a t e 刨cc o n c e p t u 一 曲z a 6 0 na n dk e yt e c h n o l o g yf o rI h et r a n s f o n I I a 6 0 na n du p g r a d i n go f s h u 卜d o w nc o a lm i n e s [ J ] ．J o u m a lo fC h i n ac o a ls o c i e t y ，2 0 l7 ， 4 2 6 1 3 5 5 1 3 6 5 ． [ 2 ]丁雅斌，梅江平，张文昌，等．基于单目视觉的并联机器人末端 位姿检测[ J ] ．机械工程学报，2 0 1 4 ，5 0 2 1 1 7 4 一1 7 9 ． D I N GY a b i n ，M E IJ i a n g p i n g ，Z H A N GW e n c h a n g ，e ta 1 ．P o s j t i o na n d o r i e n t a t i o nm e a s u r e m e n to fp a r a l l e lm b o tb a s e do nm o n o c u l a rv i s i o n [ J ] ．J o u m a lo f M e c h a n i c a lE “g i n e e r i n g ，2 0 1 4 ，5 0 2 1 1 7 4 一1 7 9 ． [ 3 ]陶云飞，宗凯，张敏骏，等．基于i G P s 的掘进机单站多点分时机 身位姿测量方法[ J ] ．煤炭学报，2 0 1 5 ，4 0 1 1 2 6 1 l 一2 6 1 6 ． T A OY u n f e i ，Z O N GK a i ，Z H A N GM i H i u n ，e ta 1 ．Ap o s i t i o na n do r i e n t a t i o nm e a s u r e m e n tm e t h o do fs i n g l e s t a t i o n ，m u l t i p o i n ta n dt i m e ． s h a r i n gf o rm a d h e a d e rb o d vb a s e d0 ni G P S [ J ] ．J o u r n a lo fC h i n a C o a lS o c i e t y ，2 0 1 5 ，4 0 1 1 2 6 1 1 2 6 1 6 ． [ 4 ]杨文辉，林嘉睿，高扬，等．双护盾硬岩隧道掘进机位姿测量系 统[ J ] ．光学学报