为了满足低成本和批量化装车的需要，本文选用览沃激光雷达开展实验。如图 1所示，作为一款非重复式扫描的半固态激光雷达，其视场中的点云覆盖率会随着积分时间的增大而增加，从而扫描到更多场景特征。

图  1  览沃激光雷达非重复扫描结果

Figure 1.  Illustration Results of Scan Pattern of Livox Horizon

由于览沃激光雷达的视场角受限，本文采用多激光雷达安装方式拓宽了系统的检测范围，提升了系统冗余度，如图 2所示。普通运营车辆仅需安装两个前视的浩界激光雷达，而轨道检测车辆需要装备8个雷达，该平台分为3个模块，前视模块由2个浩界雷达组成，左右两侧模块都由1个泰览雷达和2个浩界雷达组成。

图  2  轨道检测车辆设计

Figure 2.  Design of Rail Maintenance Vehicle

作为一个拥有多探测传感器的系统，各传感器之间的外参需要精确标定到统一坐标系下才能应用。虽然对基于点云特征和环境标志的多雷达自动标定方法的研究较多，但是这些方法大都要求各个雷达间有一定的重复视场。考虑到本文设计的平台多雷达间并无重复视场，而且由于铁路安全规定限制了标定，只能在狭小的维护车间内进行，因此设计了一种基于参数传递的多雷达标定方法。首先，两个前视雷达与全景相机间采用基于PnP（perspective-n-point）^[8]算法的标定方法进行雷达到相机的外参标定；然后，对全景相机各相机的内参及各相机间的外参进行标定，同时完成左右两侧模块中3个激光雷达的外参标定；最后，由于全景相机覆盖了前视两个雷达和左右侧前视雷达的视场，只需要利用PnP算法标定全景相机和左右侧模块中两个前视雷达的外参，即可确定整个系统中所有雷达相对于全景相机的外参，达到标定整个系统的目的。系统中的两款组合导航设备则由全站仪进行与全景相机之间的外参标定。

利用工业交换机将多激光雷达连接到同一网段至主机，采用基于秒脉冲（pulse per second，PPS）信号的GPS时间同步。多雷达间的同步策略如图 3所示，设雷达$P^{\mathrm{1}}$为主雷达，其他7个雷达为从属雷达。对于系统中的每一个雷达，其单帧测量周期为0.1 s，考虑到时间延迟等因素的影响，从属雷达不能与主雷达严格同步。因此本文定义8个雷达合并后扫描帧$F_n$的起始时间为主雷达每帧的起始时间，而结束时间为最后一个从属雷达帧的结束时间。按照时间将惯性测量单元（inertial measurement unit，IMU）和实时动态差分定位（real time kinematic，RTK）观测值分配给不同雷达扫描帧作运动补偿。

图  3  多雷达同步策略

Figure 3.  Synchronization Policy of Multi-LiDAR System

本系统首先将RTK观测的经纬度转换为横轴墨卡托投影坐标系，再输入多雷达的定位建图系统参与优化。

假设在任意时刻，轨道车辆的状态$\mathit{x}$可以表示为：

式中，$\mathit{p}$、$\mathit{v}$、$\mathit{q}$分别表示车辆的位置、速度、姿态矩阵；${\mathit{b}}_a$和${\mathit{b}}_g$分别表示IMU中的加速度计和陀螺仪噪声。

整个系统利用输入的多个激光雷达的原始点云、IMU和RTK的位姿进行车身的状态估计。本文采用基于图优化的紧组合策略融合所有传感器参与定位建图任务。因子图中包含因子和节点，其中因子代表各种传感器的观测输入，而节点则是车身的状态信息。本文采用的因子图结构如图 4所示，共包括3种因子，分别为雷达里程计因子、IMU预积分因子和RTK因子。每当轨道车辆运行距离超过一个阈值，就有一个新的位姿节点加入因子图。在整个因子图更新的同时，所有节点、位姿都进行了优化。

图  4  采用的因子图结构

Figure 4.  Structure of the Applied Factor Graph

由于轨道检测车辆运营时间长、里程远，为了确保系统中的因子数量不随时间增加而大幅度增加，从而降低整个系统运行效率，本系统采用了基于关键帧的滑动窗口法使参与优化的因子限制在一定数量内，极大地提高了系统效率和长时间运行的稳定性。

由文献[9]可知，IMU预积分的残差计算如下：

式中，${\widehat{\mathit{z}}}_{k+\mathrm{1}}^k={[\begin{array}{ccc}{\widehat{\mathbf{\alpha }}}_{{\mathrm{B}}_{\mathrm{k}+\mathrm{1}}}^{{\mathrm{B}}_{\mathrm{k}}}& {\widehat{\mathbf{\beta }}}_{{\mathrm{B}}_{\mathrm{k}+\mathrm{1}}}^{{\mathrm{B}}_{\mathrm{k}}}& {\widehat{\mathbf{\gamma }}}_{{\mathrm{B}}_{\mathrm{k}+\mathrm{1}}}^{{\mathrm{B}}_{\mathrm{k}}}\end{array}]}^{\mathrm{T}}$为从k到k+1时刻的IMU预积分残差，${\widehat{\mathit{\alpha }}}_{B_{k+\mathrm{1}}}^{B_k}、{\widehat{\mathit{\beta }}}_{B_{k+\mathrm{1}}}^{B_k}、{\widehat{\mathit{\gamma }}}_{B_{k+\mathrm{1}}}^{B_k}$分别表示位移、速度、姿态方向上的变化；上角标$W、B$分别$\mathrm{表}\mathrm{示}$世界坐标系、轨道车辆的车身坐标系；${\mathit{R}}_W^{B_k}$表示从世界坐标系转化为车身坐标系的旋转矩阵；${\mathit{g}}^W={[\begin{array}{ccc}\mathrm{0}& \mathrm{0}& g\end{array}]}^{\mathrm{T}}$表示重力方向矩阵；$∆t_k^{}$代表k到k+1时刻的时间变化；符号$\otimes$表示两个四元数间的乘法运算；${[]}_{\mathrm{2}:\mathrm{4}}$表示从四元数中取$[xyz]$分量。

