施工隧道多传感器融合定位方法研究.pdf

犇 犗 犐：施工隧道多传感器融合定位方法研究黎胜根，向宙，肖正航，崔昌（中国铁建重工集团股份有限公司，湖南 长沙 ）摘要：针对隧道环境线面几何特征少且里程方向退化问题，提出一种融合激光雷达、惯性测量单元（）、轮速计、超宽带（）等多传感器信息的误差状态卡尔曼滤波（）定位方法，并根据隧道结构的特殊性提出隧道结构化点云地图生成方法；根据隧道施工台车点云模型及里程，结合隧道设计数据生成隧道结构化点云地图，将雷达扫描点云与地图通过正态分布变换（）算法进行匹配；将点云匹配结果与其他传感器信息一起对 的 递推方程进行观测更新，降低定位系统非线性的影响。与 定位方法进行对比分析，结果显示 定位方法能解决隧道环境几何特征少带来的前端匹配精度低的问题，且多传感器融合可降低里程方向定位误差，提高系统整体定位精度。关键词：隧道；激光雷达；多传感器融合；误差状态卡尔曼滤波（）；正态分布变换（）中图分类号：文献标志码：文章编号：（）基金项目：湖南省创新型省份建设专项经费资助（）；中国铁建股份有限公司年度科研计划（）传统的主要依靠人力的隧道施工方式作业效率低、事故率高，已无法满足现代化建设需求，少人化、无人化的施工作业模式是未来隧道施工的发展方向。无人驾驶技术可实现车辆在无人操作情况下从始发地到目标地，赋予隧道装备无人驾驶能力进而实现隧道施工无人化目标。在隧道环境中实现无人驾驶，最大难题是车辆在隧道中的定位。在城市道路环境下，无人驾驶汽车的定位主要依靠全球导航卫星系 统（），然而 隧 道内 为 全 球 定 位 系统（）拒止环境，必须依靠室内定位方式获取位置和姿态信息。隧道中常见定位方式主要是测量 （射频识别）、超宽带（）、等信号的到达时间或信号强度值，这些方式的定位精度受遮挡及距离等环境影响过大，平均精度为亚米级。彭学军等采用构建隧道定位系统实现人员定位，但不足以用于隧道施工装备的自动驾驶。多传感器融合定位无疑是隧道环境高精度定位的最佳选择。张辰东等采用惯性导航系统（）与 进行组合定位，列车在隧道中高速运动状况下仍能保持米级定位精度。然而施工隧道环境复杂多变，且自动驾驶施工装备定位级别要求高。激光雷达在无人驾驶领域应用广泛，可快速获取精准、全面的环境信息，在隧道环境中加入激光雷达信息可有效提高定位精度。江勇等在类似于隧道的长直炉膛中基于 算法融合三维激光雷达、气压计信息进行定位，定位误差可控制在以内，证明融合雷达信息可有效提高定位精度。基于雷达的定位研究是自动驾驶定位技术的研究热点，在 提出后，许多研究者针对各种环境及硬件对其进行了优化。为降低运算成本，文献 提出采用两步法配准点云，减小建图量级并增加因子图优化，将 优 化 为 轻 便 型 的 。文献 提出 ，它适用于 固态激光雷达。基于 的定位方法都避免不了根据扫描点云提取线面特征，然而隧道形状为圆柱面，隧道内可用的其他线面特征非常少，线面特征提取方法不适用于隧道。文献 采用正态分布变换（）算法实现扫描点云与地图配准，该方法不依赖线面特征，更适用于隧道环境。隧道环境属于退化环境，仅依靠激光雷达信息无法确定车辆在里程方向上的位置。针对施工隧道环境特殊性带来的定位难问题，本 文 提 出 一 种 融 合 激 光 雷 达、惯 性 测 量 单元（）、轮速计、的施工隧道定位方法。点云数据处理结构化隧道点云地图生成隧道线位姿确定隧道施工时，其设计数据已经完善，因此隧道中公路与汽运总第 期犎 犻 犵 犺 狑 犪 狔 狊犃 狌 狋 狅 犿 狅 狋 犻 狏 犲犃 狆 狆 犾 犻 犮 犪 狋 犻 狅 狀 狊某一里程点的姿态是确定的，一般由平曲线与竖曲线决定。已知隧道起始点的半径为犚狅，隧道结束点的半径为犚犲，某一位置狆的里程为犾，采用四节点 公式逼近隧道线进行积分，则该点的位姿为：狓狆狓狅犾狀犻犚犻 狅犓狅犾 狏犻犓狅 犲犾犾狏犻（）狔狆狔狅犾狀犻犚犻 狅犓狅犾 狏犻犓狅 犲犾犾狏犻（）狆狅犓狅犾犓狅 犲犾犾狏犻（）狕狆狓狆犿烅烄烆（）式中：狓狆、狔狆、狕狆、狆为该点坐标及航向角度；狓狅、狔狅、狕狅、狅为 坐 标 及 航 向 角 度 起 始 值；犚犻、狏犻为 的节点参数；犓狅、犓狅 犲分别为起始点曲率和起始点与终止点的曲率差；犾为曲线总长；犿为竖曲线抛物线系数。由于隧道高程变化一般在千分之几，忽略俯仰角及横滚角的变化，即可确定隧道线上某一确定点的位姿。地图生成隧道施工中，可扫描获得的点云分为种，分别为隧道施工作业台车点云狆、初期支护段隧道壁面点云狆、二次衬砌段点云狆。通过台车里程推算台车在隧道坐标系下位姿犜，结合隧道与地图坐标关系犜，得到台车在地图坐标系下的点云狆。按同样的方法可将初期支护段点云转化到地图坐标系下。二次衬砌段轮廓和隧道设计轮廓非常接近，且二次衬砌段里程一般最长，直接通过隧道设计数据生成二次衬砌段点云并转化到地图坐标系下。按式（）合成以上种点云即可获得隧道点云地图（见图 ）。狆犜犜狆（）图隧道结构化地图流程图二次衬砌台车点云图防水板台车点云图栈桥点云图喷拱台车点云图结构化生成的点云地图犖 犇 犜点云匹配 算法的基本原理点云匹配方法分为两类，其中一种是通过扫描点云的几何特征配准两帧点云，这种方法突出特征点云的权重，过滤点云的其他非特征点，在几何特征明显的环境中往往能表现出较好的匹配效果。但隧道环境几何特征较少，因此采用另一种点云匹配方法即全点云匹配方式。考虑到迭代最近点（）算法匹配效果受相对运动影响较大且受限于初值质量，极有可能陷入局部优化，选择 算法匹配当公路与汽运 年月前帧扫描点云与地图点云，获得运动车辆在地图坐标系下的位姿最佳估计值。算法基于概率统计学原理，对于样本狓犽；犽，狀，先将参考点云进行网格化，获得三维空间中最小单元体素犮犻；然后按式（）、式（）计算每个犮犻内的正态分布模型犖（，）（为均值，为标准差）；将目标点云中每个点按照位姿投影到参考点云的每个体素中，按式（）计算每个点在每个体素中的概率得分狊。狀狀犽狓犽（）狀狀犽（狓犽）（狓犽）（）狊 （狓）（狓）（）待计算目标点在参考点云正态分布模型下的最大似然估计为：犿犼犳（犜（狆，狓）（）式中：犜（狆，狓）为转换函数；犳为概率密度函数。对优化目标取负对数，得：狀犽（犳（犜（狆，狓犽）（）忽略常数部分，优化目标为：犜犿犼（犜（狆，狓犼）犼）犼（犜（狆，狓犼）犼）（）采用最小二乘法求解优化目标即可得到匹配结果。扫描点云与隧道地图的 匹配由于隧道点云地图需要消耗大量内存资源，将地图进行分段处理，通过位姿初值及当前帧扫描点云范围确定所选地图区域；将当前帧扫描点云插入 树（）中，遍历所有扫描点并找到与每个扫描点距离最近的体素中心，确定扫描点所在的体素编码；建立当前帧点云的 匹配优化模型，扩展单点向量为点云空间向量，优化模型为：犜（犜 犡犝）犈（犜 犡犝）（）式中：犜为转换矩阵；犡为当前帧所有点组成的三维点云向量；犈为点云向量对应的协方差矩阵，又称信息矩阵；犝为点云均值。鉴于模型犉函数对犜的雅可比矩阵犑与犈均为稀疏矩阵，可降低 逆矩阵的求解难度，采用 算法（法）求解模型的最优匹配估计位姿（见图）。图当前帧点云与地图犖 犇 犜匹配犈 犛 犓 犉多传感器信息融合犈 犛 犓 犉递推模型优化位姿估计是一个非线性系统，普通的卡尔曼滤波（）只能对线性系统进行求解。扩展卡尔曼滤波（）将非线性系统通过一阶泰勒展开进行线性化，忽略了二次项之后的估计值，会造成较大估计误差。误差状态卡尔曼滤波（）算法将一个系统的状态空间分为种，分别为系统状态的真实值、名义值即不考虑系统噪声的推算值、真实值与名义值之差即误差值，系统的非线性程度集中于名义值，相对而言，误差值的线性化程度高，对误差值进行卡尔曼估计的准确性远高于直接对名义值进行估计，通过误差值与名义值之和得到的系统物理状态值往往具有更高的精度。通过 航迹推算 模型，变量名称及符号见表。表变量名称及符号物理名称真实状态值名义状态值误差状态值状态空间犚 狓狋狓狓位移犚狊狋狊狊速度犚狏狋狏狏姿态四元数犚狇狋狇狇旋转矩阵（）犚狋犚犚三轴角犚狋加速度计零犚犪犫 狋犪犫犪犫陀螺仪零偏犚犫 狋犫犫注：犚代表实数；（）表示李群中的三维旋转群。年 第期黎胜根，等：施工隧道多传感器融合定位方法研究 的真实值连续状态方程为：狊 狋狏狋狏 狋犚狋（犪犿犪犫 狋犪狀）犵狋狇 狋狇狋（犿犫 狋狀）犪 犫 狋犪 犫 狋烅烄烆（）式中：犪犿为加速度测量值；犵为重力加速度；犿为角速度测量值；犪狀为加速度测量误差；狀为角速度测量误差；代表四元数乘法；犪为加速度计零偏；为陀螺仪零偏。名义值的状态方程忽略了噪声及零偏方向的影响，其形式为：狊 狏狏 犚（犪犿犪犫）犵狇 狇（犿犫）犪 犫 犫烅烄烆（）误差状态值为真实值与名义值的差值，其形式为：狊狏 狏犚（犪犿犪犫）犚 犪犫犚 犪狀（犿犫）犫狀 犪 犫犪 犫烅烄烆（）将连续状态离散化，则名义状态值的状态方程为：狊犽 狊犽狏犽狋犚犽（犪犿 犽犪犫 犽）犵狋狏犽 狏犽犚犽（犪犿 犽犪犫 犽）犵狋狇犽狇犽狇犽（犿 犽犫 犽）狋犪犫（犽）犪犫 犽犫（犽）犫 犽烅烄烆（）误差状态值的离散状态方程为：狊犽狊犽狏犽狋狏犽犚犽（犪犿 犽犪犫 犽）犽犚犽犪犫 犽狋狏犽狑狏犽犽犚犽（犿 犽犫 犽）狋犽犫 犽狋狑 犽犪 犫（犽）犪犫 犽狑犪 犽 犫（犽）犫 犽狑 犽烅烄烆（）式中：犽表示离散时刻；狑狏 犽、狑 犽、狑犪 犽、狑 犽分别表示加速度与角速度系统噪声导致的速度误差分量、姿态误差分量、加速度计零偏误差分量、陀螺仪零偏误差分量，其协方差矩阵表示为：犠犞犪狀狋犐犠狀狋犐犠犃犪狋 犐犠狋 犐式中：犐为单位向量。令：狓犽狊犽，狏犽，狇犽，犪犫 犽，犫 犽，狓 犽狊犽，狏犽，狇犽，犪犫 犽，犫 犽，狌犿 犽犪犿 犽，犿 犽，狑犽狑狏 犽，狑 犽，狑犪 犽，狑 犽，得到名义状态变量的递推方程：狓犽犳（狓犽，狌犿 犽）（）将误差状态方程进行卡尔曼滤波的线性化，获得误差状态值的一阶泰勒展开线性化近似值递推方程：狓犽犳（狓犽，狓犽，狌犿 犽，狑犽）犉犡 犽（狓犽，狌犿 犽）狓犽犉犠 犽狑犽（）式中：犉犡 犽、犉犠 犽为误差传递函数对误差状态和噪声的雅可比矩阵。误差状 态 值 的 卡 尔 曼 滤 波 状 态 及 协 方 差 方程为：狓犽犉犡 犽（狓犽，狌犿 犽）狓犽犘犽犉犡 犽犘犽犉犡 犽犉犠 犽犙犠犉犠 犽（）式中：犘犽为 估 计 值 的 协 方 差 矩 阵；犙犠为 协 方 差矩阵。模型的雅可比矩阵及噪声协方差为：犉犡 犽犐 犐狋犐犚犽（犪犿 犽犪犫 犽）狋犚犽狋犚犽（犪犿 犽犪犫 犽）狋犐狋犐犐熿燀燄燅犉犠 犽 犐 犐 犐 犐熿燀燄燅犙犠犠犞犠犠犃犠熿燀燄燅公路与汽运 年月多传感器信息观测更新自动驾驶中一般使用低精度的 惯性测量单元，其零漂及精度决定仅依靠惯性测量单元进行定位极不准确，必须加入其他传感器信息对惯性测量单元的预测估计值进行优化来提高定位精度。为此，在定位系统中加入激光雷达、轮速计、种传感器信息作为观测对 预测进行优化。考虑到隧道环境在里程方向的退化及轮速计受轮胎打滑的影响，加入进行约束。激光雷达狕犾通过 匹配对位姿进行估计，轮速计狕狏对车辆前进方向速度进行估计，获得基站与车体安装标签犿之间的距离狕犱，则观测方程为：狕犾犽狊犽犚犽狆犾犚犽熿燀燄燅狕狏犽 狏犽狕犱犽（犚犽狆犿狊犽狌）（犚犽狆犿狊犽狌）烅烄烆（）式中：狆犾、狆犿分别为雷达和标签到惯性测量单元坐标系的杆臂长；狌为基站在全局坐标系中的位置。定位递推方程为：狕犽犺（狓犽）狑狕 犽（）式中：狕犽为观测值；犺为观测函数；狑狕 犽为测量白噪声。将式（）根据递推方程进行线性化，得到观测对误差状态值的雅可比矩阵犎犽，则卡尔曼滤波后验更新为：犓犽犘犽犎犽（犎犽犘犽犎犽犞犽）狓犽犓犽（狕犽犺（狓犽）犘犽（犐犓犽犎犽）犘犽烅烄烆（）通过链式法则对误差状态值求一阶雅可比矩阵，其值为：犎犺狓狋狓狋狓狋犎犡犃 狓式中：犎犡见式（）；犎犞；犃 狓为名义值对误差状态值的雅可比矩阵 见式（）。犎犡犐犚犽狆犾 犎犞（犚犽狆犿）狊犽狌（狊犽狌）（犚犽狆犿）熿燀燄燅（）狓狇狓狇狔狇狕狇狑狇狕狇狔狇狕狇狑狇狓狇狔狇狓狇狑熿燀燄燅（）真实估计值为：狓犽狓犽狓犽（）式中：为姿态更新符号。更新后将误差状态值及误差协方差进行复位，用于下一次迭代。试验结果分析在宜昌市老林岗隧道进行试验。该隧道进深约为 ，隧道场景见图，具备所有施工隧道应有的环境要素。试验平台为一台自动驾驶底盘，在移动平台上搭载禾塞 机械式激光雷达及 固态激光雷达、北微 惯性测量单元，移动平台自带轮速计输出实时速度（见图）。在隧道壁布置清研讯科基站，标签安装在平台上。图隧道场景图试验平台将平台前轴中心作为车体坐标系原点、前进方向作为坐标系犡轴正方向、向上作为犣轴正方向、向左作为犢轴正方向，采用徕卡全站仪对各传感器进行标定。确定地图原点，通过已知隧道坐标点与 输出坐标点标定隧道坐标系与地图坐标系的关系。采集平台按预定路径从隧道入口驶入掌子面过程中的定位数据，采用 定位方法与 融合定位方法进行对比分析，输出频率为 。试验结果见图 和表。年 第期黎胜根，等：施工隧道多传感器融合定位方法研究图 犔 犗 犃犕方法定位结果图 犈 犛 犓 犉方法定位结果图 两种方法在犡向的定位误差图 两种方法在犢向的定位误差图 两种方法在犣向的定位误差表两种方法定位误差对比项目定位方法平均值最大值犡向误差 定位 融合定位 犢向误差 定位 融合定位 犣向误差 定位 融合定位 由图、图 可知：方法在隧道径向的定位效果较好，但扫描点云中存在横洞而地图中没有，说明里程方向存在较大误差。由图 和表可知：采用 方法进行定位，犢向、犣向误差大部分在以上，受里程方向退化环境的影响，犡方向定位误差最大达 ，隧道施工中采用这种定位方法存在极大风险；采用 传感器融合定位方法进行定位，犢向、犣向误差降低至厘米级，犡方向定位误差相较于 方法也极大降低，最大定位误差下降到 ，满足隧道施工使用要求。方法定位精度低的原因是隧道线面特征少，且受退化环境 的 影 响。传 感 器 融 合 定 位 方 法 通 过 匹配获取点云和地图匹配结果，提高了前端处理精度，并通过和轮速计约束里程方向位置，融合后定位精度比任意单一传感器的定位精度都高，且相比于普通 算法，采用 算法能有效降低系统非线性对定位精度的影响。结语本文针对施工隧道圆拱形轮廓环境线面特征少及里程方向退化无法定位的问题，设计一种施工隧道多传感器融合定位方法。采用根据扫描台车点云模型和隧道设计数据及里程生成结构化地图的方法，根据生成的地图与雷达扫描点云进行 匹配，解决隧道环境线面特征少带来的前端匹配精度低的问题；将多种传感器信息通过 算法进行融合，解决隧道环境特征退化导致里程方向定位精度差的问题；对误差值进行滤波，降低定位系统非线性误差。试验验证结果表明，多传感器融合定位方法适用于施工隧道定位，其定位精度可满足施工隧道使用要求。（下转第 页）公路与汽运 年月法。编写基于 平台的二次开发插件，实现对桥梁病害记录时间、病害种类、受损等级等病害信息的查询；将病害引入桥梁实体模型，以病害的记录时间为维度建立桥梁病害族，将历史、受损程度加深、新增病害以不同图元形式展示在桥梁三维可视化模型中，实现桥梁病害与实体模型在时间节点上的动态关联；建立基于 平台的桥梁病害数据库，添加病害的局部受损图片，将每次巡查记录的不同类型病害信息数据通过 传入数据库中，确保桥梁病害信息的更新与数据的完整性。参考文献：，：，（），（）：，（）：李沅璋，赵月悦福厦铁路泉州湾特大桥工程 技术应用研究铁路技术创新，（）：高增奎 技术在南昌港口大道标市政桥梁施工中的应用铁路技术创新，（）：李成涛，章世祥基于 技术的桥梁病害信息三维可视化研究公路，（）：二次开发基础教程上海：同济大学出版社，周文哲，刘晓平，游涛，等 技术在混凝土施工温控监测中的应用研究交通科学与工程，（）：施博文，许红胜，颜东煌，等 正向设计中建模与分析软件间信息传递研究公路与汽运，（）：卫星，邹建豪，肖林，等基于 的钢桁梁桥裂纹病害信息 数 字 化 管 理 西 南 交 通 大 学 学 报，（）：杨洋，何飞，李洁公路桥梁病害分类与编码构建方法北方交通，（）：钟辉，李驰，孙红，等面向 模型二次开发数据提取与应用技术沈阳建筑大学学报（自然科学版），（）：吴基于 的桥梁检测信息管理与智能评估系统厦门：厦门大学，收稿日期：櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙櫙（上接第 页）参考文献：王志坚郑万高铁隧道智能化建造技术研究及展望隧道建设（中英文），（）：马二顺，李金昆，修春娣，等隧道定位方法综述 中国高科技产业化研究会智能信息处理产业化分会第十五届全国信号和智能信息处理与应用学术会议论文集北京：中国高科技产业化研究会智能信息处理产业化分会，：彭学军，刘云龙，罗运杰，等基于的隧道高精度定位系统研究公路与汽运，（）：张辰东，王兆瑞，金声震，等基于 的隧道列车高 精 度 定 位 方 法北 京 航 空 航 天 大 学 学 报，（）：汪勇，华志刚，陈建国，等锅炉密闭空间无人机定位导航系统设计动力工程学报，（）：，：（），：，：，（），：，（）：收稿日期：公路与汽运 年月