一种掘进机位姿测量方法、系统、介质、设备及终端技术方案

本发明专利技术属于掘进机施工技术领域，公开了一种掘进机位姿测量方法、系统、介质、设备及终端，在测量完棱镜1至棱镜n

A method, system, medium, equipment and terminal for measuring the position and attitude of roadheader

【技术实现步骤摘要】
一种掘进机位姿测量方法、系统、介质、设备及终端


[0001]本专利技术属于测量控制
，尤其涉及一种掘进机位姿测量方法、系统、介质、设备及终端。

技术介绍

[0002]目前，多棱镜法是隧道掘进机位姿测量的常用方法。隧道施工开始前，隧道掘进机厂家在掘进机上安装多个棱镜，并在厂内测量好这些棱镜和掘进机中心的位置关系。在施工过程中，这些棱镜和掘进机中心的位置关系不会发生变化。在掘进过程中，掘进机沿着设计轴线的方向运动，隧道施工项目的测量负责人首先在坐标位置已知的吊篮上安装全站仪和后视棱镜；然后使用全站仪依次对多个棱镜进行测量，这时可以计算得到每一个棱镜的三维坐标；将这些厂内测量和施工测量得到的棱镜坐标与掘进机首尾坐标，联立方程组，可以计算出掘进机中心在设计线路下的坐标。
[0003]但在实际使用中存在以下问题：掘进机施工过程中的由于围岩软硬的不同，导致掘进速度在一个范围内波动，掘进速度一般在10mm/min～120mm/min范围。全站仪测量棱镜包括搜索、照准、测量等几个步骤，受到环境光线、粉尘、全站仪自身电机转动速度等影响，测量一个棱镜需要花费的时间一般在10s～30s。整个测量过程中需要使用全站仪依次测量每一个棱镜，这会带来三个问题。第一个问题是隧道中一般多棱镜法只观测三个棱镜的坐标，缺少冗余观测量，一旦某个坐标测量误差较大会对最终的位姿解算精度带来巨大影响。第二个问题是测完第一个棱镜，再测量第二个棱镜时第一个棱镜又会发生位移，会导致所测量的棱镜坐标存在时空不匹配。第三个问题是整体测量周期比较长，一般会在1min～2min，导致姿态更新不及时。
[0004]通过上述分析，现有技术存在的问题及缺陷为：
[0005](1)隧道中一般多棱镜法只观测三个棱镜的坐标，缺少冗余观测量，一旦某个坐标测量误差较大会对最终的位姿解算精度带来巨大影响。
[0006](2)测完第一个棱镜，再测量第二个棱镜时第一个棱镜又会发生位移，会导致所测量的棱镜坐标存在时空不匹配。在通过这些不匹配的测量数据进行位姿解算时会带来严重的误差。
[0007](3)整体测量周期比较长，一般会在1min～2min，导致姿态更新不及时。

技术实现思路

[0008]针对现有技术存在的问题，本专利技术提供了一种掘进机位姿测量方法、系统、介质、设备及终端，尤其涉及一种基于多棱镜轨迹预测的掘进机位姿测量方法、系统、介质、设备及终端。
[0009]本专利技术是这样实现的，一种掘进机位姿测量方法，所述掘进机位姿测量方法包括：
[0010]在测量完棱镜1至棱镜n
‑
1后，测量棱镜n时预测棱镜1至棱镜n
‑
1在测量棱镜n时的坐标；读取该时刻角度传感器输出的3个姿态角，将预测的棱镜1至棱镜n
‑
1的坐标和测量的
棱镜n的坐标联立方程组，进行掘进机位姿的求解。
[0011]进一步，所述掘进机位姿测量方法还包括：
[0012]在掘进机上安装n个棱镜和m个姿态角的角度传感器；全站仪依次测量1～n个棱镜的坐标，完成第n个棱镜测量后，预测1～n
‑
1号棱镜在测量n号棱镜时刻的坐标，将坐标统一到同一时刻的同一坐标系下；预测算法采用分段卡尔曼滤波算法进行预测，预测算法为依次匹配掘进机所处的设计线路的线性，建立状态方程，并与观测值融合，进行卡尔曼滤波预测。
[0013]进一步，所述掘进机位姿测量方法包括以下步骤：
[0014]步骤一，利用全站仪依次测量掘进机上多个棱镜的坐标，利用安装在掘进机上的角度传感器测量多个姿态角；
[0015]步骤二，利用分段卡尔曼滤波算法预测多个棱镜在同一时刻下的坐标；
[0016]步骤三，采用坐标转换模型建立测量方程组，利用最小二乘法对位姿进行求解。
[0017]进一步，所述多个棱镜使用2个及2个以上的棱镜，优选棱镜数量为3个。
[0018]所述角度传感器采用倾角传感器或陀螺仪等传感器，所述姿态角使用1个、2个或者3个姿态角，优选为3个姿态角。
[0019]状态方程根据掘进机当前所处的设计线路段的线型进行确定，分别按照直线段、缓和曲线段和圆曲线段进行确定。
[0020]进一步，所述掘进机位姿测量方法还包括：
[0021](1)构建状态方程
[0022]1)直线状态方程
[0023]X(k+1)＝ΦX(k)+ΓW(k)；
[0024]X＝[N E H dl v a]T
；
[0025][0026]2)缓和曲线状态方程
[0027][0028]其中，
[0029]X＝[N E H l dl v a θ dθ β]T
；
[0030][0031]M
11
＝cos(θ
k
)
·
cos(β
k
)
·
cos(α)
‑
sin(θ
k
)
·
cos(β
k
)
·
sin(α)；
[0032]M
12
＝
‑
dl
k
·
sin(θ
k
)
·
cos(β
k
)
·
cos(α)
‑
dl
k
·
cos(θ
k
)
·
cos(β
k
)
·
sin(α)；
[0033]M
12
＝
‑
dl
k
·
cos(θ
k
)
·
sin(β
k
)
·
cos(α)+dl
k
·
cos(θ
k
)
·
sin(β
k
)
·
sin(α)；
[0034]M
21
＝cos(θ
k
)
·
cos(β
k
)
·
sin(α)+sin(θ
k
)
·
cos(β
k
)
·
cos(α)；
[0035]M
22
＝
‑
dl
k
·
sin(θ
k
)
·
cos(β
k
)
·
cos(α)+dl
k
·
cos(θ
k
)
·
cos(β
k
)
·
cos(α)；
[0036]M
23
＝
‑
dl...

