一种矿用电机车自主避障方法和系统

1.本发明涉及避障技术领域，特别涉及一种基于动态障碍物提取的矿用电机车自主避障方法和系统。


背景技术：

2.电机车是煤矿辅助运输的主要方式，承担着人员、设备和材料等运输的重要任务。目前完全依赖司机控制车辆，而司机疲劳等人为因素易导致非主观超速、闯红灯、车辆追尾等安全问题。
3.现有的煤矿电机车在使用中主要采用人工避障与语音预警避障，多应用于室外环境或环境良好室内场所。在限定空间避障方面，主要通过人为干涉、语音预警和人工调度室通知等方式；在人工避障中存在大量的司机操作不当导致的车祸；在语音预警避障等方式中由于无法实现动态避障，导致机车在遇到障碍物时需要停车等待，这也大幅度的降低了工作效率。


技术实现要素：

4.基于此，本发明有必要针对上述技术问题，提供一种矿用电机车自主避障方法和系统。
5.本发明实施例提供一种矿用电机车自主避障方法，包括：
6.获取矿井下巷道的点云图，并根据点云图构建矿井下巷道的动态地图；
7.实时获取矿井下电机车周围的激光雷达数据，并从激光雷达数据中提取动态障碍物位置信息；
8.根据障碍物位置信息、障碍物相对电机车速度差和动态地图，生成动态避障速度控制地图；
9.将障碍物位置转换到避障速度控制地图上，当障碍物的位置处于相应的速度限制区域时，发送控制速度指令；
10.根据控制速度指令，实时控制电机车减速避障或刹车避障。
11.进一步地，所述动态地图的构建，包括：
12.激光雷达扫描井下巷道，将三维空间物体分布转换成点云图；
13.对点云图中的环境特征进行提取；所述环境特征包括：巷道墙壁特征、轨道特征、特殊标识物；
14.在对点云图中的环境特征进行提取的同时，结合电机车速度、rfid标识卡在不同位置进行对比匹配，依次构建矿井下巷道的动态地图。
15.进一步地，所述动态障碍物位置信息的提取，包括：
16.对激光雷达数据进行分组分离，获得激光点的集合；
17.对激光点的集合进行线段模型参数的辨识得到带参数的线段集，并对带参数的线段集进行融合得到各个线段障碍物的集合；
18.根据各个线段障碍物的集合，确定动态障碍物位置信息。
19.进一步地，所述对激光雷达数据进行分组分离，获得激光点的集合，具体包括：
20.通过对满足条件的点集进行分离，激光点gi测量应满足以下条件：
21.g
i,i-1
《d
group
r
idp
22.上式中，g
i,i-1
为激光点gi与g
i-1
之间的距离，ri为激光点g
i-1
的测量值，d
group
为用户定义的属于同一组激光之间的距离，d
p
为距离的容量；
23.满足条件的两组激光数据则分为同一组，否则gi属于新的一个分组，得出激光点的集合；
24.确定集合中两端点的连线位置，采用迭代法找出集合中最远的点；最远点gj距离连线为dj；
25.如果dj满足dj》ds r
jdp
，将点集分为两个，最远点dj为分界。
26.进一步地，所述对激光点的集合进行线段模型参数的辨识得到带参数的线段集，并对带参数的线段集进行融合得到各个线段障碍物的集合，具体包括：
27.参数辨识：点集使用amx bmy cmz＝0模型进行表示，am,bm与cm通过最小二乘法求得，(x,y,z)为点的坐标，参数辨识结束后，得到带参数的线段集；
28.线段融合：判断两个线段是否相近，判断条件d0《dm，其中d0代表两个线段端点的距离，dm代表定义属于同一物体的距离阙值，将线段集中线段进行不断迭代计算，融合符合条件的线段；
29.在不同点集之间同样进行参数辨识和线段融合步骤，将两个点集合并成一个点集。
30.进一步地，所述动态避障速度控制地图的生成，包括：
31.将第n帧数据与第n 1帧的数据进行匹配，对每个扫描周期的位置进行校正，实现对障碍物的相对电机车速度进行更新；
32.规定电机车运行方向为正，若障碍物与电机车运行方向相反则速度为负值，速度差计算：
[0033]v差
＝v
车-v
障碍物
[0034]
根据障碍物相对机车速度差，根据动态地图构建方法生成以障碍物相对电机车位置信息为基础的动态避障地图。
[0035]
进一步地，所述速度限制区域，包括：
[0036]
障碍物的位置距离电机车0～10m的区域为紧急停车区域；
[0037]
障碍物的位置距离电机车10～20m的区域为制动区域；
[0038]
障碍物的位置距离电机车20～40m的区域为减速区域；
[0039]
障碍物的位置距离电机车40～60m的区域为预警区域。
[0040]
进一步地，所述根据控制速度指令，实时控制电机车减速避障或刹车避障，具体包括：
[0041]
当障碍物的距离在紧急停车区域外，通过改变电机车的速度，减小障碍物相对电机车的速度，实现自动减速避障；
[0042]
当障碍物的距离在紧急停车区域时，通过下发指令，电机车速度减小到0，刹车避障。
[0043]
一种矿用电机车自主避障系统，包括：
[0044]
激光雷达，用于获取矿井下巷道的激光雷达数据和矿井下电机车周围的激光雷达数据；
[0045]
车载控制器，用于根据矿井下巷道的激光雷达数据获取矿井下巷道的点云图，并根据点云图构建矿井下巷道的动态地图；从矿井下电机车周围的激光雷达数据中提取障碍物位置信息；根据障碍物位置信息、障碍物相对电机车速度差和动态地图，生成动态避障速度控制地图；
[0046]
整车控制器，用于将障碍物位置转换到避障速度控制地图上，当障碍物的位置处于相应的速度限制区域时，发送控制速度指令；
[0047]
电机控制器，用于根据控制速度指令，实时控制电机车实现减速避障或刹车避障。
[0048]
本发明实施例提供的上述矿用电机车自主避障方法和系统，与现有技术相比，其有益效果如下：
[0049]
本发明采用激光雷达构建动态地图，加入动态障碍物信息的获取以及避障控制地图生成，对原有的车载速度控制器免于改动，有效的利用了之前系统的其他部分；通过检测障碍物的位置以及运动状态，预测障碍物的位置，将障碍物的位置更新到新生成的避障控制地图中，有效的对机车速度进行控制，实现有轨机车的动态避障；根据激光数据，在得知机车速度的前提下，对计算所得的速度差可有效的区分静态与动态障碍物，以采用不同的避障方式，实现机车安全避障。总之，电机车的自主避障技术为实现煤矿井下电机车辅助驾驶及自主驾驶提供有力的技术支持，对建设智慧矿山、煤矿减人增效、提高辅助运输安全性能有重要意义。
附图说明
[0050]
图1为一个实施例中提供的矿用电机车自主避障系统框图；
[0051]
图2为一个实施例中提供的电机车整体结构示意图；
[0052]
图3为一个实施例中提供的动态地图构建流程示意图
[0053]
图4为一个实施例中提供的障碍物位置信息提取流程示意图；
[0054]
图5为一个实施案例中动态避障地图模型图；
[0055]
图6为一个实施例中提供的动态避障速度控制地图建立流程示意图；
[0056]
图7为一个实施例中提供的矿用电机车自主避障方法流程示意图。
[0057]
附图标记：
[0058]
1、电机车；2、控制箱；3、侧板；4、推板；5、套筒；6、铲板；7、车轮；8、安装杆；9、装载箱。
具体实施方式
[0059]
为了使本技术的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本技术进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本技术，并不用于限定本技术。
[0060]
一个实施例中，本发明提供的一种矿用电机车自主避障方法，具体包括：
[0061]
1、煤矿井下动态地图构建
[0062]
机车装载前后两个雷达，各位于机车的前后车头位置；激光雷达的作用主要是以10hz以上的频率扫描周边环境，并将数据传递给车载控制器，车载控制器根据点云分布图进行动态地图构建，使用前后两组激光雷达主要是机车存在双向驾驶且雷达的视角具有局限性；故当机车前进时，前雷达进行工作，反之则后雷达工作。步骤如下：
[0063]
①
激光雷达扫描井下巷道，将三维空间物体分布转换成点云图；
[0064]
在车载自动驾驶控制器内，模型基于ros系统，通过pcl点云库函数采集激光传感器的点云实时数据，点云数据自带时间戳属性，系统通过该属性确定连续多帧点云数据的前后顺序；激光传感器扫描的频率是10hz，100ms可成像一幅点云图像，连续多帧点云图像的叠加就是根据点云数据的前后顺序，将多张点云图像叠加起来形成一张更密集的点云图像；激光传感器内置imu传感器，imu提供的是一个相对的定位信息，它的作用是测量相对于起点物体所运动的路线，主要提供的参数有x、y、z、roll、pitch、yaw，依据这些参数可以模拟出由点云组成的三维立体空间。
[0065]
通过can总线等通信方式，激光传感器获取电机车实时行驶速度，根据某一刻电机车行驶速度可以确定点云图像前一帧与后一帧的相对位置关系，调整后一帧点云图像并将其叠加到前一帧的点云图像上，以此类推可叠加多帧连续的点云图像，形成一幅相对稳定、点云稠密的图像。
[0066]
叠加点云数据后，模型更真实还原了巷道的点云图，当电机车行驶过程中出现频繁的颠簸、抖动时，建立一个虚拟三维空间，将该三维空间z＝0的平面确定为实际地面；采用最小二乘法对单帧巷道点云图像的某一竖直平面的点云数据进行直线拟合，求出回归直线相对于虚拟平面的斜率和竖直方向的截距，根据斜率和截距调整该单帧的所有点云数据，使其与虚拟平面重合，然后将调整后的这一帧数据进行叠加，最终得到一个稳定、清晰的巷道模型。
[0067]
首先设置回归直线方程：
[0068]
z＝kx b
[0069]
其中，斜率为k，y轴截距为b；由于地面直线不可能垂直于x轴，因此可以使用截距式直线方程来作为回归直线方程；单帧点云图像的某一竖直平面的点集为：
[0070]
{(x,z)|(x1,z1),(x2,z2),...(xn,zn)}
[0071]
求所有点到直线的误差平方和：
[0072][0073]
由极值定理可知，误差方程一阶导数等于0处取得极值，因此分别对其关于k和b求导，解k,b值使得误差函数取最小值；可得：
[0074][0075]
整理后可得：
[0076][0077]
其中求出k，b的值后带入回归方程，即可得到拟合的直线方程；根据斜率k，调整这一帧点云图像的角度，使其与虚拟平面水平；根据截距b，平移点云图像，使其与虚拟平面在同一水平；这样得到的每一帧点云图像都是在同一水平，达到最大程度的重合，得到一副连续多帧叠加、稳定、清晰的巷道点云图。
[0078]
②
环境特征提取；
[0079]
车载控制器对点云图进行处理，进行环境特征提取，比如巷道墙壁特征，轨道特征、特殊标识物等。
[0080]
③
特征匹配对齐。
[0081]
电机车速度已知，通过can通信实时发送给车载控制器。激光雷达产生的点云图包含物体坐标(x,y,z,roll,pitch,yaw)，分别为三维坐标x，y，z，x旋转角度，y旋转角度，z旋转角度。
[0082]
不同于地面建图方式，煤矿井下存在特征不明显的区间，电机车移动时，在雷达扫描范围内进行特征提取的同时，需结合电机车速度、rfid标识卡在不同位置进行对比匹配，具体地，在机车运行过程中，将提取的环境特征和建立的地图模型进行匹配，即机车在运行过程中雷达会实时扫描周围环境，进而会形成一个周围环境点云图，在
①
中所建好的地图是整个巷道全程点云图，匹配就是将此时的点云图与整个巷道点云图每一段进行对比相似度，根据一些特征点确定机车所在整个巷道的位置信息；所述无源标识卡rfid校正为：在巷道特征不明显、巷道关键位置，如：巷道转弯处，限速区段，禁行区段等采用无源标识卡rfid进行定位校正；无源标识卡rfid安装在井下巷道轨道中间，矿用本安型读卡器放于电机车底部用于扫描rfid无源标识卡；每个无源标识卡rfid拥有唯一地址信息id，结合已创建地图在巷道关键点处放置rfid卡；电机车运行过程中通过矿用本安型读卡器扫描rfid卡，通过tcp/ip协议将rfid的内置地址编码发送给无人驾驶控制器，控制器反向解析得到电机车的位置，以此对激光雷达定位进行修正并对巷道关键点进行雷达定位和rfid定位，提高自主定位系统的可靠性；依次建立煤矿井下巷道地图模型。图3为建图流程。
[0083]
2、获取激光雷达数据并从激光数据中提取障碍物位置信息，如图4为示意图：
[0084]
①
通过分析每个激光点与下一个激光点之间的距离变化，对一帧激光数据的激光进行分组得出点集；
[0085]
②
点集分离。
[0086]
a.通过对满足条件的点集进行分离，激光点gi测量应满足以下条件：
[0087]gi,i-1
《d
group
r
idp
[0088]
上式中，g
i,i-1
为激光点gi与g
i-1
之间的距离，ri为激光点g
i-1
的测量值，d
group
为用户定义的属于同一组激光之间的距离，d
p
为距离的容量，满足公式的gi与gi-1激光数据属于同一组合中，否则gi属于新的一个组合。
[0089]
b.通过第一步将激光点数据进行分组，其中符合同一条件的激光组成一个点集，故全部激光点会出现n个点集。
[0090]
c.在每一个点集中，确定集合中两端点的连线位置，采用迭代法找出集合中最远的点；最远点gj距离连线为dj。
[0091]
d.在一个点集中，如果dj满足dj》ds r
vdp
，将点集分为两个，最远点gj作为分界，其中ds指用户定义的中值曲率下属于同一组激光的距离，rv代表点集中激光点gv的测量值。
[0092]
需要说明的是，步骤a～d，先将激光根据条件进行分组，符合条件的激光数据会被放在一个点集中，点集的个数无限制；是对属于同一个点集的激光数据进行区分，主要是处理一下部分点集的数据曲率过大且数据量大的问题，会把同一个点集合的数据根据条件分为两组。
[0093]
③
再对分离后的点集进行线段模型参数的辨识得到带参数的线段集，最后对带参数的线段集进行融合得到各个线段障碍物的集合。
[0094]
a.对分离后的集合进行参数辨识；点集使用amx bmy cmz＝0模型进行表示；am,bm与cm通过最小二乘法求得,(x,y,z)为点的坐标；
[0095]
b.参数辨识结束后，得到带参数的线段集；
[0096]
c.可能不同的线段代表着同一物体的不同部分，对线段进行融合；判断两个线段是否相近，判断条件d0《dm,其中d0代表两个线段端点的距离，dm代表定义属于同一物体的距离阙值。将线段集中线段进行不断迭代计算，融合符合条件的线段；
[0097]
d.在不同点集之间使用同样方法，将两个点集合并成一个点集。
[0098]
④
得出线段集合中的障碍物模型。
[0099]
3、建立避障控制地图的具体步骤如下：
[0100]
①
通过匹配的方法，将第n帧数据与第n 1帧的数据进行匹配，对每个扫描周期的位置进行校正，实现对障碍物的相对电机车速度进行更新；通过匹配的方法，机车在运动过程中雷达会依其扫描周期对环境进行扫描，并根据雷达激光点的数据进行分析；通过前面建立的地图，当雷达扫描的环境与地图某个路段相似度极高且位置信息相同时即可让机车知道目前所处的位置；在知道位置后，雷达扫描的动态移动物会被加载到建好的地图上，此时刻的数据被记为第n帧数据，在下一个扫描周期以同样的逻辑判断物体所在地图的位置其数据记为第n 1帧，对上一帧时物体所在的位置进行更新，即可知道物体的位移量，再根据两次扫描的时间间隔即可算出障碍物的移动速度。
[0101]
规定电机车运行方向为正，若障碍物与电机车运行方向相反则速度为负值，速度差计算：
[0102]v差
＝v
车-v
障碍物
[0103]
②
根据障碍物相对机车速度差，根据步骤1所述动态地图构建方法生成以障碍物相对电机车位置信息为基础的动态避障地图。动态避障地图主要是根据速度差所带来的相对位置信息进行生成的。当上式中v
差
小于0，即代表障碍物与机车同向运行且速度大于机车故无需避障；当v
差
大于0有两种情况，一种时同向运行且机车速度大于障碍物、另一种则代表相向而行，这里采用位置信息进行生成地图。这里建图的方法步骤1中已经详述。概述一下与步骤1的区别：由于静态下雷达扫描的范围在60-80米左右，根据雷达的扫描构建动态70米动态避障地图，机车在运行过程中实时地图呈滚动式输出，就以车头作为始端、终点为
前方70米的位置。在该段地图中分为预警区域、减速区域、制动区域以及紧急停车区域(这些位置相对地图不变，但避障地图实时更新即也随之变化)。在行驶过程中，当出现障碍物时，该地图就会被启动且实时更新。如图5、图6所示。
[0104]
其中当出现多个障碍物时，地图上会根据距离最近的障碍物的位置信息进行处理。
[0105]
4、得出v
障碍物
以及相对速度差v
差
，根据速度与距离关系即可算出障碍物相对机车的距离，再将计算出的距离信息加载到动态避障地图上，通过在系统上设置相应的避障区域，在不同的避障区域设置相应的减速控制。如图7所示。
[0106]
5、根据障碍物与机车的相对距离实现灵活的避障，当v
差
较大时但距离较远时，实现减速避障；当距离较近时，实现刹车避障。根据障碍物的速度与距离，在紧急避障区域外，实行减速避障；通过改变电机车的速度，进而减小障碍物相对电机车的速度，实现自动减速避障，当障碍物的距离在紧急避障区时，通过下发指令，电机车速度减小到0，刹车避障。
[0107]
综上所述，本发明实施例通过煤矿井下动态地图构建，激光雷达以10hz以上的频率扫描周边环境，并将数据传递给车载控制器，车载控制器根据点云分布图进行动态地图构建；通过雷达对环境扫描，提取动态障碍物运行速度与位置；根据点云分布设置合理的避障区域融合矿下精准定位技术，生成避障控制地图。即本发明实施例采用激光雷达，结合动态地图与动态障碍物提取技术，提高系统适应性，稳定性及可靠性。
[0108]
还有，由于电机车的刹车系统也是引起事故的主要原因之一，在电机车行驶过程中，车辆无法对轨道上的杂物进行清理，使得轨道上堆积的煤炭杂物会影响车轮和刹车部件的正常使用，严重的情况下会导致车辆脱轨。
[0109]
基于上述问题，本发明实施例提供了以下技术方案：
[0110]
电机车包括刹车机构，刹车机构包含有车轮和刹车盘，电机车的底部两侧均转动安装有若干个车轮，刹车盘通过固定轴与电机车的车轴连接，车轮安装在刹车盘远离车轴的一端。
[0111]
电机车的一端底部安装有铲板，且电机车的端部在铲板的上方安装有两个可水平移动的推板，推板的一端连接有横杆，电机车的内部下方设置有安装槽，横杆的一端延伸至安装槽的内部且通过若干根拉簧与安装槽的内端连接，安装槽的两侧内壁均沿长度方向设置有滑轨，横杆的两侧均沿长度方向设置有与滑轨相适配的滑槽，滑轨滑动安装在滑槽的内部，横杆远离推板的一端底部两侧均连接有竖杆，竖杆远离推板的一侧底部转动安装有滚轮，且安装槽内部靠近推板的一端与竖杆之间安装有第一气囊，第一气囊的底部连接有多根风管；电机车的底部两侧均转动安装有粉碎杆，粉碎杆的顶部延伸至安装槽的内部连接有齿轮，且横杆的外壁连接有与齿轮相啮合的齿条。
[0112]
靠近推板一端的固定轴外部沿圆周方向连接有若干个支撑杆，支撑杆远离固定轴的一端连接有端部锤，端部锤可与滚轮接触。
[0113]
一个实施例中，提供的一种矿用电机车自主避障系统，该系统包括：
[0114]
激光雷达，用于获取矿井下巷道的激光雷达数据和矿井下电机车周围的激光雷达数据。
[0115]
车载控制器，用于根据矿井下巷道的激光雷达数据获取矿井下巷道的点云图，并根据点云图构建矿井下巷道的动态地图；从矿井下电机车周围的激光雷达数据中提取障碍
物位置信息；根据障碍物位置信息、障碍物相对电机车速度差和动态地图，生成动态避障速度控制地图。
[0116]
整车控制器，用于将障碍物位置转换到避障速度控制地图上，当障碍物的位置处于相应的速度限制区域时，发送控制速度指令。
[0117]
电机控制器，用于根据控制速度指令，实时控制电机车实现减速避障或刹车避障。
[0118]
需要说明的是，关于系统的具体限定可以参见上文中对于方法的限定，在此不再赘述。上述方法中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
[0119]
以上所述实施例仅表达了本技术的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本技术构思的前提下，还可以做出若干变形和改进，这些都属于本技术的保护范围。因此，本技术专利的保护范围应以所附权利要求为准。