基于近红外双目立体视觉的悬臂式掘进机截割部定位研究 王学.pdf

该【基于近红外双目立体视觉的悬臂式掘进机截割部定位研究 王学 】是由【碧痕】上传分享，文档一共【12】页，该文档可以免费在线阅读，需要了解更多关于【基于近红外双目立体视觉的悬臂式掘进机截割部定位研究 王学 】的内容，可以使用淘豆网的站内搜索功能，选择自己适合的文档，以下文字是截取该文章内的部分文字，如需要获得完整电子版，请下载此文档到您的设备，方便您编辑和打印。:.
工矿自动化
JournalofMineAutomation
ISSN1671-251X,CN32-1627/TP
《工矿自动化》网络首发论文
题目:基于近红外双目立体视觉的悬臂式掘进机截割部定位研究
作者:王学,周红旭,张雷,王华英
DOI:.1671-
收稿日期:2022-02-27
网络首发日期:2022-07-15
引用格式:王学,周红旭,张雷,
割部定位研究[J/OL].工矿自动化.
/-
网络首发:在编辑部工作流程中,稿件从录用到出版要经历录用定稿、排版定稿、整期汇编定稿等阶
段。录用定稿指内容已经确定,且通过同行评议、主编终审同意刊用的稿件。排版定稿指录用定稿按照期
刊特定版式(包括网络呈现版式)排版后的稿件,可暂不确定出版年、卷、期和页码。整期汇编定稿指出
版年、卷、期、页码均已确定的印刷或数字出版的整期汇编稿件。录用定稿网络首发稿件内容必须符合《出
版管理条例》和《期刊出版管理规定》的有关规定;学术研究成果具有创新性、科学性和先进性,符合编
辑部对刊文的录用要求,不存在学术不端行为及其他侵权行为;稿件内容应基本符合国家有关书刊编辑、
出版的技术标准,正确使用和统一规范语言文字、符号、数字、外文字母、法定计量单位及地图标注等。
为确保录用定稿网络首发的严肃性,录用定稿一经发布,不得修改论文题目、作者、机构名称和学术内容,
只可基于编辑规范进行少量文字的修改。
出版确认:纸质期刊编辑部通过与《中国学术期刊(光盘版)》电子杂志社有限公司签约,在《中国
学术期刊(网络版)》出版传播平台上创办与纸质期刊内容一致的网络版,以单篇或整期出版形式,在印刷
出版之前刊发论文的录用定稿、排版定稿、整期汇编定稿。因为《中国学术期刊(网络版)》是国家新闻出
版广电总局批准的网络连续型出版物(ISSN2096-4188,CN11-6037/Z),所以签约期刊的网络版上网络首
发论文视为正式出版。
:.
췸싧쫗랢쪱볤ꎺ2022-07-1511:33:49
췸싧쫗랢뗘횷ꎺ.
工矿自动化
第48卷JournalofMineAutomation
DOI:.1671-
基于近红外双目立体视觉的悬臂式掘进机截割部定位研究
王学1,2,周红旭1,张雷1,2,王华英1,2
(,河北邯郸056038;
新中心,河北邯郸056038)
摘要:针对现有掘进机无法实时定位、定位不准确、视觉定位中相机视野被遮挡导致定位失败等问题,
提出了一种将基于近红外双目立体视觉的悬臂式掘进机截割部位姿测量方案与基于一维卷积神经网络(1D-
CNN)的磁场辅助定位方法相结合的悬臂式掘进机截割部定位方案。在悬臂式掘进机机身与机臂安装近红外
LED标靶,以LED作为近红外标靶构建掘进机特征信息,通过图像处理、位姿解算实现掘进机机身与截割
部的三维空间定位。双目立体视觉相机安装在巷道顶部,随着掘进机掘进加快,掘进机与双目立体视觉相机
距离逐渐增加,造成双目图像获取失败,进而导致视觉解算截割部的位姿失败。针对该问题,引入基于1D-
CNN的掘进机截割部磁场辅助定位方法。在掘进机机身两侧安装三轴数字磁场计,并在机臂处安装永磁体,
以磁场的强度分量和双目立体视觉获取的位姿数据作为训练数据,构建1D-CNN模型,输出在视觉测量失
效情况下掘进机截割部位姿。为验证该定位方案的可行性,从深度信息和掘进机机身及截割部位姿2个方
面进行验证,结果表明:机身测量误差在10mm以内,%以内。截割部测量误差在50
mm以内,相对误差在1%以内;掘进机机身与截割部间的相对位姿误差在,俯仰角的均方误差为
,。上述误差在巷道作业允许范围内,说明采用基于近红外双目立体视
觉的悬臂式掘进机截割部位姿测量方案进行实时视觉掘进机姿态解算,具有有效性和可靠性。并对基于1D-
CNN的掘进机截割部磁场辅助定位方法进行了有效性验证,为模拟井下复杂磁场环境,在掘进机附件随机
添加了干扰磁源,结果表明:该方法对掘进机截割部俯仰角、偏航角、翻滚角预测值与测量的真实值基本吻
合,预测的俯仰角、偏航角、,,,说明基于1D-CNN的掘进
机截割部磁场辅助定位方法能够较好地弥补在视觉定位失效下的掘进机定位需求。采用近红外双目立体视
觉的悬臂式掘进机截割部位姿测量方案与基于1D-CNN的磁场辅助定位方法相结合,解决了井下复杂环境导
致掘进机截割部位姿实时定位的问题。
关键词:悬臂式掘进机;截割部位姿;截割部实时定位;近红外双目视觉;位姿解算;一维卷积神经网
络;磁场辅助定位
中图分类号:TD421文献标志码:A
0引言定位方法。文献[4]通过读取全站仪激光光斑距目标
自“十三五”以来,我国煤炭行业自主科研创新板中心的偏移量来推断当前隧道的偏差,实现隧道
能力得到了大幅改善,但是创新推动能力依旧不足内掘进机的跟踪和引导。但全站仪无法实时自主测
[1]。GB50213—2010《煤矿井巷工程质量验收规范》量且不能适应井下掘进状况。文献[5]采用惯性导航
规定:我国煤矿掘进断面的允许偏差规定为-技术实现掘进机实时测量、自主测量等功能。然而,
50~+150mm。但是煤矿井下巷道掘进中掘进机截割由于系统的时间偏差、成本高、初始对准时间较长、
部位姿通常用固定在巷道顶板上的点光源来指示掘掘进机抖动幅度大、巷道测量环境较差等缺陷,增
进方向,通过判断激光投影点在巷道掘进断面上的加了累积误差。文献[6-7]开发了一种基于iGPS的掘
位置,确定下一步的掘进方向[2]。该方法存在掘进精进机测量系统,以基于随机误差传递方程来评估掘
度有限、测量工作量大、生产效率低、人身不安全等进机的空间角度和位移。该方法的测量介质为激光,
问题,很难保证巷道成形质量达到要求。因此,为了掘进机作业时存在遮挡问题,同时激光发射装置结
实现掘进机的智能切割控制,亟需提高悬臂式掘进构复杂,激光发射装置易损坏,在复杂工况下,其巷
机截割部位姿参数的自动实时检测[3]。道掘进精度会受到很大影响。文献[8-9]采用超宽带
为实现巷道无人化掘进,许多学者提出了多种技术对掘进机实现位姿实时检测,但由于巷道环境:.
工矿自动化
第48卷JournalofMineAutomation
复杂,金属材料设备较多,电磁信号复杂,易导致超
宽带信号发生漫反射,使得掘进机位姿测量误差偏
大、失败等情况。文献[10]提出了一种基于激光束的
悬臂式掘进机位姿测量系统。结合坐标系转换关系
得到掘进机机身在巷道坐标系中的位姿信息,但测
量距离的增加或相机视野被遮挡等问题易导致测量
失效。
针对现有掘进机无法实时定位、定位不准确、
视觉定位中相机视野被遮挡导致定位失败等问题,图1悬臂式掘进机机身及截割部位姿测量系统结构

