[go: up one dir, main page]

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

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

Info

Publication number
CN112650241B
CN112650241B CN202011524208.5A CN202011524208A CN112650241B CN 112650241 B CN112650241 B CN 112650241B CN 202011524208 A CN202011524208 A CN 202011524208A CN 112650241 B CN112650241 B CN 112650241B
Authority
CN
China
Prior art keywords
point
boundary
laser
wall
error
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011524208.5A
Other languages
English (en)
Other versions
CN112650241A (zh
Inventor
阳春华
余伶俐
况宗旭
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Central South University
Original Assignee
Central South University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Central South University filed Critical Central South University
Priority to CN202011524208.5A priority Critical patent/CN112650241B/zh
Publication of CN112650241A publication Critical patent/CN112650241A/zh
Application granted granted Critical
Publication of CN112650241B publication Critical patent/CN112650241B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种智能掘进机横向优化控制方法及系统，通过激光雷达生成巷道左右巷壁边界线，并基于边界线设计横向最优控制器，不依赖定位及地图信息，有效解决了井下定位、建图难度大的问题；基于掘进机运动学模型与左右巷壁的边界线约束生成了不同铰接角速度下的轨迹簇，设计控制评价函数选择最优运动轨迹计算最优期望铰接角，确保系统满足高精度控制需求；根据轨迹相对中心线的平均误差、最大误差、终端误差进行巷壁碰撞分级预警，降低了司机(安全员)在井下的驾驶难度，提高了掘进机井下作业的安全性。

Description

智能掘进机横向优化控制方法及系统
技术领域
本发明涉及掘进机跟踪控制领域，特别是一种智能掘进机横向优化控制方法及系统。
背景技术
掘进机跟踪控制是无人矿山研究的重要内容之一，是掘进机井下无人化作业的关键技术，对于提高矿山装备智能化水平、实现“绿色矿山”具有重要应用价值。实现掘进机跟踪控制需要克服井下环境的特殊性，满足高精度控制需求，尤其是避免掘进机与巷壁发生碰撞。在实际应用中，仍有以下问题有待解决：
(1)井下环境光线暗，掘进机驾驶难度大，而且司机(安全员)很难判断驾驶过程中是否有碰撞风险。目前，关于掘进机避碰的研究多以避碰装置或被动安全为主，还未有公开的考虑掘进机与井下巷壁避碰的横向控制方法；
(2)井下巷道狭窄，一般要求横向控制误差不超过30cm，传统的几何控制算法很难满足高精度控制要求，现代最优控制方法(如LQR、MPC等)又存在计算量大、实时性差、可能无可行解等问题。因此，有必要设计一种新的掘进机横向最优控制方法，平衡计算量与控制精度的需求。
发明内容
本发明所要解决的技术问题是，针对现有技术不足，提供一种智能掘进机横向优化控制方法及系统，在缺乏定位与地图信息的情况下实现掘进机井下精确横向跟踪控制与避碰。
为解决上述技术问题，本发明所采用的技术方案是：一种智能掘进机横向优化控制方法，包括以下步骤：
S1、扫描矿井巷道，获取巷道左巷壁边界点、右巷壁边界点的位置坐标，并拟合生成左、右巷壁的边界线；
S2、构建掘进机运动学模型，并基于所述运动学模型与左、右巷壁的边界线生成不同铰接角速度下的轨迹簇；
S3、遍历轨迹簇中的每条轨迹，根据每条轨迹相对中心线的平均误差、最大误差、终端误差以及期望控制输入设计控制评价函数；所述中心线是指左巷壁的边界线与右巷壁的边界线之间的中线，且左巷壁的边界线与右巷壁的边界线关于该中线对称；
S4、根据控制评价函数，从轨迹簇中选择最优运动轨迹，并计算最优期望铰接角。
本发明的以上步骤通过激光雷达生成巷道左右巷壁边界线，不依赖定位及地图信息，规避了井下定位、建图难度大的问题；基于控制评价函数设计了掘进机横向最优控制方法，能减小控制误差，满足井下高精度控制要求。
优选地，S4、根据所述最优运动轨迹相对中心线的平均误差、最大误差、终端误差进行巷壁碰撞分级预警；
步骤S4的优选中进行了巷壁碰撞分级预警，降低了司机(安全员)在井下的驾驶难度，提高了掘进机井下作业的安全性。优选地，还包括：
S5、根据最优期望铰接角更新掘进机位置与姿态，判断当前位置是否到达终点，若到达终点则停止，否则返回步骤S1。
步骤S5中，通过距离车辆前向激光雷达的最远边界点判断是否到达终点，实现了了掘进机终点自动停车，避免由于井下光线过暗，司机(安全员)未及时停车，导致出现碰撞事故。
步骤S1的具体实现过程包括：
1)掘进机前向激光雷达扫描矿井巷道，得到球坐标系下的激光点集合Ωpoint＝{Pointi|i＝1、2、…、N-1、N}，其中，Pointi＝(ri,ωi,αi)表示第i个激光点，N为集合中激光点的个数；激光雷达与第i个激光点的连线为Ri，Ri在xoy平面的投影为
Figure BDA0002849971840000021
ωi表示Ri与
Figure BDA0002849971840000022
之间的夹角，αi表示Ri与激光雷达正y轴方向的夹角，ri表示激光雷达到第i个激光点的距离；x,y,z轴是指激光雷达三维直角坐标系的坐标轴；所述激光雷达三维直角坐标系是指激光雷达的传感器坐标系；
2)将球坐标系下的每个激光点均转换到车辆坐标系下，得到车辆坐标系下的激光点集合ΘpointInVeh＝{PointInVehi|i＝1、2、...、N-1、N}，其中，车辆坐标系是以掘进机前桥中点为原点，以车辆航向为
Figure BDA0002849971840000023
轴方向，以垂直车辆航向向左的方向为
Figure BDA0002849971840000031
轴方向，以垂直
Figure BDA0002849971840000032
轴和
Figure BDA0002849971840000033
轴且竖直向上的方向为
Figure BDA0002849971840000034
轴方向；
Figure BDA0002849971840000035
Figure BDA0002849971840000036
分别表示车辆坐标系下第i个激光点的
Figure BDA0002849971840000037
轴、
Figure BDA0002849971840000038
轴与
Figure BDA0002849971840000039
轴坐标；
3)滤除激光点集合ΘpointInVeh＝{PointInVehi|i＝1、2、...、N-1、N}中高度hi小于hground的激光点，其中，hi＝abs(zi)，abs表示取绝对值，hground表示地面激光点的高度阈值，hground取值一般较小，需根据实际路面来调试；步骤3)滤除了地面的激光点，使得扫描得到的激光点分布在巷道中心线左右两侧；
4)将经步骤3)处理后的激光点集合ΦpointInWall＝{PointInWallc|c＝1、2、...、M-1、M}投影到车辆坐标系的
Figure BDA00028499718400000310
平面，得到二维激光点集合ΞPointInTwodim＝{PointInTwodimc|c＝1、2、…、M-1、M}，其中
Figure BDA00028499718400000311
表示第c个二维激光点；步骤4)将激光点投影到地面将边界线拟合从三维空间转换到二维平面，对问题进行了合理的简化；
5)从所述二维激光点集合ΞPointInTwodim＝{PointInTwodimc|c＝1、2、…、M-1、M}中提取左、右巷壁的边界点，采用三次样条插值分别拟合左、右巷壁的边界点，生成左、右巷壁边界线；优选地，从所述二维激光点集合ΞPointInTwodim＝{PointInTwodimc|c＝1、2、…、M-1、M}中提取左、右巷壁的边界点的具体实现过程包括：
5a)从车辆坐标系原点开始，沿着
Figure BDA00028499718400000312
轴按照固定间隔spacep进行等间隔采样，直至采样到集合ΞPointInTwodim中
Figure BDA00028499718400000313
轴坐标的最大值处，将采样点集合记为PSamplingPoint＝{SamplingPointj|j＝1、2、…、J-1、J}，
Figure BDA00028499718400000314
其中，j表示第j个采样点，
Figure BDA00028499718400000315
是第j个采样点的横坐标，J为采样点的个数；
5b)遍历采样点集合PSamplingPoint中的每个采样点；对于每个所述采样点，确定集合ΞPointInTwodim中距离该采样点最近的左侧激光点和右侧激光点，即得到该采样点对应的左巷壁边界点和右巷壁边界点；将所有采样点对应的左巷壁边界点的集合记为BoundLeftPoint＝{LeftPointj|j＝1、2、…、J-1、J}，将所有采样点对应的右巷壁边界点的集合记为BoundRightPoint＝{RightPointj|j＝1、2、…、J-1、J}；
Figure BDA0002849971840000041
表示第j个左巷壁边界点，
Figure BDA0002849971840000042
表示第j个右巷壁边界点，
Figure BDA0002849971840000043
表示第j个左巷壁边界点的
Figure BDA0002849971840000044
坐标，
Figure BDA0002849971840000045
表示第j个左巷壁边界点的
Figure BDA0002849971840000046
坐标，
Figure BDA0002849971840000047
表示第j个右巷壁边界点的
Figure BDA0002849971840000048
坐标，
Figure BDA0002849971840000049
表示第j个左巷壁边界点的
Figure BDA00028499718400000410
坐标，上标left表示左侧巷壁，上标right表示右侧巷壁；优选地，确定集合ΞPointInTwodim中距离任一采样点最近的左侧激光点和右侧激光点的具体实现过程包括：①计算当前采样点与集合ΞPointInTwodim中每个激光点的距离；②集合ΞPointInTwodim中
Figure BDA00028499718400000411
轴坐标为正值且与所述当前采样点距离最小的激光点即为左巷壁边界点，集合ΞPointInTwodim中
Figure BDA00028499718400000412
轴坐标为负值且与所述当前采样点距离最小的激光点即为右巷壁边界点；
步骤5)的方法相当于是考虑了巷道中最靠近内侧的激光点，即巷壁上最突出的激光点，相比于传统的方法，更能有效避免掘进机撞上巷壁上凸起的岩石，更能保证掘进机井下自动驾驶的安全性。
步骤S1中，将激光扫描得到的边界点转换到车辆坐标系，在车辆坐标系下拟合生成边界线。与在球坐标系或者三维直角坐标系下直接拟合相比，车辆坐标系的前向方向与车辆航向方向重合，能更直观地表示车辆与边界线的相对关系，更便于计算横向误差。
步骤2)中将球坐标系下的每个激光点均转换到车辆坐标系下，其中第i个激光点的具体实施过程如下：
1)利用以下转换公式将第i个激光点从球坐标系转换到激光雷达三维直角坐标系：
Figure BDA0002849971840000051
其中，xi,yi,zi是第i个激光点在激光雷达三维直角坐标下的坐标；
2)将第i个激光点从倾斜的激光雷达三维直角坐标系转换到水平直角坐标系，转换公式为：
Figure BDA0002849971840000052
Figure BDA0002849971840000053
是指第i个激光点在激光雷达水平直角坐标下的坐标，β为激光雷达向下倾斜的角度；若激光雷达未向下倾斜，则β＝0；
3)将第i个激光点从水平直角坐标系转换到车辆坐标系，转换公式为：
Figure BDA0002849971840000054
其中，
Figure BDA0002849971840000055
为激光雷达水平直角坐标系
Figure BDA0002849971840000056
轴与车辆坐标系
Figure BDA0002849971840000057
的夹角；Δx、Δy、Δz为激光雷达相对于车辆坐标系原点的偏移量，偏移量与激光雷达的安装位置有关。
由于在实际应用中，需要激光线束与地面相交，且注意安装位置的合理性，激光雷达与车辆参考点的航向方向一般都存在一定的倾斜角度和偏移量。但是，车辆控制是以车辆坐标系原点(或车辆参考点)的航向方向为依据，因此有必要对边界点进行倾斜角度和偏移量的矫正，以减小传感器安装造成的控制误差。步骤S2的具体实现过程包括：
步骤1，以掘进机前桥中点为参考点，构建掘进机运动学模型如下：
Figure BDA0002849971840000058
其中，(xf,yf)为前桥中点坐标，θf为前桥航向角，v为前桥车速，γ为掘进机铰接角，lf为前桥轴距，lr为后桥轴距；
步骤2，分别向左、右巷壁边界线预瞄
Figure BDA0002849971840000061
轴长度为lpre的距离，得到左预瞄点
Figure BDA0002849971840000062
和右预瞄点
Figure BDA0002849971840000063
计算预瞄距离lpre：lpre＝lmin+p·v；其中，lmin表示最短预瞄距离，p为时间常数；
步骤3，以车辆坐标系原点到预瞄点连线的中垂线与
Figure BDA0002849971840000064
轴的交点为转向中心，计算得到左转向半径
Figure BDA0002849971840000065
和右转向半径
Figure BDA0002849971840000066
Figure BDA0002849971840000067
其中，
Figure BDA0002849971840000068
Figure BDA0002849971840000069
sign()为取符号函数；则左边界约束和右边界约束下的转向半径范围为
Figure BDA00028499718400000610
步骤4，考虑掘进机执行机构的左右转向半径限幅
Figure BDA00028499718400000611
与左边界约束、右边界约束下的转向半径范围
Figure BDA00028499718400000612
求交集得到转向半径区间为[-∞,Rright]∪[Rleft,+∞]；
步骤5，根据掘进机运动学模型以及转向半径区间，计算得到转向半径区间[-∞,Rright]∪[Rleft,+∞]对应的铰接角速度区间为
Figure BDA00028499718400000613
其中
Figure BDA00028499718400000614
Figure BDA00028499718400000615
计算公式如下：
Figure BDA00028499718400000616
步骤6，将铰接角速度区间
Figure BDA0002849971840000071
按照长度为spaceγ的固定间隔采样进行离散化，将离散后的铰接角速度序列记为Υγ＝{γk|k＝1、2、…、K-1、K}，其中，k表示铰接角速度序列中的第k个铰接角速度，K为铰接角速度序列中离散铰接角速度数量，
Figure BDA0002849971840000072
接下来，将铰接角速度序列Υγ中的铰接角速度分别代入掘进机运动学模型，推算得到不同铰接角速度下的
Figure BDA0002849971840000073
轴长度为lpre的轨迹簇；其中，轨迹簇中共有K条轨迹，k表示轨迹簇中的第k条轨迹。
步骤S2中，基于掘进机运动学模型与左右巷壁的边界线约束生成了不同铰接角速度下的轨迹簇，考虑了运动学约束、边界线约束、掘进机执行机构的转向半径限幅等，限制了车辆避碰的安全运动范围。另外，步骤S2中是采用控制输入采样的方法进行轨迹簇生成，与Lattice状态采样(参考百度Apollo方案)生成轨迹簇等常规方法相比，其显著优点是考虑了车辆的运动特性，因此该方法可以直接利用最优运动轨迹对应的控制输入进行掘进机的横向控制。
步骤S3的具体实现过程包括：
i)将轨迹簇中每一条轨迹的
Figure BDA0002849971840000074
轴按照长度为
Figure BDA0002849971840000075
的固定间隔采样进行离散化，得到离散后的轨迹点
Figure BDA0002849971840000076
并计算得到每个轨迹点对应左巷壁边界线和右巷壁边界线的最短距离
Figure BDA0002849971840000077
其中，j表示轨迹中的第j个轨迹点，j＝1、2、…、J-1、J，J为轨迹簇中每条轨迹的轨迹点数量，
Figure BDA0002849971840000078
ii)将每个轨迹点对应左巷壁边界线和右巷壁边界线的最短距离
Figure BDA0002849971840000079
相减，得到每条轨迹所对应相对中心线的误差errorDisk,j；
iii)根据errorDisk,j计算得到每条轨迹相对中心线的平均误差
Figure BDA00028499718400000710
最大误差
Figure BDA00028499718400000711
终端误差
Figure BDA00028499718400000712
iv)设计控制评价函数如下：
Figure BDA0002849971840000081
其中，w1、w2、w3、w4为权重系数，w1、w4为常数。
步骤S3中，根据每条轨迹相对中心线的平均误差、最大误差、终端误差以及期望控制输入设计控制评价函数，全面考虑了各类控制误差与控制输入，更符合井下高性能控制需求；
权重系数w2、w3采用如下的自适应权重配置策略：
Figure BDA0002849971840000082
其中，w2,2＞＞w2,1、w3,2＞＞w3,1；
Figure BDA0002849971840000083
表示最大误差界值；
Figure BDA0002849971840000084
为终端误差界值。
本发明的自适应权重配置策略，当最大误差或者终端误差超过界值时给予较大的惩罚，能有效控制最大误差与终端误差在安全的误差界内，避免掘进机发生碰撞危险。
步骤S4中，选择最小的控制评价函数fmin所对应最优运动轨迹，进一步确定最优运动对应的铰接角速度
Figure BDA0002849971840000085
并计算得到最优期望铰接角γopt：
Figure BDA0002849971840000086
其中，Tc为控制周期；γopt,last为上一周期的最优期望铰接角，γopt,last的初始值为0。
基于控制评价函数最小化的原则选择最优运动轨迹，并进一步确定最优运动轨迹对应的铰接角速度，相比于采用二次规划等优化的方法进行计算，实时性高、计算量更小，同时避免了无可行解的问题。
步骤S4中，根据最优运动轨迹相对中心线的平均误差
Figure BDA0002849971840000087
最大误差
Figure BDA0002849971840000088
终端误差
Figure BDA0002849971840000089
计算巷壁碰撞预警等级的具体实现过程包括：
A)根据最大误差
Figure BDA00028499718400000810
计算最大误差等级：
Figure BDA00028499718400000811
其中，
Figure BDA00028499718400000812
Figure BDA0002849971840000091
为常数；根据平均误差
Figure BDA0002849971840000092
计算平均误差等级：
Figure BDA0002849971840000093
其中，
Figure BDA0002849971840000094
Figure BDA0002849971840000095
为常数；根据终端误差
Figure BDA0002849971840000096
计算终端误差等级：
Figure BDA0002849971840000097
其中，
Figure BDA0002849971840000098
Figure BDA0002849971840000099
为常数；
B)根据误差等级levelmax、levelave、levelend计算巷壁碰撞预警等级：
Figure BDA00028499718400000910
其中，warn_value＝levelmax·weightmax+levelave·weightave+levelend·weightend；warn_level＝4为四级风险预警，建议安全员接管；warn_level＝3为三级风险预警，提醒安全员注意；warn_level＝2为二级风险预警，warn_level＝1为一级风险预警，此时为较安全状态，可进行自动驾驶；weightmax、weightave、weightend为常数。
根据轨迹相对中心线的平均误差、最大误差、终端误差进行巷壁碰撞分级预警，降低了司机(安全员)在井下的驾驶难度，提高了掘进机井下作业的安全性。而且，基于运动学模型推算的运动轨迹进行碰撞预警，考虑了未来一段时间的潜在风险，使得掘进机的避碰更加具有预见性。
步骤S5中，判断当前位置是否到达终点的具体实现过程包括：从二维激光点集合ΞPointInTwodim＝{PointInTwodimc|c＝1、2、…、M-1、M}中找到
Figure BDA00028499718400000911
轴坐标最大的点PointBound，即为最远边界点；判断最远边界点的
Figure BDA00028499718400000912
轴坐标值是否小于终点判别阈值rend；若小于终点判别阈值rend，则表示到达终点，否则表示还未到达终点。
通过距离车辆前向激光雷达的最远边界点判断是否到达终点，实现了了掘进机终点自动停车功能，避免由于井下光线过暗，司机(安全员)未及时停车，导致在终点出现碰撞事故；
一种智能掘进机横向控制系统，包括计算机设备；所述计算机设备被配置或编程为用于执行上述方法的步骤。
与现有技术相比，本发明所具有的有益效果为：
(1)相比于基于SLAM或者惯导进行导航控制的方案，本发明通过激光雷达生成巷道左右巷壁边界线，并基于边界线设计横向最优控制器，不依赖定位及地图信息，有效规避了井下定位、建图难度大的问题；
(2)考虑掘进机运动学模型与左右巷壁的边界线约束，通过对控制输入进行采样，生成不同铰接角速度对应的轨迹簇，设计控制评价函数选择最优运动轨迹并计算最优期望铰接角，融合了采样与最优化理论，平衡了计算量与控制精度的两难问题；
(3)根据轨迹相对中心线的平均误差、最大误差、终端误差进行巷壁碰撞分级预警，降低了司机(安全员)在井下的驾驶难度，提高了掘进机井下作业的安全性。
附图说明
图1为整体流程图；
图2(a)为球坐标系-激光雷达三维直角坐标系转换示意图；
图2(b)为倾斜的激光雷达三维直角坐标系-水平直角坐标系转换示意图；
图2(c)为激光雷达安装三视图；
图2(d)为车辆坐标系示意图；
图3为掘进机运动学模型示意图；
图4为转向半径计算示意图；
图5为巷壁碰撞预警等级曲线图；
图6为井下巷道全局示意图；
图7为掘进机实际车速曲线；
图8(a)为不同方法的横向控制误差曲线图；
图8(b)为不同方法的铰接角曲线图。
具体实施方式
本发明实施例以湖南创远高新机械有限责任公司提供的井下掘进机为原型，在湖南某金属矿进行实验，提供的一种智能掘进机横向优化控制方法，如图1所示。包括以下步骤：
步骤一：掘进机前向激光雷达扫描矿井巷道，获取巷道左右巷壁边界点的位置坐标，并拟合生成左右巷壁的边界线，具体步骤如下：
(1)掘进机前向激光雷达扫描矿井巷道，得到球坐标系下的激光点集合Ωpoint＝{Pointi|i＝1、2、…、N-1、N}，其中，Pointi＝(ri,ωi,αi)表示第i个激光点，N为集合中激光点的个数；激光雷达与第i个激光点的连线为Ri，Ri在xoy平面的投影为
Figure BDA0002849971840000111
ωi表示Ri与
Figure BDA0002849971840000112
之间的夹角，αi表示Ri与激光雷达正y轴方向的夹角，ri表示激光雷达到第i个激光点的距离；其中，x,y,z轴是指激光雷达三维直角坐标的坐标轴；
本实施例中，采用的是velodyne 16线激光雷达，激光连接线指向的方向为激光雷达三维直角坐标的y轴正方向，垂直激光雷达横截面向上的方向为z轴正方向，垂直y轴与z轴向左的方向为x轴正方向。
(2)将球坐标系下的每个激光点均转换到车辆坐标系下，得到车辆坐标系下的激光点集合ΘpointInVeh＝{PointInVehi|i＝1、2、…、N-1、N}，其中，
Figure BDA0002849971840000113
Figure BDA0002849971840000114
分别表示车辆坐标系下第i个激光点的
Figure BDA0002849971840000115
轴、
Figure BDA0002849971840000116
轴与
Figure BDA0002849971840000117
轴坐标；。第i个激光点进行坐标系转换的具体过程如下：
1)将第i个激光点从球坐标系转换到激光雷达三维直角坐标系，如图2(a)所示，坐标系转换公式为：
Figure BDA0002849971840000118
其中，xi,yi,zi是指第i个激光点在激光雷达三维直角坐标下的坐标。
2)若激光雷达向下倾斜，将第i个激光点从倾斜的激光雷达三维直角坐标系转换到水平直角坐标系，如图2(b)所示，转换公式为：
Figure BDA0002849971840000121
其中，
Figure BDA0002849971840000122
是指第i个激光点在激光雷达水平直角坐标下的坐标，β为激光雷达向下倾斜的角度，若激光雷达不向下倾斜，则β＝0；本实例中，激光雷达向下倾斜安装，β＝0.12rad。
3)将第i个激光点从水平直角坐标系转换到车辆坐标系，转换公式为：
Figure BDA0002849971840000123
其中，
Figure BDA0002849971840000124
为激光雷达水平直角坐标系
Figure BDA0002849971840000125
轴与车辆坐标系
Figure BDA0002849971840000126
的夹角。
如图2(c)所示，Δx、Δy、Δz为激光雷达相对于车辆坐标系原点的偏移量；本实例中Δx＝0.1m、Δy＝0.9m、Δz＝1.6m。
如图2(d)所示，以掘进机前桥中点为车辆坐标系原点，车辆坐标系是指以车辆航向为
Figure BDA0002849971840000127
轴方向(前向)，以垂直车辆航向向左的方向为
Figure BDA0002849971840000128
轴方向(侧向)，以垂直
Figure BDA0002849971840000129
轴和
Figure BDA00028499718400001210
轴且竖直向上的方向为
Figure BDA00028499718400001211
轴方向(天向)；
(3)滤除激光点集合ΘpointInVeh＝{PointInVehi|i＝1、2、…、N-1、N}中高度hi小于hground的激光点，其中，hi＝abs(zi)，abs表示取绝对值，hground表示地面激光点的高度阈值；具体做法是：遍历集合中所有的激光点，将高度小于hground的激光点从集合中移除，得到滤除后的激光点集合ΦpointInWall＝{PointInWallc|c＝1、2、…、M-1、M},M为滤除后剩下的激光点个数，
Figure BDA00028499718400001212
Figure BDA00028499718400001213
分别表示车辆坐标系下第c个激光点的前向、侧向与天向坐标；
(4)将滤除后的激光点集合ΦpointInWall＝{PointInWallc|c＝1、2、…、M-1、M}投影到车辆坐标系的
Figure BDA0002849971840000131
平面，具体做法是：令所有激光点的
Figure BDA0002849971840000132
轴坐标为0，得到二维激光点集合ΞPointInTwodim＝{PointInTwodimc|i＝1、2、…、M-1、M}，其中
Figure BDA0002849971840000133
(5)从二维激光点集合ΞPointInTwodim＝{PointInTwodimc|c＝1、2、…、M-1、M}中提取左右巷壁的边界点，具体做法是：
1)从车辆坐标系原点开始，沿着
Figure BDA0002849971840000134
轴按照固定间隔spacep进行等间隔采样，直到集合ΞPointInTwodim中
Figure BDA0002849971840000135
轴坐标的最大值处结束；将采样点集合记为PSamplingPoint＝{SamplingPointj|j＝1、2、…、J-1、J}，
Figure BDA0002849971840000136
其中，j表示第j个采样点，
Figure BDA0002849971840000137
是第j个采样点的横坐标，J为采样点的个数；本实例中，采样间隔spacep＝0.1m；
2)遍历采样点集合PSamplingPoint中的每个采样点；对于每个所述采样点，确定集合ΞPointInTwodim中距离该采样点最近的左侧激光点和右侧激光点，即得到该采样点对应的左巷壁边界点和右巷壁边界点；将所有采样点对应的左巷壁边界点的集合记为BoundLeftPoint＝{LeftPointj|j＝1、2、…、J-1、J}，将所有采样点对应的右巷壁边界点的集合记为BoundRightPoint＝{RightPointj|j＝1、2、…、J-1、J}；
Figure BDA0002849971840000138
表示第j个左巷壁边界点，
Figure BDA0002849971840000139
表示第j个右巷壁边界点，
Figure BDA00028499718400001310
表示第j个左巷壁边界点的
Figure BDA00028499718400001311
坐标，
Figure BDA00028499718400001312
表示第j个左巷壁边界点的
Figure BDA00028499718400001313
坐标，
Figure BDA00028499718400001314
表示第j个右巷壁边界点的
Figure BDA00028499718400001315
坐标，
Figure BDA00028499718400001316
表示第j个左巷壁边界点的
Figure BDA00028499718400001317
坐标，上标left表示左侧巷壁，上标right表示右侧巷壁；优选地，确定集合ΞPointInTwodim中距离任一采样点最近的左侧激光点和右侧激光点的具体实现过程包括：①计算当前采样点与集合ΞPointInTwodim中每个激光点的距离；②集合ΞPointInTwodim中
Figure BDA00028499718400001318
轴坐标为正值且与所述当前采样点距离最小的激光点即为左巷壁边界点，集合ΞPointInTwodim中
Figure BDA00028499718400001319
轴坐标为负值且与所述当前采样点距离最小的激光点即为右巷壁边界点。
(6)采用三次样条插值拟合左右巷壁的边界点生成左右巷壁边界线，具体如下：
采用三次样条插值拟合左侧巷壁边界点集合BoundLeftPoint，得到左侧巷壁边界线的分段三次多项式为：
Figure BDA0002849971840000141
其中，左侧巷壁的边界线是由J-1条三次多项式组成，
Figure BDA0002849971840000142
表示左侧巷壁第j个和第j+1个边界点连线的三次方系数；
采用三次样条插值拟合右侧巷壁边界点集合BoundRightPoint，得到右侧巷壁边界线的分段三次多项式为：
Figure BDA0002849971840000143
其中，右侧巷壁的边界线也是由J-1条三次多项式组成，
Figure BDA0002849971840000144
表示右侧巷壁第j个和第j+1个边界点连线的三次方系数。
分段三次多项式中未考虑
Figure BDA0002849971840000145
轴坐标是车辆控制领域的常规操作，即进行车辆横向控制时忽略车辆竖直方向(
Figure BDA0002849971840000146
轴方向)的运动。
在本实施例中，所述掘进机是装配了激光雷达的铰接式车辆，分为前车体与后车体，前后车体由铰接盘连接，利用铰接进行转向，最大铰接角为0.35rad(铰接角的机械限制)。
本实施例通过激光雷达生成巷道左右巷壁边界线，并基于边界线设计横向最优控制器，不依赖定位及地图信息，有效规避了井下定位、建图难度大的问题；另外，与现有技术手段不同的是，本实施例通过将所有的激光点投影到车辆坐标系的
Figure BDA0002849971840000147
平面，并提出最靠近
Figure BDA0002849971840000148
轴的激光点，相当于是提取了巷壁中最突出的位置用来拟合生成边界线，相比于将巷壁作为二维平面或者根据巷壁与地面相交处生成边界线的方式，本实施例的方法考虑了矿井巷壁凹凸不平的特性，生成的边界线更符合安全约束。
步骤二：构建掘进机运动学模型，并基于运动学模型与左右巷壁的边界线生成不同铰接角速度下的轨迹簇，具体步骤如下：
(1)如图3所示，以掘进机前桥中点为参考点，构建掘进机运动学模型如下：
Figure BDA0002849971840000151
其中，(xf,yf)为前桥中点坐标，θf为前桥航向角，v为前桥车速，γ为掘进机铰接角，lf为前桥轴距，lr为后桥轴距；本实施例中，掘进机的前桥轴距lf＝1.4m、后桥轴距lr＝1.76m。
(2)分别向左、右巷壁边界线预瞄
Figure BDA0002849971840000152
轴长度为lpre的距离，得到左预瞄点
Figure BDA0002849971840000153
和右预瞄点
Figure BDA0002849971840000154
采用经验公式计算预瞄距离lpre，公式如下：
lpre＝lmin+p·v；
其中，lmin表示最短预瞄距离，p为时间常数，本实施例中lmin＝3m、p＝0.1s。
(3)如图4所示，以车辆坐标系原点(前桥中点)到预瞄点连线的中垂线与
Figure BDA0002849971840000155
轴的交点为转向中心，计算得到左转向半径
Figure BDA0002849971840000156
和右转向半径
Figure BDA0002849971840000157
如下：
Figure BDA0002849971840000158
其中，αleft为左预瞄偏差角，αright为右预瞄偏差角，
Figure BDA0002849971840000159
Figure BDA00028499718400001510
于是，左右边界约束下的转向半径范围为
Figure BDA00028499718400001511
(4)考虑掘进机执行机构的转向半径限幅
Figure BDA00028499718400001512
与左右边界约束下的转向半径范围
Figure BDA00028499718400001513
求交集得到转向半径区间为[-∞,Rright]∪[Rleft,+∞]，其中，
Figure BDA0002849971840000161
本实施例中，规定掘进机左转向半径为正值，右转向半径为负值，即
Figure BDA0002849971840000162
Figure BDA0002849971840000163
(5)根据掘进机运动学模型以及转向半径区间，计算得到转向半径区间[-∞,Rright]∪[Rleft,+∞]对应的铰接角速度区间为
Figure BDA0002849971840000164
其中
Figure BDA0002849971840000165
Figure BDA0002849971840000166
计算公式如下：
Figure BDA0002849971840000167
其中，
Figure BDA0002849971840000168
分别与转向半径为Rright、Rleft、-∞、+∞对应，γ是指掘进机上一周期的铰接角，在实际应用中，γ与γopt,last的含义及数值相同。
以
Figure BDA0002849971840000169
为例，其计算原理具体如下：
首先，根据掘进机运动学模型有：
Figure BDA00028499718400001610
而后，根据速度与角速度的关系可得：
Figure BDA00028499718400001611
最后，联立以上两式可得：
Figure BDA00028499718400001612
变形得：
Figure BDA00028499718400001613
(6)将铰接角速度区间
Figure BDA00028499718400001614
按照长度为spaceγ的固定间隔采样进行离散化，将离散后的铰接角速度序列记为Υγ＝{γk|k＝1、2、…、K-1、K}，其中，k表示铰接角速度序列中的第k个铰接角速度，K为铰接角速度序列中离散铰接角速度数量，
Figure BDA0002849971840000171
接下来，将铰接角速度序列Υγ中的铰接角速度分别代入掘进机运动学模型，推算得到不同铰接角速度下的
Figure BDA0002849971840000172
轴长度为lpre的轨迹簇；其中，轨迹簇中共有K条轨迹，k表示轨迹簇中的第k条轨迹。本实施例中spaceγ＝0.01rad。
步骤三：遍历轨迹簇中的每条轨迹，根据每条轨迹相对中心线的平均误差、最大误差、终端误差以及期望控制输入设计控制评价函数，具体步骤如下：
(1)将轨迹簇中每一条轨迹的
Figure BDA0002849971840000173
轴按照长度为
Figure BDA0002849971840000174
的固定间隔采样进行离散化，得到离散后的轨迹点
Figure BDA0002849971840000175
并计算得到每个轨迹点对应左、右巷壁边界线的最短距离
Figure BDA0002849971840000176
(见CN110796852B)，其中，j表示轨迹中的第j个轨迹点，j＝1、2、…、J-1、J，J为轨迹簇中每条轨迹的轨迹点数量，
Figure BDA0002849971840000177
本实施例中
Figure BDA0002849971840000178
(2)将每个轨迹点对应左右巷壁的最短距离
Figure BDA0002849971840000179
相减计算得到每条轨迹所对应相对中心线的误差errorDisk,j，即：
Figure BDA00028499718400001710
(3)根据errorDisk,j计算得到每条轨迹相对中心线的平均误差、最大误差、终端误差，计算方法如下：
Figure BDA00028499718400001711
其中，
Figure BDA00028499718400001712
表示第k条轨迹中J个轨迹点相对中心线的误差绝对值之和，max[abs(errorDisk,j)]表示第k条轨迹的J个轨迹点相对中心线的最大误差的绝对值，abs(errorDisk,J)表示第k条轨迹的第J个轨迹点相对中心线的误差绝对值。
(4)设计控制评价函数如下：
Figure BDA0002849971840000181
其中，w1、w2、w3、w4为权重系数，w1、w4为常数，本实施例中w1＝0.8，w4＝0.5，w2、w3如下：
Figure BDA0002849971840000182
Figure BDA0002849971840000183
其中，w2,2＞＞w2,1、w3,2＞＞w3,1；本实施例中，w2,1取值为1，w2,2取值为15，w3,1取值为1.5，w3,2取值为15；
Figure BDA0002849971840000184
表示最大误差界值，在本实施例中取值为0.5，
Figure BDA0002849971840000185
为终端误差界值，在本实施例中取值为0.1；w2、w3表达式的物理含义是，当最大误差或者终端误差超过界值时给予较大的惩罚，以控制最大误差与终端误差在有限的误差界内。
步骤四：根据控制评价函数，从轨迹簇中选择最优运动轨迹，并计算最优期望铰接角；优选地，根据所述最优运动轨迹相对中心线的平均误差、最大误差、终端误差进行巷壁碰撞分级预警；
步骤四中计算最优期望铰接角的具体做法为：选择最小的控制评价函数fmin所对应轨迹，进一步确定其对应的铰接角速度
Figure BDA0002849971840000186
并计算得到最优期望铰接角γopt，计算公式如下：
Figure BDA0002849971840000187
其中，Tc为控制周期，本实施例中Tc＝100ms，γopt,last为上一周期的最优期望铰接角，γopt,last的初始值为0。
步骤四中进行巷壁碰撞分级预警的具体做法为：根据最优运动轨迹相对中心线的平均误差
Figure BDA0002849971840000191
最大误差
Figure BDA0002849971840000192
终端误差
Figure BDA0002849971840000193
计算巷壁碰撞预警等级，计算方法如下：
(1)根据最大误差
Figure BDA0002849971840000194
计算最大误差等级，规则如下：
Figure BDA0002849971840000195
本实施例中，
Figure BDA0002849971840000196
(2)根据平均误差
Figure BDA0002849971840000197
计算平均误差等级，规则如下：
Figure BDA0002849971840000198
本实施例中，
Figure BDA0002849971840000199
(3)根据终端误差
Figure BDA00028499718400001910
计算终端误差等级，规则如下：
Figure BDA00028499718400001911
本实施例中，
Figure BDA00028499718400001912
(4)根据误差等级levelmax、levelave、levelend计算计算巷壁碰撞预警等级，具体如下：
1)量化巷壁碰撞预警危险程度，计算如下：
warn_value＝levelmax·weightmax+levelave·weightave+levelend·weightend；
本实施例中，weightmax＝3，weightave＝1，weightend＝1。
2)划分巷壁碰撞预警等级，规则如下：
Figure BDA0002849971840000201
其中，warn_level＝4为四级风险预警，建议安全员接管；warn_level＝3为三级风险预警，提醒安全员注意；warn_level＝2为二级风险预警，warn_level＝1为一级风险预警，此时为较安全状态，可以进行自动驾驶；本实施例中，warnBound＝5。
根据轨迹相对中心线的平均误差、最大误差、终端误差进行巷壁碰撞分级预警，降低了司机(安全员)在井下的驾驶难度，提高了掘进机井下作业的安全性。
本实施例对应的巷壁碰撞分级预警的实车测试结果如图5所示，根据实测结果可以看出，掘进机在井下自动驾驶的过程中，大多数时间为一级预警，最高预警等级为三级，未触发四级高风险预警，全程均为自动驾驶状态。对比图6可知，二级及三级预警是对应掘进机转弯的过程中，此时掘进的位置、姿态变化较大，需要提醒司机(安全员)注意，也符合实际驾驶经验。
步骤五：根据最优期望铰接角更新掘进机位置与姿态，判断当前位置是否到达终点，若到达终点则停止，否则回到步骤一继续循环。
步骤五中判断当前位置是否到达终点的具体方法是，首先，从二维激光点集合ΞPointInTwodim＝{PointInTwodimc|c＝1、2、…、M-1、M}中找到
Figure BDA0002849971840000202
轴坐标最大的点PointBound，即为最远边界点；而后，判断最远边界点的
Figure BDA0002849971840000203
轴坐标值是否小于终点判别阈值rend；若小于终点判别阈值rend，则表示到达终点，否则表示还未到达终点。本实施例中rend取值为2m。
本实施例应用了一种新的智能掘进机横向优化控制方法及系统，基于掘进机运动学模型与左右巷壁的边界线生成了不同铰接角速度下的轨迹簇，设计控制评价函数选择最优轨迹，考虑了轨迹相对中心线的平均误差、最大误差、终端误差以及控制输入，确保系统满足高精度控制需求，融合了采样与最优化理论，平衡了计算量与控制精度的两难问题。
本实施例在某金属矿进行实车测试，井下巷道呈现如图6所示的“J”字形。实验过程中，掘进机初始车速为0，设置期望车速为定值4.5m/s，实际车速变化如图7所示。智能掘进机横向控制的实车测试结果如图8(a)、图8(b)所示，根据图8(a)可以看出，相比几何控制方法与LQR最优控制方法，本实施例提供的方法的横向控制误差整体上是最小的；几何控制方法的控制误差最大，这是因为几何控制器未考虑误差的长期变化，入弯、出弯的过程中不能及时调整，导致误差迅速上升；LQR最优控制实现和求解最复杂，因为LQR需要用迭代的方法求解黎卡提方程，而本实施例的方法和几何控制方法均无需进行复杂的求解。根据图8(b)可以看出三种方法所计算得到的铰接角均不超过0.35rad，满足最大铰接角的机械限制，但是几何控制器的铰接角整体上最大，比较接近掘进机转向极限，这是由于本实施例提供的方法以及LQR最优控制方法进行优化时均考虑了控制输入最小化的问题，即以尽量小的铰接角实时误差尽可能得减小，而几何控制器没有考虑这一点。

