移动扫码阅读
移动扫码阅读
无轨胶轮车在国内外煤矿的辅助运输系统中得到了广泛应用[1-3]。然而由于人员密集度高和劳动强度大的特点，无轨胶轮车辅助运输仍是煤矿生产中安全隐患突出的环节[4-5]。实现这类辅助运输车辆无人驾驶是煤矿安全生产的现实需求，也是智慧化矿山建设的必然趋势[6]。当前关于煤矿用辅助运输车辆无人驾驶技术的研究仍处于起步阶段，相关文献较少。
车辆自主定位是无人驾驶技术体系中的重要一环。定位技术用于建立车辆与环境地图之间的相互关系，也被称为位置估计。在移动机器人领域，概率技术因其鲁棒性好和模型依赖程度低等优点已成为研究热点和发展趋势[7-8]。移动机器人(涵盖无人驾驶车辆)的概率定位方法基本都源于贝叶斯滤波。得到广泛应用的扩展卡尔曼滤波(EKF)定位和无迹卡尔曼滤波(UKF)定位将机器人位姿的置信度表示为一个参数化的高斯分布[9]。其主要局限是不能解决全局定位和绑架问题，此外需要从传感器测量提取特征进行处理。DELLAERT等[10]提出了基于粒子滤波的定位方法，并将这一方法命名为术语蒙特卡罗定位(Monte Carlo Localization，MCL)。粒子滤波通过状态采样表示置信度，是贝叶斯滤波的一种非参数实现形式[11]。MCL易于实现并且适用范围广，受到了广泛关注。大量的研究工作针对算法中出现的粒子缺乏问题[12-13]，粒子退化和建议分布机制缺陷问题[14-16]，以及粒子集规模自适应调整优化等方面展开[17-18]。
笔者以煤矿用辅助运输车辆的自主精确定位问题为研究对象，在场景及需求分析的基础上，讨论了激光测距扫描的适用性，提出了将巷道内车辆的精确定位维持在一个典型场景所对应的区域地图范围内进行的策略；基于车辆的速度运动模型和激光测距的似然域模型，论述了蒙特卡罗定位方法的实现，提出了调节似然域模型不确定性范围，从而避免定位算法失败的方法，仿真结果验证了该方法有效,实现了车辆在煤矿井下巷道的自主定位。
煤矿用车辆自主行驶时，在巷道延伸的纵向方向和垂直于巷道壁的侧向方向存在不同的定位要求。纵向方向上，在一个重要地点所对应的有限区域范围内，车辆需要获取精确位置坐标；而在一段仅作为通道存在的狭长直巷内则无需该信息，即需要时精确定位。侧向的情况则不同，为确保在规定的狭窄车道内安全行驶，车辆需要时刻了解自身相对巷道中心线基准的位置和转向姿态角，即实时精确定位。
尽管基于特征测量的定位方法能够实时获得车辆的全部三自由度位姿估计，但在井下巷道环境应用时存在一定的局限。考虑对重要地标进行观测的情况，由于狭窄巷道空间走向的单一性，地标只能依线性排列设置，从而长距离测量将不可避免。此时，特征方位角的测量噪声将对车辆侧向定位精度产生较大制约。并且由于侧向连续实时定位的要求，在车辆一次观测的可及范围内至少需要设置1个地标，这在煤矿长距离巷道环境下工程实现代价较大。
综合上述定位需求，侧向的激光测距扫描为车辆自主定位提供了一个可供选择的合理解决方案。测距扫描提供原始测量数据，而不是特征测量。此时侧向位置和姿态角能够获得实时的测量更新；对于纵向位置，当车辆未到达重要区域而不需要精确定位时，可以维持在一个多假设状态。
煤矿井下车辆行驶的巷道环境的一个典型场景如图1所示，图中同时绘出了车辆侧向测距扫描的情形，其中，A、B、C、D为图中4个固定点，x′和y′为车体坐标系的2个坐标轴，θ为车辆局部坐标系相对于全局坐标系的转角，逆时针旋转为正。在一段狭长直巷的中间某个位置发生巷道的交汇连接，该位置处出现的可能是硐室、联络巷或交叉巷。通常对于车辆来说这里对应一个重要地点或场所，车辆行驶的全域环境基本可认为是一定数量的该典型场景的集合。
图1 矿用车辆行驶的巷道环境的一个典型场景
Fig.1 Typical scene of laneways in which vehicle for coal mine operates
基于分层定位技术，提出将车辆精确定位问题维持在一个典型场景所对应的区域地图范围内进行的策略，并进一步详细讨论蒙特卡罗定位方法的实现。需要强调说明的是，任何一个典型场景内只包含1处巷道交汇。这样划分的意义在于，在某一场景内的全局定位问题下，当传感器未观测到巷道交汇处时，位姿置信度可能呈现1个多峰分布，在纵向方向维持多个位置假设；而当传感器一旦观测到巷道交汇地点时，置信度迅速收敛到1个单峰分布，车辆将确信自己获得了正确的位置估计。如果1个典型场景中包含多处巷道交汇，通常在观测到2个以上巷道交汇地点前置信度将保持多峰分布，即维持多假设的情况，车辆有可能因此错过在某个重要地点获得正确位置估计的时机，即未能实现“需要时精确定位”，从而影响正常行驶。
定义全局坐标系如图1所示，其中x轴沿巷道中心线定义，y轴在巷道横截面内方向垂直于巷道壁。车辆在全局坐标系下的位姿状态变量为
(1)
式中，(x,y)为车辆局部坐标系原点在全局坐标系下的坐标值。
在概率体系下，k时刻车辆的位姿由置信度bel(ξk)描述，其定义为后验概率分布p(ξk|ο1:k,u1:k)。
bel(ξk)≐p(ξk|ο1:k,u1:k)
(2)
式中，ο1:k和u1:k分别为k时刻以前所有的测量值和控制量。
当车辆首次进入某个典型场景时，首先面对的是在这个区域地图范围内的全局定位问题，同时考虑到车辆侧向的激光测距扫描提供的是原始测量值而非特征测量，从而EKF或UKF定位方法不适用于这一定位问题的解决。MCL通过一个状态采样获得的粒子集描述置信度bel(ξk)，能够解决位置跟踪、全局定位以及绑架问题，并且可以处理传感器的原始测量值而无需提取特征，算法按照如下步骤实现：
1)给定k-1时刻的粒子集χk-1，当前k时刻的uk、οk，以及环境地图m。
2)对所有j=1，2，…，N(N为粒子数量)进行采样。
(3)
得到预测状态的样本集
同时计算每个
所对应的权值为
(4)
3)在
中重复进行N次采样，每次采样时每个粒子
被抽取的概率与其权值
成正比，从而获得k时刻的粒子集χk。
算法的第2步实现了贝叶斯滤波的控制更新过程。式(3)中
为状态转移概率，对k-1时刻的每个粒子
从该后验概率分布进行采样，产生一个状态样本，最终获得预测状态的样本集
采样同时，根据预测样本及当前测量值所对应的测量概率
计算每一个预测状态样本的权值，见式(4)。权值反映了基于预测状态的一个样本获得当前测量值的可信度。
算法的第3步根据每一个预测状态的权值大小进行重要性采样，实现了贝叶斯滤波的测量更新过程。得到的粒子集χk能够描述任意形状(如多峰分布)的置信度bel(ξk)分布。如果为简单地获取定位估计，可采用高斯近似进行密度提取。分别以样本均值和样本方差作为近似高斯分布的一阶矩和二阶矩参数，得到定位估计。
矿用辅助运输车辆一般采用阿克曼转向或铰接转向的驱动结构，其中前者用于运人车等轻型胶轮车，后者用于支架搬运车等重型车。这2类结构都具有非完整运动学约束[19]，具体来说，它们都具有1个瞬时旋转中心，控制自由度都等于2，由1个差分自由度和1个可操控自由度组成。对于阿克曼转向结构，可选取车辆纵向速度v和旋转角速度ω作为名义控制输入。而对于铰接转向结构车辆，其前后车体满足一个与铰接角相关的运动学约束关系，可选取前车体的纵向速度v及其旋转角速度ω作为名义控制输入。由此，采用统一的速度运动概率模型的采样算法完成式(3)表示的采样过程[20]。首先，向名义控制中加入控制噪声采样。
(5)
其中：randNorm(a,b2)为由服从均值为a、方差为b2的高斯分布采样一个随机数；α1～α6为可调节噪声噪声参数；
和
为加入高斯噪声的实际控制参数。进一步，由速度运动方程计算状态采样。
(6)
式中，Δt为采样间隔。
当测距仪测量到地图中的重要场所时(巷道交汇处)，车辆位姿的微小变化可能导致测量距离返回值的剧烈变化。为了使测量概率
随车辆位姿变化具有平滑性，减小因粒子缺乏而造成MCL失败的可能性，采用测距仪的似然域模型作为测量模型[20]。在该模型中，对地图m上的任一点(x,y)均可找到一个与之最近的障碍物，同时将该点到这个障碍物的距离值记为l，则(x,y)点可唯一对应一个由下式计算的概率密度。
(7)
其中：σsens与传感器相关，为测距噪声的均方根误差；
为在均值0，方差
的高斯分布函数中，取自变量l对应的概率密度。这样，地图上的所有点及其对应的概率密度构成一个似然域。
在车辆状态ξk和地图m已知条件下，1个激光测距返回的距离测量值将唯一对应地图上的1个点
设车辆的1次激光测距扫描共包含Q个点测量，假设各个点测量是条件独立的，则测量概率为
(8)
基于坐标变换，点
通过下式计算得到：
(9)
式中：
为第i个激光测距的起点在车体坐标系的位置；
为第i个激光测距的发射方向与车体坐标系x′轴的夹角，逆时针旋转为正。
这里将激光测距返回的最大距离读数视为测量失败，将其简单丢弃。则式(7)—式(9)给出了激光测距扫描的似然域测量模型，由此可根据式(4)进行定位算法中权值的计算。
直观上，在似然域模型式(7)所使用的高斯分布中选择传感器噪声的均方差σsens表示其不确定性具有显著的物理意义。但对于噪声水平较低的高精度传感器情况，这时似然域中的概率质量会集中于障碍物体附近的区域，即似然域中随着与障碍物体距离的增大对应的概率密度值急剧衰减。此时如果从运动模型获得的样本集
当中缺乏足够接近真实状态的采样时，对应的权值将很小且不能体现预测状态的重要性差别，在实现MCL时有可能造成算法失败。
为适应MCL，笔者提出不确定性范围可调的似然域模型修正方法，即将式(7)修正为
(10)
其中，σLHF为可调节参数。增大似然域不确定性σLHF使得在正确状态附近范围内的预测状态值能够获得更高的测量概率(权值)，这时重要性采样能够基于测量信息采样出更接近真实值的状态。
仿真使用图1所示的典型场景，首先设置地图参数。设定巷道宽6 m，图1中A、B、C、D点坐标分别为(10,3)、(10,11)、(15,11)、(15,3)，单位m。为简化计算,设激光测距扫描的起点位于车体坐标系的原点处。选取1次扫描中的8个点激光测距返回值作为定位的原始测量，其激光发射方向相对车体坐标系x′轴正向逆时针旋转的角度
分别为45°、75°、105°、135°、225°、255°、285°和315°。仿真过程中，基于车辆的实际位姿在地图上按照射线投射计算每个激光测距的理想测量值，然后向其加入零均值高斯噪声获得仿真用真实测量值
噪声均方根误差σsens设定为0.1 m。选取测距传感器的最大测量值为30 m。最后设置运动参数见表1。
表1 仿真用运动参数
Table 1 Motion model parameters used in simulation
参数设定值参数设定值参数设定值vk/(m·s-1)1α10.5α20.1ωk/(rad·s-1)0.001α30.01α40.1Δt/s0.1α50.002α60.005
MCL方法首先要解决全局定位问题。选取粒子数N=30 000，在地图范围内，按照均匀分布在车辆位姿状态空间进行采样，获得初始时刻粒子集，粒子在x-y平面的分布情况如图2所示。
图2 初始时刻的粒子在x-y平面的分布情况
Fig.2 The distribution of particles in the x-y plane at the initial monment
MCL仿真过程中，调节模型参数σLHF=1 m，获得了良好的估计效果。进行参数调节时，当σLHF取值较小(向σsens靠近)，则粒子基本不能收敛到真实位姿附近，与分析情况相符。仿真得到MCL过程中不同时刻的粒子在x-y平面的分布情况如图3所示。设车辆的各位姿状态是条件独立的，通过高斯近似对粒子集进行密度提取，并以高斯分布的均值作为车辆位姿估计，则车辆位置估计的情况如图4所示，车辆姿态角估计如图5所示。
图3 不同时刻的粒子在x-y平面的分布情况
Fig.3 The distribution of particles in the x-y plane at different monments
图4 MCL的车辆位置估计
Fig.4 Position estimation of vehicle by MCL
图5 MCL的车辆姿态角估计
Fig.5 Angular orientation estimation of vehicle by MCL
由图3可得，在MCL开始后的一段时间里，初始时刻均匀分布的粒子聚集在2个区域之内。此时状态置信度对应1个多峰分布，即存在多个可能的位姿假设，这一阶段反映了车辆的全局定位过程。随着MCL的进行，粒子聚集到了一个确定的区域附近，该阶段反映出车辆已进入位置跟踪过程。图4的位置估计曲线也清晰地表现出这一过程变化。当置信度具有多峰分布时，高斯近似密度不能有效拟合真实置信度分布，从而使得位置估计具有较大偏差。当置信度变为单峰分布时，高斯近似精度大幅提升，位置估计迅速逼近实际运动轨迹。事实上，结合图4和图5可以看出，在整个MCL过程中，侧向位置估计和姿态角估计都很好地反映了实际状态值，真正导致置信度多峰分布的因素是纵向位置的后验分布特性。在t≈8 s时，置信度由多峰分布收敛为单峰分布，该过程反映了车辆侧向的激光测距扫描观测到地图中的巷道交汇这一事实。综上可知，车辆的定位特性确实满足了侧向“实时精确定位”和纵向“需要时精确定位”的要求。
为对比定位性能和效果，基于相同的实际运行轨迹，对EKF定位的实现情况进行了仿真。首先，EKF定位不能解决全局定位问题，因此仿真使用了车辆真实初始位姿附近的一个窄高斯分布作为初始值进行位置跟踪。其次，EKF不能直接处理前述的车辆侧向激光测距扫描的测量信息，仿真采用基于地标特征测量的方法实现EKF定位。考虑到煤矿巷道实际环境特点，假设可同时观测的地标数量为1个，地标在地图中的设置坐标为(100,0)，单位m。最后，分别设置特征方位角的测量误差σ为0.05和0.10 rad，仿真得到EKF定位的车辆位置估计情况如图6所示。基于MCL以及2种条件下的EKF定位仿真结果，绘制各自的侧向位置估计误差曲线，如图7所示。
图6 EKF的车辆位置估计
Fig.6 Position estimation of vehicle by EKF
图7 侧向位置估计误差对比
Fig.7 Comparison of errors of lateral position estimation
计算MCL和2种条件下EKF定位的侧向估计误差统计值，见表2。
表2 侧向位置估计误差对比
Table 2 Comparison of errors of lateral position estimation
定位方法及参数侧向估计均值误差/m侧向估计均方根误差/mMCL-0.01150.0238EKF(σ=0.05rad)-0.09580.0485EKF(σ=0.10rad)-0.18020.0908
可以看出，在基于特征测量的EKF位置跟踪定位情况下，侧向的定位精度受特征方位角测量误差的影响较为显著。对于煤矿巷道环境长距离单特征测量，适宜的特征测量方法和高精度传感器成为现实制约。
1)通过定义煤矿巷道环境的典型场景，矿井全域地图可分解为若干典型场景区域地图，矿用车辆的定位问题在区域地图范围内得到解决，简化了车辆全局定位的复杂度和运算量。
2)在巷道环境中，纵向和侧向相关位姿状态估计的及时性要求不同，采用车辆速度运动模型及激光测距传感器似然域模型的蒙特卡罗定位算法能够很好地适应这一要求，解决了全局定位和位置跟踪问题。
3)为适应蒙特卡罗定位算法的特点，将似然域模型中的不确定性参数作为可调节量，当传感器噪声水平较低时，适当增大该量值能够保证算法成功运行。
4)相比巷道环境下的扩展卡尔曼滤波定位，除能够解决全局定位问题外，蒙特卡罗定位方法使侧向位置估计精度得到有效保证。
[1] 张彦禄，高 英，樊运平，等.煤矿井下辅助运输的现状与展望[J].矿山机械，2011，39(10)：6-9.
ZHANG Yanlu,GAO Ying,FAN Yunping,et al.Current situation and prospects of underground auxiliary transportation in collieries[J].Mining and Processing Equipment,2011,39(10):6-9.
[2] 郝明锐.矿用纯电动防爆车辆续驶里程提升技术研究[J].煤炭科学技术，2019，47(2)：156-160.
HAO Mingrui.Research on driving range extension technology of mine flame-proof battery electric vehicle[J].Coal Science and Technology,2019,47(2):156-160.
[3] 任志勇.纯电动防爆车辆续驶里程影响因素研究[J].煤炭科学技术，2019，47(2)：150-155.
REN Zhiyong.Research on influence factors affecting driving range of flame-proof battery electric vehicles[J].Coal Science and Technology,2019,47(2):150-155.
[4] 倪兴华.安全高效矿井辅助运输关键技术研究与应用[J].煤炭学报，2010，35(11)：1909-1915.
NI Xinghua.Research and application of key technology for safety and high efficient mine auxiliary transportation[J].Journal of China Coal Society,2010,35(11):1909-1915.
[5] 魏 臻，鲍红杰，陆 阳，等.矿井胶轮车运输监控系统信号设计与调度规则[J].煤炭学报，2010，35(3)：509-514.
WEI Zhen,BAO Hongjie,LU Yang,et al.Signal designing and dispatching rules for rubber tire vehicle transportation in mine[J].Journal of China Coal Society,2010,35(3):509-514.
[6] 王国法，赵国瑞，任怀伟.智慧煤矿与智能化开采关键核心技术分析[J].煤炭学报，2019，44(1)：34-41.
WANG Guofa,ZHAO Guorui,REN Huaiwei.Analysis on key technologies of intelligent coal mine and intelligent mining[J].Journal of China Coal Society,2019,44(1):34-41.
[7] 路 飞，田国会，刘国良，等.智能空间下基于WIFI 指纹定位与粒子滤波的服务机器人复合全局定位系统设计[J].机器人，2016，38(2)：178-184.
LU Fei,TIAN Guohui,LIU Guoliang,et al.A composed global localization system for service robot in intelligent space based on particle filter algorithm and WIFI fingerprint localization[J].Robot,2016,38(2):178-184.
[8] 季宇寒，李 寒，张 漫，等.基于激光雷达的巡检机器人导航系统研究[J].农业机械学报，2018，49(2)：14-21.
JI Yuhan,LI Han,ZHANG Man,et al.Navigation system for inspection robot based on LiDAR[J].Transactions of the Chinese Society for Agricultural Machinery,2018,49(2):14-21.
[9] 张 智，张 磊，苏 丽，等.基于人工离线特征库的室内机器人双目定位[J].哈尔滨工程大学学报，2017，38(12)：1906-1914.
ZHANG Zhi,ZHANG Lei,SU Li,et al.Binocular localization of indoor robot based on artificial offline feature-database[J].Journal of Harbin Engineering University,2017,38(12):1906-1914.
[10] DELLAERT F,FOX D,BURGARD W,et al.Monte Carlo localization for mobile robots[C].Proceedings -IEEE International Conference on Robotics and Automation,vol 2.Detroit:IEEE,1999:1322-1328.
[11] FOX D,BURGARD W,DELLAERT F,et al.Monte Carlo Localization:efficient position estimation for mobile robots[C]//Proceedings of the National Conference on Artificial Intelligence.Orlando:AAAI,1999:343-349.
[12] GUTMANN J S,FOX D.An experimental comparison of localization methods continued[C]//Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems.EPFL:IEEE,2002:454-459.
[13] 朱奇光，肖亚昆，陈卫东，等.基于萤火虫算法改进移动机器人定位方法研究[J].仪器仪表学报，2016，37(2)：323-329.
ZHU Qiguang,XIAO Yakun,CHEN Weidong,et al.Research on the improved mobile robot localization approach based on firefly algorithm[J].Chinese Journal of Scientific Instrument,2016,37(2):323-329.
[14] THRUN S,FOX D,BURGARD W.Monte Carlo localization with mixture proposal distribution[C]//Proceedings of the AAAI National Conference on Artificial Intelligence.Austin:AAAI,2000:859-865.
[15] 宋 宇，孙富春，李庆玲.移动机器人的改进无迹粒子滤波蒙特卡罗定位算法[J].自动化学报，2010，36(6)：851-857.
SONG Yu,SUN Fuchun,LI Qingling.Mobile robot Monte Carlo localization based on improved unscented particle filter[J].Acta Automatica Sinica,2010,36(6):851-857.
[16] 房 芳，马旭东，戴先中.一种新的移动机器人Monte Carlo自主定位算法[J].东南大学学报(自然科学版)，2007，37(1)：40-44.
FANG Fang,MA Xudong,DAI Xianzhong.New Monte Carlo algorithm for mobile robot self-localization[J].Journal of Southeast University(Natural Science Edition),2007,37(1):40-44.
[17] FOX D.Adapting the sample size in particle filters through KLD-sampling[J].International Journal of Robotics Research,2003,22(12):985-1003.
[18] 李天成，孙树栋.采用双重采样的移动机器人Monte Carlo定位方法[J].自动化学报，2010，36(9)：1279-1286.
LI Tiancheng,SUN Shudong.Double-resampling based Monte Carlo localization for mobile robot[J].Acta Automatica Sinica,2010,36(9):1279-1286.
[19] SIEGWART R,NOURBAKHSH I R.Introduction to autonomous mobile robots[M].Massachusetts:The MIT Press,2004.
[20] THRUN S,BURGARD W,FOX D.Probabilistic robotics[M].Massachusetts:The MIT Press,2006.