机截割部定位方案。该方案以LED作为近红外标靶positionposemeasurementsystem
构建掘进机特征信息,通过图像处理,位姿解算实悬臂式掘进机机身及截割部位姿测量方案流程
现掘进机机身与截割部的三维空间定位;为解决双如图2所示。首先根据双目立体视觉测量原理设计
目立体视觉位姿解算失效问题[11],引入基于一维卷双目立体视觉相机,并采用张正友标定原理对双目
积神经网络(OneDimensional-ConvolutionalNeural立体视觉相机进行标定,计算相机相关参数。在实
Network,1D-CNN)[12-14]的井下磁场视觉辅助定位际生产活动中,机身与相机间的外部参数误差会随
算法,通过构建神经网络模型训练磁场信号数据,着距离的增加而增加,在实际掘进中双目立体视觉
预测掘进机截割部的位姿变化,为视觉定位掘进机相机跟随掘进机前进,在掘进机停止等待过程中需
位姿提供了在视觉测量失效情况下一种新的辅助定重新进行外部参数的标定,防止积累误差。其次,在
位方案。掘进初始工作前利用迭代最近点(IterativeClosest
1悬臂式掘进机机身及截割部定位方案Point,ICP)[15]算法对掘进机机身坐标系与相机坐标
悬臂式掘进机机身及截割部位姿测量系统由悬系进行外部参数解算。根据相机标定得到的参数,
臂式掘进机、机身近红外LED标靶、机臂近红外利用相机校正技术实现图像立体校正。对近红外图
LED标靶、2台近红外相机组成的双目立体视觉相像进行图像增强、去噪,利用Canny边缘检测方法
机、三轴数字磁场计、永磁体组成,如图1所示。分割目标,并利用分散度筛选出真正的近红外LED
机身近红外LED标靶根据作业的实际情况固定在标靶,得到标靶质心坐标,根据欧氏距离等方法对
机身四周的边缘处,机臂近红外LED标靶安装在掘标靶质心进行定位、跟踪及匹配。然后,根据双目立
进机截割部护罩四周边缘处。采用近红外相机1,2体视觉测量原理计算巷道坐标系下的三维空间坐标,
对掘进工作面进行图像采集,并将双目立体视觉相在掘进机机身与截割部建立坐标系,根据ICP算法
机安装在巷道顶端,获得足够的相机视野。为防止求解巷道坐标系与机身坐标系、巷道坐标系与截割
在恶劣环境下图像采集失败、位姿测量失效等问题,部坐标系间的位姿关系。最后,根据位姿模型求解
在掘进机机身两侧各安装高精度三轴数字磁场计,掘进机截割部与机身间的位姿关系。
并在机臂处安装永磁体,以磁场的强度分量和双目由于煤矿井下掘进机的工作环境恶劣,巷道复
立体视觉获取的位姿数据作为训练数据,构建1D-杂多变,且双目立体视觉测量原理在测量距离上有
CNN模型,输出在视觉测量失效情况下掘进机截割限,导致掘进机在掘进过程中视觉位姿估计失效。
部位姿。针对该问题,引入基于1D-CNN的井下磁场视觉辅
助定位算法。在1D-CNN的基础上搭建网络模型,
该模型包括2个输入层,训练数据经过3层1D-CNN
卷积,每层卷积都经过批标准化(Batch
Normalization,BN)使其加快收敛,防止过拟合。
使用Selu激活函数后输入下一卷积层,不断进行改
善训练参数。由于特征较少故无须采用池化层,添
加平展层将张量重新整形为矢量,数据经卷积后引
入Selu激活函数连接输出层,得到悬臂式掘进机截:.
工矿自动化
第48卷JournalofMineAutomation
割部的位姿预测值。
图2悬臂式掘进机机身及截割部位姿测量方案