Claims (10)

1.一种智能掘进机横向优化控制方法，其特征在于，包括以下步骤：
S1、扫描矿井巷道，获取巷道左巷壁边界点、右巷壁边界点的位置坐标，并拟合生成左巷壁、右巷壁的边界线；
步骤S1的具体实现过程包括：
1)掘进机前向激光雷达扫描矿井巷道，得到球坐标系下的激光点集合Ωpoint＝{Pointi|i＝1、2、…、N-1、N}，其中，Pointi＝(ri,ωi,αi)表示第i个激光点，N为集合中激光点的个数；激光雷达与第i个激光点的连线为Ri，Ri在xoy平面的投影为Ri proj，ωi表示Ri与Ri proj之间的夹角，αi表示Ri与激光雷达正y轴方向的夹角，ri表示激光雷达到第i个激光点的距离；x,y,z轴是指激光雷达三维直角坐标系的坐标轴；
2)将球坐标系下的每个激光点均转换到车辆坐标系下，得到车辆坐标系下的激光点集合ΘpointInVeh＝{PointInVehi|i＝1、2、…、N-1、N}，其中，车辆坐标系是以掘进机前桥中点为原点，以车辆航向为
Figure FDA0003295372900000011
轴方向，以垂直车辆航向向左的方向为
Figure FDA0003295372900000012
轴方向，以垂直
Figure FDA0003295372900000013
轴和
Figure FDA0003295372900000014
轴且竖直向上的方向为
Figure FDA0003295372900000015
轴方向；
Figure FDA0003295372900000016
Figure FDA0003295372900000017
分别表示车辆坐标系下第i个激光点的
Figure FDA0003295372900000018
轴、
Figure FDA0003295372900000019
轴与
Figure FDA00032953729000000110
轴坐标；
3)滤除激光点集合ΘpointInVeh＝{PointInVehi|i＝1、2、…、N-1、N}中高度hi小于hground的激光点，其中，hi＝abs(zi)，hground表示地面激光点的高度阈值；zi是第i个激光点在激光雷达三维直角坐标下的z轴坐标；
4)将经步骤3)处理后的激光点集合ΦpointInWall＝{PointInWallc|c＝1、2、…、M-1、M}投影到车辆坐标系的
Figure FDA00032953729000000111
平面，得到二维激光点集合ΞPointInTwodim＝{PointInTwodimc|c＝1、2、…、M-1、M}，其中
Figure FDA00032953729000000112
表示第c个二维激光点；
5)从所述二维激光点集合ΞPointInTwodim＝{PointInTwodimc|c＝1、2、…、M-1、M}中提取左、右巷壁的边界点，采用三次样条插值分别拟合左、右巷壁的边界点，生成左、右巷壁边界线；优选地，从所述二维激光点集合ΞPointInTwodim＝{PointInTwodimc|c＝1、2、…、M-1、M}中提取左、右巷壁的边界点的具体实现过程包括：
5a)从车辆坐标系原点开始，沿着
Figure FDA0003295372900000021
轴按照固定间隔spacep进行等间隔采样，直至采样到集合ΞPointInTwodim中
Figure FDA0003295372900000022
轴坐标的最大值处，将采样点集合记为PSamplingPoint＝{SamplingPointj|j＝1、2、…、J-1、J}，
Figure FDA0003295372900000023
其中，j表示第j个采样点，
Figure FDA0003295372900000024
是第j个采样点的横坐标，J为采样点的个数；
5b)遍历采样点集合PSamplingPoint中的每个采样点；对于每个所述采样点，确定集合ΞPointInTwodim中距离该采样点最近的左侧激光点和右侧激光点，即得到该采样点对应的左巷壁边界点和右巷壁边界点；将所有采样点对应的左巷壁边界点的集合记为BoundLeftPoint＝{LeftPointj|j＝1、2、…、J-1、J}，将所有采样点对应的右巷壁边界点的集合记为BoundRightPoint＝{RightPointj|j＝1、2、…、J-1、J}；
Figure FDA0003295372900000025
表示第j个左巷壁边界点，
Figure FDA0003295372900000026
表示第j个右巷壁边界点，
Figure FDA0003295372900000027
表示第j个左巷壁边界点的
Figure FDA0003295372900000028
坐标，
Figure FDA0003295372900000029
表示第j个左巷壁边界点的
Figure FDA00032953729000000210
坐标，
Figure FDA00032953729000000211
表示第j个右巷壁边界点的
Figure FDA00032953729000000212
坐标，
Figure FDA00032953729000000213
表示第j个左巷壁边界点的
Figure FDA00032953729000000214
坐标，上标left表示左侧巷壁，上标right表示右侧巷壁；确定集合ΞPointInTwodim中距离任一采样点最近的左侧激光点和右侧激光点的具体实现过程包括：①计算当前采样点与集合ΞPointInTwodim中每个激光点的距离；②集合ΞPointInTwodim中
Figure FDA0003295372900000031
轴坐标为正值且与所述当前采样点距离最小的激光点即为左巷壁边界点，集合ΞPointInTwodim中
Figure FDA0003295372900000032
轴坐标为负值且与所述当前采样点距离最小的激光点即为右巷壁边界点；
S2、构建掘进机运动学模型，并基于所述运动学模型与左巷壁、右巷壁的边界线生成不同铰接角速度下的轨迹簇；
S3、遍历轨迹簇中的每条轨迹，根据每条轨迹相对中心线的平均误差、最大误差、终端误差以及期望控制输入设计控制评价函数；所述中心线是指左巷壁的边界线与右巷壁的边界线之间的中线，且左巷壁的边界线与右巷壁的边界线关于该中线对称；
S4、根据控制评价函数，从轨迹簇中选择最优运动轨迹，并计算最优期望铰接角；根据所述最优运动轨迹相对中心线的平均误差、最大误差、终端误差进行巷壁碰撞分级预警。
2.根据权利要求1所述的智能掘进机横向优化控制方法，其特征在于，步骤2)的具体实现过程包括：
a)利用以下转换公式将第i个激光点从球坐标系转换到激光雷达三维直角坐标系：
Figure FDA0003295372900000033
其中，xi,yi,zi是第i个激光点在激光雷达三维直角坐标下的坐标；
b)将第i个激光点从倾斜的激光雷达三维直角坐标系转换到水平直角坐标系，转换公式为：
Figure FDA0003295372900000034
Figure FDA0003295372900000035
是指第i个激光点在激光雷达水平直角坐标下的坐标，β为激光雷达向下倾斜的角度；若激光雷达未向下倾斜，则β＝0；
c)将第i个激光点从水平直角坐标系转换到车辆坐标系，转换公式为：
Figure FDA0003295372900000041
其中，
Figure FDA0003295372900000042
为激光雷达水平直角坐标系y轴与车辆坐标系
Figure FDA0003295372900000043
的夹角；Δx、Δy、Δz为激光雷达相对于车辆坐标系原点的偏移量。
3.根据权利要求1所述的智能掘进机横向优化控制方法，其特征在于，步骤S2的具体实现过程包括：
步骤1，以掘进机前桥中点为参考点，构建掘进机运动学模型如下：
Figure FDA0003295372900000044
其中，(xf,yf)为前桥中点坐标，θf为前桥航向角，v为前桥车速，γ为掘进机铰接角，lf为前桥轴距，lr为后桥轴距；
步骤2，分别向左、右巷壁边界线预瞄
Figure FDA0003295372900000045
轴长度为lpre的距离，得到左预瞄点
Figure FDA0003295372900000046
和右预瞄点
Figure FDA0003295372900000047
计算预瞄距离lpre：lpre＝lmin+p·v；
其中，lmin表示最短预瞄距离，p为时间常数；
步骤3，以车辆坐标系原点到预瞄点连线的中垂线与
Figure FDA0003295372900000048
轴的交点为转向中心，计算得到左转向半径
Figure FDA0003295372900000049
和右转向半径
Figure FDA00032953729000000410
Figure FDA00032953729000000411
其中，
Figure FDA00032953729000000415
Figure FDA00032953729000000412
sign()为取符号函数；则左边界约束和右边界约束下的转向半径范围为
Figure FDA00032953729000000413
步骤4，考虑掘进机执行机构的左右转向半径限幅
Figure FDA00032953729000000414
与左边界约束、右边界约束下的转向半径范围
Figure FDA0003295372900000051
求交集得到转向半径区间为[-∞,Rright]∪[Rleft,+∞]；
步骤5，根据掘进机运动学模型以及转向半径区间，计算得到转向半径区间[-∞,Rright]∪[Rleft,+∞]对应的铰接角速度区间为
Figure FDA0003295372900000052
其中
Figure FDA0003295372900000053
计算公式如下：
Figure FDA0003295372900000054
步骤6，将铰接角速度区间
Figure FDA0003295372900000055
按照长度为spaceγ的固定间隔采样进行离散化，将离散后的铰接角速度序列记为Υγ＝{γk|k＝1、2、…、K-1、K}，其中，k表示铰接角速度序列中的第k个铰接角速度，K为铰接角速度序列中离散铰接角速度数量；将铰接角速度序列Υγ中的铰接角速度代入掘进机运动学模型，推算得到不同铰接角速度下的
Figure FDA0003295372900000056
轴长度为lpre的轨迹簇；其中，K为轨迹簇中轨迹数量，
Figure FDA0003295372900000057
k表示轨迹簇中的第k条轨迹。
4.根据权利要求1所述的智能掘进机横向优化控制方法，其特征在于，步骤S3的具体实现过程包括：
i)将轨迹簇中每一条轨迹的
Figure FDA0003295372900000058
轴按照长度为
Figure FDA0003295372900000059
的固定间隔采样进行离散化，得到离散后的轨迹点
Figure FDA00032953729000000510
并计算得到每个轨迹点对应左巷壁边界线和右巷壁边界线的最短距离
Figure FDA00032953729000000511
其中，j表示轨迹中的第j个轨迹点，j＝1、2、…、J-1、J，J为轨迹簇中每条轨迹的轨迹点数量，
Figure FDA00032953729000000512
ii)将每个轨迹点对应左巷壁边界线和右巷壁边界线的最短距离
Figure FDA0003295372900000061
相减，得到每条轨迹相对中心线的误差errorDisk,j；
iii)根据errorDisk,j计算得到每条轨迹相对中心线的平均误差
Figure FDA0003295372900000062
最大误差
Figure FDA0003295372900000063
终端误差
Figure FDA0003295372900000064
iv)设计控制评价函数如下：
Figure FDA0003295372900000065
其中，w1、w2、w3、w4为权重系数，w1、w4为常数。
5.根据权利要求4所述的智能掘进机横向优化控制方法，其特征在于，
Figure FDA0003295372900000066
其中，w2,2＞＞w2,1、w3,2＞＞w3,1；
Figure FDA0003295372900000067
表示最大误差界值；
Figure FDA0003295372900000068
为终端误差界值。
6.根据权利要求1所述的智能掘进机横向优化控制方法，其特征在于，步骤S4中，选择最小的控制评价函数fmin所对应最优运动轨迹，进一步确定最优运动轨迹对应的铰接角速度
Figure FDA0003295372900000069
并计算得到最优期望铰接角γopt：
Figure FDA00032953729000000610
其中，Tc为控制周期；γopt,last为上一周期的最优期望铰接角，γopt,last的初始值为0。
7.根据权利要求1所述的智能掘进机横向优化控制方法，其特征在于，步骤S4中，根据最优运动轨迹相对中心线的平均误差
Figure FDA00032953729000000611
最大误差
Figure FDA00032953729000000612
终端误差
Figure FDA00032953729000000613
计算巷壁碰撞预警等级的具体实现过程包括：
A)根据最大误差
Figure FDA00032953729000000614
计算最大误差等级：
Figure FDA00032953729000000615
其中，
Figure FDA00032953729000000616
Figure FDA00032953729000000617
为常数；根据平均误差
Figure FDA00032953729000000618
计算平均误差等级：
Figure FDA0003295372900000071
其中，
Figure FDA0003295372900000072
Figure FDA0003295372900000073
为常数；根据终端误差
Figure FDA0003295372900000074
计算终端误差等级：
Figure FDA0003295372900000075
其中，
Figure FDA0003295372900000076
Figure FDA0003295372900000077
为常数；
B)根据误差等级levelmax、levelave、levelend计算巷壁碰撞预警等级：
Figure FDA0003295372900000078
其中，
warn_value＝levelmax·weightmax+levelave·weightave+levelend·weightend；warn_level＝4为四级风险预警，建议安全员接管；warn_level＝3为三级风险预警，提醒安全员注意；warn_level＝2为二级风险预警，warn_level＝1为一级风险预警，此时为较安全状态，可进行自动驾驶；weightmax、weightave、weightend为常数。
8.根据权利要求1～7之一所述的智能掘进机横向优化控制方法，其特征在于，还包括：
S5、根据最优期望铰接角更新掘进机位置与姿态，判断当前位置是否到达终点，若到达终点则停止，否则返回步骤S1。
9.根据权利要求8所述的智能掘进机横向优化控制方法，其特征在于，步骤S5中，判断当前位置是否到达终点的具体实现过程包括：从二维激光点集合ΞPointInTwodim＝{PointInTwodimc|c＝1、2、…、M-1、M}中找到
Figure FDA0003295372900000079
轴坐标最大的点PointBound，即为最远边界点；判断最远边界点的
Figure FDA0003295372900000081
轴坐标值是否小于终点判别阈值rend；若小于终点判别阈值rend，则表示到达终点，否则表示还未到达终点。
10.一种智能掘进机横向优化控制系统，其特征在于，包括计算机设备；所述计算机设备被配置或编程为用于执行权利要求1～9之一所述方法的步骤。
CN202011524208.5A 2020-12-22 2020-12-22 智能掘进机横向优化控制方法及系统 Active CN112650241B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011524208.5A CN112650241B (zh) 2020-12-22 2020-12-22 智能掘进机横向优化控制方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011524208.5A CN112650241B (zh) 2020-12-22 2020-12-22 智能掘进机横向优化控制方法及系统