本系统采用文献[10]中基于曲率大小的特征提取方式提取平面和边缘特征点，两帧间的特征点关联计算如下：

式中，$d_{E_k}$、$d_{S_k}$分别代表第k+1帧中的任意边缘、平面特征点到第k帧中的对应关系；${\mathit{P}}_k^W$表示累积到第k帧时刻的全局地图中的点坐标；$[\begin{array}{cc}{\mathit{ϵ}}_{\mathrm{1}}^W& {\mathit{ϵ}}_{\mathrm{2}}^W\end{array}]$、$[\begin{array}{ccc}{\mathit{\eta }}_{\mathrm{1}}^W& {\mathit{\eta }}_{\mathrm{2}}^W& {\mathit{\eta }}_{\mathrm{3}}^W\end{array}]$分别表示第k+1帧中提取出的与${\mathit{P}}_k^W$最邻近的边缘、平面点坐标。由此得到雷达里程计的残差因子${\mathit{r}}_{L_i}=\sum {\mathit{d}}_{E_k}+\sum {\mathit{d}}_{S_k}$。由于览沃激光雷达的棱镜式设计问题，其在0.5 m内没有点云信息返回，且在3 m内的测距误差高于分米级，因此本文只选取每一帧中距离激光雷达超过3 m的点参与估计。

系统初始在开阔场地静置数分钟，在获得RTK固定解后将其设为初始点，后续所有位姿都是基于该初始点的车辆坐标系进行优化。由于RTK数据的加入会使得系统当前维护的因子图更新，为了提高系统的运行效率，本系统仅在系统估计的位置协方差大于RTK预报的位置协方差时，将RTK观测加入因子图进行优化。

由于轨道车辆运行速度过快，当遇到特征退化区域时，雷达里程计不能正常工作。此时RTK观测可以作为初始估计进行两帧间基于迭代最近邻点法（iterative closest point，ICP）的点云配准。

实验采用搭载Ubuntu-Linux操作系统的大疆妙算2工控机作为车载计算平台，选用飞纳经纬的MiniII-D-INS组合导航设备输出RTK和IMU观测值，将迈普时空的M39设备后处理结果作为系统的位姿真值，并利用千寻的RTK服务提供两个组合导航设备的RTK改正。

利用SLAM技术建图可以实时可视化点云地图，且列车结束行程时便可得到全局点云地图，极大地提高了地图更新速度。图 5介绍了几种实验常见情况的实时建图输出。

图  5  铁路上几种典型场景实时建图结果

Figure 5.  Real Time Mapping Results of Typical Scnearios Using Our Proposed System

由图 5可以看出，系统实时输出的结果可用于环境地图更新与监测，线缆和铁轨清晰可见，可以直接进行轨道提取和异物侵限检测等后续工作。

迈普时空的M39组合导航设备配合GINS后处理软件可以进行高精度的组合导航后处理解算，本文将该处理结果作为地面真值。由于所有激光雷达都与GPS时间同步，时间戳都是GPS时，因此只需比较相同时间SLAM系统输出的位姿和后处理结果便可验证定位精度。

由于SLAM系统受到速度及场景特征丰富性的影响，本文选取4种场景进行系统定位精度对比实验，结果如表 1所示。场景A为低速特征丰富场景，轨道检测车辆缓缓进入保养段内停车，速度保持在10 km/h以下，周围铁轨、杆塔、建筑及植被等特征丰富；场景B为低速特征稀疏场景，轨道检测车辆进入单轨缓速行驶等待停车指令，周围5 m范围内均为灌木植被，特征稀疏，车速保持在20 km/h以下；场景C为高速特征丰富场景，车辆高速驶过站台，车速保持在60 km/h以上；场景D为高速特征稀疏场景，车辆高速行驶在单轨上，车速保持在70 km/h以上，周围5 m范围内均为灌木植被，特征稀疏。

由表 1可以看出，本文系统在低速特征丰富的环境下能达到厘米级的定位精度，而最差时候的精度也能保持在米级，对于非精细建图任务来说，这是一个足够可用的精度。

传统基于MMS的建图方法成本高、难以批量化装车、处理程序复杂，且难以满足地图更新的时效性需求。本文基于多个棱镜式雷达搭建了新型铁路扫描系统，并且开发了一套多雷达/IMU/RTK紧组合SLAM系统。该系统集成了多个激光雷达的点云/IMU/RTK结果，利用图优化方法紧耦合各个传感器信息，构建因子图进行状态估计和全局地图构建，在工控机上可以实时运行。在实时地图可视化的同时，满足了作业完毕即可完成地图更新的需求。经过验证，即使在高速和特征稀疏场景下，该系统仍能达到分米级定位精度。本系统目前已经在轨道检测车辆上测试了超过300 km的里程，鲁棒性和系统稳定性得到了进一步验证。基于此系统，后续可以继续开展轨道环境地物自动提取、基于unity的增强现实和虚拟现实展示技术开发。