悬臂式掘进机机身及截割部位姿空间坐标系如图像对比度和亮度、改善图像质量。
图3所示。以机身与机臂近红外LED标靶的中心为将近红外LED标靶作为掘进机位姿估计测量
原点,建立机身坐标系OfXfYfZf,机臂坐标系的重要识别特征,通过最大类间方差法(Open
OeXeYeZe;以截割部中心点作为原点,建立截割部坐TechnologySupportUnit,Otsu)分割原理计算Canny
标系OhXhYhZh;以近红外相机1,2的光心作为相机算子得到最佳阈值,对近红外图像进行处理。设近
坐标系的原点,相机的光轴设为Zc轴,垂直相机光红外图像函数为二值函数Ixy(,),假设存在阈值f将
轴向下设为Yc轴,水平垂直相机光轴设为Xc轴,建图像分为前景和后景2类(小于阈值f的为前景,
立相机坐标系Oc1Xc1Yc1Zc1和Oc2Xc2Yc2Zc2;将近红外大于阈值f的为后景),前景和后景的均值分别为
相机1坐标系设置为世界坐标系即巷道坐标系mv1和mv2,整个图像的均值为mv,图像被分为前
OwXwYwZw。景和后景的概率分别为p1和p2。
mvp1mv1p2mv2(1)
p1p21(2)
根据方差知识,可得类间方差为
2p1mv1(3)mv2p2mv2mv2
图3悬臂式掘进机机身及截割部位姿空间坐标系将式(3)化简后,可得