Publications (2)

Publication Number Publication Date
CN112650241A CN112650241A (zh) 2021-04-13
CN112650241B true CN112650241B (zh) 2021-11-23

Family

ID=75358796

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011524208.5A Active CN112650241B (zh) 2020-12-22 2020-12-22 智能掘进机横向优化控制方法及系统

Country Status (1)

Country Link
CN (1) CN112650241B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117516550B (zh) * 2024-01-04 2024-03-15 三一重型装备有限公司 路径规划方法及系统、可读存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20090003386A (ko) * 2007-06-07 2009-01-12 조병태 지하매설물을 안내하는 네비게이션
CN101713999A (zh) * 2009-11-18 2010-05-26 北京矿冶研究总院 地下自主铲运机的导航控制方法
CN103197675A (zh) * 2013-03-13 2013-07-10 北京矿冶研究总院 地下铲运机自主行驶和避障运动控制及目标路径规划方法
CN103413473A (zh) * 2013-08-22 2013-11-27 北京科技大学 一种地下矿用铰接车的驾驶模拟系统
WO2017192666A1 (en) * 2016-05-03 2017-11-09 Sunshine Aerial Systems, Inc. Autonomous aerial vehicle
CN107561552A (zh) * 2017-08-16 2018-01-09 北京矿冶研究总院 一种矿山井下无轨设备防碰撞方法与装置
CN109375632A (zh) * 2018-12-17 2019-02-22 清华大学 自动驾驶车辆实时轨迹规划方法
CN111538331A (zh) * 2020-04-24 2020-08-14 北京科技大学 一种地下无人铰接车的反应式导航方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20090003386A (ko) * 2007-06-07 2009-01-12 조병태 지하매설물을 안내하는 네비게이션
CN101713999A (zh) * 2009-11-18 2010-05-26 北京矿冶研究总院 地下自主铲运机的导航控制方法
CN103197675A (zh) * 2013-03-13 2013-07-10 北京矿冶研究总院 地下铲运机自主行驶和避障运动控制及目标路径规划方法
CN103413473A (zh) * 2013-08-22 2013-11-27 北京科技大学 一种地下矿用铰接车的驾驶模拟系统
WO2017192666A1 (en) * 2016-05-03 2017-11-09 Sunshine Aerial Systems, Inc. Autonomous aerial vehicle
CN107561552A (zh) * 2017-08-16 2018-01-09 北京矿冶研究总院 一种矿山井下无轨设备防碰撞方法与装置
CN109375632A (zh) * 2018-12-17 2019-02-22 清华大学 自动驾驶车辆实时轨迹规划方法
CN111538331A (zh) * 2020-04-24 2020-08-14 北京科技大学 一种地下无人铰接车的反应式导航方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
Automation of an Underground Mining Vehicle using Reactive Navigation and Opportunistic Localization;Elliot S. Duff;《 International Conference on Intelligent Robots and Systems》;20031031;全文 *
Autonomous Control of Underground Mining Vehicles using Reactive Navigation;Roberts J M;《International Conference on Ｒobotics and Automation》;20000430;全文 *
基于NMPC 的地下无人铲运机反应式导航系统;罗维东;《煤炭学报》;20200430;正文第1538-1542页 *
基于OGＲE 的铰接式地下矿车驾驶模拟系统;刘立;《农业机械学报》;20130831;全文 *

