本发明涉及竖井施工，具体为一种全断面竖井掘进机纠偏系统及纠偏控制算法。
背景技术：
1、竖井可用于矿石、物料、人员的运输以及通风、排水等，是矿产资源开发和地下空间建设的咽喉工程。全断面竖井掘进机是结合隧道掘进机技术、物料垂直提升技术的先进装备，可以进行高效破岩、上排渣、姿态控制自动钻进、井帮同步支护，实现在没有下部工作条件的岩石地层中精准钻凿井筒，已成为竖井建设的首选。
2、大直径井筒施工对井筒的偏斜程度有很高的要求，由于受到周围应力、温度、渗流等多场耦合作用的影响，全断面竖井掘进机在施工中常常偏离基准轴线。如果得不到及时、准确的纠偏，轻则影响施工进度，重则引起人员伤亡事故，纠偏控制能力已成为衡量全断面竖井掘进机性能的重要指标。
3、目前纠偏控制技术的竖井掘进机仍处于初期验证阶段，施工过程中掘进偏斜后的姿态调整仍为“人工远程操控”的开环模式，自动化程度有待提高，存在人工操作效率低、纠偏不及时、控制精度受人工操作经验影响等问题，因此，亟需开展全断面竖井掘进机的纠偏控制技术研究。
技术实现思路
1、本发明提供了一种全断面竖井掘进机纠偏系统及纠偏控制算法，具备提升施工过程掘进机的姿态调整自动化水平，确保竖井建设工程更安全、更高效的优点，解决了上述背景技术所提出的问题。
2、本发明提供如下技术方案：一种全断面竖井掘进机纠偏系统，包括纠偏系统和纠偏控制算法，所述纠偏系统由撑靴模块组成，
3、所述撑靴模块接收来自中央控制器的控制信号，且与竖井掘进机推进油缸配合，完成竖井掘进机纠偏过程的动作执行，所述撑靴模块的控制输入模型用于计算撑靴模块的实时控制信号。
4、优选的，所述纠偏控制算法集成于中央控制器，且所述纠偏控制算法包括全断面竖井掘进机纠偏轨迹、全断面竖井掘进机动力学模型、撑靴模块的控制输入模型；
5、所述全断面竖井掘进机纠偏轨迹负责提供全断面竖井掘进机的所有偏斜类型及各偏斜类型的纠偏施工轨迹；
6、所述全断面竖井掘进机动力学模型负责为纠偏控制算法提供被控对象的数学模型。
7、优选的，所述撑靴模块，包括有坐标轴、上框架、上层撑靴、中框架、中层撑靴、下框架、下层撑靴、支撑油缸、反力架、推进油缸、竖井设计轴线、掘进机机体轴线、刀盘、井壁、油缸缸体以及活塞杆。
8、优选的，所述上框架和下框架通过反力架相连接，所述上层撑靴的数量有四个，且四个均布的上层撑靴通过四个均布的支撑油缸安装在上框架周围，四个所述均布的中层撑靴通过四个均布的支撑油缸安装在中框架周围，四个所述均布的下层撑靴通过四个均布的支撑油缸安装在下框架周围。
9、优选的，所述中框架在上框架与下框架中间，且中框架可沿着反力架上下活动，所述上层撑靴、中层撑靴以及下层撑靴上中下三层撑靴均由所在层支撑油缸推动与井壁接触以保证固定。
10、优选的，所述支撑油缸和推进油缸皆与控制器为电性连接。
11、优选的，所述全断面竖井掘进机纠偏轨迹包括掘进机三维平面至二维平面转换、掘进机偏斜类型及各偏斜类型的纠偏施工轨迹。
12、优选的，所述掘进机三维平面至二维平面转换，且所述上框架平面中心点为圆心o，所述掘进机机体轴线向下为z轴正向，建立竖井掘进机附体坐标系o-xyz，所述掘进机偏斜类型和各偏斜类型的纠偏施工轨迹，且在xoz平面和yoz平面相同。
13、优选的，所述撑靴模块的控制输入模型包括全断面竖井掘进机动力学模型的状态空间方程、掘进机自适应积分鲁棒纠偏控制器及控制器稳定性分析。
14、一种全断面竖井掘进机纠偏系统的纠偏控制算法，
15、以xoz平面为例，xb表示上框架平面中心点至竖井设计轴线的距离，即上框架平面中心点的位移偏斜，
16、xp表示刀盘顶点至竖井设计轴线的距离，即刀盘顶点的位移偏斜；
17、β表示掘进机机体轴线和竖井设计轴线的夹角，即掘进机的角度偏斜。
18、定义位移偏斜在x轴正向一侧时为正，反之为负，定义角度偏斜β绕竖井设计轴线顺时针偏转时为正，反之为负。
19、定义xd>0表示位移偏斜阈值，ld>0表示位移偏斜的安全极限；
20、定义βd>0表示角度偏斜的安全极限。
21、参数xb和β由位姿测量模块一检测得到，参数xp由参数xb结合竖井掘进机结构尺寸换算得到，为了减小纠偏过程中竖井掘进机的结构受力，纠偏时需保证竖井掘进机的钻进趋势向着井筒中心方向，通过判断参数xp、β、xd、ld的相互大小关系，可规划出xoz平面上竖井掘进机不同姿态下的纠偏轨迹，共计分为八种情况：
22、当|xp|>ld或|β>βd，表示竖井掘进机偏斜量超出了安全运行极限，可认定设备或者环境异常，需停机检查。
23、当|β<βd且|xp|≤xd，表示竖井掘进机角度偏斜β和刀盘顶点位移偏斜xp均在允许范围内，竖井掘进机位姿状态运行正常，可继续保持当前钻进方向。
24、当β>0,|β<βd,xd<xp<ld或β<0,|β<βd,xd<-xp<ld，即图4中①和⑥，表示竖井掘进机角度偏斜β在允许范围内，刀盘顶点的位移偏斜xp超出允许范围，但当前竖井掘进机的钻进趋势是向着靠近竖井设计轴线方向的。首先，保持当前钻进方向，当满足xp＝0时，推进油缸停止，期间竖井掘进机可多次迈步。然后，控制支撑油缸的伸缩，以刀盘的顶点为转动中心，让竖井掘进机转动，将角度偏斜β调节至零，期间刀盘维持运转确保扩出纠偏所需空间，至xp＝0且β＝0后，完成纠偏；
25、当β＝0且xd<|xp|<ld，即图4中③和④，表示竖井掘进机刀盘顶点位移偏斜xp超出允许范围，掘进机角度偏斜β为0。首先，迈步使推进油缸缩回，然后控制支撑油缸的伸缩，以刀盘的顶点为转动中心，让竖井掘进机转动，将竖井掘进机的钻进趋势变化成向着井筒设计轴线方向，期间刀盘维持运转确保扩出纠偏所需空间，支撑油缸推动撑靴撑紧井壁，推进油缸伸出保持当前钻进方向，当满足xp＝0后，推进油缸停止，期间竖井掘进机可多次迈步。最后，推进油缸缩回，控制支撑油缸伸缩，以刀盘顶点为转动中心，让竖井掘进机转动，将β调节至零，期间刀盘维持运转确保扩出纠偏所需空间，至xp＝0且β＝0后，完成纠偏；
26、当β>0,|β<βd,xd<-xp<ld或β<0,|β<βd,xd<xp<ld，即图4中②和⑤，表示竖井掘进机角度偏斜β和刀盘顶点的位移偏斜xp超出允许范围，且当前竖井掘进机的钻进趋势是远离井筒设计轴线方向的。
27、首先，迈步使推进油缸缩回，然后控制支撑油缸伸缩，以刀盘顶点为转动中心，让竖井掘进机转动，将竖井掘进机的钻进趋势变化成与井筒设计轴线平行方向，刀盘维持运转确保扩出纠偏所需空间，角度偏斜调整完成后支撑油缸推动撑靴撑紧井壁，保持当前钻进方向，推进油缸伸出，钻进一段距离；迈步使推进油缸缩回，然后控制支撑油缸伸缩，以刀盘顶点为转动中心，让竖井掘进机转动，将竖井掘进机的钻进趋势变化成向着井筒轴线方向，角度偏斜调整完成后支撑油缸推动撑靴撑紧井壁，保持当前钻进方向，推进油缸伸出，当满足xp＝0后，推进油缸停止，期间竖井掘进机可多次迈步。最后，推进油缸缩回，控制支撑油缸伸缩，以刀盘顶点为转动中心，让竖井掘进机转动，将β调节至零，期间刀盘维持运转确保扩出纠偏所需空间，至xp＝0且β＝0后，完成纠偏；
28、全断面竖井掘进机动力学模型包括掘进机纠偏平面坐标设定、掘进机运动学方程、掘进机动力学方程、广义坐标下的受力分析和支撑油缸液压系统数学模型，xoz平面和yoz平面上的纠偏问题完全相同，可以任一平面为基础建立竖井掘进机动力学模型，根据已规划的纠偏轨迹，无论在xoz平面上还是在yoz平面上，纠偏动作均由四个支撑油缸配合完成。
29、以角度纠偏为例，所述掘进机纠偏平面坐标设定，为竖井掘进机xoz平面，左右两侧各有两个支撑油缸以及撑靴，撑靴支撑在已经成型的井壁上，由于每个跃步所完成的纠偏角度较小，忽略撑靴与井壁的滑移，将撑靴看作是在井壁上固定的，各支撑油缸由油缸缸体和活塞杆组成，所述油缸缸体与掘进机主框架固联，所述活塞杆与撑靴铰接，掘进机主框架在四个液压缸的推进作用下能够在平面内平移和转动，以便于纠正角度偏斜和位移偏斜，
30、建立图中所示的两个坐标系，其中坐标系{a}为惯性坐标系，该坐标系坐标原点在右侧两组撑靴连接线中点，za轴平行于已成型井壁向下，xa轴垂直于za轴，方向按右手定则确定，坐标系{b}为附体坐标系，其坐标原点在掘进机主框架上框架与掘进机机体轴线交点，zb轴与掘进机机体轴线重合，方向向下，xb轴垂直于zb轴，方向按右手定则确定，该坐标系与掘进机刚体固联，
31、bi点为支撑油缸活塞杆与撑靴铰接点，bi点为支撑油缸缸体与掘进机刚体框架连接点，d1为推进油缸缩回时，上层支撑油缸活塞杆与撑靴铰接点与刀盘顶点的垂直距离，d2为上下层支撑油缸之间的距离，d3为掘进机质心距刀盘顶点的距离，d4为两侧支撑油缸与撑靴铰接点之间的距离，d5为两侧支撑油缸与掘进机刚体框架连接点的距离。惯性坐标系{a}与附体坐标系{b}的关系用表示，坐标系{b}的坐标原点ob在坐标系{a}的坐标矢量为aob＝(xb zb)t，坐标系{b}中一点ap在坐标系{a}中可表示为：
32、
33、惯性坐标系{a}与附体坐标系{b}二维坐标转换矩阵可表示为：
34、
35、同时附体坐标系{b}与惯性坐标系{a}二维坐标转换矩阵可表示为：
36、
37、式(3)中：β表示附体坐标系相对于惯性坐标系的旋转角度。
38、掘进机纠偏时，支撑油缸伸/缩使掘进机绕刀盘顶点旋转，掘进机刀盘旋转扩出纠偏所需空间，用矢量q＝[xb zb β]t表示掘进机刚体位姿，其中绕刀盘顶点的旋转角速度ω和旋转角β的导数有以下关系：
39、
40、所述掘进机运动学方程的建立方法为：
41、由于每次迈步所需完成的纠偏角度较小，每次支撑油缸伸/缩前将推进油缸缩回，此时推进油缸的伸出长度固定为零，忽略撑靴与井壁的滑移，参考图5，各点坐标为：
42、ab1＝[-d4 -d2/2]t，ab2＝[-d4 d2/2]t，
43、ab3＝[0 -d2/2]t,ab4＝[0 d2/2]t,
44、bb1＝[-d5/2 0]t,bb2＝[-d5/2 d2]t,
45、bb3＝[d5/2 0]t,bb4＝[d5/2 d2]t
46、
47、定义li为每个支撑油缸活塞杆与撑靴铰接点至油缸缸体与主框架连接点的距离，可表示为：
48、ali＝abi-abi i＝1,2,3,4 (5)
49、则支撑活塞杆的伸/缩的速度可表示为：
50、
51、所述掘进机动力学方程的建立方法为：
52、掘进机刚体的动能为：
53、
54、式(7)中：md为掘进机刚体质量(包括掘进机刀盘、推进油缸、反力架和撑靴油缸缸体)，id为掘进机刚体绕刀盘顶点转动惯量，d3为掘进机质心距刀头顶点的距离，质量矩阵m1(q)的定义如下：
55、
56、掘进机刚体的势能为：
57、ep1＝mdg(d3-d3cosβ) (9)
58、所有支撑油缸活塞杆的动能和为：
59、
60、式(10)中：mui为支撑油缸活塞杆质量。
61、综合以上，可知竖井掘进机的总动能和总势能为：
62、
63、ep＝ep1＝mdg(d3-d3cosβ) (12)
64、根据以上动能和势能表达式可以求得拉格朗日函数
65、
66、式(13)中：qj为广义力。
67、进一步整理系统动力学表达式为
68、
69、式(14)中：m(q)为质量矩阵，λ为拉格朗日算子，重力项g(q)和哥式项定义为：
70、
71、所述广义坐标下的受力分析方法为：
72、某一时刻下支撑油缸活塞杆的速度为：
73、
74、依据广义作用力与广义坐标分析可得：
75、δlzi＝ajpiδq (17)
76、根据虚功原理，支撑油缸对系统做功为：
77、
78、每个支撑油缸可单独控制，其输出力为：
79、
80、因此，可得
81、
82、如图6所示，竖井掘进机受到的作用力主要包括：
83、(1)竖井掘进机的重力(gg)：由竖井掘进机的总重量产生向下的作用力，方向为参考惯性坐标系的za轴正向，等效作用点为c。
84、(2)井底岩体对竖井掘进机的反向推力(fp)：掘进机破岩时，井底岩体对各个滚刀反向作用力的合力，等效作用点为p。
85、(3)井壁对4个撑靴的压力(fri)：为平衡竖井掘进机各方向的摩擦力，井壁需对撑靴施加足够的压力，方向为沿支撑油缸中心轴线向内，其反作用力为液压油缸对撑靴的推力，等效作用点为fi。
86、(4)井壁对4个撑靴的摩擦力(ffi)，等效作用点为fi。
87、在图6中，d6为两撑靴与井壁接触点之间的距离，d7为掘进机上层支撑油缸到破岩力等效点的竖直距离，d8掘进机上层支撑油缸到破岩力等效点的水平距离。则各作用点坐标为：
88、
89、bc＝[0 d1-d3]t,bp＝[-d8 d7]t
90、
91、撑靴和井壁的摩擦力必须能够平衡旋转破岩和推进过程中的荷载变化，确保竖井掘进机稳定工作，在摩擦系数一定的条件下，摩擦力与支撑油缸对井壁的压力成正比。撑靴的轴向摩擦力fai与竖井掘进机的重力gd的关系为：
92、
93、式(21)中：gg为竖井掘进机的质量。
94、四个撑靴所受的摩擦力ff可表示为：
95、
96、则围岩作用力fr可表示为：
97、
98、井底岩体对竖井掘进机的反向推力fp可分解为fp1、fp2：
99、fp1＝fpsin45° , fp2＝fpcos45° (24)
100、则
101、
102、竖井掘进机的重力gg为：
103、
104、据此，可得广义力的表达为
105、
106、将式(6-29)其代入拉格朗日方程(6-17)有：
107、
108、式(28)中：τd为未知干扰项，进一步可得：
109、
110、所述支撑油缸液压系统数学模型的建立方法如下：
111、定义a1,a2分别代表液压缸无杆腔和有杆腔的有效作用面积；p1,p2分别为液压缸无杆腔和有杆腔的油压；v1,v2分别为液压缸无杆腔和有杆腔的控制体积；xv为液压控制阀的阀芯位移；b为负载端有效粘性阻尼系数；ps和pr分别为供油和回油压力。
112、液压缸无杆腔和有杆腔的压力动态方程：
113、
114、式(30)中：βe1,βe2分别为液压缸无杆腔和有杆腔液压油的弹性模量；v01,v02分别为液压缸无杆腔和有杆腔的初始容积；q1,q2分别为由液压进入/流出液压缸无杆腔和有杆腔的液压流量；ct为液压缸内泄漏系数；pl＝p1-p2为负载压差。
115、假设液压控制阀是对称且匹配的，则阀流量方程为：
116、
117、式(31)中：kq为液压控制阀流量增益，满足cd,ρ分别为液压控制阀的节流孔流量系数、液压油密度；w为液压控制阀阀芯节流孔左右两端面积梯度，函数s(*)定义为
118、
119、假设执行器两腔液压油弹性模量相同，即βe1＝βe2＝βe；液压控制阀频宽远高于系统频宽，可简化阀动态为比例环节xv＝kiu，此时有s(xv)＝s(u)，并考虑系统在一般工况下工作，即两腔压力满足0<pr<p1<ps,0<pr<p2<ps。
120、根据上述假设，可以将式(31)转化为：
121、
122、所述撑靴模块的控制输入模型包括全断面竖井掘进机动力学模型的状态空间方程、掘进机自适应积分鲁棒纠偏控制器及控制器稳定性分析。
123、所述全断面竖井掘进机动力学模型的状态空间方程建立方法如下：
124、在全断面竖井掘进机动力学模型基础上，定义状态空间变量x＝[x1,x2,x3]t＝[q,q&,p1ia1i-p2ia2i]t，进而全断面竖井掘进机动力学模型的状态空间方程可表示为：
125、
126、式(33)中：f1＝m(x1,x2)b-1，f1＝csysb-1，θ2＝βe，θ3＝ctβe，
127、所述掘进机自适应积分鲁棒纠偏控制器的设计方法如下：
128、定义z1＝x1-yd为纠偏过程的轨迹控制误差，其中yd为任一纠偏步的跟踪指令，同时定义辅助控制量z2和r如下：
129、
130、式(34)中：k1和k2为反馈增益对角矩阵。
131、根据式(34)定义，可得
132、
133、定义误差量z3＝x3-α2，其中α2为待设计的稳定函数，则式(35)可转化为
134、
135、为了确保该动态方程稳定，稳定函数α2可设计为
136、
137、式(37)中：kr为反馈增益对角矩阵；α2s为待设计的鲁棒控制项，用于应对式(33)中干扰项τd。
138、将式(37)代入式(36)，可得
139、f1r＝z3+α2s-τd-krz2 (38)
140、根据式(38)的结构，可将鲁棒控制项α2s设计为以下自适应积分形式，即
141、
142、式(39)中：为可在线自适应的鲁棒增益，γ为自适应过程参数对角矩阵。
143、将式(39)代入式(38)，并对式(38)两端求导，可得
144、
145、式(40)中：其中表示将中所有状态信息替换为相应指令信息，以便于后续稳定性分析。
146、为了确保动态方程(40)稳定，最终自适应积分鲁棒纠偏控制器设计为
147、
148、式中：k3为反馈增益对角矩阵；为不确定参数θ1,θ2,θ3的估计值，γ为用于参数自适应过程调节的对角增益矩阵。
149、所述撑靴模块的实时控制信号可由式(34)、式(37)、式(39)、式(41)共同计算得到。
150、所述控制器稳定性分析方法如下：
151、将式(41)代入式(40)，可得
152、
153、结合将式(42)代入式(33)，可得
154、
155、式中：表示参数估计误差。
156、定义lyapunov函数如下：
157、
158、式中：表示参数估计误差。
159、对lyapunov函数v求导，并代入式(34)(42)(43)，可得：
160、
161、式(45)中：z＝[z1,z2,z3,r]t。通过选取足够大的控制增益使如下矩阵λ为正定，且满足以下条件时：
162、
163、式(46)中：λmin(λ)表示矩阵λ的最小特征值。
164、则最终下式成立
165、
166、由式(47)可知函数v有界，因此z1，z2，z3，r，均有界。根据式(34)可知有界，再式(41)易判断u有界，因此闭环系统所有信号有界。
167、根据以上的有界性分析易知有界，因此函数w(z)一致连续，即当t→∞时，z→0，即系统获得渐近稳定。
168、根据稳定性分析结果可知，所设计控制算法可保证纠偏控制误差理论上趋近于零，显著提升纠偏控制精度。
169、本发明具备以下有益效果：
170、该全断面竖井掘进机纠偏系统及纠偏控制算法，可根据掘进机机体轴线与井筒设计轴线之间角度偏斜和位移偏斜，由中央控制器实时计算出撑靴模块的控制信号，控制撑靴模块完成纠偏动作执行，提升了全断面竖井掘进机纠偏自动化水平，本发明将掘进机三维偏斜分解为两个二维偏斜，分别针对两个二维偏斜，提出了基于动力学模型补偿和抗扰设计的纠偏控制策略，降低了纠偏控制策略设计难度，提升了纠偏控制精度。
171、本发明适用于全断面竖井掘进机施工过程中的所有偏斜类型。
1.一种全断面竖井掘进机纠偏系统，其特征在于，包括
2.根据权利要求1所述的一种全断面竖井掘进机纠偏系统，其特征在于：所述撑靴模块，包括有上框架(1)、上层撑靴(2)、中框架(3)、中层撑靴(4)、下框架(5)、下层撑靴(6)、反力架(7)、支撑油缸(8)、推进油缸(9)、竖井设计轴线(10)、掘进机机体轴线(11)、刀盘(12)、井壁(13)、油缸油缸缸体(14)以及活塞杆(15)。
3.根据权利要求2所述的一种全断面竖井掘进机纠偏系统，其特征在于：所述上框架(1)和下框架(5)通过反力架(7)相连接，所述上层撑靴(2)的数量有四个，且四个均布的上层撑靴(2)通过四个均布的支撑油缸(8)安装在上框架(1)周围，四个所述均布的中层撑靴(4)通过四个均布的支撑油缸(8)安装在中框架(3)周围，四个所述均布的下层撑靴(6)通过四个均布的支撑油缸(8)安装在下框架(5)周围。
4.根据权利要求3所述的一种全断面竖井掘进机纠偏系统，其特征在于：所述中框架(3)在上框架(1)与下框架(5)中间，且中框架(3)可沿着反力架(7)上下活动，所述上层撑靴(2)、中层撑靴(4)以及下层撑靴(6)上中下三层撑靴均由所在层支撑油缸(8)推动与井壁(13)接触以保证固定；
5.一种全断面竖井掘进机纠偏系统的纠偏控制算法，采用如权利要求1-4任一项所述的全断面竖井掘进机纠偏系统，包括以下步骤：
6.根据权利要求5所述的一种全断面竖井掘进机纠偏系统的纠偏控制算法，其特征在于：所述纠偏控制算法集成于中央控制器，且所述纠偏控制算法还包括全断面竖井掘进机动力学模型和撑靴模块控制输入模型；
7.根据权利要求6所述的一种全断面竖井掘进机纠偏系统的纠偏控制算法，其特征在于：具体包括以下步骤，建立图中所示的两个坐标系，其中坐标系{a}为惯性坐标系，该坐标系坐标原点在右侧两组撑靴连接线中点，za轴平行于已成型井壁(13)向下，xa轴垂直于za轴，方向按右手定则确定，坐标系{b}为附体坐标系，其坐标原点在掘进机主框架上框架(1)与掘进机机体轴线(12)交点，zb轴与掘进机机体轴线(11)重合，方向向下，xb轴垂直于zb轴，方向按右手定则确定，该坐标系与掘进机刚体固联；
8.根据权利要求6所述的一种全断面竖井掘进机纠偏系统的纠偏控制算法，其特征在于：具体包括以下步骤，d6为两撑靴与井壁接触点之间的距离，d7为掘进机上层支撑油缸到破岩力等效点的竖直距离，d8掘进机上层支撑油缸到破岩力等效点的水平距离，则各作用点坐标为：
9.根据权利要求6所述的一种全断面竖井掘进机纠偏系统的纠偏控制算法，其特征在于：所述撑靴模块的撑靴模块控制输入模型包括全断面竖井掘进机动力学模型的状态空间方程、掘进机自适应积分鲁棒纠偏控制器及控制器稳定性分析。
10.根据权利要求9所述的一种全断面竖井掘进机纠偏系统的纠偏控制算法，其特征在于：所述全断面竖井掘进机动力学模型的状态空间方程建立方法如下：