spacecoordinatesystem2pp2(4)
12mv2mv1
2近红外标靶图像特征提取、定位与匹配
阈值的确定是控制边缘检测效果的关键,将通

煤矿井下作业面受高粉尘、高湿度、强振动和测双阈值检测步骤中,获取不同目标的图像边缘特
电磁等干扰[16]的影响,双目立体视觉相机采集到的征。将获取到的真正边缘轮廓信息后进行拟合,根
图像质量较差且伴有噪声,因此采用滤波、平滑等据分散度方法进行特征选择剔除非标靶的干扰目标。
预处理技术对立体校正后的图像进行处理,以提高近红外LED标靶提取如图4所示。可看出近红外:.
工矿自动化
第48卷JournalofMineAutomation
LED标靶的特征提取与识别效果较好。
(a)近红外相机拍摄的现场(b)Otsu法加Canny边缘检测的分割图(c)利用分散度算法筛选出的目标标靶
图4近红外LED标靶特征提取结果

,分别计算右相机图像各个重
xyll11,
悬臂式掘进机双目视觉位姿测量方法中利用重
心进行特征点匹配,利用图像的矩来计算近红外心平移后的特征点之间的欧氏距离D。
xre,yre
LED标靶轮廓重心坐标,以一阶矩和零阶矩来计算
重心,实现近红外双目图像中标靶中心的精确定位。(5)
Dxlexre2yleyre2
匹配过程如下:
(1)采集目标轮廓信息。计算左相机图像目提取左相机图像第1个特征点与距右相
xyll11,
标边缘轮廓点像素坐标xle,yle,取X方向和Y方向机图像标靶重心平移后的特征点欧氏距离最近的最
近邻点与次邻近点。提取这2个关键点,用最近邻
坐标值计算均值xle,yle;计算右相机图像目标边缘
点欧氏距离Dm除以次邻近点的欧氏距离Dm+1得到
轮廓点像素坐标x,y,取X方向和Y方向坐标比率。
rere
D(6)
均值为x,y。其中le与re分别为左相机图像与Dm
rerem1
右相机图像第e个边缘轮廓点。当<,则左相机图像第1个特征点与最
(2)计算X方向和Y方向的差值。将右相机
近邻点为1对匹配点。否则就返回重新匹配。依次
图像边缘点像素坐标X方向的平均值与左相机图像
边缘点像素坐标X方向的平均值相减,得X方向上类推,将左相机图像中所有特征点进行匹配。
3悬臂式掘进机位姿估计
的差值x,同理计算Y方向上的差值y。

(3)计算右相机图像质心平移后的新坐标点。将巷道坐标系与机身坐标系下的3D坐标作为
将右相机图像重心坐标与差值x,相减,即右输入,通过刚性变换(旋转变换和平移变换)使其转
y
换到同一坐标系下。设巷道坐标系下目标点集为
相机图像标靶重心平移至左相机图像处,得到右相
机图像各个重心平移到左相机图像后新坐标点SXYZ,,,机身坐标系下参考点集为JXYZ(,,),
。将机身坐标系下参考点集通过刚性变换到巷道坐标
xle,yle
系下目标点集,其转换关系为
(4)计算欧氏距离。以左相机图像第一个特:.
工矿自动化
第48卷JournalofMineAutomation
XX式中M为映射矩阵。
三维点云围绕xyz,,轴旋转,可得到如下转换关
YY
M(7)系。
ZZ

11
XXt1000cos0sin0cossin00x
YYt0cossin00100sincos00
(8)=y
ZZt0sincos0sin0cos00010z

1000**********
式中:为偏航角;为俯仰角;为翻滚角;tttxyz,,
kkk111N2
dSJNi1ii
(9)
xyz,,轴旋转的偏移量。
分别为围绕
(5)当dk1≥时,则返回步骤(1),直到dk1<

