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

1.本发明属于测量控制技术领域，尤其涉及一种掘进机位姿测量方法、系统、介质、设备及终端。


背景技术：

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


技术实现要素：

8.针对现有技术存在的问题，本发明提供了一种掘进机位姿测量方法、系统、介质、设备及终端，尤其涉及一种基于多棱镜轨迹预测的掘进机位姿测量方法、系统、介质、设备及终端。
9.本发明是这样实现的，一种掘进机位姿测量方法，所述掘进机位姿测量方法包括：
10.在测量完棱镜1至棱镜n-1后，测量棱镜n时预测棱镜1至棱镜n-1在测量棱镜n时的坐标；读取该时刻角度传感器输出的3个姿态角，将预测的棱镜1至棱镜n-1的坐标和测量的
棱镜n的坐标联立方程组，进行掘进机位姿的求解。
11.进一步，所述掘进机位姿测量方法还包括：
12.在掘进机上安装n个棱镜和m个姿态角的角度传感器；全站仪依次测量1～n个棱镜的坐标，完成第n个棱镜测量后，预测1～n-1号棱镜在测量n号棱镜时刻的坐标，将坐标统一到同一时刻的同一坐标系下；预测算法采用分段卡尔曼滤波算法进行预测，预测算法为依次匹配掘进机所处的设计线路的线性，建立状态方程，并与观测值融合，进行卡尔曼滤波预测。
13.进一步，所述掘进机位姿测量方法包括以下步骤：
14.步骤一，利用全站仪依次测量掘进机上多个棱镜的坐标，利用安装在掘进机上的角度传感器测量多个姿态角；
15.步骤二，利用分段卡尔曼滤波算法预测多个棱镜在同一时刻下的坐标；
16.步骤三，采用坐标转换模型建立测量方程组，利用最小二乘法对位姿进行求解。
17.进一步，所述多个棱镜使用2个及2个以上的棱镜，优选棱镜数量为3个。
18.所述角度传感器采用倾角传感器或陀螺仪等传感器，所述姿态角使用1个、2个或者3个姿态角，优选为3个姿态角。
19.状态方程根据掘进机当前所处的设计线路段的线型进行确定，分别按照直线段、缓和曲线段和圆曲线段进行确定。
20.进一步，所述掘进机位姿测量方法还包括：
21.(1)构建状态方程
22.1)直线状态方程
23.x(k 1)＝φx(k) γw(k)；
24.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]m11
＝cos(θk)
·
cos(βk)
·
cos(α)-sin(θk)
·
cos(βk)
·
sin(α)；
[0032]m12
＝-dlk·
sin(θk)
·
cos(βk)
·
cos(α)-dlk·
cos(θk)
·
cos(βk)
·
sin(α)；
[0033]m12
＝-dlk·
cos(θk)
·
sin(βk)
·
cos(α) dlk·
cos(θk)
·
sin(βk)
·
sin(α)；
[0034]m21
＝cos(θk)
·
cos(βk)
·
sin(α) sin(θk)
·
cos(βk)
·
cos(α)；
[0035]m22
＝-dlk·
sin(θk)
·
cos(βk)
·
cos(α) dlk·
cos(θk)
·
cos(βk)
·
cos(α)；
[0036]m23
＝-dlk·
cos(θk)
·
sin(βk)
·
cos(α)-dlk·
sin(θk)
·
sin(βk)
·
cos(α)；
[0037][0038][0039]
3)圆曲线状态方程
[0040]
圆曲线与缓和曲线结构类似，只有角度增量计算有区别因此对于公式有：
[0041]m31
＝0；
[0042]
[0043]
其中，n为北坐标，e为东坐标，h为高程，dl为油缸增量，α为设计线路的方位角，t为测量周期，β为俯仰角，α为方位角，l为前进距离；速度v和加速度a通过测量得到。
[0044]
(2)构建观测方程
[0045]
掘进机前进距离通过油缸行程传感器测量得到，棱镜坐标通过全站仪测量得到，采用油缸行程传感器的行程和全站仪测量得到的棱镜的坐标作为观测量。
[0046]
采用全站仪测量的棱镜坐标和油缸行程传感器测量的油缸行程作为观测值，列出观测方程：
[0047]
z＝[n e h l]
t
；
[0048][0049][0050]
(3)预测更新
[0051]
1)直线段采用kalman滤波直接进行预测：
[0052][0053]
p(k 1|k)＝φ(k 1|k)p(k)φ
t
(k 1|k) q(k 1)；
[0054]
k(k 1)＝p(k 1|k)h
t
(k 1)[h(k 1)p(k 1|k)h
t
(k 1) r(k 1)]-1
；
[0055][0056]
p(k 1)＝[i-k(k 1)h(k 1)]p(k 1|k)；
[0057]
2)对于缓和曲线和圆曲线段因为存在非线性函数，对线性化后的模型采用扩展kalman进行预测，则：
[0058]
[0059]
p(k 1|k)＝φ(k 1|k)p(k)φ
t
(k 1|k) q(k 1)；
[0060]
k(k 1)＝p(k 1|k)h
t
(k 1)[h(k 1)p(k 1|k)h
t
(k 1) r(k 1)]-1
；
[0061][0062]
p(k 1)＝[i-k(k 1)h(k 1)]p(k 1|k)。
[0063]
进一步，所述掘进机位姿测量方法还包括：
[0064]
角度传感器测量掘进机的m个姿态角，根据所述预测后的棱镜组坐标和姿态角，联立方程组；以最小二乘准则作为估计条件，求误差改正最优解。
[0065]
本发明的另一目的在于提供一种应用所述的掘进机位姿测量方法的掘进机位姿测量系统，所述掘进机位姿测量系统包括：
[0066]
姿态角测量模块，用于利用全站仪依次测量掘进机上多个棱镜的坐标，利用安装在掘进机上的角度传感器测量多个姿态角；
[0067]
棱镜坐标预测模块，用于利用分段卡尔曼滤波算法预测多个棱镜在同一时刻下的坐标；
[0068]
位姿测量模块，用于采用坐标转换模型建立测量方程组，利用最小二乘法对位姿进行求解。
[0069]
本发明的另一目的在于提供一种计算机设备，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如下步骤：
[0070]
在测量完棱镜1至棱镜n-1后，测量棱镜n时预测棱镜1至棱镜n-1在测量棱镜n时的坐标；读取该时刻角度传感器输出的3个姿态角，将预测的棱镜1至棱镜n-1的坐标和测量的棱镜n的坐标联立方程组，进行掘进机位姿的求解。
[0071]
本发明的另一目的在于提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如下步骤：
[0072]
在测量完棱镜1至棱镜n-1后，测量棱镜n时预测棱镜1至棱镜n-1在测量棱镜n时的坐标；读取该时刻角度传感器输出的3个姿态角，将预测的棱镜1至棱镜n-1的坐标和测量的棱镜n的坐标联立方程组，进行掘进机位姿的求解。
[0073]
本发明的另一目的在于提供一种信息数据处理终端，所述信息数据处理终端用于实现所述的掘进机位姿测量系统。
[0074]
结合上述的技术方案和解决的技术问题，请从以下几方面分析本发明所要保护的技术方案所具备的优点及积极效果为：
[0075]
第一、针对上述现有技术存在的技术问题以及解决该问题的难度，紧密结合本发明的所要保护的技术方案以及研发过程中结果和数据等，详细、深刻地分析本发明技术方案如何解决的技术问题，解决问题之后带来的一些具备创造性的技术效果。具体描述如下：
[0076]
本发明为解决掘进机施工中多棱镜姿态测量精度低的难题，提出基于多棱镜轨迹预测的掘进机位姿测量方法。本发明根据掘进机所处设计线路的位置，分别建立直线段、缓和曲线段、圆曲线段的状态方程，与测量的棱镜坐标进行卡尔曼滤波，预测出在同一时刻下的棱镜坐标。再以棱镜误差最小为目标，对多个棱镜坐标和多个姿态角进行最小二乘的位姿求解。相较于现有多棱镜方法，本发明提供的基于多棱镜轨迹预测的掘进机位姿测量方法能够显著提高测量精度和测量频率。
[0077]
第二，把技术方案看做一个整体或者从产品的角度，本发明所要保护的技术方案具备的技术效果和优点，具体描述如下：
[0078]
本发明解决了因棱镜测量不同步产生的位置误差，提高了位姿测量的精度。
[0079]
第三，作为本发明的权利要求的创造性辅助证据，还体现在以下几个重要方面：
[0080]
(1)本发明的技术方案填补了国内外业内技术空白：
[0081]
国内外现有的掘进机配备的多棱镜法的导向系统，在掘进机施工时测量误差较大，均需要在掘进机停机的状态下才能进行准确的位姿测量。本发明可以实现掘进机在掘进施工时，动态准确测量掘进机位姿。
[0082]
(2)本发明的技术方案是否解决了人们一直渴望解决、但始终未能获得成功的技术难题：采用多棱镜法导向系统的掘进机施工时为了测量准确的位姿，往往需要停下来进行位姿测量以保证测量精度，这严重影响了施工效率。动态、实时、准确的测量掘进机位姿是多棱镜法掘进机位姿测量面临的重要问题。本发明解决了多棱镜法测量时因掘进机移动带来的测量误差，可以实现多棱镜法掘进机位姿的实时动态测量，可以实现施工效率的提升。
附图说明
[0083]
为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例中所需要使用的附图做简单的介绍，显而易见地，下面所描述的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下还可以根据这些附图获得其他的附图。
[0084]
图1是本发明实施例提供的掘进机位姿测量方法流程图；
[0085]
图2是本发明实施例提供的掘进机位姿测量方法原理图；
[0086]
图3是本发明实施例提供的掘进机位姿测量系统结构框图；
[0087]
图4是本发明实施例提供的分段卡尔曼滤波预测的流程图；
[0088]
图5是本发明实施例提供的现有方案的仿真结果；
[0089]
图6是本发明实施例提供的本发明的仿真结果。
[0090]
图中：1、姿态角测量模块；2、棱镜坐标预测模块；3、位姿测量模块。
具体实施方式
[0091]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0092]
针对现有技术存在的问题，本发明提供了一种掘进机位姿测量方法、系统、介质、设备及终端，下面结合附图对本发明作详细的描述。
[0093]
一、解释说明实施例。为了使本领域技术人员充分了解本发明如何具体实现，该部分是对权利要求技术方案进行展开说明的解释说明实施例。
[0094]
如图1所示，本发明实施例提供的掘进机位姿测量方法包括以下步骤：
[0095]
s101，利用全站仪依次测量掘进机上多个棱镜的坐标，利用安装在掘进机上的角度传感器测量多个姿态角；
[0096]
s102，利用分段卡尔曼滤波算法预测多个棱镜在同一时刻下的坐标；
[0097]
s103，采用坐标转换模型建立测量方程组，利用最小二乘法对位姿进行求解。
[0098]
本发明实施例提供的掘进机位姿测量方法原理图如图2所示。
[0099]
如图3所示，本发明实施例提供的掘进机位姿测量系统包括：
[0100]
姿态角测量模块1，用于利用全站仪依次测量掘进机上多个棱镜的坐标，利用安装在掘进机上的角度传感器测量多个姿态角；
[0101]
棱镜坐标预测模块2，用于利用分段卡尔曼滤波算法预测多个棱镜在同一时刻下的坐标；
[0102]
位姿测量模块3，用于采用坐标转换模型建立测量方程组，利用最小二乘法对位姿进行求解。
[0103]
本发明实施例提供的分段卡尔曼滤波预测的流程图如图4所示。
[0104]
下面结合具体实施例对本发明的技术方案作进一步的说明。
[0105]
实施例1采用全站仪测量棱镜坐标和倾角传感器测量姿态角
[0106]
本发明提供一种基于多棱镜轨迹预测的掘进机位姿测量方法，用以解决现有掘进机多棱镜测量精度低的技术问题。
[0107]
本发明解决上述技术问题的技术方案如下：
[0108]
掘进机上安装3个棱镜和2个姿态角的角度传感器。全站仪依次测量1～3个棱镜的坐标，完成第n个棱镜测量后，预测1～n-1号棱镜在测量n号棱镜时刻的坐标，将其坐标统一到同一时刻的同一坐标系下。预测算法可以采用分段卡尔曼滤波算法进行预测，预测算法为依次匹配掘进机所处的设计线路的线性，建立状态方程，然后与观测值融合，进行卡尔曼滤波预测。其中，
[0109]
1.状态方程
[0110]
1)直线状态方程
[0111]
x(k 1)＝φx(k) γw(k)(1)
[0112]
x＝[nehdlva]
t
(2)
[0113][0114]
2)缓和曲线状态方程
[0115][0116]
其中，
[0117]
x＝[nehldlvaθdθβ]
t
(5)
[0118][0119]m11
＝cos(θk)
·
cos(βk)
·
cos(α)-sin(θk)
·
cos(βk)
·
sin(α)(7)
[0120]m12
＝-dlk·
sin(θk)
·
cos(βk)
·
cos(α)-dlk·
cos(θk)
·
cos(βk)
·
sin(α)(8)
[0121]m12
＝-dlk·
cos(θk)
·
sin(βk)
·
cos(α) dlk·
cos(θk)
·
sin(βk)
·
sin(α)(9)
[0122]m21
＝cos(θk)
·
cos(βk)
·
sin(α) sin(θk)
·
cos(βk)
·
cos(α)(10)
[0123]m22
＝-dlk·
sin(θk)
·
cos(βk)
·
cos(α) dlk·
cos(θk)
·
cos(βk)
·
cos(α)(11)
[0124]m23
＝-dlk·
cos(θk)
·
sin(βk)
·
cos(α)-dlk·
sin(θk)
·
sin(βk)
·
cos(α)(12)
[0125][0126][0127]
3)圆曲线状态方程
[0128]
圆曲线与缓和曲线结构类似，只有角度增量计算有区别因此对于公式4有：
[0129]m31
＝0(15)
[0130][0131]
其中，n为北坐标，e为东坐标，h为高程，dl为油缸增量，α为设计线路的方位角，t为
测量周期，β为俯仰角，α为方位角，l为前进距离。速度v和加速度a可以通过测量得到。
[0132]
2.观测方程
[0133]
掘进机前进距离可以通过油缸行程传感器测量得到，棱镜的坐标可以通过全站仪测量得到，因此采用油缸行程传感器的行程和全站仪测量得到的棱镜的坐标作为观测量。
[0134]
采用全站仪测量的棱镜坐标，和油缸行程传感器测量的油缸行程作为观测值。可列出观测方程：
[0135]
z＝[nehl]
t
(17)
[0136][0137][0138]
3.预测更新
[0139]
1)直线段可以采用kalman滤波直接进行预测
[0140][0141]
p(k 1|k)＝φ(k 1|k)p(k)φ
t
(k 1|k) q(k 1)(21)
[0142]
k(k 1)＝p(k 1|k)h
t
(k 1)[h(k 1)p(k 1|k)h
t
(k 1) r(k 1)]-1
(22)
[0143][0144]
p(k 1)＝[i-k(k 1)h(k 1)]p(k 1|k)
[0145]
(24)
[0146]
2)对于缓和曲线和圆曲线段因为存在非线性函数，对其线性化后的模型采用扩展kalman进行预测有：
[0147]
[0148]
p(k 1|k)＝φ(k 1|k)p(k)φ
t
(k 1|k) q(k 1)(26)
[0149]
k(k 1)＝p(k 1|k)h
t
(k 1)[h(k 1)p(k 1|k)h
t
(k 1) r(k 1)]-1
(27)
[0150][0151]
p(k 1)＝[i-k(k 1)h(k 1)]p(k 1|k)(29)
[0152]
该方法考虑了轨迹的速度和加速度的变化，可以精确预测棱镜的坐标。
[0153]
同时，倾角传感器测量掘进机的2个姿态角。根据所述预测后的棱镜组坐标和姿态角，联立方程组。以最小二乘准则作为估计条件，求误差改正最优解。
[0154]
本发明为解决掘进机施工中多棱镜姿态测量精度低的难题，提出基于多棱镜轨迹预测的掘进机位姿测量方法，相较于未预测的模型，本方法能提高测量精度和测量频率。
[0155]
实施例2采用全站仪测量棱镜坐标和陀螺仪测量姿态角
[0156]
本实例如图2所示，共采用全站仪测量3个棱镜和陀螺仪测量3个姿态角。
[0157]
1、全站仪首先测量位于位置1的1号棱镜，然后再测量位于位置2的2号棱镜，接着测量位于位置3的3号棱镜的坐标。测量3号棱镜坐标时，读取陀螺仪输出的3个姿态角。
[0158]
采用分段卡尔曼滤波预测方法预测测量3号棱镜时1号棱镜和2号棱镜的坐标。然后将预测的棱镜1、棱镜2的坐标和测量的棱镜3的坐标一起联立方程组，进行掘进机位姿的求解。
[0159]
2、全站仪再测量位于位置4的1号棱镜坐标，读取陀螺仪输出的3个姿态角。采用分段卡尔曼滤波预测方法预测测量1号棱镜时2号棱镜和3号棱镜的坐标。然后将预测的棱镜2、棱镜3的坐标和测量的棱镜1的坐标一起联立方程组，进行掘进机位姿的求解。
[0160]
3、全站仪再测量位于位置5的2号棱镜坐标，读取陀螺仪输出的3个姿态角。采用分段卡尔曼滤波预测方法预测测量2号棱镜时1号棱镜和3号棱镜的坐标。然后将预测的棱镜1、棱镜3的坐标和测量的棱镜2的坐标一起联立方程组，进行掘进机位姿的求解。
[0161]
4、修正陀螺仪随时间发散的误差。位置6为掘进机停止掘进的状态，此刻，依次测量的棱镜1、棱镜2和棱镜3的坐标，求解掘进机位姿，并将计算的姿态角重新标定陀螺仪角度。
[0162]
5、继续下一个测量循环。
[0163]
二、应用实施例。为了证明本发明的技术方案的创造性和技术价值，该部分是对权利要求技术方案进行具体产品上或相关技术上的应用实施例。
[0164]
应用例
[0165]
仅采用全站仪测量棱镜坐标
[0166]
本实例采用全站仪测量3个棱镜坐标。
[0167]
1、全站仪首先测量位于位置1的1号棱镜，然后再测量位于位置2的2号棱镜，接着测量位于位置3的3号棱镜的坐标。采用分段卡尔曼滤波预测方法预测测量3号棱镜时1号棱镜和2号棱镜的坐标。然后将预测的棱镜1、棱镜2的坐标和测量的棱镜3的坐标一起联立方程组，进行掘进机位姿的求解。
[0168]
2、全站仪再测量位于位置4的1号棱镜坐标。采用分段卡尔曼滤波预测方法预测测量1号棱镜时2号棱镜和3号棱镜的坐标。然后将预测的棱镜2、棱镜3的坐标和测量的棱镜1的坐标一起联立方程组，进行掘进机位姿的求解。
[0169]
3、全站仪再测量位于位置5的2号棱镜坐标。采用分段卡尔曼滤波预测方法预测测
量2号棱镜时1号棱镜和3号棱镜的坐标。然后将预测的棱镜1、棱镜3的坐标和测量的棱镜2的坐标一起联立方程组，进行掘进机位姿的求解。
[0170]
4、继续下一个测量循环。
[0171]
三、实施例相关效果的证据。本发明实施例在研发或者使用过程中取得了一些积极效果，和现有技术相比的确具备很大的优势，下面内容结合试验过程的数据、图表等进行描述。
[0172]
为验证本发明的效果，进行仿真实验如下：
[0173]
模拟生成一段三个棱镜和三个姿态角的样本数据包括直线段、缓和曲线段和圆曲线段。其中直线段长29m，缓和曲线段长50m，圆曲线长40m，转弯半径200m。分别采用现有方案和本发明的进行结果对比，见图5图6。
[0174]
可以看到现有方案误差一直较大，尤其在曲线段误差逐渐放大。而本发明的方案误差整体均较小。
[0175]
(1)现有的方案中，n坐标偏差刚开始速度不大时约为0mm，但是很快增加到20～30mm；e坐标偏差在直线段偏差为5mm左右，到曲线段之后增加到8～12mm；h坐标偏差一直在-4～5mm跳动。方位角、俯仰角在-0.2～0.2
°
之间跳动，滚动角在-0.15～0.15范围跳动。
[0176]
(2)本发明的方案中，n坐标、e坐标、h坐标的偏差均小于2mm。方位角、俯仰角偏差约在0.05
°
跳动，滚动角偏差均小于0.1
°
。
[0177]
应当注意，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行系统，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁盘、cd或dvd-rom的载体介质、诸如只读存储器(固件)的可编程的存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软件实现，也可以由上述硬件电路和软件的结合例如固件来实现。
[0178]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。