基于TSOA定位原理混合算法的掘进机位姿检测方法.pdf

第4 4 卷第4 期 2 0 1 9 年4 月 煤 炭学 报 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 4N o ．4 A p r ． 2 0 1 9 移动阅读 刘超，符世琛，成龙，等．基于T S O A 定位原理混合算法的掘进机位姿检测方法[ J ] ．煤炭学报，2 0 1 9 ，4 4 4 1 2 5 5 1 2 6 4 ．d o i 1 0 ．1 3 2 2 5 ／j ．c n k i ．j c c s ．2 0 1 8 ．1 4 3 2 L I UC h a o ，F US h i c h e n ，C H E N GL o n g ，e ta 1 ．P o s ed e t e c t i o nm e t h o db a s e do nh y b r i da l g o r i t h mo fT S O Ap o s i t i o n i n g p r i n c i p l ef o rr o a d h e a d e r [ J ] ．J o u r n a lo fC h i n aC o a lS o c i e t y ，2 0 1 9 ，4 4 4 1 2 5 5 1 2 6 4 ．d o i 1 0 ．1 3 2 2 5 ／j ．c n k i ．j C C S ． 2 0 1 8 ．1 4 3 2 基于T S O A 定位原理混合算法的掘进机位姿检测方法 刘超，符世琛，成龙，刘丹，沈阳，吴淼 中国矿业大学 北京 机电与信息工程学院，北京1 0 0 0 8 3 摘要为实现掘进机的位姿检测，提出了一种面向掘进机的混合算法的位姿检测方法。基于超宽 带 U h r a - w i d e b a n d 测距和 T i m eS u m m a t i o no fA r r i v a l ，T S O A 定位原理，系统地推导了混合算法的 计算过程，将间接法得到的定位点初始坐标代入T a y l o r 级数展开法，循环迭代，通过混合算法得到 了掘进机的坐标值，将坐标值代入姿态角解算公式得到了掘进机的姿态角。实际实验中需要其他 测量方式的标定才可验证真值，且任何测量方式均有误差，因此基于M A T L A B 进行仿真分析研究 规律。绘制了混合算法程序流程图，仿真对比了间接法和混合算法在1 5m 和9 0m 的定位点空间 分布状态、三轴误差，以及2 种算法在1 0 1 0 0m 的测量范围内定位点均方根误差和三轴均方根误 差变化情况，分析了混合算法在1 5m 和9 0m 的姿态角精度，仿真结果表明混合算法的定位性能 优于间接法，精度高于间接法，姿态角误差可控制在0 ．0 0 8 。以下。搭建了掘进机位姿检测系统实 验平台，在模拟巷道中进行了实验验证，完整地采集了U W B 测距的相关数据，在M A T L A B 中通过 曲线拟舍绘制了误差随距离变化的曲线图，得到了误差随测量距离变化的规律，实验结果表明在 3 9 4m 测量范围内，X 轴误差可控制在4C B 以内，Y 轴误差可达到毫米级，Z 轴误差随测量距离 增大而增大。为实现综掘装备的自主导控提供了理论基础。 关键词掘进机位姿检测；混合算法；T S O A 定位原理；超宽带；T a y l o r 级数展开；精度分析 中图分类号T D 4 2 1 ．5 ；T D 6 7 6文献标志码A文章编号0 2 5 3 - 9 9 9 3 2 0 1 9 0 4 - 1 2 5 5 1 0 P o s ed e t e c t i o nm e t h o db a s e do nh y b r i da l g o r i t h mo fT S O Ap o s i t i o n i n g p r i n c i p l ef o rr o a d h e a d e r L I UC h a o ，F US h i c h e n ，C H E N GL o n g ，L I UD a n ，S H E NY a n g ，W UM i a o S c h o o l ∥M e c h a n i c a lE l e c t r o n i ca n dI n f o r m a t i o nE n g i n e e r i n g ，C h i n aU n i v e r s i t yo f M i n i n ga n dT e c h n o l o g y B e r i n g ，B e i j i n g1 0 0 0 8 3 ，C h i n a A b s t r a c t I no r d e rt or e a l i z et h ep o s ed e t e c t i o no ft h er o a d h e a d e r ，ap o s ed e t e c t i o nm e t h o df o rt h eh y b r i da l g o r i t h mw a s p r o p o s e d ．A c c o r d i n gt ot h ep o s i t i o n i n gp r i n c i p l eo fT i m eS u m m a t i o no fA r r i v a l T S O A ，t h ec a l c u l a t i o np r o c e s so ft h e h y b r i da l g o r i t h mw a ss y s t e m a t i c a l l yd e r i v e d ．T h ei n i t i a lc o o r d i n a t e so ft h ep o s i t i o n i n gp o i n to b t a i n e db yt h ei n d i r e c t m e t h o dw e r eb r o u g h ti n t ot h eT a y l o rs e r i e se x p a n s i o n ，l o o pi t e r a t i o n ，t h ec o o r d i n a t ev a l u e so ft h er o a d h e a d e rw e r eo b t a i n e db yt h eh y b r i da l g o r i t h m ．T a k i n gt h ec o o r d i n a t ev a l u ei n t ot h ea t t i t u d e a n g l es o l u t i o nf o r m u l a ，t h ea t t i t u d ea n g l e o ft h er o a d h e a d e rw a so b t a i n e d ．T h ea c t u a lm e a s u r e m e n tr e q u i r e dt h ec a l i b r a t i o no fo t h e rm e a s u r e m e n tm e t h o d st ov e i l ． f yt h et r u ev a l u e ．A n ym e a s u r e m e n tm e t h o dh a de r r o r s ，S Ot h es i m u l a t i o na n a l y s i sw a sc a r r i e do u ti nM A T L A B ．T h e f l o wc h a ao ft h eh y b r i da l g o r i t h mp r o g r a mw a sd r a w n ．T h es i m u l a t i o nc o m p a r e dt h es p a t i a ld i s t r i b u t i o na n dt r i a x i a le r 一 收稿日期2 0 1 8 1 0 2 7修回日期2 0 1 9 0 2 0 2责任编辑郭晓炜 基金项目国家自然科学基金面上资助项目 5 1 8 7 4 3 0 8 ；国家自然科学基金重点资助项目 U 1 6 1 0 2 0 0 0 3 作者简介刘超 1 9 9 3 一 ，男，云南曲靖人，硕士研究生。E - m a i l l i u c h a 0 9 3 5 4 1 6 3 ．c o n l 通讯作者符世琛 1 9 9 1 一 ，男，河北邢台人，博士。E - m a i l f u s h i e h e n 0 5 2 6 1 6 3 ．c o r n 万方数据 煤炭 学报 2 0 1 9 年第4 4 卷 r o ro fb o t hi n d i r e c ta n dh y b r i da l g o r i t h m sa t15ma n d9 0m ．I tc o m p a r e dt h eR M S Ee r r o ro ft h ep o s i t i o n i n gp o i n ta n d t h et h r e e a x i sR M S Ee r r o ro ft h et w oa l g o r i t h m si n1 0 1 0 0m ．T h ea t t i t u d ea n g l ea c c u r a c yo ft h eh y b r i da l g o r i t h mw a s a n a l y z e da t 15ma n d9 0m ．T h er e s u l t ss h o wt h a tt h ep o s i t i o n i n gp e r f o r m a n c eo ft h eh y b r i da l g o r i t h mi sb e t t e rt h a nt h e i n d i r e c tm e t h o d ．H y b r i da l g o r i t h mi sm o r ea c c u r a t et h a ni n d i r e c tm e t h o d ．T h ea t t i t u d ea n g l ee r r o rc a nb ec o n t r o l l e db e l o w0 ．0 0 8 。．T h ee x p e r i m e n t a lp l a t f o r mo ft h er o a d h e a d e rp o s ed e t e c t i o ns y s t e mw a sb u i l t ．E x p e r i m e n t sw e r ec a r r i e do u t i nt h es i m u l a t e dr o a d w a y ，a n dt h er e l e v a n td a t ao fu l t r a - w i d e b a n dr a n g i n gw a sc o l l e c t e dc o m p l e t e l y ．T h ec u r v eo fe r r o r w i t hd i s t a n c ew a sp l o t t e db yc u r v ef i t t i n gi nM A T L A B ．T h el a wo fe r r o rw i t hm e a s u r e m e n td i s t a n c ew a so b t a i n e d ．T h e e x p e r i m e n tr e s u l t ss h o wt h a tt h eX a x i se r r o rc a nb ec o n t r o l l e dw i t h i n4c ma n dt h eY - a x i se r r o rc a nr e a c hm i l l i m e t e r ， Z - a x i se r r o ri n c r e a s e sa st h em e a s u r e dd i s t a n c ei n c r e a s e sw i t h i n3 9 4m ．T h i sm e t h o dp r o v i d e sab a s i sf o rt h er e a l i z a - t i o no ft h ep o s i t i o nd e t e c t i o no fr o a d h e a d e r ． K e yw o r d s r o a d h e a d e rp o s ed e t e c t i o n ；h y b r i da l g o r i t h m ；p o s i t i o n i n gp r i n c i p l eo fT i m eS u m m a t i o no fA r r i v a l ；U l t r a w i d e b a n d ；T a y l o rs e r i e se x p a n s i o n ；p r e c i s i o na n a l y s i s 智能化开采成为煤炭安全高效开采的发展方向 与必然趋势⋯。无人采掘装备是当前国际煤炭领域 的发展前沿埋J 。在地下巷道掘进过程中，传统掘进 方法效率低，精度有限，而且井下矿工长期处于高温、 高湿、高浓度粉尘环境，健康严重受损，井下环境复 杂，事故频发，增加了矿工的工作强度及危险性，因此 我国亟待研制无人采掘装备旧J 。要实现综掘工作面 的无人化，综掘装备自主导控是其关键，掘进机的位 姿检测方法是实现综掘装备自主导控的核心，具有重 要的研究意义。 自2 0 世纪9 0 年代以来，国外主要产煤大国多以 露天矿为主，在井下实际运用掘进机位姿检测技术的 报道较少，多数技术集中在盾构掘进机中L 4J 。日 本G Y R O 系统。5 1 使用陀螺仪检测盾构掘进机的姿态 角，环境温度的变化对陀螺仪的机械有较大影响，该 系统实用性不佳，只能作为人工测量的辅助参考。德 国艾柯夫公司‘6 1 的掘进机具有方向控制和自动成型 以及对故障监控的功能，但位姿检测精度不高，受环 境影响较大，在矿井中未见成熟应用案例。 近年来，国内学者对掘进机的位姿检测方法进行 了许多研究。陶云飞“ o 提出了一种基于激光导向系 统的位姿测量方法、周玲玲等一提出了一种基于双 激光标靶图像识别的位姿检测方法、童敏明等归。提 出了一种多传感器的定位系统、杜雨馨等刨提出了 一种基于机器视觉的位姿检测系统、贾文浩等‘1 u 提 出了一种基于i G P S 的掘进机定位系统、吴淼等纠提 出了一种基于空间交汇测量技术的位姿自主测量方 法、黄东等引提出了一种基于视觉／惯导的位姿测量 方法。从上述研究可以发现，目前掘进机的位姿检测 方法主要有惯性导航、机器视觉、激光、全站仪、无线 电等。不同的定位方法定位精度也各不相同，惯性导 航【1 4 1 由于导航信息经过积分产生，所以定位误差随 时间增大，精度差；基于激光、机器视觉的定位环境适 应性较差，巷道中粉尘较大，会干扰可见光及红外线 的传播，对成像会产生严重的影响；基于全站仪的定 位需要专业测量人员参与，巷道环境复杂，增加了全 站仪的工作难度；无线电定位主要包括W i F i 、B l u e t o o t h 等，但上述方法精度不高，而超宽带 U l t r a w i d e b a n d ，U W B 测量范围广、系统组成简单、抗干扰 能力强，局域范围内具有高精度测距功能，在掘进机 静止或者移动过程中均能提供较高的定位精度。基 于U W B 测距，符世琛等。1 5 。1 6 1 提出了一种基于T O A 定位原理的掘进机自主定位定向方法，但该原理需要 保证基站和终端的时基必须是同步的，同时该原理存 在位置模糊解‘17 I ，影响精度。 笔者基于U W B 测距，根据T S O A 定位原理，提出 了一种面向掘进机的混合算法的位姿检测方法，建立 了观测方程组，详细地推导了混合算法的计算过程； 基于M A T L A B 仿真对比了间接法和混合算法的定位 点空间分布、三轴误差及均方根误差，分析了混合算 法的姿态角精度，在模拟巷道进行了实验验证，代入 混合算法得到三维坐标，与标定值进行对比，得到了 误差随距离变化的规律。 1 面向掘进机的超宽带位姿检测系统 1 ．1U W B 定位技术 U W B 测距引是利用纳秒甚至皮秒级的极窄脉 冲来实现信息的传输，U W B 信号的相对带宽必须大 于0 ．2 ，绝对带宽大于5 0 0M H z ，具有恒定的波达时间 分辨率，可解算出准确的测距信息。它具有抗干扰能 力强、多径分辨能力强、系统容量大、传输速率高、安 全性高、成本低和功耗低等优点。 通过U W B 信号无线电波口⋯，测量目标的方向、 距离、距离差等定位参量，实现位置坐标求解，定位过 万方数据 第4 期刘超等基于T S O A 定位原理混合算法的掘进机位姿检测方法 程如图1 所示。利用U W B 测距进行掘进机的位姿 检测，关键是获得掘进机机身定位点的坐标。首先由 一个或多个位置已知的基站发射无线电信号，无线电 信号的电参量 如振幅、频率、相位、时间等 中的一 个或多个携带着定位参量信息，经电波传播到P C 端；其次P C 端接收并处理该无线电信号，并根据电 波传播特性，由电参量得到定位参量 如来波方向、 距离、距离差、高度等 ；再次根据得到的定位参量及 位置已知的基站，获得多个相对于各基站的位置面； 最后由多个位置面根据相应的定位算法，得到掘进机 机定位点的坐标。 位置已知的基站 基 站 发 射 无 线 电 信 口 可 位置已知的基站 刮鍪斟 位置已知的基站 位置已知的基站 位置面l 位置面2 位置面3 位置面” 图1U W B 定位结构 F i g ．1 U l t r a w i d e b a n dp o s i t i o n i n gs t r u c t u r e 目 标 位 置 1 ．2T S O A 定位原理 测距和定位 T i m eS u m m a t i o no fA r r i v a l ，T S O A 原理旧0 。是基于波达时间和，通过信号同步网络实现 在时域、频域、空域上的严格同步，各站将测得的目标 数据通过数据传递网进行传递，发射站只发射信号， 接收站通过接收待测目标反射的发射站信号，测量从 发射站到待测目标以及待测目标到接收站的距离和。 在一个特定空间，保持与两个指定基准点距离和相等 的点的轨迹是以接收站、发射站为焦点的椭球面，该 面称为椭圆球位置面。在测量没有误差的情况下， U W B 测距模块一旦得到接收站和发射站的距离和 P ，那么待测目标一定在由P 所确定的椭圆球位置面 上，其定位原理如图2 所示。 设定4 个定位基站4 ，B ，C ，D ，基站A 为发射站， 只发射信号，其位置坐标为 蜀，K ，乙 ，基站B ，c ，D 为接收站，只接收信号，其基站群位置坐标为 x 。，I ， Z ， ，其中i B ，C ，D 。接收站通过接收目标反射的发 射站信号，测量从发射站到目标的距离n ，以及目标 到接收站的距离r 。。掘进机的机身节点E 的位置坐 标为 x 。，K ，z 。 ；第i 个接收站获得的观测量为P 。， 解算出3 个椭圆球的交点，得出目标点的坐标值，则 机身定位点位置坐标的T S O A 观测方程组为 式中， r 。 、厅i 了可可可F 1 了了弦■乏y r 。 、厅i 了可可可F 玎了_ 百■乏y r 。 、厅瓦了丽可T i l 万‘可■巧了 r 。 、厅i 了丽可可F 1 了了弦■乏y 图2T S O A 定位原理 F i g ．2 T S O Ap o s i t i o n i n gp r i n c i p l e 2 基于间接法与T a y l o r 级数展开法的混合 算法 位置坐标的解算方法有很多种，但是无论采用哪 种方法均需求解非线性方程。方程 1 就是非线性 方程组，然而在工程实现中，求解非线性方程并不是 一件容易的事，同时，U W B 测距存在一定的误差，测 距误差的引入会导致椭圆球位置面不相交于一点或 者根本没有交点。综上所述，对掘进机机身的定位问 题就由原来的非线性方程精确求解问题转换成非线 性优化的最优估计问题。 间接法与T a y l o r 级数展开法各有优缺点，二者都 是将非线性问题转化为线性问题。间接法可以通过 直接计算得到一个初始点坐标，性能稳定。T a y l o r 级 数展开法为了防止收敛到局部最优点以及减少迭代 次数，初始点的估计值需要靠近真实值，但在实际中 不易实现，虽然一般都能收敛到真实点，但迭代过程 的收敛性无法保证，同时每一次迭代都需要对矩阵求 逆，运算量大。而由间接法得到的定位点估计值可以 解决初始点这一问题，满足T a y l o r 算法对初始点的要 求，文献[ 1 8 ] 表明T a y l o r 级数算法的精度高于间接 法。因此，在本文中提出了将间接法与T a y l o r 级数展 开法进行混合，通过间接法得到初始点坐标值后，代 入T a y l o r 级数展开法进行循环迭代消除误差，从而可 以得到更精确的掘进机机身位姿。 B C D r r r A A 4 r r r B C D P P P ，I____J、I___【 万方数据 1 2 5 8 煤炭学报 2 0 1 9 年第4 4 卷 2 ．1 间接法 在间接法中，首先把r 看成一个已知量，从而解 得如，K ，磊是 的线性函数，求出机身定位点E 的 坐标估计值，具体过程如下 通过对方程组 1 进行移项，平方，整理化简，得 式中， X 。一X B X 。 K K K Z A Z B Z E K B P B n 蜀一x c 也 y 4 一l ，c K 乙一 f 2 Z c Z ￡ K c P c h 、’ X A X D X E Y A Y o Y E Z 一 Z D Z E K D P D r A k ≯1 厂2 。 x l 圬 z 1 一 x ； 蟾 磊 ] { K c 了1L ，2 。 x l E z j 一 砭 蟾 z ； ] 【％ 虿1 [ P ； 霹 圬 z j 一 碥 圪 z f l [ x A x BY ～一y 8Z A zB _ ＼ A l 蜀一X cK Y cZ 一Z cl ， ＼．x A X8Y 4 一Y DZ 一Z D ] x 料6 慑矧 T X E - X A X E - 。X B 6 ∥Y F - - Y , ．Y E - 。Y a ． Z E - - Z A 警卜吁∽饥 S T E - S A S T E - X C 川半 半卜Z E 三AZ A 。 Z E - 。Z c 6 z 叱∞一c 6 ， X T E - S A 半卜Y E - - Y 4 半卜Z E - - Z A 警p 。∞一。， 将式 6 转化为矩阵形式，则有 A 塑．盥 nr B X E X A 。X E x C ’。。。。。1。。。。。。。。。。。。。。。。。～ “r c 生二生．生二叠 Y E Y A 。Y E Y B ‘。。。。。。。。。。‘‘。。。。。。～ h％ 燮。燮 7 c YE Y t 。Y E Y D 。。。。。。。。。。。。‘。。。。。--。。。⋯ A 6 Z 塑．盟 “％ 生二垒．生二叠 hr c 生二互．兰 二鱼 r dr 仃 可得到占的最小二乘解为 6 A 7 A _ A 1 Z 8 3 判断选择的判据￡是否小于给定的门限s 。， 舻州6 v ；Z 誊P c r A ；i i ] r c 7 其中s J6 ，I J6 ，l J6 。} ，设定s 。 0 ．1 ，若s 大于 门限，则用式 9 代替对目标的上次估计值，并重复 步骤 2 ， 3 。 万方数据 第4 期 刘超等基于T S O A 定位原理混合算法的掘进机位姿检测方法 1 2 5 9 设定机身定位ll 设定巷道后方4 个基 点初始坐标I站初始坐标 将机身定位点坐标分将基站坐标分别 别赋予相应标量中赋予相应标量中 根据课题自相关研究。生成1．L 。 4 个均值为0 ，标准差为o ．0 2 的ll 计算各基革到机皋 随机变量作为测距误差l 定位点的真实距离 莉雨而五葡吾西i 翮厂叫将各基站坐标代入矩阵肘。 絮鼎骥警I - t 陌西函丽焉丽丽 上塑塑划叫搿粼霜础氓 - 求X [ i n v M A ’M A ] ’M0 M 。 得到定位点坐标，代入关于 ，的方程求解，求得r ．，，’ 将玳入矩阵肼B 。 抽仁【i n v 帆4 蜂 】 心4 U s 利用T 对l o r 级数对 初始值进行优化 初始值鼍，匕，Z 将各基站坐标、定位点 初始值和带有随机误差 的距离代入矩阵M 。，M 。 X f X 一。 y E 2 y E 彤， z ． z 6 ． J 。2 衄“ 气”M 。 】 朋0 4 M 。 得到定位点的最终估计位置 茸， i 鼍 d ”匕． y 占，，z 。． z d 图3 混合算法程序流程 F i g ．3H y b r i da l g o r i t h mp r o g r a mf l o wc h a r t f x E } xE 七8 x { Y E 卜K 6 y 9 ＼ZE } zE 8 z 4 若8 小于门限，则停止迭代，定位点E 的最 终估计位置为趣，，K ，，Z 。。。 r X 朗 X F 6 、 { K 。 Y E 6 r 1 0 【Z ￡l Z f 6 z 3 混合算法精度分析 3 ．1 定位点分布 为了探究定位点的分布，设定基站坐标A 0 ，1 0 ， 0 ，B 2 ，5 ，0 ，C 一2 ，5 ，0 ，D 2 ，0 ，5 ，E 点为掘进机 机身定位点，坐标为 0 ，n ，0 。采用美国T i m ed o m a i n 公司生产的P 4 4 0U W B 测距模块，宽带为3 ．1 ～ 5 ．3G H z ，中心频率为4 ．3G H z ，经过课题组的大量实 验验证口卜22 I ，该模块在1 0 0m 狭长封闭空间的误差 基本都小于0 ．0 21 1 3 ，在视距传播的条件下的测距参 量一般呈现高斯分布，因此将均值为0 、标准差为 0 ．0 2m 的测距误差代入间接法和混合算法，得到定 位点的三维坐标，并进行10 0 0 次定位仿真，将仿真 结果所得到的均值作为最终定位点的坐标值，n 分别 取1 5m 和9 0m 。 当n 为1 5m 时，得到2 种算法的定位点空间分 布如图4 所示。在间接法中，定位点在空间呈椭球状 分布，定位点比较密集，三轴的误差可以达到厘米级， 而在混合算法中，定位点在空间呈发射状分布，定位 点发散，三轴的误差可以达到毫米级以上，相比间接 法，精度有较为明显的提高。 a 间接法定位点分布 b 混合算法定位点分布 图41 5m 定位点分布 F i g ．4 15m p 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 对1 5m 这个定位点进行定位精度分析，在M A T L A B 中利用2 种算法测量1 0 0 次，得到如图5 所示的 三轴误差仿真曲线。在间接法中，最大误差在lc m 以内，y 轴误差较小，X 轴、z 轴次之；在混合算法中， 三轴的误差在毫米级以上，x 轴误差较小，l ，轴、z 轴 无较大差异，该算法精度高于间接法。 当n 为9 0m ，2 种算法的定位点分布如图6 所 示。二者在空间中均呈椭球状，定位点比较密集，但 混合算法的精度高于问接法，可达毫米级。 在9 0m ，用2 种算法在M A T L A B 中对定位点测 量1 0 0 次，与真值比较，得到如图7 所示的三轴误差 曲线。由图7 可知，间接法的误差基本维持在4c m 以内，y 轴误差最小，x 轴、z 轴误差无明显差异，而 混合算法三轴的误差达毫米级以上，l ，轴误差最小， 混合算法的精度有较大提高。 3 ．2 均方根误差分析 在定位过程中，无论何种定位方法，由于受测量 设备和传播环境的影响，测量的定位参量都会存在误 差，从而导致定位误差的存在，并且采用不同的定位 算法，定位误差的大小也不一样。均方根误差。23 。是 裂 万方数据 1 2 6 0 煤炭 学报 2 0 1 9 年第4 4 卷 0 0 E0 商一0 0 0 9 0 ． 1 ．5 O 呈一1 ．5 剁 3 联 一3 ．0 4 ．5 测量次数 a 间接法三轴误差曲线 差 02 04 06 08 01 0 0 测量次数 b 混合算法三轴误差曲线 仿真曲线 图69 0m 定位点分布 F i g ．6 9 0m p 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 0 ．0 8 0 ．0 4 暑 剿0 嗤 一0 ．0 4 0 ．0 8 a 间接法三轴误差曲线 矗丽秆硼，勰＆鼎国，腻A 2 裂1 6 5 翠l 零6 节警’。曲艮 测量次数 b 混合算法‘轴误差曲线 图79 0m 三轴误差仿真曲线 F i g ．7 9 0m e t e rt r i a x i a le r r o rc u r v e s 评价定位精度的一个十分重要的指标，均方根误差越 小，定位精度越高，因此此处采用均方根误差作为定 位算法精度的评判指标。 选取3 ．1 节中的基站坐标，E 为定位点，坐标 为 0 ，r t ，0 ，r t 从1 0m 处开始，每隔1m 移动一次，直 到移动到1 0 0m 处，在M A T L A B 中根据2 种算法对 其进行仿真，得到2 种算法的均方根误差随距离的变 化曲线如图8 所示。由图8 可知，在1 0 ～1 0 0m 内， 间接法的均方根误差控制在4 ．5c m 以内，随着距离 的增大均方根误差也在增大，而在混合算法中，均方 根误差达到毫米级以上，相比间接法，精度明显更高。 0 3 6 0 2 7 0 1 8 0 0 9 ，、L - ．．．．‘．．．．．．j ．．．．．．J ．．．．．．L ．．．．．．L ．．．．．．1 ．．．．．．』．．．．．．J - - - - - - J 。1 02 0 3 0 4 0 5 0 6 0 7 08 09 01 0 0 测量距离／m a 间接法均方根误差曲线 3 ．5r 卜 测量距离／m b 混合算法均’J 根l X 军【} } l 线 图8 均方根误差随距离变化曲线 F i g ．8 R M S Ee r r o rc u r v e sw i t hd i s t a n c e 为了探究三轴的定位精度随定位距离的变化关 系，在M A T L A B 中进行仿真模拟，得到三轴的均方根 误差随定位距离的变化曲线如图9 所示。由图9 可 知，1 0 ～1 0 0m 内，在间接法中，三轴均方根误差随测 量距离增大而增大，y 轴的均方根误差较小，在5m m 以下，x 轴和z 轴次之，没有明显差异；在混合算法 中，三轴的均方根误差达到毫米级以下，X 轴和y 轴 均方根误差较小。 由2 种算法的定位点均方根误差曲线和三轴均 方根误差曲线可知，混合算法的均方根误差可达到毫 米级以上，精度明显高于间接法。 3 ．3 姿态角精度分析 图1 0 为掘进机机身定位点布置示意图，3 个定 位点布置在同一水平面上，形成一个等腰三角形，掘 进机机身视为一个刚体。结合E B Z l 6 0 悬臂式掘进 机的尺寸，图中￡，为定位点E 到定位点F 的距离，L 为定位点E 到掘进机中轴线的距离，定位点E 到定 位点G 之间的距离为2m ，定位点F 到L 之间的距 离为2m 。 万方数据 第4 期刘超等基于T S O A 定位原理} 昆合算法的掘进机位姿检测方法 量 删 鉴 皑 惶 曩 暑 口 2 ＼ 删 嗤 皿譬 * 橱 墨 0 3 6 0 2 7 0 1 8 0 0 9 O 卜 心 图9 三轴均方根误差随距离变化曲线 F i g ．9 T r i a x i a lR M S Ee r r o rc u r v e sw i t hd i s t a n c e 图1 0 掘进机机身定位点布置 F i g ．1 0L a y o u to ft h er o a d h e a d e rf u s e l a g el o c a t i n gp o i n t 4 个基站的坐标与3 ．1 节中相同，基站对机身的 3 个定位点E ，F ，G 进行测距，利用混合算法解算出 定位点的坐标，代入位姿解算公式6 I ，可得到掘进机 的航向角O t 、俯仰角B 、横滚角7 。 。X E XF．L l 旷射毗肌瓦～m 8 1 ni ／3 a r c t a n 磋 ⋯ Z G Z E r 盯毗卸瓢 分别选取1 5m 和9 0m 进行位姿参数精度分析。 在1 5m 中，E 1 ，1 3 ，0 ，F 0 ，1 5 ，0 ，G 一1 ，1 3 ，0 ， 9 0m 中，E 1 ，8 8 ，0 ，F 0 ，9 0 ，0 ；G 一1 ，8 8 ，0 ，可得 到如图1 1 所示的位姿参数精度分布曲线。由图可 知，在1 5m 处，掘进机的航向角、俯仰角、横滚角的精 度可达到0 ．0 0 5 。左右，其中航向角精度最高；在9 0m 处，各姿态角的精度可控制在0 ．0 0 8 。以下。随着定 位距离的增大，定位误差也随之增大，同样，位姿误差 也随着定位距离的增大而增大。 一 。 剁 嗤 蜊 娅 f ≮ } i { ] j 嗤 诖鲢 娅 测量次数 a 1 5 m 位姿解算精度 测量次数 b 9 0 m 位姿解算精度 图1 1 位姿解算精度曲线 F i g ．11A c c u r a c yC H I v e so fp o s e 4 实验验证 煤矿的巷道可近似为狭长封闭空间，实验选择在 与巷道相似的狭长封闭的楼道中进行，二者在其空间 变化上是相似的，重点研究该位姿检测系统及其算法 的精度，以及空问结构对U W B 模块的影响。采用的 U W B 测距模块是美国T i m ed o m a i n 公司生产的P 4 4 0 模块。准备工作如下 1 在电脑上安装P 4 4 0 测距软件。 2 将模块1 ，2 ，3 ，