A localization and navigation method for underground mine autonomous driving based on local geometric topology map
-
摘要: 无人驾驶技术在提高效率、节省成本、减少安全隐患等方面具有巨大优势。针对目前地下环境中定位导航方案实施难度大、成本高、构建地图耗时长等问题，提出了一种基于局部几何−拓扑地图的地下矿自动驾驶定位导航方法。设计了一种局部几何−拓扑地图，井下环境的路网主体结构由拓扑地图表示，该地图上定义了巷道（边）和交叉路口（节点），在每个节点中存储以该节点为中心构建的局部几何地图，用以实现节点处的精确定位。提出了一种基于局部几何−拓扑地图的定位方法，使用基于激光雷达的交叉路口检测算法与交叉路口定位算法进行车辆全局定位。设计了一种基于自适应模型预测控制(MPC)的轨迹跟随算法，保证车辆在交叉路口大曲率转向时的路径跟踪精度。使用三维物理仿真平台构建了地下矿的仿真环境与车辆仿真模型，仿真结果表明：该方法能够实现地下矿自动驾驶定位导航功能，在各种类型交叉路口的定位误差均在0.2 m以内，可以满足自动驾驶的定位精度要求；在整个行驶过程中车辆始终保持较为平稳的行驶状态和较小的跟踪误差。与目前依赖于5G、UWB等技术的定位导航方法相比，该方法仅依赖于激光雷达与惯性测量单元2种车身传感器，在控制设备成本上具有极大优势。Abstract: Unmanned driving technology has enormous advantages in improving efficiency, saving costs and reducing safety hazards. In the current implementation of localization and navigation solutions in underground environments, there are problems of implementation difficulties, high costs, and time-consuming construction of maps. In order to solve the above problems, a localization and navigation method for underground mine autonomous driving based on local geometric topology map is proposed. A local geometric topology map has been designed. The main structure of the underground environment road network is represented by a topology map. The map defines roadways (sides) and intersections (nodes), and stores a local geometric map built around the node in each node to achieve precise positioning at the node. A localization method based on local geometric topology map is proposed, which uses a LiDAR-based intersection detection algorithm and intersection localization algorithm for global vehicle localization. A trajectory-following algorithm based on adaptive model predictive control (MPC) has been designed to ensure the path-tracking precision of vehicles turning at high curvature intersections. A simulation environment and vehicle simulation model for underground mines are constructed by using a 3D physical simulation platform. The simulation results show that this method can achieve underground mine autonomous driving localization and navigation functions. The positioning errors are within 0.2 m at various types of intersections, meeting the positioning localization precision requirements of autonomous driving. Throughout the entire driving process, the vehicle maintains a relatively stable driving state and a small tracking error. Compared with the current localization and navigation methods that rely on technologies such as 5G and UWB, this method only relies on two types of vehicle sensors: LiDAR and inertial measurement unit. It has great advantages in controlling equipment costs.
-
0. 引言
无人驾驶技术在提高效率、节省成本、减少安全隐患等方面具有巨大优势。尤其是在地下矿的恶劣环境中，存在安全事故频发、司机招聘困难、管理运营成本高等问题，无人化运输正成为重点研究方向[1-4]。然而，由于井下的“长廊效应”、无定位信号等复杂工况环境[5]，传统的即时定位与地图构建（Simultaneous Localization and Mapping，SLAM）方法无法适用[6]。目前井下无人驾驶采用的方案是在井下大规模部署5G基站，融合超宽带（Ultra Wide Band，UWB）系统[7]、高精度地图、车载惯导与惯性测量单元[8]等实现定位与导航[9-10]。在新矿山中一般配备5G基站[11]和UWB系统[12-14]，这种方案尚属可行，然而在老矿山中部署5G基站的成本高、难度大，这种方案并不适用。
S. Radacina Rusu[15]提出了一种基于射频识别 (Radio Frequency Identification，RFID)的节点地图定位导航方法，以RFID标签为独特的地标，将大型井下环境整体地图分割成较小的地图，通过粒子滤波实现车辆定位。然而，为保证定位的精准度，该方法要求每隔50~300 m安装1个RFID标签，在井下环境中布置存在难度大、成本高的缺陷。E. J. Larson[16]提出了基于超声波测距仪的定位算法，使用扩展卡尔曼滤波器，融合阿克曼车辆动力学与迭代最近邻点算法(Iterative Closest Point，ICP)结果，获取车辆位置。但该算法测量较远距离的目标时，回波信号较弱，测量精度较低，影响定位精度。M. Mascaró等[17]提出了一种适用于隧道内车辆的拓扑地图及基于该地图的自动驾驶系统，实现了不依赖于全球导航卫星系统（Global Navigation Satellite System，GNSS）的定位功能，然而基于ICP配准得到的陆标定位误差较大，且提取陆标点耗时较长，在井下环境中应用存在构建地图耗时长、鲁棒性差的问题。
针对上述问题，本文提出一种仅依赖于激光雷达与惯性测量单元的低成本地下矿自动驾驶定位导航方法。主要创新如下：① 提出了一种局部几何−拓扑地图，能够在不影响定位精度的前提下，以较小的空间复杂度描述井下道路网络环境。② 基于ICP与正态分布点云算法(Normal Distribution Transform，NDT)的点云配准特点，将二者进行融合，实现一种兼顾鲁棒性与精确性的车辆定位。③ 设计了一种基于自适应模型预测控制(Model Predictive Control，MPC)的轨迹跟随算法，保证车辆在交叉路口大曲率转向时的路径跟踪精度。
1. 局部几何−拓扑地图
目前常用的地图类型包括公制几何地图和拓扑地图。公制几何地图利用传感器数据对环境进行详细描述，建立环境的几何表示，如图1所示[18]。然而，一方面，公制几何地图易受车辆地图制作和航位推算不准确的影响；另一方面，在较大的应用环境中，里程计的漂移也会给地图构建带来困难，并使地图的全局一致性难以维护。拓扑地图将整个环境抽象为一系列拓扑节点，依据在环境中的相对位置构建边，如图2所示。拓扑地图能够以较低的计算、存储资源实现车辆粗略定位及后续路径规划[19-20]。
本文结合2种地图格式，设计了一种局部几何−拓扑地图。井下环境的路网主体结构由拓扑地图表示，该地图上定义了井下2种主要的环境组成部分：巷道（边）和交叉路口（节点）。巷道是可以往返穿越的单一特征道路部分，对应于2个交叉路口之间的单个路径或轨迹。在拓扑地图上，代表巷道的边将2个代表交叉路口的节点连接起来；1个节点可以连接多条边，从而构成路网结构。在每个节点中存储以该节点为中心构建的局部几何地图，用以实现节点处的精确定位，节点处的精确定位是进行车辆导航的必要前提。
局部几何−拓扑地图结构如图3所示。节点中存储以下信息：① 唯一的节点ID。② 与节点连接的边的数量。③ 与节点连接的边的具体信息，包括巷道ID、通向各巷道的统一基准下的偏航角和连接点局部坐标。代表巷道的边中存储以下信息：① 唯一的边ID。② 边连通的节点ID。③ 边所代表巷道的宽度和长度。通过节点ID，可以从几何地图数据库中查找相对应交叉路口的局部几何地图，每个交叉路口均有且仅有1个与之对应的三维点云地图。
一个三岔路口的拓扑地图如图4所示，红色虚线框中的部分为节点，该区域的三维点云地图即为局部几何−拓扑地图中需保存的局部几何地图，为交叉路口局部定位提供依据。与节点相连的3条巷道构成蓝色实线所示的3条路径，3条路径以参数形式保存在地图中，为车辆导航提供参考轨迹。
2. 基于局部几何−拓扑地图的定位
基于局部几何−拓扑地图的定位分为2个部分：巷道中的定位和交叉路口处的定位。因此，在定位时首先要检测车辆所处区域。
2.1 区域检测
考虑到激光雷达光束在巷道方向探测距离大、在其他方向探测距离小的特性，通过计算各偏航角上的车辆探测距离，构建激光雷达光束模型（图5），并获得光束模型距离−角度直方图（图6）。
当探测距离过大时，激光雷达无法接收返回的信号，会出现探测距离为0的无效数据。为解决该问题，对光束模型数据进行归一化处理，并将无效数据替换为1，从而获得归一化的光束模型距离−角度直方图[21]，如图7所示。基于交叉路口处会出现多个波峰、每个波峰代表1条岔路的直方图特征，可以判断车辆目前正位于交叉路口处，且当前交叉路口的岔路为4条，分布在0，90，180，245°方向上。
交叉路口检测算法伪代码如图8所示。该算法的输入为1组距离测量值D，参数包括距离阈值d（distance_threshold）和巷道最小宽度阈值W（width_threshold），输出为交叉路口标志、交叉路口连接的巷道数量及位置。首先，计算距离−角度直方图的所有峰值，并过滤小于距离阈值d的峰值，得到代表巷道存在的峰值；其次，在每个峰值所在角度左侧和右侧搜索探测距离小于距离阈值d的角度，得到该峰值所代表的巷道开口位置；然后，计算巷道开口宽度，使用巷道最小宽度阈值W过滤不符合巷道特征的检测结果；最后，统计当前岔路数量及位置，并据此判断是否位于交叉路口处。
交叉路口检测算法的实际运行效果如图9所示。车辆在交叉路口处识别出5条岔路，并获得了每条岔路的具体位置。
2.2 拓扑地图全局定位
当车辆在已知规划路径行驶时，能够得知当前所在节点及其相邻节点，因此当使用交叉路口检测算法检测到抵达新节点时，将通过激光雷达获取的当前节点点云与前一节点相邻节点的点云进行点云匹配，从而获得车辆当前所在节点ID。通过这种全局定位算法获取车辆实时位置，该实时位置由拓扑地图节点ID或边ID表示。
全局定位算法流程如图10所示。首先设定无人车初始位置，在实时定位中，使用交叉路口检测算法进行交叉路口检测。当检测到抵达新的节点时，在拓扑地图中查找与当前位置相邻的节点，并提取相邻节点的三维点云地图作为目标匹配点云[22]，提取当前无人车所在节点通道在该相邻路口节点点云地图中的水平角度作为NDT匹配的水平旋转角度先验信息，以当前帧环境点云为输入点云，通过NDT配准算法进行匹配，根据实际应用情况设定阈值，若匹配误差小于所设阈值，则认为匹配成功，更新当前位置到匹配成功且相似度最高的节点处，完成一次定位工作。在下一次检测到抵达节点时，循环上述步骤。
2.3 交叉路口局部定位
交叉路口局部定位是地下矿自动驾驶的关键技术。当车辆由巷道进入交叉路口时，首先根据拓扑地图中存储的拓扑关系得知所抵达节点的ID，然后提取该节点中存储的局部三维点云地图，与车载激光雷达实时三维点云进行配准，获取车辆位姿。
常用的三维点云配准方法有ICP配准算法和NDT配准算法2种。ICP配准算法精度高，但由于算法本身缺陷，最终迭代结果可能会陷入局部最优，需要提供一个较好的位姿初值，适用于位移、姿态小范围变化的配准[23-24]。NDT 配准算法精度一般，但在所给位姿初值的误差较大时也可以匹配成功，具有较好的鲁棒性，适用于位移、姿态大范围变化的配准[25-26]。本文使用ICP与NDT组合的配准方法：在初次配准时，采用NDT配准算法快速、准确地得到车辆位姿；在后续的连续定位中，采用ICP配准算法得到精度较高的车辆位姿。通过将2种算法结合，本文的定位算法在快速性和鲁棒性上均具有良好表现。
交叉路口局部定位算法伪代码如图11所示。该算法的输入为激光雷达三维实时点云C和交叉路口节点标志符M，输出为车辆的位姿信息。首先，从拓扑地图得到由巷道进入当前交叉路口的配准位姿初值（pose）；然后，根据前一时刻位姿并行使用ICP和NDT算法进行配准，当配准结果满足要求，即适应度小于阈值（fitness_threshold）时，使用配准结果对位姿状态进行更新；最后，考虑到初次配准位姿初值误差可能较大，本文对适应度阈值采用逐渐收敛的方法，在每次位姿更新结束后减小适应度阈值，直至小于所设定的阈值（minFitness_threshold）。
![]()
图 11 交叉路口局部定位算法伪代码Figure 11. Pseudo-code of local positioning algorithm for intersection为保证定位稳定性，将当前定位结果与上一次定位结果进行对比，当移动距离或偏航角变化量超出预期值时，对定位结果进行过滤，并重新开始定位，直至获得良好定位结果。
3. 基于自适应MPC的轨迹跟随控制
鉴于拓扑地图由边和节点2个部分组成，轨迹跟随控制模块具有2种跟踪模式：① 当车辆抵达交叉路口时，根据下一目标节点ID在拓扑地图中查找对应的路径轨迹点，并使用MPC算法计算车辆前轮转角，控制车辆进入指定巷道，如图12所示，其中白色部分为激光雷达点云，绿色线段为参考轨迹。② 当车辆进入巷道时，则沿巷道行驶，直至抵达新的交叉路口，如图13所示，其中白色部分为激光雷达点云，红色部分为车身，绿色线段为车道边线，黄色线段为车辆参考轨迹。
汽车的动力学特性非常复杂，尽管复杂的车辆动力学模型能够较好地反映车辆的实际运行状态，但并不利于控制问题的研究。因此本文采用简单有效的二自由度车辆模型进行轨迹跟随控制算法的设计。二自由度车辆模型如图14所示。L为车辆模型的轴距；θ为车身在全局坐标系下与水平线之间的夹角，记为车身偏角；φ为前轮转角；（x，y）为车身后轴中心点在全局坐标系下的坐标；ρ为车辆转向半径。
为了方便描述车辆在空间中的运动，忽略轮胎侧滑对轨迹跟踪控制的影响，得到汽车运动学模型，其微分方程为
$$ \left\{ \begin{array}{l}\dot{x}=v{{\rm{cos}}} \ \theta \\ \dot{y}=v{{\rm{sin}}} \ \theta \\ \dot{\theta }=\dfrac{v}{L}{{\rm{tan}}} \ \varphi \end{array}\right. $$ (1) 式中v为车身前进速度。
根据上述运动学模型，得到输入$ {\boldsymbol{u}}={\left[v \;\; \varphi \right]}^{\mathrm{T}} $、状态量$ {\boldsymbol{X}}={\left[x \;\; y \;\; \theta \right]}^{\mathrm{T}} $的控制系统的状态空间方程：
$$ \dot{{\boldsymbol{X}}}=f\left({\boldsymbol{X}},{\boldsymbol{u}}\right) $$ (2) 用下标r表示参考轨迹点处的对应变量。在参考轨迹点利用泰勒级数将状态空间方程展开并忽略高阶项，得
$$ \begin{split} {\dot{{\boldsymbol{X}}}}_{{\rm{r}}}= & f\left({{\boldsymbol{X}}}_{{\rm{r}}},{{\boldsymbol{u}}}_{{\rm{r}}}\right)+ {\left.\frac{\partial f\left({\boldsymbol{X}},{\boldsymbol{u}}\right)}{\partial {\boldsymbol{X}}}\right|}_{{\boldsymbol{u}}={{\boldsymbol{u}}}_{{\rm{r}}}}^{{\boldsymbol{X}}={{\boldsymbol{X}}}_{{\rm{r}}}}\left({\boldsymbol{X}}-{{\boldsymbol{X}}}_{{\rm{r}}}\right) + \\ & {\left.\frac{\partial f\left({\boldsymbol{X}},{\boldsymbol{u}}\right)}{\partial {\boldsymbol{u}}}\right|}_{{\boldsymbol{u}}={{\boldsymbol{u}}}_{{\rm{r}}}}^{{\boldsymbol{X}}={{\boldsymbol{X}}}_{{\rm{r}}}}\left({\boldsymbol{u}}-{{\boldsymbol{u}}}_{{\rm{r}}}\right) \end{split}$$ (3) 进一步可以得到线性化的无人驾驶车辆误差模型：
$$\begin{split} \dot {\tilde {\boldsymbol{X}}} = & \left[\begin{array}{c}\dot{x}-{\dot{x}}_{{\rm{r}}}\\ \dot{y}-{\dot{y}}_{{\rm{r}}}\\ \dot{\varphi }-{\dot{\varphi }}_{{\rm{r}}}\end{array}\right]=\left[\begin{array}{ccc}0& 0& -{v}_{{\rm{r}}}{\rm{sin}} \ {\theta }_{{\rm{r}}}\\ 0& 0& {v}_{{\rm{r}}}{\rm{cos}} \ {\theta }_{{\rm{r}}}\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}x-{x}_{{\rm{r}}}\\ y-{y}_{{\rm{r}}}\\ \theta -{\theta }_{{\rm{r}}}\end{array}\right] + \\ & \left[\begin{array}{cc}{\rm{cos}} \ {\theta }_{{\rm{r}}}& 0\\ {\rm{sin}} \ {\theta }_{{\rm{r}}}& 0\\ \dfrac{{\rm{tan}} \ {\varphi }_{{\rm{r}}}}{L}& \dfrac{{v}_{{\rm{r}}}}{L{\rm{cos}} \ (2{\varphi }_{{\rm{r}}})}\end{array}\right]\left[\begin{array}{c}v-{v}_{{\rm{r}}}\\ {\boldsymbol{u}}-{{\boldsymbol{u}}}_{{\rm{r}}}\end{array}\right] \end{split} $$ (4) 式中$\dot {\tilde {\boldsymbol{X}}} $为误差。
将式（4）简写为
$$ \dot{\tilde{{\boldsymbol{X}}}}\left(k\right)={\boldsymbol{A}}\tilde{{\boldsymbol{X}}}\left(k\right)+{\boldsymbol{B}}\tilde{{\boldsymbol{u}}}\left(k\right) $$ (5) 式中：k为向前预测的步数；${{\boldsymbol{A}}}=\left[ \begin{array}{ccc}0& 0& -{v}_{{\rm{r}}}{\rm{sin}} \ {\theta }_{{\rm{r}}}\\ 0& 0& {v}_{{\rm{r}}}{\rm{cos}} \ {\theta }_{{\rm{r}}}\\ 0& 0& 0\end{array} \right] $；${\tilde{{\boldsymbol{X}}}}\left(k\right) =\left[ \begin{array}{l}x - {x_{\rm{r}}}\\y - {y_{\rm{r}}}\\\theta - {\theta _{\rm{r}}}\end{array} \right] $；${{\boldsymbol{B}}}=\left[ \begin{array}{cc}{\rm{cos}} \ {\theta }_{{\rm{r}}} & 0\\ {\rm{sin}} \ {\theta }_{{\rm{r}}} & 0\\ \dfrac{{\rm{tan}} \ {\varphi }_{{\rm{r}}}}{L} & \dfrac{{v}_{{\rm{r}}}}{L{\rm{cos}} \ (2{\varphi }_{{\rm{r}}})}\end{array} \right]$；$\tilde{{\boldsymbol{u}}}\left(k\right)=\left[ \begin{array}{l}v - {v_{{\rm{r}}}}\\{\boldsymbol{u}} - {{\boldsymbol{u}}_{{\rm{r}}}}\end{array} \right]$。
对式（5）进行前向欧拉离散化：
$$ \dot {\tilde {\boldsymbol{X}}} \left(k\right)=(\tilde{{\boldsymbol{X}}}\left(k+1\right)-\tilde{{\boldsymbol{X}}}\left(k\right))/T $$ (6) 式中T为采样时间间隔。
由式（5）和式（6）可得
$$ \dot{{\boldsymbol{X}}}\left(k+1\right)=\left(T{\boldsymbol{A}}+{\boldsymbol{E}}\right)\tilde{{\boldsymbol{X}}}\left(k\right)+T{\boldsymbol{B}}\tilde{{\boldsymbol{u}}}\left(k\right) $$ (7) 式中E为单位向量。
令$ \bar{{\boldsymbol{A}}}=T{\boldsymbol{A}}+{\boldsymbol{E}} $，$ \bar{{\boldsymbol{B}}}=T{{\boldsymbol{B}}} $，得
$$ \dot{{\boldsymbol{X}}}\left(k+1\right)=\bar {{\boldsymbol{A}}}\tilde{{\boldsymbol{X}}}\left(k\right)+\bar{{\boldsymbol{B}}}\tilde{{\boldsymbol{u}}}\left(k\right) $$ (8) 设计预测控制的目标函数，应考虑系统状态量的偏差和控制量。因此，设计目标函数为
$$\begin{split} \mathrm{m}\mathrm{i}\mathrm{n}J\left(U\right)=&\sum _{k=0}^{N-1}\left({\tilde{{\boldsymbol{X}}}^{{\rm{T}}} \left(k\right)}{{\boldsymbol{Q}}}\tilde{{\boldsymbol{X}}}\left(k\right)+ {\tilde{{\boldsymbol{u}}}^{{\rm{T}}}\left(k\right)} {\boldsymbol{R}}\tilde{{\boldsymbol{u}}}\left(k\right)\right)+ \\& {\tilde{{\boldsymbol{X}}}^{{\rm{T}}}\left(N\right)} {{{\boldsymbol{Q}}}}_{{\rm{f}}}\tilde{{\boldsymbol{X}}}\left(N\right) \end{split}$$ (9) 式中：$ U\mathrm{为} $控制序列；N为系统的预测步长；${\boldsymbol{Q}} $为给定状态代价矩阵；$ {\boldsymbol{R}} $为输入代价矩阵；$ {{{\boldsymbol{Q}}}}_{{\rm{f}}} $为最终状态代价矩阵，${\boldsymbol{Q}} $，$ {{\boldsymbol{Q}}}_{{\rm{f}}} $，R均为正定矩阵。
在忽略系统方向和动力延迟的情况下，为保证控制的连续性，根据经验给定速度和加速度的阈值，控制量${\boldsymbol{ u}} $和控制增量$ \Delta {\boldsymbol{u}} $的约束条件为
$$ \begin{gathered} {{\boldsymbol{u}}_{\min }}(t + k) \leqslant {\boldsymbol{u}}(t + k) \leqslant {{\boldsymbol{u}}_{\max }}(t + k) \end{gathered} $$ (10) $$ \begin{gathered} \Delta {{\boldsymbol{u}}_{\min }}(t + k) \leqslant \Delta {\boldsymbol{u}}(t + k) \leqslant \Delta {{\boldsymbol{u}}_{\max }}(t + k) \end{gathered} $$ (11) 式中：${{\boldsymbol{u}}_{\min }} $，${{\boldsymbol{u}}_{\max }} $分别为u的最小值和最大值； t为当前预测时刻；$\Delta {{\boldsymbol{u}}_{\min }} $和$\Delta {{\boldsymbol{u}}_{\max }} $分别为$\Delta {\boldsymbol{u}}$的最小值和最大值。
由于井下交叉路口处转弯曲率较大，对无人驾驶车辆的路径跟随精度要求较高，传统的MPC无法根据实际道路情况自动调整控制需求。为保证控制精度，本文参考B. Chen等[27]提出的一种基于道路曲率的变预测步长策略：
$$ N = {{Y(8 + 12}} {{\text{exp}}({-{{ w}} {{G}}}})) $$ (12) $$ G = \frac{1}{{P - 1}}\sum\limits_{i = 0}^{P - 1} {\left| {{y_{\rm{r}}}^{\prime \prime }({x_{\rm{r}}}_{,i + 1})} \right|} $$ (13) 式中：Y(·)为四舍五入函数；w为收缩因子；G为期望道路二阶微分的平均数（PGC）；P为参考轨迹点数；${x}_{{\rm{r}}, i+1}$为参考轨迹点向前预测的第i个点的横坐标。
车辆定位频率稳定在10 Hz以上，因此合理采样时间为0.1 s，将直线行驶预测时域选取为0.8~2.0 s，则合理的预测步长$ {N_{\rm{p}}} \in [8,20] $。PGC反映轨迹的曲率大小，车辆直线行驶时PGC越大，预测步长越小。由以上公式可以看到，预测时域被设计成对较小PGC值更敏感，而对较大PGC值不敏感。因此，当车辆前方的路径几何形状快速变化时，可以将预测步长减小到适当值，以提高跟踪性能；当车辆前方为直线道路时，将预测步长增加到适当值，以降低横向加速度，提高车辆稳定性。
4. 仿真测试与结果分析
4.1 硬件在环仿真测试
为验证本文算法在井下环境中的表现，使用硬件在环设备进行仿真测试。工控机硬件环境为Intel Core i5−9400处理器，主频为2.90 GHz，操作系统为Ubuntu 20.04。上位机环境为Intel Core i7−3610QM处理器，主频为2.3 GHz，操作系统为Ubuntu 20.04。自动驾驶系统基于机器人编程框架ROS(C++)实现，并部署到工控机上；然后在Gazebo中搭建地下矿环境及无人驾驶汽车模型，并部署到硬件在环设备上。地下矿自动驾驶系统框架如图15所示，Gazebo中的地下矿环境及无人驾驶车辆模型分别如图16、图17所示。地下矿巷道宽度为6 m，车载激光雷达为3台Velodyne 32线激光雷达。
地下矿环境拓扑地图如图18所示。为测试定位方法的真实可行性，根据拓扑地图制定了4条测试行驶路线，尽可能覆盖地下矿环境所有道路，以验证系统的鲁棒性。路线1： 1→2→11→12→13→16→17→18；路线2： 1→2→3→4→5→6→7→18→17→1；路线3：1→2→3→4→9→10→11→14→15→10→3→2→1；路线4：2→3→4→9→8→5→6→7→18→17→16→13→14→11→10→3→2→1。
设定实验参考车速为6 m/s，在4条行驶路线上获得的车辆行驶轨迹如图19所示，其中红色线条为车辆行驶轨迹。
对路线4中11→10→3路段的自动驾驶效果进行分析，该路段行驶轨迹如图20所示。车辆从下方巷道驶入交叉路口，右转驶出交叉路口，并进入巷道直线行驶。该过程中，车辆首先检测到进入交叉路口，开始进行交叉路口处的局部定位，然后调取拓扑地图中存储的11→10→3标定轨迹点，使用自适应MPC轨迹跟随算法计算出车辆的期望前轮转角及期望加速度，进行车辆控制。
4.2 定位性能验证
为验证交叉路口局部定位算法效果，使用3种算法进行测试：① 仅使用ICP算法进行定位。② 仅使用NDT算法进行定位。③ 使用ICP与NDT组合算法进行定位。各算法的计算耗时和点云配准距离差平均值分别如图21和图22所示。组合算法的耗时在大部分情况下大于NDT算法耗时，与ICP算法耗时接近，平均计算耗时为27.94 ms；组合算法的点云配准距离差平均值基本小于ICP与NDT 算法的点云配准距离差平均值。
![]()
图 22 不同算法的点云配准距离差平均值对比Figure 22. Comparison of average distance differences in point cloud registration of different algorithms在交叉路口处基于局部几何地图的组合定位算法误差如图23所示。x轴方向误差最大值为0.22 m，误差均值为0.11 m；y轴方向误差最大值为0.23 m，误差均值为0.09 m。第7 s之前，定位精度较高，误差始终保持在0.1 m以内；第7 s之后，由于激光雷达光束被墙壁阻挡，导致探测范围受限，沿巷道方向的y轴定位精度略微下降，但误差保持在0.2 m以内。
4.3 控制性能验证
使用自适应MPC轨迹跟随算法计算出的期望前轮转角如图24所示。最大值限定在0.6 rad，即34.38°以内，较小的控制量可以保证车辆的行驶稳定性，期望速度恒定为6 m/s。
实际车辆纵向速度如图25所示。车辆纵向速度在开始转弯后略微下降，最大速度偏差为0.63 m/s，驶入右侧巷道后逐渐加速至参考车速。车辆横摆角及横摆角速度如图26所示。车辆驶入交叉路口后车身向右偏转，车辆横摆角速度最大值为0.33 rad/s，即19 （°）/s，驶入右侧巷道后进入稳定的直线行驶状态。由图25和图26可以看出，在整个行驶过程中车辆始终保持较为平稳的行驶状态。
车辆横向偏差及横摆角偏差如图27所示。横向偏差最大值为0.074 m，横摆角偏差最大值为0.16 rad，即9.17°，车辆在整个行驶过程中能够保持较小的跟踪误差。
4.4 普适性验证
对4条路线上节点2、3、12处的交叉路口定位误差进行了测试，验证所提定位算法对于不同类型交叉路口的普适性。车辆行驶轨迹和定位误差曲线如图28−图30所示。分析节点2定位误差曲线可知：刚进入交叉路口时，沿巷道方向定位误差较大，使用自适应阈值的局部定位算法可以在0.6 s时迅速将定位误差收敛至0.15 m以内；车辆处于交叉路口中时，x 轴方向的平均定位误差为0.065 6 m，y 轴方向的平均定位误差为0.127 8 m。在节点3处，x轴方向的平均定位误差为0.009 m，y轴方向的平均定位误差为0.015 m。在节点12处，x轴方向的平均定位误差为0.157 6 m，y轴方向的平均定位误差为0.121 9 m。该定位算法在各种类型交叉路口的定位误差均在0.2 m以内，可以满足自动驾驶的定位精度要求。
由定位误差曲线可以看出，车辆定位精确受车辆在交叉路口中位置的影响，位置越靠近中心，则定位精度越高。当车辆远离巷道时，定位精度降低，且定位偏离方向与巷道方向一致，对于在巷道中进行的车道保持行驶并无影响。
5. 结论
1） 提出了一种基于局部几何−拓扑地图的地下矿自动驾驶定位导航方法。设计了几何地图与拓扑地图相融合的先验局部几何−拓扑地图结构；使用基于激光雷达的交叉路口检测算法与交叉路口定位算法进行车辆全局定位；使用自适应MPC算法进行车辆轨迹跟随控制。
2） 在ROS中实现整套自动驾驶系统，并在三维物理仿真平台中搭建地下矿环境及无人驾驶汽车模型进行联合仿真。结果表明：所提方法能够基于井下环境局部几何−拓扑地图，通过定位算法实现车辆定位功能，最终完成井下的自动驾驶定位导航任务。
3） 下一步研究计划是将测试移到真实条件下，扩展当前的解决方案，增加无人车动态避障路径规划功能，并设计完善的交叉路口多车协同通行方法。
-
![]()
图 11 交叉路口局部定位算法伪代码
Figure 11. Pseudo-code of local positioning algorithm for intersection
![]()
图 22 不同算法的点云配准距离差平均值对比
Figure 22. Comparison of average distance differences in point cloud registration of different algorithms
-
[1] SCHEDING S,DISSANAYAKE G,NEBOT E M,et al. An experiment in autonomous navigation of an underground mining vehicle[J]. IEEE Transactions on Robotics & Automation,2001,15(1):85-95.
[2] DUFF E S, ROBERTS J M, CORKE P I. Automation of an underground mining vehicle using reactive navigation and opportunistic localization[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems, Las Vegas, 2003:3775-3780.
[3] MEECH J,PARREIRA J. An interactive simulation model of human drivers to study autonomous haulage trucks[J]. Procedia Computer Science,2011,6(1):118-123.
[4] SZREK J, ZIMROZ R, WODECKI J, et al. Application of the infrared thermography and unmanned ground vehicle for rescue action support in underground mine-the AMICOS project[J]. Remote Sensing, 2021, 13（1）. DOI: 10.3390/rs13010069.
[5] 张元刚,刘坤,白猛,等. 井下多传感器组合导航系统[J]. 工矿自动化,2019,45(7):10-16. ZHANG Yuangang,LIU Kun,BAI Meng,et al. Underground multi-sensor integrated navigation system[J]. Industry and Mine Automation,2019,45(7):10-16.
[6] 杨林,马宏伟,王岩,等. 煤矿巡检机器人同步定位与地图构建方法研究[J]. 工矿自动化,2019,45(9):18-24. YANG Lin,MA Hongwei,WANG Yan,et al. Research on method of simultaneous localization and mapping of coal mine inspection robot[J]. Industry and Mine Automation,2019,45(9):18-24.
[7] CHEHRI A,FORTIER P,TARDIF P M. UWB-based sensor networks for localization in mining environments[J]. Ad Hoc Networks,2009,7(5):987-1000. DOI: 10.1016/j.adhoc.2008.08.007
[8] 杜京义,郭金宝,张渤. 井下无人驾驶列车惯性导航定位系统[J]. 工矿自动化,2018,44(9):5-9. DU Jingyi,GUO Jinbao,ZHANG Bo. Inertial navigation and positioning system for underground driverless train[J]. Industry and Mine Automation,2018,44(9):5-9.
[9] 周李兵. 煤矿井下无轨胶轮车无人驾驶系统研究[J]. 工矿自动化,2022,48(6):36-48. ZHOU Libing. Research on unmanned driving system of underground trackless rubber-tyred vehicle in coal mine[J]. Journal of Mine Automation,2022,48(6):36-48.
[10] 陈伟. 煤矿井下精确定位系统研究[J]. 工矿自动化,2019,45(12):86-90. CHEN Wei. Research on precise positioning system of coal mine underground[J]. Industry and Mine Automation,2019,45(12):86-90.
[11] 孙继平,江嬴. 矿井车辆无人驾驶关键技术研究[J]. 工矿自动化,2022,48(5):1-5,31. SUN Jiping,JIANG Ying. Research on key technologies of mine unmanned vehicle[J]. Journal of Mine Automation,2022,48(5):1-5,31.
[12] 李论. 基于RSSI的煤矿巷道高精度定位算法研究[D]. 徐州: 中国矿业大学, 2015. LI Lun. Research of precision positioning algorithm based on RSSI in coal mine tunnel[D]. Xuzhou: China University of Mining and Technology, 2015.
[13] 唐丽均,吴畏. 煤矿井下精确定位系统中的节点设计[J]. 煤炭工程,2015,47(6):18-20. TANG Lijun,WU Wei. Node design in accurate positioning system for underground coal mine[J]. Coal Engineering,2015,47(6):18-20.
[14] 游小荣,裴浩,霍振龙. 一种基于UWB的三边定位改进算法[J]. 工矿自动化,2019,45(11):19-23. YOU Xiaorong,PEI Hao,HUO Zhenlong. An improved trilateral positioning algorithm based on UWB[J]. Industry and Mine Automation,2019,45(11):19-23.
[15] RADACINA RUSU S. Real-time localization in large-scale underground environments using RFID-based node maps[D]. Ottawa: Carleton University, 2011.
[16] LARSON E J. Vehicle localization in an underground mine using ultrasonic sensors[D]. Golden: Colorado School of Mines, 2012.
[17] MASCARÓ M,PARRA-TSUNEKAWA I,TAMPIER C,et al. Topological navigation and localization in tunnels-application to autonomous load-haul-dump vehicles operating in underground mines[J]. Applied Sciences,2021,11(14):6547. DOI: 10.3390/app11146547
[18] MARSHALL J,BARFOOT T,LARSSON J. Autonomous underground tramming for center-articulated vehicles[J]. Journal of Field Robotics,2008,25(6):400-421.
[19] HABIB M K, YUTA S. Map representation and path planning for mobile robots in a building environment[C]. IEEE International Workshap on Intelligent Robots, Tokyo, 1988: 173-178.
[20] KUIPERS B,BRUYN Y T. A robot exploration and mapping strategy based on a semantic hierarchy of spacial representations[J]. Robotics and Autonomous Systems,1991,8:47-63. DOI: 10.1016/0921-8890(91)90014-C
[21] LI Qingquan,CHEN Long,ZHU Quanwen,et al. Intersection detection and recognition for autonomous urban driving using a virtual cylindrical scanner[J]. Intelligent Transport Systems,2014,8(3):244-254. DOI: 10.1049/iet-its.2012.0202
[22] 崔文,薛棋文,李庆玲,等. 基于三维点云地图和ESKF的无人车融合定位方法[J]. 工矿自动化,2022,48(9):116-122. CUI Wen,XUE Qiwen,LI Qingling,et al. Unmanned vehicle fusion positioning method based on 3D point cloud map and ESKF[J]. Journal of Mine Automation,2022,48(9):116-122.
[23] BESL P J,MCKAY H D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence,1992,14(2):239-256.
[24] 顾文华,周波,戴先中. 基于ICP匹配算法的室内移动机器人定位[J]. 华中科技大学学报(自然科学版),2013,41(增刊1):262-266. GU Wenhua,ZHOU Bo,DAI Xianzhong. ICP matching algorithm-based localization of indoor mobile robots[J]. Journal of Huazhong University of Science and Technology(Natural Science Edition),2013,41(S1):262-266.
[25] BIBER P, STRASSER W. The normal distributions transform: a new approach to laser scan matching[C]. IEEE/RSJ International Conference on Intelligent Robots & Systems, Las Vegas, 2003: 2743-2748.
[26] 余洪山,付强,孙健,等. 面向室内移动机器人的改进3D−NDT点云配准算法[J]. 仪器仪表学报,2019,40(9):151-161. YU Hongshan,FU Qiang,SUN Jian,et al. Improved 3D-NDT point cloud registration algorithm for indoor mobile robot[J]. Chinese Journal of Scientific Instrument,2019,40(9):151-161.
[27] CHEN B, TSAI C, LEE K. Path-following steering controller of automated lane change system with adaptive preview time[C]. IEEE International Conference on Systems, Man, and Cybernetics, Hong Kong, 2015: 2522-2526.
-
期刊类型引用(2)
1. 胡青松，李鹤，罗渝嘉，王柳笛，寇志豪，李世银. 多径与非视距影响下的精确目标定位关键技术. 金属矿山. 2024(11): 173-184 . 
百度学术
2. 王佳恒，王天鑫，王晋淼. 基于体素的地下矿山三维点云地图增量式更新方法. 采矿技术. 2024(06): 243-247 . 百度学术
其他类型引用(6)
下载:
计量
- 文章访问数: 134
- HTML全文浏览量: 78
- PDF下载量: 31
- 被引次数: 8
