智能掘进机横向优化控制方法及系统

1.本发明涉及掘进机跟踪控制领域，特别是一种智能掘进机横向优化控制方法及系统。


背景技术：

2.掘进机跟踪控制是无人矿山研究的重要内容之一，是掘进机井下无人化作业的关键技术，对于提高矿山装备智能化水平、实现“绿色矿山”具有重要应用价值。实现掘进机跟踪控制需要克服井下环境的特殊性，满足高精度控制需求，尤其是避免掘进机与巷壁发生碰撞。在实际应用中，仍有以下问题有待解决：
3.(1)井下环境光线暗，掘进机驾驶难度大，而且司机(安全员)很难判断驾驶过程中是否有碰撞风险。目前，关于掘进机避碰的研究多以避碰装置或被动安全为主，还未有公开的考虑掘进机与井下巷壁避碰的横向控制方法；
4.(2)井下巷道狭窄，一般要求横向控制误差不超过30cm，传统的几何控制算法很难满足高精度控制要求，现代最优控制方法(如lqr、mpc等)又存在计算量大、实时性差、可能无可行解等问题。因此，有必要设计一种新的掘进机横向最优控制方法，平衡计算量与控制精度的需求。


技术实现要素：

5.本发明所要解决的技术问题是，针对现有技术不足，提供一种智能掘进机横向优化控制方法及系统，在缺乏定位与地图信息的情况下实现掘进机井下精确横向跟踪控制与避碰。
6.为解决上述技术问题，本发明所采用的技术方案是：一种智能掘进机横向优化控制方法，包括以下步骤：
7.s1、扫描矿井巷道，获取巷道左巷壁边界点、右巷壁边界点的位置坐标，并拟合生成左、右巷壁的边界线；
8.s2、构建掘进机运动学模型，并基于所述运动学模型与左、右巷壁的边界线生成不同铰接角速度下的轨迹簇；
9.s3、遍历轨迹簇中的每条轨迹，根据每条轨迹相对中心线的平均误差、最大误差、终端误差以及期望控制输入设计控制评价函数；所述中心线是指左巷壁的边界线与右巷壁的边界线之间的中线，且左巷壁的边界线与右巷壁的边界线关于该中线对称；
10.s4、根据控制评价函数，从轨迹簇中选择最优运动轨迹，并计算最优期望铰接角。
11.本发明的以上步骤通过激光雷达生成巷道左右巷壁边界线，不依赖定位及地图信息，规避了井下定位、建图难度大的问题；基于控制评价函数设计了掘进机横向最优控制方法，能减小控制误差，满足井下高精度控制要求。
12.优选地，s4、根据所述最优运动轨迹相对中心线的平均误差、最大误差、终端误差进行巷壁碰撞分级预警；
13.步骤s4的优选中进行了巷壁碰撞分级预警，降低了司机(安全员)在井下的驾驶难度，提高了掘进机井下作业的安全性。优选地，还包括：
14.s5、根据最优期望铰接角更新掘进机位置与姿态，判断当前位置是否到达终点，若到达终点则停止，否则返回步骤s1。
15.步骤s5中，通过距离车辆前向激光雷达的最远边界点判断是否到达终点，实现了了掘进机终点自动停车，避免由于井下光线过暗，司机(安全员)未及时停车，导致出现碰撞事故。
16.步骤s1的具体实现过程包括：
17.1)掘进机前向激光雷达扫描矿井巷道，得到球坐标系下的激光点集合ω
point
＝{point
i
|i＝1、2、
…
、n
‑
1、n}，其中，point
i
＝(r
i
,ω
i
,α
i
)表示第i个激光点，n为集合中激光点的个数；激光雷达与第i个激光点的连线为r
i
，r
i
在xoy平面的投影为ω
i
表示r
i
与之间的夹角，α
i
表示r
i
与激光雷达正y轴方向的夹角，r
i
表示激光雷达到第i个激光点的距离；x,y,z轴是指激光雷达三维直角坐标系的坐标轴；所述激光雷达三维直角坐标系是指激光雷达的传感器坐标系；
18.2)将球坐标系下的每个激光点均转换到车辆坐标系下，得到车辆坐标系下的激光点集合θ
pointinveh
＝{pointinveh
i
|i＝1、2、...、n
‑
1、n}，其中，车辆坐标系是以掘进机前桥中点为原点，以车辆航向为轴方向，以垂直车辆航向向左的方向为轴方向，以垂直轴和轴且竖直向上的方向为轴方向；轴方向；分别表示车辆坐标系下第i个激光点的轴、轴与轴坐标；
19.3)滤除激光点集合θ
pointinveh
＝{pointinveh
i
|i＝1、2、...、n
‑
1、n}中高度h
i
小于h
ground
的激光点，其中，h
i
＝abs(z
i
)，abs表示取绝对值，h
ground
表示地面激光点的高度阈值，h
ground
取值一般较小，需根据实际路面来调试；步骤3)滤除了地面的激光点，使得扫描得到的激光点分布在巷道中心线左右两侧；
20.4)将经步骤3)处理后的激光点集合φ
pointinwall
＝{pointinwall
c
|c＝1、2、...、m
‑
1、m}投影到车辆坐标系的平面，得到二维激光点集合ξ
pointintwodim
＝{pointintwodim
c
|c＝1、2、
…
、m
‑
1、m}，其中表示第c个二维激光点；步骤4)将激光点投影到地面将边界线拟合从三维空间转换到二维平面，对问题进行了合理的简化；
21.5)从所述二维激光点集合ξ
pointintwodim
＝{pointintwodim
c
|c＝1、2、
…
、m
‑
1、m}中提取左、右巷壁的边界点，采用三次样条插值分别拟合左、右巷壁的边界点，生成左、右巷壁边界线；优选地，从所述二维激光点集合ξ
pointintwodim
＝{pointintwodim
c
|c＝1、2、
…
、m
‑
1、m}中提取左、右巷壁的边界点的具体实现过程包括：
22.5a)从车辆坐标系原点开始，沿着轴按照固定间隔space
p
进行等间隔采样，直至采样到集合ξ
pointintwodim
中轴坐标的最大值处，将采样点集合记为p
samplingpoint
＝{samplingpoint
j
|j＝1、2、
…
、j
‑
1、j}，其中，j表示第j个采样点，是第j个采样点的横坐标，j为采样点的个数；
23.5b)遍历采样点集合p
samplingpoint
中的每个采样点；对于每个所述采样点，确定集合ξ
pointintwodim
中距离该采样点最近的左侧激光点和右侧激光点，即得到该采样点对应的左巷
壁边界点和右巷壁边界点；将所有采样点对应的左巷壁边界点的集合记为bound
leftp
o
int
＝{leftpoint
j
|j＝1、2、
…
、j
‑
1、j}，将所有采样点对应的右巷壁边界点的集合记为bound
rightp
o
int
＝{rightpoint
j
|j＝1、2、
…
、j
‑
1、j}；表示第j个左巷壁边界点，表示第j个右巷壁边界点，表示第j个左巷壁边界点的坐标，表示第j个左巷壁边界点的坐标，表示第j个右巷壁边界点的坐标，表示第j个左巷壁边界点的坐标，上标left表示左侧巷壁，上标right表示右侧巷壁；优选地，确定集合ξ
pointintwodim
中距离任一采样点最近的左侧激光点和右侧激光点的具体实现过程包括：
①
计算当前采样点与集合ξ
pointintwodim
中每个激光点的距离；
②
集合ξ
pointintwodim
中轴坐标为正值且与所述当前采样点距离最小的激光点即为左巷壁边界点，集合ξ
pointintwodim
中轴坐标为负值且与所述当前采样点距离最小的激光点即为右巷壁边界点；
24.步骤5)的方法相当于是考虑了巷道中最靠近内侧的激光点，即巷壁上最突出的激光点，相比于传统的方法，更能有效避免掘进机撞上巷壁上凸起的岩石，更能保证掘进机井下自动驾驶的安全性。
25.步骤s1中，将激光扫描得到的边界点转换到车辆坐标系，在车辆坐标系下拟合生成边界线。与在球坐标系或者三维直角坐标系下直接拟合相比，车辆坐标系的前向方向与车辆航向方向重合，能更直观地表示车辆与边界线的相对关系，更便于计算横向误差。
26.步骤2)中将球坐标系下的每个激光点均转换到车辆坐标系下，其中第i个激光点的具体实施过程如下：
27.1)利用以下转换公式将第i个激光点从球坐标系转换到激光雷达三维直角坐标系：
[0028][0029]
其中，x
i
,y
i
,z
i
是第i个激光点在激光雷达三维直角坐标下的坐标；
[0030]
2)将第i个激光点从倾斜的激光雷达三维直角坐标系转换到水平直角坐标系，转换公式为：换公式为：是指第i个激光点在激光雷达水平直角坐标下的坐标，β为激光雷达向下倾斜的角度；若激光雷达未向下倾斜，则β＝0；
[0031]
3)将第i个激光点从水平直角坐标系转换到车辆坐标系，转换公式为：其中，为激光雷达水平直角坐标系轴与车辆坐标系的夹角；δx、δy、δz为激光雷达相对于车辆坐标系原点的偏移量，偏移量与激光雷达的安装位置有关。
[0032]
由于在实际应用中，需要激光线束与地面相交，且注意安装位置的合理性，激光雷
达与车辆参考点的航向方向一般都存在一定的倾斜角度和偏移量。但是，车辆控制是以车辆坐标系原点(或车辆参考点)的航向方向为依据，因此有必要对边界点进行倾斜角度和偏移量的矫正，以减小传感器安装造成的控制误差。步骤s2的具体实现过程包括：
[0033]
步骤1，以掘进机前桥中点为参考点，构建掘进机运动学模型如下：
[0034][0035]
其中，(x
f
,y
f
)为前桥中点坐标，θ
f
为前桥航向角，v为前桥车速，γ为掘进机铰接角，l
f
为前桥轴距，l
r
为后桥轴距；
[0036]
步骤2，分别向左、右巷壁边界线预瞄轴长度为l
pre
的距离，得到左预瞄点和右预瞄点计算预瞄距离l
pre
：l
pre
＝l
min
+p
·
v；其中，l
min
表示最短预瞄距离，p为时间常数；
[0037]
步骤3，以车辆坐标系原点到预瞄点连线的中垂线与轴的交点为转向中心，计算得到左转向半径和右转向半径
[0038]
其中，其中，sign()为取符号函数；则左边界约束和右边界约束下的转向半径范围为
[0039]
步骤4，考虑掘进机执行机构的左右转向半径限幅与左边界约束、右边界约束下的转向半径范围求交集得到转向半径区间为[
‑
∞,r
right
]∪[r
left
,+∞]；
[0040]
步骤5，根据掘进机运动学模型以及转向半径区间，计算得到转向半径区间[
‑
∞,r
right
]∪[r
left
,+∞]对应的铰接角速度区间为其中其中计算公式如下：
[0041][0042]
步骤6，将铰接角速度区间按照长度为space
γ
的固定间隔采样进行离散化，将离散后的铰接角速度序列记为υ
γ
＝{γ
k
|k＝1、2、
…
、k
‑
1、k}，其中，k表示铰接角速度序列中的第k个铰接角速度，k为铰接角速度序列中离散铰接角速度数量，接下来，将铰接角速度序列υ
γ
中的铰接角速度分别代入掘进机运动学模型，推算得到不同铰接角速度下的轴长度为l
pre
的轨迹簇；其中，轨迹簇中共有k条轨迹，k表示轨迹簇中的第k条轨迹。
[0043]
步骤s2中，基于掘进机运动学模型与左右巷壁的边界线约束生成了不同铰接角速度下的轨迹簇，考虑了运动学约束、边界线约束、掘进机执行机构的转向半径限幅等，限制了车辆避碰的安全运动范围。另外，步骤s2中是采用控制输入采样的方法进行轨迹簇生成，与lattice状态采样(参考百度apollo方案)生成轨迹簇等常规方法相比，其显著优点是考虑了车辆的运动特性，因此该方法可以直接利用最优运动轨迹对应的控制输入进行掘进机的横向控制。
[0044]
步骤s3的具体实现过程包括：
[0045]
i)将轨迹簇中每一条轨迹的轴按照长度为的固定间隔采样进行离散化，得到离散后的轨迹点并计算得到每个轨迹点对应左巷壁边界线和右巷壁边界线的最短距离其中，j表示轨迹中的第j个轨迹点，j＝1、2、
…
、j
‑
1、j，j为轨迹簇中每条轨迹的轨迹点数量，
[0046]
ii)将每个轨迹点对应左巷壁边界线和右巷壁边界线的最短距离相减，得到每条轨迹所对应相对中心线的误差errordis
k,j
；
[0047]
iii)根据errordis
k,j
计算得到每条轨迹相对中心线的平均误差最大误差终端误差
[0048]
iv)设计控制评价函数如下：
[0049][0050]
其中，w1、w2、w3、w4为权重系数，w1、w4为常数。
[0051]
步骤s3中，根据每条轨迹相对中心线的平均误差、最大误差、终端误差以及期望控制输入设计控制评价函数，全面考虑了各类控制误差与控制输入，更符合井下高性能控制需求；
[0052]
权重系数w2、w3采用如下的自适应权重配置策略：
[0053]
其中，w
2,2
＞＞w
2,1
、w
3,2
＞＞w
3,1
；表示最大误差界值；为终端误差界值。
[0054]
本发明的自适应权重配置策略，当最大误差或者终端误差超过界值时给予较大的惩罚，能有效控制最大误差与终端误差在安全的误差界内，避免掘进机发生碰撞危险。
[0055]
步骤s4中，选择最小的控制评价函数f
min
所对应最优运动轨迹，进一步确定最优运动对应的铰接角速度并计算得到最优期望铰接角γ
opt
：其中，t
c
为控制周期；γ
opt,last
为上一周期的最优期望铰接角，γ
opt,last
的初始值为0。
[0056]
基于控制评价函数最小化的原则选择最优运动轨迹，并进一步确定最优运动轨迹对应的铰接角速度，相比于采用二次规划等优化的方法进行计算，实时性高、计算量更小，同时避免了无可行解的问题。
[0057]
步骤s4中，根据最优运动轨迹相对中心线的平均误差最大误差终端误差计算巷壁碰撞预警等级的具体实现过程包括：
[0058]
a)根据最大误差计算最大误差等级：
[0059]
其中，其中，为常数；根据平均误差计算平均误差等级：其中，其中，为常数；根据终端误差计算终端误差等级：其中，其中，为常数；
[0060]
b)根据误差等级level
max
、level
ave
、level
end
计算巷壁碰撞预警等级：
[0061][0062]
其中，warn_value＝level
max
·
weight
max
+level
ave
·
weight
ave
+level
end
·
weight
end
；warn_level＝4为四级风险预警，建议安全员接管；warn_level＝3为三级风险预
警，提醒安全员注意；warn_level＝2为二级风险预警，warn_level＝1为一级风险预警，此时为较安全状态，可进行自动驾驶；weight
max
、weight
ave
、weight
end
为常数。
[0063]
根据轨迹相对中心线的平均误差、最大误差、终端误差进行巷壁碰撞分级预警，降低了司机(安全员)在井下的驾驶难度，提高了掘进机井下作业的安全性。而且，基于运动学模型推算的运动轨迹进行碰撞预警，考虑了未来一段时间的潜在风险，使得掘进机的避碰更加具有预见性。
[0064]
步骤s5中，判断当前位置是否到达终点的具体实现过程包括：从二维激光点集合ξ
pointintwodim
＝{pointintwodim
c
|c＝1、2、
…
、m
‑
1、m}中找到轴坐标最大的点pointbound，即为最远边界点；判断最远边界点的轴坐标值是否小于终点判别阈值r
end
；若小于终点判别阈值r
end
，则表示到达终点，否则表示还未到达终点。
[0065]
通过距离车辆前向激光雷达的最远边界点判断是否到达终点，实现了了掘进机终点自动停车功能，避免由于井下光线过暗，司机(安全员)未及时停车，导致在终点出现碰撞事故；
[0066]
一种智能掘进机横向控制系统，包括计算机设备；所述计算机设备被配置或编程为用于执行上述方法的步骤。
[0067]
与现有技术相比，本发明所具有的有益效果为：
[0068]
(1)相比于基于slam或者惯导进行导航控制的方案，本发明通过激光雷达生成巷道左右巷壁边界线，并基于边界线设计横向最优控制器，不依赖定位及地图信息，有效规避了井下定位、建图难度大的问题；
[0069]
(2)考虑掘进机运动学模型与左右巷壁的边界线约束，通过对控制输入进行采样，生成不同铰接角速度对应的轨迹簇，设计控制评价函数选择最优运动轨迹并计算最优期望铰接角，融合了采样与最优化理论，平衡了计算量与控制精度的两难问题；
[0070]
(3)根据轨迹相对中心线的平均误差、最大误差、终端误差进行巷壁碰撞分级预警，降低了司机(安全员)在井下的驾驶难度，提高了掘进机井下作业的安全性。
附图说明
[0071]
图1为整体流程图；
[0072]
图2(a)为球坐标系
‑
激光雷达三维直角坐标系转换示意图；
[0073]
图2(b)为倾斜的激光雷达三维直角坐标系
‑
水平直角坐标系转换示意图；
[0074]
图2(c)为激光雷达安装三视图；
[0075]
图2(d)为车辆坐标系示意图；
[0076]
图3为掘进机运动学模型示意图；
[0077]
图4为转向半径计算示意图；
[0078]
图5为巷壁碰撞预警等级曲线图；
[0079]
图6为井下巷道全局示意图；
[0080]
图7为掘进机实际车速曲线；
[0081]
图8(a)为不同方法的横向控制误差曲线图；
[0082]
图8(b)为不同方法的铰接角曲线图。
具体实施方式
[0083]
本发明实施例以湖南创远高新机械有限责任公司提供的井下掘进机为原型，在湖南某金属矿进行实验，提供的一种智能掘进机横向优化控制方法，如图1所示。包括以下步骤：
[0084]
步骤一：掘进机前向激光雷达扫描矿井巷道，获取巷道左右巷壁边界点的位置坐标，并拟合生成左右巷壁的边界线，具体步骤如下：
[0085]
(1)掘进机前向激光雷达扫描矿井巷道，得到球坐标系下的激光点集合ω
p
o
int
＝{point
i
|i＝1、2、
…
、n
‑
1、n}，其中，point
i
＝(r
i
,ω
i
,α
i
)表示第i个激光点，n为集合中激光点的个数；激光雷达与第i个激光点的连线为r
i
，r
i
在xoy平面的投影为ω
i
表示r
i
与之间的夹角，α
i
表示r
i
与激光雷达正y轴方向的夹角，r
i
表示激光雷达到第i个激光点的距离；其中，x,y,z轴是指激光雷达三维直角坐标的坐标轴；
[0086]
本实施例中，采用的是velodyne 16线激光雷达，激光连接线指向的方向为激光雷达三维直角坐标的y轴正方向，垂直激光雷达横截面向上的方向为z轴正方向，垂直y轴与z轴向左的方向为x轴正方向。
[0087]
(2)将球坐标系下的每个激光点均转换到车辆坐标系下，得到车辆坐标系下的激光点集合θ
pointinveh
＝{pointinveh
i
|i＝1、2、
…
、n
‑
1、n}，其中，1、n}，其中，分别表示车辆坐标系下第i个激光点的轴、轴与轴坐标；。第i个激光点进行坐标系转换的具体过程如下：
[0088]
1)将第i个激光点从球坐标系转换到激光雷达三维直角坐标系，如图2(a)所示，坐标系转换公式为：
[0089][0090]
其中，x
i
,y
i
,z
i
是指第i个激光点在激光雷达三维直角坐标下的坐标。
[0091]
2)若激光雷达向下倾斜，将第i个激光点从倾斜的激光雷达三维直角坐标系转换到水平直角坐标系，如图2(b)所示，转换公式为：
[0092][0093]
其中，是指第i个激光点在激光雷达水平直角坐标下的坐标，β为激光雷达向下倾斜的角度，若激光雷达不向下倾斜，则β＝0；本实例中，激光雷达向下倾斜安装，β＝0.12rad。
[0094]
3)将第i个激光点从水平直角坐标系转换到车辆坐标系，转换公式为：
[0095][0096]
其中，为激光雷达水平直角坐标系轴与车辆坐标系的夹角。
[0097]
如图2(c)所示，δx、δy、δz为激光雷达相对于车辆坐标系原点的偏移量；本实例
中δx＝0.1m、δy＝0.9m、δz＝1.6m。
[0098]
如图2(d)所示，以掘进机前桥中点为车辆坐标系原点，车辆坐标系是指以车辆航向为轴方向(前向)，以垂直车辆航向向左的方向为轴方向(侧向)，以垂直轴和轴且竖直向上的方向为轴方向(天向)；
[0099]
(3)滤除激光点集合θ
pointinveh
＝{pointinveh
i
|i＝1、2、
…
、n
‑
1、n}中高度h
i
小于h
ground
的激光点，其中，h
i
＝abs(z
i
)，abs表示取绝对值，h
ground
表示地面激光点的高度阈值；具体做法是：遍历集合中所有的激光点，将高度小于h
ground
的激光点从集合中移除，得到滤除后的激光点集合φ
pointinwall
＝{pointinwall
c
|c＝1、2、
…
、m
‑
1、m},m为滤除后剩下的激光点个数，点个数，分别表示车辆坐标系下第c个激光点的前向、侧向与天向坐标；
[0100]
(4)将滤除后的激光点集合φ
pointinwall
＝{pointinwall
c
|c＝1、2、
…
、m
‑
1、m}投影到车辆坐标系的平面，具体做法是：令所有激光点的轴坐标为0，得到二维激光点集合ξ
pointintwodim
＝{pointintwodim
c
|i＝1、2、
…
、m
‑
1、m}，其中
[0101]
(5)从二维激光点集合ξ
pointintwodim
＝{pointintwodim
c
|c＝1、2、
…
、m
‑
1、m}中提取左右巷壁的边界点，具体做法是：
[0102]
1)从车辆坐标系原点开始，沿着轴按照固定间隔space
p
进行等间隔采样，直到集合ξ
pointintwodim
中轴坐标的最大值处结束；将采样点集合记为p
samplingpoint
＝{samplingpoint
j
|j＝1、2、
…
、j
‑
1、j}，其中，j表示第j个采样点，是第j个采样点的横坐标，j为采样点的个数；本实例中，采样间隔space
p
＝0.1m；
[0103]
2)遍历采样点集合p
samplingpoint
中的每个采样点；对于每个所述采样点，确定集合ξ
pointintwodim
中距离该采样点最近的左侧激光点和右侧激光点，即得到该采样点对应的左巷壁边界点和右巷壁边界点；将所有采样点对应的左巷壁边界点的集合记为bound
leftpoint
＝{leftpoint
j
|j＝1、2、
…
、j
‑
1、j}，将所有采样点对应的右巷壁边界点的集合记为bound
rightpoint
＝{rightpoint
j
|j＝1、2、
…
、j
‑
1、j}；表示第j个左巷壁边界点，表示第j个右巷壁边界点，表示第j个左巷壁边界点的坐标，表示第j个左巷壁边界点的坐标，表示第j个右巷壁边界点的坐标，表示第j个左巷壁边界点的坐标，上标left表示左侧巷壁，上标right表示右侧巷壁；优选地，确定集合ξ
pointintwodim
中距离任一采样点最近的左侧激光点和右侧激光点的具体实现过程包括：
①
计算当前采样点与集合ξ
pointintwodim
中每个激光点的距离；
②
集合ξ
pointintwodim
中轴坐标为正值且与所述当前采样点距离最小的激光点即为左巷壁边界点，集合ξ
pointintwodim
中轴坐标为负值且与所述当前采样点距离最小的激光点即为右巷壁边界点。
[0104]
(6)采用三次样条插值拟合左右巷壁的边界点生成左右巷壁边界线，具体如下：
[0105]
采用三次样条插值拟合左侧巷壁边界点集合bound
leftpoint
，得到左侧巷壁边界线的分段三次多项式为：
[0106]
[0107]
其中，左侧巷壁的边界线是由j
‑
1条三次多项式组成，表示左侧巷壁第j个和第j+1个边界点连线的三次方系数；
[0108]
采用三次样条插值拟合右侧巷壁边界点集合bound
rightpoint
，得到右侧巷壁边界线的分段三次多项式为：
[0109][0110]
其中，右侧巷壁的边界线也是由j
‑
1条三次多项式组成，表示右侧巷壁第j个和第j+1个边界点连线的三次方系数。
[0111]
分段三次多项式中未考虑轴坐标是车辆控制领域的常规操作，即进行车辆横向控制时忽略车辆竖直方向(轴方向)的运动。
[0112]
在本实施例中，所述掘进机是装配了激光雷达的铰接式车辆，分为前车体与后车体，前后车体由铰接盘连接，利用铰接进行转向，最大铰接角为0.35rad(铰接角的机械限制)。
[0113]
本实施例通过激光雷达生成巷道左右巷壁边界线，并基于边界线设计横向最优控制器，不依赖定位及地图信息，有效规避了井下定位、建图难度大的问题；另外，与现有技术手段不同的是，本实施例通过将所有的激光点投影到车辆坐标系的平面，并提出最靠近轴的激光点，相当于是提取了巷壁中最突出的位置用来拟合生成边界线，相比于将巷壁作为二维平面或者根据巷壁与地面相交处生成边界线的方式，本实施例的方法考虑了矿井巷壁凹凸不平的特性，生成的边界线更符合安全约束。
[0114]
步骤二：构建掘进机运动学模型，并基于运动学模型与左右巷壁的边界线生成不同铰接角速度下的轨迹簇，具体步骤如下：
[0115]
(1)如图3所示，以掘进机前桥中点为参考点，构建掘进机运动学模型如下：
[0116][0117]
其中，(x
f
,y
f
)为前桥中点坐标，θ
f
为前桥航向角，v为前桥车速，γ为掘进机铰接角，l
f
为前桥轴距，l
r
为后桥轴距；本实施例中，掘进机的前桥轴距l
f
＝1.4m、后桥轴距l
r
＝1.76m。
[0118]
(2)分别向左、右巷壁边界线预瞄轴长度为l
pre
的距离，得到左预瞄点和右预瞄点采用经验公式计算预瞄距离l
pre
，公式如下：
[0119]
l
pre
＝l
min
+p
·
v；
[0120]
其中，l
min
表示最短预瞄距离，p为时间常数，本实施例中l
min
＝3m、p＝0.1s。
[0121]
(3)如图4所示，以车辆坐标系原点(前桥中点)到预瞄点连线的中垂线与轴的交点为转向中心，计算得到左转向半径和右转向半径如下：
[0122][0123]
其中，α
left
为左预瞄偏差角，α
right
为右预瞄偏差角，为右预瞄偏差角，
[0124]
于是，左右边界约束下的转向半径范围为
[0125]
(4)考虑掘进机执行机构的转向半径限幅与左右边界约束下的转向半径范围求交集得到转向半径区间为[
‑
∞,r
right
]∪[r
left
,+∞]，其中，
[0126]
本实施例中，规定掘进机左转向半径为正值，右转向半径为负值，即本实施例中，规定掘进机左转向半径为正值，右转向半径为负值，即
[0127]
(5)根据掘进机运动学模型以及转向半径区间，计算得到转向半径区间[
‑
∞,r
right
]∪[r
left
,+∞]对应的铰接角速度区间为其中其中计算公式如下：
[0128][0129]
其中，分别与转向半径为r
right
、r
left
、
‑
∞、+∞对应，γ是指掘进机上一周期的铰接角，在实际应用中，γ与γ
opt,last
的含义及数值相同。
[0130]
以为例，其计算原理具体如下：
[0131]
首先，根据掘进机运动学模型有：
[0132][0133]
而后，根据速度与角速度的关系可得：
[0134][0135]
最后，联立以上两式可得：
[0136][0137]
变形得：
[0138][0139]
(6)将铰接角速度区间按照长度为space
γ
的固定间隔采样进行离散化，将离散后的铰接角速度序列记为υ
γ
＝{γ
k
|k＝1、2、
…
、k
‑
1、k}，其中，k表示铰接角速度序列中的第k个铰接角速度，k为铰接角速度序列中离散铰接角速度数量，接下来，将铰接角速度序列υ
γ
中的铰接角速度分别代入掘进机运动学模型，推算得到不同铰接角速度下的轴长度为l
pre
的轨迹簇；其中，轨迹簇中共有k条轨迹，k表示轨迹簇中的第k条轨迹。本实施例中space
γ
＝0.01rad。
[0140]
步骤三：遍历轨迹簇中的每条轨迹，根据每条轨迹相对中心线的平均误差、最大误差、终端误差以及期望控制输入设计控制评价函数，具体步骤如下：
[0141]
(1)将轨迹簇中每一条轨迹的轴按照长度为的固定间隔采样进行离散化，得到离散后的轨迹点并计算得到每个轨迹点对应左、右巷壁边界线的最短距离(见cn110796852b)，其中，j表示轨迹中的第j个轨迹点，j＝1、2、
…
、j
‑
1、j，j为轨迹簇中每条轨迹的轨迹点数量，本实施例中
[0142]
(2)将每个轨迹点对应左右巷壁的最短距离相减计算得到每条轨迹所对应相对中心线的误差errordis
k,j
，即：
[0143][0144]
(3)根据errordis
k,j
计算得到每条轨迹相对中心线的平均误差、最大误差、终端误差，计算方法如下：
[0145][0146]
其中，表示第k条轨迹中j个轨迹点相对中心线的误差绝对值之和，max[abs(errordis
k,j
)]表示第k条轨迹的j个轨迹点相对中心线的最大误差的绝对值，abs(errordis
k,j
)表示第k条轨迹的第j个轨迹点相对中心线的误差绝对值。
[0147]
(4)设计控制评价函数如下：
[0148][0149]
其中，w1、w2、w3、w4为权重系数，w1、w4为常数，本实施例中w1＝0.8，w4＝0.5，w2、w3如下：
[0150][0151][0152]
其中，w
2,2
＞＞w
2,1
、w
3,2
＞＞w
3,1
；本实施例中，w
2,1
取值为1，w
2,2
取值为15，w
3,1
取值为1.5，w
3,2
取值为15；表示最大误差界值，在本实施例中取值为0.5，为终端误差界值，在本实施例中取值为0.1；w2、w3表达式的物理含义是，当最大误差或者终端误差超过界值时给予较大的惩罚，以控制最大误差与终端误差在有限的误差界内。
[0153]
步骤四：根据控制评价函数，从轨迹簇中选择最优运动轨迹，并计算最优期望铰接角；优选地，根据所述最优运动轨迹相对中心线的平均误差、最大误差、终端误差进行巷壁碰撞分级预警；
[0154]
步骤四中计算最优期望铰接角的具体做法为：选择最小的控制评价函数f
min
所对应轨迹，进一步确定其对应的铰接角速度并计算得到最优期望铰接角γ
opt
，计算公式如下：
[0155][0156]
其中，t
c
为控制周期，本实施例中t
c
＝100ms，γ
opt,last
为上一周期的最优期望铰接角，γ
opt,last
的初始值为0。
[0157]
步骤四中进行巷壁碰撞分级预警的具体做法为：根据最优运动轨迹相对中心线的平均误差最大误差终端误差计算巷壁碰撞预警等级，计算方法如下：
[0158]
(1)根据最大误差计算最大误差等级，规则如下：
[0159][0160]
本实施例中，
[0161]
(2)根据平均误差计算平均误差等级，规则如下：
[0162][0163]
本实施例中，
[0164]
(3)根据终端误差计算终端误差等级，规则如下：
[0165][0166]
本实施例中，
[0167]
(4)根据误差等级level
max
、level
ave
、level
end
计算计算巷壁碰撞预警等级，具体如下：
[0168]
1)量化巷壁碰撞预警危险程度，计算如下：
[0169]
warn_value＝level
max
·
weight
max
+level
ave
·
weight
ave
+level
end
·
weight
end
；
[0170]
本实施例中，weight
max
＝3，weight
ave
＝1，weight
end
＝1。
[0171]
2)划分巷壁碰撞预警等级，规则如下：
[0172][0173]
其中，warn_level＝4为四级风险预警，建议安全员接管；warn_level＝3为三级风险预警，提醒安全员注意；warn_level＝2为二级风险预警，warn_level＝1为一级风险预警，此时为较安全状态，可以进行自动驾驶；本实施例中，warnbound＝5。
[0174]
根据轨迹相对中心线的平均误差、最大误差、终端误差进行巷壁碰撞分级预警，降低了司机(安全员)在井下的驾驶难度，提高了掘进机井下作业的安全性。
[0175]
本实施例对应的巷壁碰撞分级预警的实车测试结果如图5所示，根据实测结果可以看出，掘进机在井下自动驾驶的过程中，大多数时间为一级预警，最高预警等级为三级，未触发四级高风险预警，全程均为自动驾驶状态。对比图6可知，二级及三级预警是对应掘进机转弯的过程中，此时掘进的位置、姿态变化较大，需要提醒司机(安全员)注意，也符合实际驾驶经验。
[0176]
步骤五：根据最优期望铰接角更新掘进机位置与姿态，判断当前位置是否到达终点，若到达终点则停止，否则回到步骤一继续循环。
[0177]
步骤五中判断当前位置是否到达终点的具体方法是，首先，从二维激光点集合ξ
pointintwodim
＝{pointintwodim
c
|c＝1、2、
…
、m
‑
1、m}中找到轴坐标最大的点pointbound，即为最远边界点；而后，判断最远边界点的轴坐标值是否小于终点判别阈值r
end
；若小于终点判别阈值r
end
，则表示到达终点，否则表示还未到达终点。本实施例中r
end
取值为2
m
。
[0178]
本实施例应用了一种新的智能掘进机横向优化控制方法及系统，基于掘进机运动学模型与左右巷壁的边界线生成了不同铰接角速度下的轨迹簇，设计控制评价函数选择最优轨迹，考虑了轨迹相对中心线的平均误差、最大误差、终端误差以及控制输入，确保系统满足高精度控制需求，融合了采样与最优化理论，平衡了计算量与控制精度的两难问题。
[0179]
本实施例在某金属矿进行实车测试，井下巷道呈现如图6所示的“j”字形。实验过程中，掘进机初始车速为0，设置期望车速为定值4.5m/s，实际车速变化如图7所示。智能掘进机横向控制的实车测试结果如图8(a)、图8(b)所示，根据图8(a)可以看出，相比几何控制
方法与lqr最优控制方法，本实施例提供的方法的横向控制误差整体上是最小的；几何控制方法的控制误差最大，这是因为几何控制器未考虑误差的长期变化，入弯、出弯的过程中不能及时调整，导致误差迅速上升；lqr最优控制实现和求解最复杂，因为lqr需要用迭代的方法求解黎卡提方程，而本实施例的方法和几何控制方法均无需进行复杂的求解。根据图8(b)可以看出三种方法所计算得到的铰接角均不超过0.35rad，满足最大铰接角的机械限制，但是几何控制器的铰接角整体上最大，比较接近掘进机转向极限，这是由于本实施例提供的方法以及lqr最优控制方法进行优化时均考虑了控制输入最小化的问题，即以尽量小的铰接角实时误差尽可能得减小，而几何控制器没有考虑这一点。