【技术保护点】

【技术特征摘要】
1.一种掘进机位姿测量方法，其特征在于，所述掘进机位姿测量方法包括：在测量完棱镜1至棱镜n
‑
1后，测量棱镜n时预测棱镜1至棱镜n
‑
1在测量棱镜n时的坐标；读取该时刻角度传感器输出的3个姿态角，将预测的棱镜1至棱镜n
‑
1的坐标和测量的棱镜n的坐标联立方程组，进行掘进机位姿的求解。2.如权利要求1所述的掘进机位姿测量方法，其特征在于，所述掘进机位姿测量方法还包括：在掘进机上安装n个棱镜和m个姿态角的角度传感器；全站仪依次测量1～n个棱镜的坐标，完成第n个棱镜测量后，预测1～n
‑
1号棱镜在测量n号棱镜时刻的坐标，将坐标统一到同一时刻的同一坐标系下；预测算法采用分段卡尔曼滤波算法进行预测，预测算法为依次匹配掘进机所处的设计线路的线性，建立状态方程，并与观测值融合，进行卡尔曼滤波预测。3.如权利要求1所述的掘进机位姿测量方法，其特征在于，所述掘进机位姿测量方法包括以下步骤：步骤一，利用全站仪依次测量掘进机上多个棱镜的坐标，利用安装在掘进机上的角度传感器测量多个姿态角；步骤二，利用分段卡尔曼滤波算法预测多个棱镜在同一时刻下的坐标；步骤三，采用坐标转换模型建立测量方程组，利用最小二乘法对位姿进行求解。4.如权利要求1所述的掘进机位姿测量方法，其特征在于，多个棱镜使用2个及2个以上的棱镜，优选棱镜数量为3个；所述角度传感器采用倾角传感器或陀螺仪等传感器，所述姿态角使用1个、2个或者3个姿态角，优选为3个姿态角；状态方程根据掘进机当前所处的设计线路段的线型进行确定，分别按照直线段、缓和曲线段和圆曲线段进行确定。5.如权利要求1所述的掘进机位姿测量方法，其特征在于，所述掘进机位姿测量方法还包括：(1)构建状态方程1)直线状态方程X(k+1)＝ΦX(k)+ΓW(k)；X＝[N E H dl v a]
T
；2)缓和曲线状态方程
其中，X＝[N E H l dl v a θ dθ β]
T
；M
11
＝cos(θ
k
)
·
cos(β
k
)
·
cos(α)
‑
sin(θ
k
)
·
cos(β
k
)
·
sin(α)；M
12
＝
‑
dl
k
·
sin(θ
k
)
·
cos(β
k
)
·
cos(α)
‑
dl
k
·
cos(θ
k
)
·
cos(β
k
)
·
sin(α)；M
12
＝
‑
dl
k
·
cos(θ
k
)
·
sin(β
k
)
·
cos(α)+dl
k
·
cos(θ
k
)
·
sin(β
k
)
·
sin(α)；M
21
＝cos(θ
k
)
·
cos(β
k
)
·
sin(α)+sin(θ
k
)
·
cos(β
k
)
·
cos(α)；M
22
＝
‑
dl
k
·
sin(θ
k
)
·
cos(β
k
)
·
cos(α)+dl
...

【专利技术属性】
技术研发人员：朱国力，贺泊宁，张代林，
申请(专利权)人：华中科技大学，
类型：发明
国别省市：