Also Published As

Publication number Publication date
CN112650241A (zh) 2021-04-13

Similar Documents

Publication Publication Date Title
CN115525047B (zh) 一种具备多型避障方式的车辆局部轨迹规划方法及系统
CN109669461B (zh) 一种复杂工况下自动驾驶车辆决策系统及其轨迹规划方法
CN110286683B (zh) 一种履带式移动机器人的自主行驶路径跟踪控制方法
CN110262495B (zh) 可实现移动机器人自主导航与精确定位的控制系统及方法
CN113276848A (zh) 一种智能驾驶换道避障轨迹规划、跟踪控制方法及系统
CN108106623B (zh) 一种基于流场的无人车路径规划方法
US8233663B2 (en) Method for object formation
CN113720346B (zh) 基于势能场和隐马尔可夫模型的车辆路径规划方法及系统
CN114194215B (zh) 一种智能车辆避障换道轨迹规划方法及系统
CN109270933A (zh) 基于圆锥曲线的无人驾驶避障方法、装置、设备及介质
CN112577506B (zh) 一种自动驾驶局部路径规划方法和系统
CN109635672A (zh) 一种无人驾驶的道路特征参数估计方法
Kim et al. Automated complex urban driving based on enhanced environment representation with GPS/map, radar, lidar and vision
CN111967094B (zh) 一种基于Mobileye提供车道线方程的后向车道线推算方法
Qiu et al. Hierarchical control of trajectory planning and trajectory tracking for autonomous parallel parking
CN114942642B (zh) 一种无人驾驶汽车轨迹规划方法
CN113581201A (zh) 一种基于势场模型的无人汽车避撞控制方法及系统
CN111452786B (zh) 一种无人车辆避障方法及系统
CN114815853B (zh) 一种考虑路面障碍特征的路径规划方法和系统
CN116337045A (zh) 一种基于karto和teb的高速建图导航方法
CN112650241B (zh) 智能掘进机横向优化控制方法及系统
CN115230729A (zh) 一种自动驾驶避障方法及系统、存储介质
CN117413233A (zh) 轨道计划装置
CN115454086A (zh) 一种基于模型预测控制算法的车辆主动避撞控制方法
CN116952244A (zh) 一种结合驾驶员认知风险和车辆失稳风险的局部路径规划方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant