CN104360687B - 一种地下铲运机多模式自主行驶控制方法 - Google Patents

Abstract

本发明公开了一种地下铲运机多模式自主行驶控制方法，针对井下巷道环境条件的复杂状况，将地下铲运机目标路径规划、偏差计算及自主行驶控制有机结合起来，克服了地下铲运机目标路径规划难题及行驶轨迹与目标路径偏差求取难题，具有测量系统存储和计算数据量小、控制系统运行速度快、系统稳定性好的优点；另外，通过将地下铲运机偏差计算及自主行驶控制划分为四种计算控制模式，克服了单一模式的局限性，通过在不同的井下巷道环境条件下，运用不同的偏差计算控制模式或将其组合使用，在减少计算量和控制复杂性的同时，较好地适用井下巷道各种环境条件，取得地下铲运机较理想的自主行驶控制效果。

Description

一种地下铲运机多模式自主行驶控制方法

技术领域

本发明涉及矿冶技术领域，尤其涉及一种地下铲运机多模式自主行驶控制方法。

背景技术

随着现有矿产资源逐步全面转入地下开采，采矿条件愈来愈恶劣，对人的安全威胁也愈来愈大，遥控采矿技术应运而生。地下铲运机是地下无轨采矿的关键设备，实现地下铲运机在井下巷道内无人操纵、自主行驶，可使操作人员远离井下恶劣、危险的工作环境，保护铲运机司机人员安全、提高采矿作业效率并降低采矿成本。

地下铲运机自主行驶面临两个难题，一个是目标路径规划难题，另一个是行驶轨迹与目标路径偏差求取难题。为了降低井下巷道开拓成本，井下巷道的形状和尺寸一般允许有一定程度的不规则情况存在，巷道侧壁表面也是凹凸不平，另外巷道侧壁还有可能与地面倾斜，在巷道侧壁还可能安放有风筒或其他设备障碍物等，使得巷道内理想的目标路径并不总是巷道的路面中线，加上地下铲运机铰接车身尺寸结构所固有的行驶轨迹特性，即在巷道转弯处，如果地下铲运机沿着该处的巷道路面中线行驶，则地下铲运机的车身外廓点会更多地靠近外侧巷道侧壁，即车身外廓距离巷道内侧壁的间距大，而外侧的间距小，所以在巷道转弯处路面中线并不是理想的目标路径，此时理想的目标路径应该是适当靠近巷道内侧的曲线，加上地下巷道形状和尺寸的不规则及障碍物需要避让等，因此，地下巷道的目标路径规划是非常困难的。对于常规的控制原理来说，为了实现跟踪控制需要有与目标路径相比较得到的轨迹偏差，井下巷道侧壁现状尺寸不规则，墙壁表面凹凸不平，还存在避车硐室、岔路口等，即使设计出目标路径，利用激光扫描器扫描光束求取地下铲运机轨迹相对于目标路径的偏差也是十分困难，另外，限于地下铲运机的车身结构限制，激光扫描器可能无法安装在理想位置，对偏差计算也会带来不利影响。

发明内容

本发明的目的是提供一种地下铲运机多模式自主行驶控制方法，可以根据地下巷道的环境来选择不同的控制模式，从而有效的进行地下铲运机自主行驶控制。

本发明的目的是通过以下技术方案实现的：

一种地下铲运机多模式自主行驶控制方法，该方法包括：

根据巷道环境测量系统测量到的地下巷道的环境数据及地下巷道环境条件，来确定用于控制所述地下铲运机在地下巷道中自主行驶的计算控制模式，再根据确定的计算控制模式实现地下铲运机在地下巷道中的自主行驶；

其中，根据所述巷道环境测量系统测量到的地下巷道的环境数据及地下巷道环境条件来确定地下巷道的复杂度；

若地下巷道的整体复杂度低于最低预设级别，则使用基于航向角偏差和横向位置偏差的扫描光束三角计算偏差控制模式来实现地下铲运机在地下巷道中的自主行驶；

若地下巷道的整体复杂度低于预设级别，且当前地下巷道的曲率半径大于预设值时，则使用基于扫描数据综合偏差的扫描光束对称计算偏差控制模式来实现地下铲运机在地下巷道中的自主行驶；

若地下巷道的整体复杂度高于最高预设级别，则使用基于环境模型函数的人工示教控制模式来实现地下铲运机在地下巷道中的自主行驶；

若地下巷道局部区域的复杂度高于最高预设级别，则在该局部区域使用基于里程计和陀螺仪的轨迹偏差推算控制模式来实现地下铲运机在地下巷道中的自主行驶。

所述基于航向角偏差和横向位置偏差的扫描光束三角计算偏差控制模式来实现地下铲运机在地下巷道中的自主行驶包括：

地下铲运机上的激光扫描器在180°范围内扫描巷道数据，扫描光束每隔1°进行一次激光测距，获得181个长度测量数据，表示为L₀，L₁，L₂，...，L₁₈₀；

以所述180°范围区域分为左侧扫描区域与右侧扫描区域，并分别选取n条左侧扫描区域的扫描光束与m条右侧扫描区域的扫描光束；其中，20≤m≤70，20≤n≤70；

根据选择的m条右侧扫描区域的扫描光束计算地下铲运机相对于巷道右侧壁的航向角偏差

其中，i表示m条右侧扫描区域的扫描光束的光束序号，i＝0,1,2,...,m；A表示右侧扫描区域扫描光束对的夹角；

根据选择的n条左侧扫描区域的扫描光束计算地下铲运机相对于巷道左侧壁的航向角偏差

其中，i表示n条左侧扫描区域的扫描光束的光束序号，i＝180-n，180-n+1，…，180-B；B表示左侧扫描区域扫描光束对的夹角；

则地下铲运机相对于巷道中线的航向角偏差β由下式计算：

以当前巷道中线为局部区域目标路径，计算每个激光扫描光束相对于该目标路径垂直方向的投影长度以及并计算对投影长度数据的最小值：

则地下铲运机上激光扫描中心相对巷道中线的当前横向位置偏差δ由下式计算：

基于计算出的航向角偏差β与横向位置偏差δ计算融合偏差e：

e＝K₁δ+K₂β_；

其中，K₁与K₂为预先存储的反馈系数，取值均在0-1之间，且K₁+K₂＝1；

将所述融合偏差e作为控制量输入给自主行驶控制器，以融合偏差e趋于零为控制目标，来实现地下铲运机在地下巷道中的自主行驶。

所述基于扫描数据综合偏差的扫描光束对称计算偏差控制模式来实现地下铲运机在地下巷道中的自主行驶包括：

地下铲运机上的激光扫描器在180°范围内扫描巷道数据，扫描光束每隔1°进行一次激光测距，获得181个长度测量数据，表示为L₀，L₁，L₂，...，L₁₈₀；并在每个采样和计算周期，存储该组数据，在下一个采样和计算周期更新该组数据；

在每个采样和计算周期，计算每对左右对称扫描光束测距的差值与左右对称扫描光束测距之和的比值，然后求和，构成综合偏差W，计算公式如下：

其中，i表示左右对称区域中右侧区域参与计算的扫描光束序号，i＝0,1,2,...,m；m表示右侧区域参与计算的扫描光束序号的最大值，m的取值范围为，20≤m≤70；

将计算得到的综合偏差W作为控制量输入给自主行驶控制器，以综合偏差W趋于零为控制目标，来实现地下铲运机在地下巷道中的自主行驶。

所述基于环境模型函数的人工示教控制模式来实现地下铲运机在地下巷道中的自主行驶包括：

预先计算人工示教路径环境模型函数W_m(s)，其步骤如下：选择一目标路径为行驶轨迹S，地下铲运机上的激光扫描器在180°范围内重复扫描巷道数据，扫描光束每隔1°进行一次激光测距，获得181个长度测量数据，表示为L₀，L₁，L₂，...，L₁₈₀，并在每一次扫描后更新前一组数据；

从起点开始至终点所述地下铲运机每行驶一段距离d均提取当前所存储的数据来计算该组数据的综合偏差W_m(s)并存储，综合偏差W_m(s)计算公式如下：

其中，i表示左右对称区域中右侧区域参与计算的扫描光束序号，i＝0,1,2,...,m；m表示右侧区域参与计算的扫描光束序号的最大值，m的取值范围为，20≤m≤70；

从起点开始至终点所计算到的综合偏差W_m(s)构成了地下铲运机沿着该行驶轨迹为曲线坐标轴的人工示教路径环境模型函数W_m(s)；

再基于所述行驶轨迹S自主行驶，从起点开始所述地下铲运机每行驶一段距离d均提取当前所存储的数据来计算该组数据的综合偏差W_g(s)，并计算与对应的综合偏差W_m(s)之间的差值ΔW(s)：

ΔW(s)＝W_g(s)-W_m(s)；

以每一次计算出的出的差值ΔW(s)作为控制量输入给自主行驶控制器，以差值ΔW(s)趋于零为控制目标，来实现地下铲运机在地下巷道中的自主行驶。

所述基于里程计和陀螺仪的轨迹偏差推算控制模式来实现地下铲运机在地下巷道中的自主行驶包括：

在地下铲运机驶入所述局部区域时，利用里程计和陀螺仪的测量数据来推算轨迹偏差，并根据推算出的轨迹偏差作为控制量输入给自主行驶控制器，来实现地下铲运机在地下巷道中的自主行驶。

由上述本发明提供的技术方案可以看出，本发明针对井下巷道环境条件的复杂状况，将地下铲运机目标路径规划、偏差计算及自主行驶控制有机结合起来，克服了地下铲运机目标路径规划难题及行驶轨迹与目标路径偏差求取难题，具有测量系统存储和计算数据量小、控制系统运行速度快、系统稳定性好的优点；另外，通过将地下铲运机偏差计算及自主行驶控制划分为四种计算控制模式，即扫描光束三角计算偏差控制模式、扫描光束对称计算偏差控制模式、人工示教控制模式和轨迹偏差推算控制模式，克服了单一模式的局限性，通过在不同的井下巷道环境条件下，运用不同的偏差计算控制模式或将其组合使用，在减少计算量和控制复杂性的同时，较好地适用井下巷道各种环境条件，取得地下铲运机较理想的自主行驶控制效果。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他附图。

图1为本发明实施例提供的一种地下铲运机多模式自主行驶控制方法的示意图；

图2为本发明实施例提供的地下铲运机激光扫描光束示意图；

图3为本发明实施例提供的地下铲运机激光扫描光束角度偏差计算示意图；

图4为本发明实施例提供的地下铲运机横向位置偏差和航向角偏差计算示意图；

图5为本发明实施例提供的在模式一下地下铲运机行驶轨迹和车身外廓点轨迹曲线图；

图6为本发明实施例提供的在模式二下地下铲运机行驶轨迹和车身外廓点轨迹曲线图；

图7为本发明实施例提供的人工示教目标路径曲线和地下铲运机跟踪轨迹曲线图；

图8为本发明实施例提供的地下铲运机在跟踪直线目标路径及局部大尺寸避障时轨迹曲线图。

具体实施方式

下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明的保护范围。

实施例

地下铲运机工作时需要在狭窄多弯的地下巷道内行驶，由于地下地质条件及矿脉因素地下巷道尺寸不规则，巷道壁表面凹凸不平，个别巷道壁局部还有较大起伏，所以实际的巷道中理想的目标路径往往是很复杂的曲线，使得在图纸上规划目标路径是十分困难的事情，费时费力。

地下铲运机采用前后车体中央铰接结构，通过改变前后车体相对角度来实现转向，即中央铰接转向。地下铲运机行驶轨迹复杂，车身外廓点在转弯时的轨迹更是复杂多变，不易控制，加上地下巷道宽度尺寸狭小，车身外廓与巷道壁的距离仅1米左右，巷道壁表面凹凸不平，个别巷道壁还有较大突出，轨迹偏差计算难度很大，目标路径确定起来难度很大，巷道的宽窄尺寸也变化较大，加上巷道侧壁存放设备等需要避让，加大了行驶难度，即使是有经验的司机，在井下驾驶地下铲运机时刮蹭巷道壁也时有发生。因此结合地下铲运机的铰接结构及运行轨迹特点，找到一种综合了地下铲运机结构特点与尺寸参数因素及巷道弯曲情况和宽窄情况及凹凸巷道壁情况的自动轨迹规划和轨迹偏差计算及行驶控制的方法是十分必要的，使得地下铲运机在地下巷道内实际的自主行驶轨迹是最优或接近最优的，此时铲运机车身外廓距离左右巷道壁接近等距离，达到自动规划目标路径及行驶控制的目的。

本发明提出一种将目标路径规划及偏差计算进行联合分析计算的方法，简化独立目标路径的规划设计和传统意义的轨迹偏差的计算步骤，将地下铲运机目标路径规划和利用激光扫描光束计算轨迹偏差结合在一起来考虑，虽然这样计算的轨迹偏差已经不是完全经典意义上的轨迹偏差了(可理解为是一种合适的控制量)，但却提供了一种新的方法，在分析研究的基础上通过试验调整参与计算的扫描光束数量，达到实际上的较理想的地下铲运机的自主行驶控制效果，以此方法和步骤来克服地下铲运机自主行驶面临的目标路径规划和轨迹偏差求取的双重困难。考虑到地下巷道环境的复杂性，采用单一的计算与控制模式的方法，很难应对地下巷道不同路段、不同环境下存在的各种问题，所以需要根据巷道各种路段环境的不同情况在每一路段采取不同的处理方法，即多种模式的测量与控制方式，地下铲运机自主行驶时在不同模式之间的切换，可由安装在巷道侧壁的信标来触发，这样才能更好地解决地下铲运机沿地下巷道自主行驶问题。

基于激光扫描测量数据的地下铲运机多模式自主行驶控制方法的示意图见图1。根据巷道环境测量系统(包括：激光扫描测量系统、里程测量系统、转角测量系统与惯性测量系统)测量到的地下巷道的环境数据及地下巷道环境条件，来确定用于控制所述地下铲运机在地下巷道中自主行驶的计算控制模式，再根据确定的计算控制模式来控制自主行驶控制系统。其中，根据所述巷道环境测量系统测量到的地下巷道的环境数据及地下巷道环境条件来确定地下巷道的复杂度；确定地下巷道复杂度的方法有很多，本发明不对其进行限定。示例性的，可以先进行扫描获取扫描数据，再根据现场考察获得巷道环境条件信息，将两者数据信息相结合确定地下巷道的复杂度；在确定复杂度时可以考虑多种因素，例如，巷道尺寸、巷道弯曲情况与宽窄情况、障碍物等。另外，本方法不过分依赖于对目标路径的严格公式表达，也不苛求相对于地下铲运机定位参考点相对于目标路径的严格意义的偏差计算，以地下铲运机车身在巷道中的较佳的安全运行效果为目标，通过对激光扫描光束测量数据的适当处理，得到PID自主行驶控制器的较佳控制输入量，PID自主行驶控制器将该输入量进行适当处理后输出控制电压，驱动地下铲运机转向控制系统，使地下铲运机车体能够根据巷道情况自主转向，保持车身外廓与巷道侧壁及障碍物的恰当距离，实现地下铲运机在井下各种巷道环境条件下的自主行驶。

地下铲运机对周围环境的感知主要是由安装在地下铲运机车体上的激光扫描器实现的。在地下铲运机的前后车体上分别安装了前、后180°激光扫描器，朝着车辆前进方向扫描的激光扫描器称为前向激光扫描器，朝着车辆后方扫描的激光扫描器称为后向激光扫描器，当地下铲运机在井下巷道行驶时，由前向激光扫描器扫描数据计算得到铲运机前车体的跟踪轨迹偏差作为铲运机整机的跟踪轨迹偏差，后向激光扫描器扫描数据仅用于防碰撞检测，不参与轨迹偏差计算，地下自主铲运机激光扫描光束示意图参见图2，激光扫描器在180°范围内扫描环境，扫描光束一般每隔1°(也可为0.5°，考虑到井下巷道侧壁凹凸不平，1°扫描间隔已经够用了)测量一个反射光束长度，共获得181个长度测量数据，表示为L₀，L₁，L₂，...，L₁₈₀，选用的激光扫描器量程为80m。其中前向激光扫描测量数据既用于地下自主铲运机行驶轨迹偏差计算，又用于井下巷道障碍检测与避障计算。考虑到井下巷道形状狭长和激光扫描光束扇形分布特点，在行驶轨迹偏差计算时，可以将前向激光扫描器扫描光束测量数据划分成三个区域，前向区域、右侧区域(以扫描范围的0°为起始点)和左侧区域(以扫描范围的180°为终止点)，可根据井下巷道实际情况进行区域大小的设置。前向区域激光测量数据主要用于车辆距巷道转弯处、铲矿点等距离的判断，左侧区域激光测量数据和右侧区域激光测量数据主要用于计算车辆的横向位置偏差和航向角偏差。

针对井下巷道环境条件的复杂情况和地下铲运机的车身结构特点，将地下铲运机目标路径规划及偏差计算结合起来，并划分为四种计算控制模式，即扫描光束三角计算偏差控制模式、扫描光束对称计算偏差控制模式、人工示教控制模式和轨迹偏差推算控制模式。其中模式一适合在巷道环境条件良好时应用(即地下巷道的整体复杂度低于最低预设级别)，模式二适合在巷道环境条件良好(即地下巷道的整体复杂度低于最低预设级别)且巷道曲率半径较大时应用，模式三适合在巷道环境条件较恶劣时应用(即地下巷道的整体复杂度高于最高预设级别)，模式四适合在巷道局部环境条件恶劣时应用(地下巷道局部区域的复杂度高于最高预设级别)。需要强调的是，上述所提到的各个复杂度级别可以由用户预先设定，可由用户根据实际情况或需求来设定，也可由用户根据其经验进行相应的设定。

为了便于理解，下面结合附图2-8对本发明做详细的介绍。

一、模式一(扫描光束三角计算偏差控制模式)。

由于井下巷道狭窄、形状和尺寸不规则，车辆沿单侧巷道侧壁行驶会因距离墙壁过近而增加行车风险，车辆沿巷道中线行驶可以充分利用巷道宽度，可使车辆左右外廓距离巷道侧壁间距更加均匀，提高车辆行驶时的安全性和快速性。

激光扫描器工作时从右到左(逆时针)扫描巷道侧壁，扫描180度后又返回到从右到左扫描巷道侧壁，用右侧区域的扫描光束计算地下铲运机相对于右侧巷道墙壁的偏差，用左侧区域的扫描光束计算地下铲运机相对于左侧巷道墙壁的偏差，用两束成一定夹角的扫描光束测量数据构成的三角形来计算相对于巷道墙壁的偏差。下面以右侧区域的偏差计算为例来说明计算方法和步骤，左侧区域的偏差计算方法与右侧相同。取激光扫描光束对的计算夹角为A，用扫描0°光束所测量到的距离L₀和A角度所在光束所测量到的距离L_A用三角形来计算偏差，计算地下铲运机相对该对光束端点确定的假想线段的角度偏差，保持光束夹角A不变，计算车辆相对下一对光束L₀₊₁和L_A+1端点确定的假想线段的角度偏差，以此类推，得到一系列角度偏差。一般用于计算的光束对夹角可在5°-40°范围选取，在巷道转弯处扫描巷道内侧的光束夹角要取小值，巷道曲率半径越小，光束夹角取值也要越小。在接近直线的巷道，光束对夹角A较佳的选取范围为25°-30°。参与计算的光束测量数据量也随巷道曲直情况要有所变化，参与计算的扫描光束数量m可在20-70范围内选取(对应20°-70°角度范围)。在巷道转弯处，参与计算的巷道内侧的扫描光束数量要减少，巷道曲率半径越小，参与计算的巷道内侧的扫描光束数量也要越小，在接近直线的巷道扫描光束数量取50-60比较好。由于测量数据是随着巷道壁的凹凸不平波动的，这些角度偏差计算值大小也是波动的，用该系列角度偏差的均值来代表车辆相对于巷道右侧壁的航向角偏差可较好克服巷道凹凸不平的影响，达到数值计算滤波的作用。同理可求出地下铲运机相对于巷道左侧壁的航向角偏差将和求均值，得到车辆对巷道中线的航向角偏差β。地下铲运机激光扫描光束角度偏差计算图，见图3。

由图中几何关系，右侧区域地下铲运机相对一对光束L_i和L_A+i端点确定的假想线段的角度偏差可通过下列方程得到:

其中，L_i为序号为i的扫描光束测量数据，L_A+i为序号为A+i的扫描光束测量数据,i＝0,1,2,m,...,180，代表激光扫描光束序号，也代表该光束的角度值，对于不同的巷道条件，m的最佳值可通过试验来优化，m的取值范围为，20≤m≤70，共有m-A+1对数据加入β_r角度偏差的计算；β_ri—(A+i)为车辆相对一对光束L_i和L_A+i端点确定的假想线段的角度偏差；N₁H₁为光束L_i和L_A+i端点的距离；A表示右侧扫描区域扫描光束对的夹角；P表示激光头的光束中心的位置。

解上述方程可得：

同理左侧区域可得：

其中，i表示n条左侧扫描区域的扫描光束的光束序号，i＝180-n，180-n+1，…，180-B；B表示左侧扫描区域参与计算的扫描光束对夹角；

对于接近于直线巷道的路段情况，m与n可以取相同的值，即m＝n，式中i的初始值为180-n，对于转弯巷道情况，m与n可以取不同的值，以提高地下铲运机在转弯巷道的通过性能。

对角度偏差β_ri—(A+i)求均值，得到地下铲运机相对于巷道右侧壁的航向角偏差

同理，可得地下铲运机相对于巷道左侧壁的航向角偏差

地下铲运机横向位置偏差和航向角偏差计算图见图4。综合左右侧巷道壁航向角偏差的计算值，将和求均值，得到车辆相对巷道中线的航向角偏差β：

以巷道中线为当前的局部区域目标路径，计算左右两侧每个激光扫描光束相对于该目标路径垂直方向的投影长度以及并计算对投影长度数据的最小值：

综合左右侧巷道壁横向位置偏差的计算值则地下铲运机上激光扫描中心相对巷道中线的当前横向位置偏差δ由下式计算：

用车辆相对巷道中线的航向角偏差β及横向位置偏差δ来控制车辆，使车辆尽量靠近巷道中线行驶，提高行车安全性和快速性，提高作业效率，并降低碰撞风险。在航向角和位置偏差计算过程中数据得到滤波，得到更具代表性的计算结果。该方法通过直接计算铲运机轨迹偏差数值，不必进行数据与图形之间的转换，减少了数据存储，加快了运算速度，实现基于激光扫描数据的轨迹偏差计算，为导航控制器提供了可靠的输入数据。

本发明实施例中，基于计算出的航向角偏差β与横向位置偏差δ计算融合偏差e：e＝K₁δ+K₂β，用于作为地下铲运机自主行驶控制器的偏差输入。当下一个计算周期时，重复上述计算，得到一组新的航融合偏差e用于自主行驶控制器下一个计算周期时的偏差输入。融合偏差e的式子中，K₁与K₂为预先存储的反馈系数，取值均在0-1之间，且K₁+K₂＝1；将所述融合偏差e作为控制量输入给自主行驶控制器，以融合偏差e趋于零为控制目标，经过PID自主行驶控制器处理后，输出控制电压控制铲运机转向系统动作，实现地下铲运机自主跟踪目标路径。

下面介绍自主行驶控制算法：

1、给定K_p、K_i、K_d数值，给定l₁、l₂数值、给定T数值，给定K₁与K₂数值。

其中，比例系数K_p的取值范围为，1≤K_p≤800，取比例系数K_p＝20。积分系数采样周期T选取范围为，50≤T≤300毫秒，取采样周期T＝0.1秒，积分时间常数T_i的取值范围为，0≤T_i≤50秒，取积分时间常数T_i＝1秒，则积分系数K_i＝0.6。微分系数微分时间常数T_d的取值范围为，0≤T_d≤8，取微分时间常数T_d＝0，则微分系数K_d＝0。l₁为铲运机前方驱动桥的中点到铰接中心线的距离，l₂为铲运机后方驱动桥的中点到铰接中心线的距离，对于给定的2立方地下铲运机，l₁＝1.270m，l₂＝1.270m。K₁与K₂为反馈系数，取值均在0-1之间，且K₁+K₂＝1。针对不同车辆的上述参数最佳值可由实验来确定。

2、当k＝1时，设初值e(k-1)＝e(1-1)＝0_，e(k-2)＝e(1-2)＝0，u(k-1)＝u(1-1)＝0，即起点位置信标1触发，发出铲运机自主行驶开始信号。

3、输入转向角当前值，α(k)＝α(1)，输入转向角前一采样时刻的数值，α(k-1)＝α(1-1)。

4、输入铲运机车速当前值，v_g(k)＝v_g(1)。

5、输入航向角偏差初始值，β(1)＝β，β为前面航向角偏差计算值。

6、输入横向位置偏差初始值，δ(1)＝δ，δ为前面横向位置偏差计算值。

7、输入初始铲运机纵向位置s(1)，可取s(1)＝0。

8、计算融合反馈e(1)：e(1)＝K₁δ(1)+K₂β(1)_。

9、融合反馈e(1)经PID控制器校正后，计算输出转向控制电压u(1)：u(1)＝u(1-1)+Δu(1)

其中，u(1-1)为前一采样周期时刻控制电压，Δu(1)为转向控制电压的增量，且Δu(1)＝K_p[e(1)-e(1-1)]+K_ie(1)+K_d[e(1)-2e(1-1)+e(1-2)]，其中，e(1-1)为e(1)前一采样周期时刻的融合反馈值，e(1-2)为e(1-1)前一采样周期时刻的融合反馈值。

10、为下一时刻作准备

(1)计算地下铲运机行驶里程s(1+1)：s(1+1)＝s(1)+v_g(1)T_。

(2)根据上面地下铲运机行驶里程s(1+1)，输出对应该处的光束夹角及参与计算的激光束数量，为下次计算做准备。

(3)转移存储α(1)→α(1-1)，e(1-1)→e(1-2)，e(1)→e(1-1)，u(1)→u(1-1)。

当2≤k＜z时：

其中，采样次数z是由下一个信标决定的，根据需要，在一些关键地点安放定位信标，车辆行驶到达信标点后控制方式可切换到其他控制模式。

11、输入转向角当前值α(k)，输入转向角前一采样时刻的数值α(k-1)。

12、输入铲运机车速当前值v_g(k)。

13、输入航向角偏差推算值β(k)＝β，β为前面航向角偏差计算值。

14、输入横向位置偏差推算值δ(k)＝δ，δ为前面横向位置偏差计算值。

15、计算融合反馈e(k)：e(k)＝K₁δ(k)+K₂β(k)_。

16、融合反馈e(k)经PID控制器校正后，计算输出转向控制电压u(k)的方法为：u(k)＝u(k-1)+Δu(k)。

其中，u(k-1)为前一采样周期时刻控制电压，Δu(k)为转向控制电压的增量，且Δu(k)＝K_p[e(k)-e(k-1)]+K_ie(k)+K_d[e(k)-2e(k-1)+e(k-2)]，其中，e(k-1)为e(k)前一采样周期时刻的融合反馈值，e(k-2)为e(k-1)前一采样周期时刻的融合反馈值。

重复上述步骤，实现井下巷道目标路径的自动规划和激光测量参数的数值计算滤波，得到自主行驶转向控制指令，为地下铲运机实现自主行驶打下测控方法基础。

该方法适合在巷道环境条件良好时应用，所需存储的数据量小，根据巷道侧壁曲率情况，可分别设定巷道左右侧扫描光束参与计算的数量和光束夹角，所以，地下铲运机跟踪轨迹的控制效果较好。另外，该控制模式允许前向和后向的激光扫描器与车体中线非对称安装布置，激光扫描器的布置灵活性较大。

另外，本发明实施例还利用上述模式一进行了仿真计算，以验证其合理性。

取PID的比例系数K_p＝30，取PID的微分系数K_d＝0，取PID的积分系数K_i＝0.6，横向位置偏差反馈系数K₁＝0.4，取航向角偏差反馈系数K₂＝0.6，取车速v_g＝8km/h，对于给定的2立方地下铲运机，取l₁＝1.270m，l₂＝1.270m。为了降低仿真计算量，左右两侧区域仅取三对光束对进行计算，在地下铲运机行驶里程0—15m范围内时，取右侧参与计算的光束对为0°-30°，10°-40°，20°-50°；左侧参与计算的光束对为130°-160°，140°-170°，150°-180°。在这一段路程时，左右光束对的计算夹角相同，都是30°。在行驶里程15—28m范围内时，取左侧参与计算的光束对为155°-160°，165°-170°，175°-180°，此时光束夹角为5°，取右侧参与计算的光束对为0°-30°，10°-40°，20°-50°。过了行驶里程28m后进入直线段，恢复左侧参与计算的光束对为130°-160°，140°-170°，150°-180°，取右侧参与计算的光束对为0°-30°，10°-40°，20°-50°。针对上述参数设置，运用模式一(扫描光束三角计算偏差控制模式)控制方法进行仿真，地下铲运机行驶轨迹和车身外廓点轨迹曲线图(模式一)见图5。由图5可见地下铲运机在直+弯巷道内行驶时，车身重要外廓点的轨迹基本上是与巷道壁保持比较均匀的间距，提高了行车安全性和快速性。

二、模式二(扫描光束对称计算偏差控制模式)。

地下铲运机上的激光扫描器在180°范围内扫描巷道数据，扫描光束每隔1°进行一次激光测距，获得181个长度测量数据，表示为L₀，L₁，L₂，...，L₁₈₀；并在每个采样和计算周期，存储该组数据，在下一个采样和计算周期更新该组数据。

在每个采样和计算周期，计算每对左右对称扫描光束测距的差值与左右对称扫描光束测距之和的比值，然后求和，构成综合偏差W，计算公式如下：

其中，W为综合偏差，无量纲；i表示左右对称区域中右侧区域参与计算的扫描光束序号，i＝0,1,2,...,m；m表示右侧区域参与计算的扫描光束序号的最大值，也代表该光束的角度数值，m的取值范围为，20≤m≤70；

综合偏差W将巷道弯曲变化情况，车身转向角变化情况、激光扫描器前后安装位置情况及航向角偏差、横向位置偏差情况进行全面综合，包含丰富的信息。在每个采样和计算周期中，不断重复存储当前的激光扫描光束测量值，计算出当前的综合偏差W。

将计算得到的综合偏差W作为控制量输入给PID自主行驶控制器，以综合偏差W趋于零为控制目标，经过PID自主行驶控制器处理后，输出控制电压控制铲运机转向系统动作，在实现地下巷道目标路径自动规划的同时，实现地下铲运机自主行驶控制。

该方法适合在巷道环境条件良好时应用，具有计算过程简单，需要存储的数据量小，计算量小，系统响应快速的优点。另外，该方法还应当将激光扫描器安装在地下铲运机车体左右对称中线上。另外，本发明实施例还利用上述模式二进行了仿真计算，以验证其合理性。

取比例放大系数K_p＝30，取微分系数K_d＝0.3，取PID的积分系数K_i＝0，在地下铲运机行驶全程采用的参与计算的扫描光束角度i分别为0°，5°，10°，15°，20°，25°。取车速v_g＝8km/h。针对上述参数设置，运用模式二(扫描光束对称计算偏差控制模式)进行仿真，地下铲运机行驶轨迹和车身外廓点轨迹曲线图(模式二)见图6。由图6可见地下铲运机在直+弯巷道内行驶时，车身重要外廓点的轨迹基本上是与巷道壁保持比较均匀的间距，提高了行车安全性和快速性。

三、模式三(人工示教控制模式)。

预先计算人工示教路径环境模型函数W_m(s)，其步骤如下：选择一目标路径为行驶轨迹S(可以由有经验的驾驶员，根据其经验选择较佳的目标路径为行驶轨迹)，地下铲运机上的激光扫描器在180°范围内重复扫描巷道数据，扫描光束每隔1°进行一次激光测距，获得181个长度测量数据，表示为L₀，L₁，L₂，...，L₁₈₀，并在每一次扫描后更新前一组数据；

从起点开始至终点所述地下铲运机每行驶一段距离d(0.05m≤d≤0.3m)均提取当前所存储的数据来计算该组数据的综合偏差W_m(s)并存储，综合偏差W_m(s)计算公式如下：

其中，i表示左右对称区域中右侧区域参与计算的扫描光束序号，i＝0,1,2,...,m；m表示右侧区域参与计算的扫描光束序号的最大值，也代表该光束的角度数值，m的取值范围为，20≤m≤70；

这样在人工驾驶铲运机从巷道起始点到终点的行驶过程中，就以地下铲运机的行驶里程S为纵向坐标轴，记录下一系列激光扫描光束测量值计算得到的特定的综合偏差W_m(s)。对于相同的井下巷道环境，如果地下铲运机在井下巷道中的行驶轨迹不同，得到的综合偏差W_m(s)数据会不同；对于相同的地下铲运机行驶轨迹，如果井下巷道环境不同，得到的综合偏差W_m(s)数据也会不同；对于相同的井下巷道环境和相同的目标路径，激光扫描器在车辆上的安装位置不同，在地下铲运机行驶过程中激光扫描器也会扫描测量到不同的距离数据，计算出的综合偏差W_m(s)数据也会不同；另外，地下铲运机的结构尺寸参数及转向性能也会反映在激光扫描数据中。综合偏差W_m(s)数据中既包含巷道环境(巷道形状尺寸及巷道侧壁表面凹凸不平)信息，目标路径(路径曲率变化情况)信息，也包含了激光扫描器的安装位置信息及地下铲运机的转向结构信息等，由此得到了针对单个地下铲运机和某一井下巷道环境所特有的，从起点开始至终点所计算到的综合偏差W_m(s)所构成的沿着该目标路径(行驶轨迹S)为曲线坐标轴的人工示教路径环境模型函数W_m(s)。

上述综合偏差W_m(s)中的s可以理解为具体的值，环境模型函数W_m(s)中的s是一个变量，可以是不同的值(即包含综合偏差W_m(s)中所有s的值)。

再基于所述行驶轨迹S自主行驶，从起点开始所述地下铲运机每行驶一段距离d均提取当前所存储的扫描数据来计算该组数据的综合偏差W_g(s)(与综合偏差W_m(s)计算方法一样)，并计算与对应的综合偏差W_m(s)之间的差值ΔW(s)：

ΔW(s)＝W_g(s)-W_m(s)；

以每一次计算出的差值ΔW(s)作为控制量输入给自主行驶控制器，以差值ΔW(s)趋于零为控制目标，经过PID自主行驶控制器处理后，输出控制电压控制铲运机转向系统动作，实现地下铲运机自主跟踪人工示教路径的目的。

该方法能够实现在复杂巷道条件下的目标路径人工智能规划，在此情况下具有目标路径规划简单方便的优点。与现有技术相比不必存储地下巷道侧壁的扫描数据，所需存储的数据量和计算量大为减少。该模式与模式一、模式二相比，需要沿着地下铲运机行驶里程不断存储环境数据W_m(s)，需要存储的数据量较大。另外，目标路径跟踪精度还要受到里程计测量精度及轮胎打滑的影响，对里程计精度要求较高，当轮胎打滑严重时，这时里程计测得的里程与实际里程会有较大偏差，控制效果变差，为了恢复控制精度，需要在巷道侧壁每隔一段距离安放信标，当地下铲运机行驶到信标处时，用信标处对应的里程数据更新车辆存储器中的里程数据，从而计算和控制精度得到恢复。

另外，本发明实施例还利用上述模式三进行了仿真计算，以验证其合理性。

取比例放大系数K_p＝30，取微分系数K_d＝0.3，取PID的积分系数K_i＝0，在人工示教过程中，在地下铲运机在行驶全程采用的参与计算的扫描光束角度i分别为0°，10°，20°，30°，40°，取车速v_g＝8km/h。针对上述参数设置，运用模式三(人工示教控制模式)进行仿真，人工示教目标路径曲线和地下铲运机跟踪轨迹曲线图见图7。由图7可见地下铲运机在直+弯巷道内行驶时，地下铲运机并没有向巷道空间较大的区域行驶，其行驶轨迹(虚线曲线)几乎与人工示教目标路径曲线(实线曲线)重合，实现了由人工示教目标路径引导的地下铲运机的自主行驶。

四、模式四(轨迹偏差推算控制模式)。

当井下巷道局部区域的环境条件恶劣时，上述三种模式的控制效果可能都不理想，在地下铲运机驶入该局部区域时，利用里程计和陀螺仪的测量数据并基于常规的算法来推算轨迹偏差，在较短的时间内，轨迹偏差推算方法可有较高的计算精度，当地下铲运机驶过该困难路段后，再转回采用上述三种偏差计算控制模式。该方法仅可短时应用，时间加长后累积误差将显著加大。

另外，本发明实施例还利用上述模式四进行了仿真计算，以验证其合理性。

设地下自主铲运机沿着直巷道行驶，铲运机从原点(0,0)开始向右运行，路径中包含一处避障路段，该处避障距离横向偏移距离-3m，避障信号作用距离20m，起点为13m处，终止点为33m处，取车速v_g＝4km/h,并且设地下铲运机存在初始的横向位置偏差和航向角偏差，取铲运机初始具有横向偏差δ₀＝1m，取航向角初始偏差分别为β₀＝-30°，β₀＝-15°，β₀＝-0°，β₀＝+15°，β₀＝+30°，取初始车辆转向角α₀＝0°，地下铲运机在跟踪直线目标路径及局部大尺寸避障时轨迹曲线图见图8，由图8中轨迹曲线可见铲运机跟踪与避障控制情况良好，自主行驶控制保持良好的稳定性。

五、组合模式。

针对井下巷道的不同路段的不同环境条件，可将上述模式适当组合使用，每种路段采用选用一种最合适的模式，不同的路段选用不同的模式，在各路段和各模式之间的切换由安装在巷道侧壁的信标来触发，实现各控制模式之间的有序结合，在减少计算量和控制复杂性的同时，取得地下铲运机较理想的自主行驶控制效果。

本发明与现有技术相比具有如下优点和效果：

(1)本发明针对井下巷道环境条件的复杂状况和地下铲运机的车身结构特点，将地下铲运机目标路径规划、偏差计算及自主行驶控制有机结合起来，克服了地下铲运机目标路径规划难题及行驶轨迹与目标路径偏差求取难题，具有测量系统存储和计算数据量小、控制系统运行速度快、系统稳定性好的优点。

(2)通过将地下铲运机偏差计算及自主行驶控制划分为四种计算控制模式，即扫描光束三角计算偏差控制模式、扫描光束对称计算偏差控制模式、人工示教控制模式和轨迹偏差推算控制模式，克服了单一模式的局限性，通过在不同的井下巷道环境条件下，运用不同的偏差计算控制模式或将其组合使用，在减少计算量和控制复杂性的同时，较好地适用井下巷道各种环境条件，取得地下铲运机较理想的自主行驶控制效果。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例可以通过软件实现，也可以借助软件加必要的通用硬件平台的方式来实现。基于这样的理解，上述实施例的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM，U盘，移动硬盘等)中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (4)

1.一种地下铲运机多模式自主行驶控制方法，其特征在于，该方法包括：

根据巷道环境测量系统测量到的地下巷道的环境数据及地下巷道环境条件，来确定用于控制所述地下铲运机在地下巷道中自主行驶的计算控制模式，再根据确定的计算控制模式实现地下铲运机在地下巷道中的自主行驶；

其中，根据所述巷道环境测量系统测量到的地下巷道的环境数据及地下巷道环境条件来确定地下巷道的复杂度；

若地下巷道的整体复杂度低于最低预设级别，则使用基于航向角偏差和横向位置偏差的扫描光束三角计算偏差控制模式来实现地下铲运机在地下巷道中的自主行驶；

若地下巷道的整体复杂度低于最低预设级别，且当前地下巷道的曲率半径大于预设值时，则使用基于扫描数据综合偏差的扫描光束对称计算偏差控制模式来实现地下铲运机在地下巷道中的自主行驶；

若地下巷道的整体复杂度高于最高预设级别，则使用基于环境模型函数的人工示教控制模式来实现地下铲运机在地下巷道中的自主行驶；

若地下巷道局部区域的复杂度高于最高预设级别，则在该局部区域使用基于里程计和陀螺仪的轨迹偏差推算控制模式来实现地下铲运机在地下巷道中的自主行驶；

其中，所述基于扫描数据综合偏差的扫描光束对称计算偏差控制模式来实现地下铲运机在地下巷道中的自主行驶包括：

地下铲运机上的激光扫描器在180°范围内扫描巷道数据，扫描光束每隔1°进行一次激光测距，获得181个长度测量数据，表示为L₀，L₁，L₂，...，L₁₈₀；并在每个采样和计算周期，存储该组数据，在下一个采样和计算周期更新该组数据；

在每个采样和计算周期，计算每对左右对称扫描光束测距的差值与左右对称扫描光束测距之和的比值，然后求和，构成综合偏差W，计算公式如下：

$W=\underset{i=0}{\overset{i=m}{\Σ}}\frac{L_{180-i}-L_i}{L_{180-i}+L_i};$

其中，i表示左右对称区域中右侧区域参与计算的扫描光束序号，i＝0,1,2,...,m；m表示右侧区域参与计算的扫描光束序号的最大值，m的取值范围为，20≤m≤70；