或迭代次数k大于预设的最大迭代次数,算法结
ICP配准是一种准确可靠的自由曲面配准方法。束。
其算法用于寻找目标点集S和参考点集J之间的刚
当算法结束后有效收敛到局部最小值,则最后1次
性变换,使2个匹配数据在某种度量标准下满足最迭代得到的目标点集即为最终的目标点集。
优匹配。假设目标点集S的坐标为

SSZiN3,1,2,,,NS为目标点集中第N个
iiS搭建模拟测量掘进机位姿实验平台,分别在机
目标点,参考点集J的坐标为身与机臂安装近红外LED标靶。掘进机机身近红
外LED标靶在巷道坐标系(即相机坐标系)下的
JJZiN3,1,2,,,NJ为参考点集中第N个
iiJaaa
111213
目标点,第i个参考点集Ji经过k次迭代后的坐标
a21a22a23
为。计算目标点集点集为,A,其中,a为方
JJZiNiiJkk3,1,2,,S与参Aaaay
313233
的对应点之间的变换矩阵a41a42a43
考点集J经过k次迭代后
并更新原始变换,直到目标点集与参考点集间的距
向上轴心距,在机身坐标系下的点集为A,
离小于给定的阈值。ICP算法步骤如下:
(1)计算第i个参考点集Ji经过k次迭代后的a11a12a13
对应点与第i个目标点集Si经过k次迭代后的对应
a21a22a23
点的对应关系,使得。A近红外LED标
JSiikkmin。掘进机机臂
a31a32a33

(2)计算经过k次迭代后旋转矩阵Rk和平移矩aaa
414243
阵Tk,满足关系Nkkkk。2
i1RSTJiimin靶在巷道坐标系下的点集为B,
(3)运用经过k次迭代后旋转矩阵Rk与平移矩b11b12b13
阵Tk变换目标点集S的位置,即
b21b22b23
。B,其中,b为掘进机截割头中
SSSRSTSSiiiikkkkkk+1+1,bbb
313233
(4)将经过k+1次迭代后的均方误差dk+1作为
b41b42b43

目标函数。:.
工矿自动化
第48卷JournalofMineAutomation
心点距离机臂中心的初始位置,设置初始位置为
0,在机身坐标系下的点集为B,截割部坐标系的平移矩阵,0。其中为
Tad
4
bd
b11b12b13
掘进机截割部的液压关节处的伸缩量d=0。
b21b22b23
B4双目视觉位姿辅助估计
。
b31b32b33

b41b42b43
将处于不同强度范围的数据进行归一化处理,
通过ICP算法使得不同坐标系下的空间点数据用于网络训练。
合并到同一坐标系中,即得出巷道坐标系与掘进机
CnminC(11)
机身坐标系的变换矩阵。根据位姿CnmaxCminC
H1,H1=RT11
01
解算模型可得巷道坐标系与掘进机机臂坐标系的变式中:C为输入变量的实测值集合;C为第n个输
n
换矩阵H2,RT。
H2=0122
入变量的实测值;Cn为第n个输入变量归一化的值。
由悬臂式掘进机的构造可知掘进机机臂通过回
转结构和液压系统与机身进行连接,-CNN截割部位姿估计网络模型
臂的上下摆动与左右回转。通过位姿解算模型可得3层卷积层的一维卷积核大小均为3,卷积核的
掘进机机身与机臂在巷道坐标系下的位姿信息,可
数量依次为16,32,64,模型使用Adam优化方法
计算出掘进机机身与机臂间的位姿关系。训练参数,。经Selu激活函数激活
以巷道坐标系为中间量,根据巷道坐标系与机对输入信息进行非线性变换后输入下一卷积层,不
身坐标系、巷道坐标系与机臂坐标系的关系,计算断改善训练参数,批大小为64,迭代次数为1000。
掘进机机身坐标系与机臂坐标系的变换矩阵H3,数据经卷积后引入Selu激活函数连接输出层,经过
不断改进优化,最后得到1D-CNN截割部位姿估计
RT。网络模型。
H3=0133


HHHR1R双目立体视觉相机安装在巷道顶部,掘进机截2T1R2R1R2T1T210
31201
割部在前方工作面进行实时切割煤层,但随着掘进
由此可解算出机身坐标系与机臂坐标系间的变机掘进加快,掘进机距离增加或双目立体视觉相机
换关系,即旋转矩阵R3与平移矩阵T3。根据偏航角被外物遮挡等因素,造成双目图像获取失败,进而
、俯仰角、翻滚角解算出绕机臂坐标系x,y,导致视觉解算截割部位姿失败。采用视觉与磁场相
z轴的旋转角度、、。结合的掘进机测量策略,以保证掘进机截割部实时
综上,可计算出掘进机在巷道坐标系下掘进机测量的要求。
机身与机臂的空间位姿及机臂相对于机身的空间位双目视觉测量失效下磁场辅助定位相机前移如
姿。图5所示。随着掘进机不断前进,双目立体视觉相
因截割部与掘进机机臂的运动相同,则掘进机机与掘进机距离不断增加,当掘进距离较远时,近
机身坐标系与截割部坐标系的变换矩阵红外LED标靶采集出现缺失,此时位姿解算模型位
HH43,
姿解算失败。为保证掘进机位姿解算的实时连续性,
磁场辅助定位算法开始工作,掘进机两侧磁场计采
截割部平移矩阵为TTT534,T4为机臂坐标系与
集的磁场信号数据经过数据处理,调用已经训练好
的1D-CNN位姿估计模型(视觉位姿解算数据实时
更新,磁场信号数据训练模型需不断进行更新,以:.
工矿自动化
第48卷JournalofMineAutomation
符合掘进机当时的巷道磁场环境),输出此时掘进掘进机位姿2个方面对掘进机测量进行验证分析。
机截割部位姿信息。双目立体视觉相机从地点A移本实验由2部MakoG-223BNIR近红外视觉相
动到适宜工作的地点B,双目立体视觉相机开始正机搭建双目立体视觉相机测量系统,由8组LED灯
常工作。以此类推掘进机不断掘进,保证掘进机截组成近红外LED标靶,采用690nm工业激光测距
割部位姿解算一直保持连续性,其中网络模型需实模块对近红外LED标靶进行深度信息测量,采用角
时训练,即掘进机边掘进边训练,以防止井下磁场度传感器对掘进机截割部相对机身坐标系的位姿变
异常变化带来的模型失效。换信息进行测量。
在实验过程中模拟掘进机截割部作切割煤层运
动,将高精度传感器安装在机臂中间位置,对截割
部的空间位置进行检测,双目立体视觉相机测量系
统对截割部模拟切割煤层过程进行实时数据采集,
获取数据后传输至计算机,计算机对图像进行分析
处理,计算出截割部与机身的三维空间信息及截割
部与机身的位姿信息,最后根据截割部与机身的转
图5双目视觉测量失效下磁场辅助定位相机前移换关系解算截割部空间位姿。

forwardunderthefailureofbinocularvisionmeasurement变化如图6所示。可看出在掘进机机身静止、截割
5实验结果与分析部作截割煤层运动时,机身空间坐标未出现明显波
动,截割部空间坐标发生变化,空间坐标变化符合

实际截割部运动状态,说明基于近红外双目立体视
为验证近红外双目立体视觉的悬臂式掘进机截觉空间位置定位方法可行。
割部及机身位姿测量方案的可行性,从深度信息和
图6掘进机机身与截割部空间坐标变化

在巷道坐标系下掘进机机身与截割部空间深度对误差在1%以内,该误差在井下煤矿施工标准误差
及误差见表1。可看出掘进机机身在Z轴方向的测允许范围内,说明该系统满足掘进过程中悬臂式掘
量误差在10mm以内,%以内。进机机身及截割部空间位置的精确测量需求。
截割部在Z轴方向的测量误差在50mm以内,相
表1掘进机机身与截割部空间深度及误差
Table1Spacedepthanderrorofroadheaderbodyandcuttingpart
机身截割部
序号
测量值/mm真实值/mm误差/mm相对误差%测量值/mm真实值/mm误差/mm相对误差%
:.
工矿自动化
第48卷JournalofMineAutomation




巷道坐标系下的机身与截割部位姿变化如图7部空间位姿角发生变化,其位姿角变化符合实际截
所示。可看出在掘进机机身静止、截割部作截割煤割部运动状态,说明基于近红外双目立体视觉空间
层运动时,机身空间位姿角未出现明显波动,截割姿态定位方法可行。
图7巷道坐标系下的机身与截割部位姿变化
Fig