将计算得到的综合偏差W作为控制量输入给自主行驶控制器，以综合偏差W趋于零为控制目标，来实现地下铲运机在地下巷道中的自主行驶。

2.根据权利要求1所述的方法，其特征在于，所述基于航向角偏差和横向位置偏差的扫描光束三角计算偏差控制模式来实现地下铲运机在地下巷道中的自主行驶包括：

地下铲运机上的激光扫描器在180°范围内扫描巷道数据，扫描光束每隔1°进行一次激光测距，获得181个长度测量数据，表示为L₀，L₁，L₂，...，L₁₈₀；

以所述180°范围区域分为左侧扫描区域与右侧扫描区域，并分别选取n条左侧扫描区域的扫描光束与m条右侧扫描区域的扫描光束；其中，20≤m≤70，20≤n≤70；

根据选择的m条右侧扫描区域的扫描光束计算地下铲运机相对于巷道右侧壁的航向角偏差

${\stackrel{\‾}{\mathrm{\β}}}_r=\frac{1}{m-A+1}\underset{i=0}{\overset{(m-A)}{\Σ}}(arccos\frac{L_i-L_{A+i}\cos A}{\sqrt{{L_i}^2+{L_{A+i}}^2-2L_i{L}_{A+i}\cos A}}-\frac{\mathrm{\π}}{2}-\frac{\mathrm{\π}}{180}i);$

其中，i表示m条右侧扫描区域的扫描光束的光束序号，i＝0,1,2,...,m；A表示右侧扫描区域扫描光束对的夹角；

根据选择的n条左侧扫描区域的扫描光束计算地下铲运机相对于巷道左侧壁的航向角偏差

${\stackrel{\‾}{\mathrm{\β}}}_l=\frac{1}{n-B+1}\underset{i=180-n}{\overset{180-B}{\Σ}}\left(\arccos \frac{L_{B+i}-L_i\cos B}{\sqrt{{L_i}^2+{L_{B+i}}^2-2L_i{L}_{B+i}\cos B}}-\frac{\mathrm{\π}}{2}-\frac{\mathrm{\π}}{180}\[180-\left(B+i\right)\]\right)$

其中，i表示n条左侧扫描区域的扫描光束的光束序号，i＝180-n，180-n+1，…，180-B；B表示左侧扫描区域扫描光束对的夹角；

则地下铲运机相对于巷道中线的航向角偏差β由下式计算：

$\mathrm{\β}=\frac{1}{2}({\stackrel{\‾}{\mathrm{\β}}}_r+{\stackrel{\‾}{\mathrm{\β}}}_l);$

以当前巷道中线为局部区域目标路径，计算每个激光扫描光束相对于该目标路径垂直方向的投影长度以及并计算对投影长度数据的最小值：

$\left\{\begin{array}{c}{d}_{r\min }={\left\{L_i\cos \[\frac{\mathrm{\π}}{180}i-\mathrm{\β}\]\right\}}_{\min },i=0,1,\mathrm{...},m\\ d_{l\min }={\left\{L_i\cos \[\frac{\mathrm{\π}}{180}\left(180-i\right)+\mathrm{\β}\]\right\}}_{\min },i=180-n,180-n+1,\mathrm{...},180\end{array}\right.;$

则地下铲运机上激光扫描中心相对巷道中线的当前横向位置偏差δ由下式计算：

$\mathrm{\δ}=\frac{1}{2}\[d_{lmin}-d_{rmin}\];$

基于计算出的航向角偏差β与横向位置偏差δ计算融合偏差e：

e＝K₁δ+K₂β；

其中，K₁与K₂为预先存储的反馈系数，取值均在0-1之间，且K₁+K₂＝1；

将所述融合偏差e作为控制量输入给自主行驶控制器，以融合偏差e趋于零为控制目标，来实现地下铲运机在地下巷道中的自主行驶。

3.根据权利要求1所述的方法，其特征在于，所述基于环境模型函数的人工示教控制模式来实现地下铲运机在地下巷道中的自主行驶包括：

预先计算人工示教路径环境模型函数W_m(s)'，其步骤如下：选择一目标路径为行驶轨迹S，地下铲运机上的激光扫描器在180°范围内重复扫描巷道数据，扫描光束每隔1°进行一次激光测距，获得181个长度测量数据，表示为L₀，L₁，L₂，...，L₁₈₀，并在每一次扫描后更新前一组数据；

从起点开始至终点所述地下铲运机每行驶一段距离d均提取当前所存储的数据来计算该组数据的综合偏差W_m(s)并存储，综合偏差W_m(s)计算公式如下：

$W_m\left(s\right)=\underset{i=0}{\overset{i=m}{\Σ}}\frac{L_{180-i}-L_i}{L_{180-i}+L_i};$

其中，i表示左右对称区域中右侧区域参与计算的扫描光束序号，i＝0,1,2,...,m；m表示右侧区域参与计算的扫描光束序号的最大值，m的取值范围为，20≤m≤70；

从起点开始至终点所计算到的综合偏差W_m(s)构成了地下铲运机沿着该行驶轨迹为曲线坐标轴的人工示教路径环境模型函数W_m(s)'；

再基于所述行驶轨迹S自主行驶，从起点开始所述地下铲运机每行驶一段距离d均提取当前所存储的数据来计算该组数据的综合偏差W_g(s)，并计算与对应的综合偏差W_m(s)之间的差值ΔW(s)：

ΔW(s)＝W_g(s)-W_m(s)；

以每一次计算出的出的差值ΔW(s)作为控制量输入给自主行驶控制器，以差值ΔW(s)趋于零为控制目标，来实现地下铲运机在地下巷道中的自主行驶。

4.根据权利要求1所述的方法，其特征在于，所述基于里程计和陀螺仪的轨迹偏差推算控制模式来实现地下铲运机在地下巷道中的自主行驶包括：

在地下铲运机驶入所述局部区域时，利用里程计和陀螺仪的测量数据来推算轨迹偏差，并根据推算出的轨迹偏差作为控制量输入给自主行驶控制器，来实现地下铲运机在地下巷道中的自主行驶。