CN115127547B - 一种基于捷联惯导***和图像定位的隧道检测车定位方法 - Google Patents

Abstract

本发明一种基于捷联惯导***和图像定位的隧道检测车定位方法，先求隧检车在地球表面的纬度和经度；隧检车前端和后端的单目面阵相机先后对隧道内各车道间的间断虚线进行拍摄，两次间断虚线相同，以标识线端点和相机底部中心点为特征点对隧检车的横摆角解算，横摆角和编码器的位置量融合处理，再结合隧检车进入隧道的初始时刻位置，得到隧检车在地球坐标系下隧道内的运动轨迹；将纬度转化成地球坐标系下的数值记为M，将M与X比较，绝对值记为O，将经度转化成地球坐标系下的数值记为N，将N与Y比较，绝对值记为P，当O和P均在设定误差值内，用M和N定位隧检车，否则将M和N、X和Y用卡尔曼滤波器融合，融合后的数值定位隧检车。

Description

一种基于捷联惯导***和图像定位的隧道检测车定位方法

技术领域

本发明属于隧道检测定位领域，具体为一种基于捷联惯导***和图像定位的隧道检测车定位方法。

背景技术

随着交通运输行业的持续发展，隧道的数量和长度不断增加。对隧道进行定期的检测是公路正常运行和人身安全的重要保障。传统的隧道检测通常是人工作业，检测时间长且往往需要封锁隧道，具有效率低、检测强度大、加重交通压力等缺陷。

现有技术有使用搭载单目面阵相机、激光器、可见光等装置的隧道检测车对隧道内壁进行检测。然而在较长的隧道内，卫星信号较弱甚至几乎没有，依靠卫星来确定隧道检测车的位置在实际隧道检测中很难实现。

发明内容

针对现有技术中存在的问题，本发明提供一种基于捷联惯导***和图像定位的隧道检测车定位方法，能准确反映隧道检测车的运载方位，实现隧道检测车在隧道内的快速检测。

本发明是通过以下技术方案来实现：

一种基于捷联惯导***和图像定位的隧道检测车定位方法，包括如下步骤：

S1，先利用加速度计得到隧道检测车的位置更新微分方程，之后求解所述的位置更新微分方程，得到隧道检测车在地球表面的纬度和经度；

隧道检测车的前端单目面阵相机先对隧道内各车道间的间断虚线进行拍摄，隧道检测车的后端单目面阵相机之后对隧道内各车道间的间断虚线进行拍摄，两次拍摄的间断虚线相同，然后以标识线端点和单目面阵相机底部中心点为特征点对隧道检测车的横摆角进行解算，将得到的横摆角和编码器的位置量进行融合处理，再结合隧道检测车进入隧道的初始时刻位置，得到隧道检测车在地球坐标系下隧道内的运动轨迹，运动轨迹为X和Y的点集，其中X对应隧道检测车在地球坐标系下x方向的数值，Y对应隧道检测车在地球坐标系下y方向的数值；

S2，将隧道检测车在地球表面的纬度转化成地球坐标系下的数值后记为M，将M与S1所述运动轨迹中的X进行比较，所得的绝对值记为O，将隧道检测车在地球表面的经度转化成地球坐标系下的数值后记为N，将N与S1所述运动轨迹中的Y进行比较，所得的绝对值记为P，当O和P均在设定误差值以内，用M和N定位隧道检测车，当O和P中至少有一个大于设定误差值，将M和N，以及S1所述运动轨迹中的X和Y在卡尔曼滤波器中进行融合，用融合后的数值定位隧道检测车。

优选的，S1中所述的位置更新微分方程如下所示：

其中，L为隧道检测车在地球表面的纬度，v_y为隧道检测车在坐标系b中y₁方向上的速度分量，R_M为隧道检测车所在点对应地球子午圈的半径，h为海拔高度；λ为隧道检测车在地球表面的经度，v_x为隧道检测车在坐标系b中x₁方向上的速度分量，R_N为隧道检测车所在点对应卯酉圈的曲率半径；

在坐标系b中，以隧道检测车的质心为原点，沿隧道检测车轴线向右为x₁轴正方向，沿垂直x₁轴向前为y₁轴正方向。

优选的，S1中前端单目面阵相机、后端单目面阵相机提取两幅相同图像的特征点集，分别记为N₁、N₂，之后找出N₁中任意一个特征点a与N₂中欧氏距离最小和次最小的特征点b和特征点c，特征点a和特征点b的欧氏距离、特征点a和特征点c的欧氏距离分别为d₁、d₂。若d₁和d₂的比值小于阈值则判定特征点a和特征点b为匹配点对，记为(a,b)，按照相同的过程得到两幅图像的匹配点对集合，之后剔除所述匹配点对集合中的误匹配，再选取标识线端点为特征点。

进一步，从所述匹配点对集合中找出任意一个特征点与其他匹配的特征点相交点数目大于3的匹配点对，判定这些匹配点对为错误匹配对，将错误匹配对剔除，之后计算剩余匹配点对的欧氏距离比值r_i，将r_i按照递增排序组成新的匹配点对集合，再将新的匹配点对集合平均分成四组，从第一组中任意选择四对匹配点对，若所述的四对匹配点对全不是内点，舍弃第一组且不对剩余匹配点对进行判定，若第一组中有两对匹配点对是内点，再对第一组内剩余匹配点对进行内点的判定，对剩余组进行相同操作，选择内点数最多的组作为最优模型，完成匹配点对集合中误匹配的剔除。

优选的，S1将前端单目面阵相机提取到的标识线端点和前端单目面阵相机底部中心点进行世界坐标系变换，分别记为(X₁,Y₁)和(X₂,Y₂)；将后端单目面阵相机提取到的标识线端点和后端单目面阵相机底部中心点进行世界坐标系变换，分别记为(X₁′,Y₁′)，(X₂′,Y₂′)；前端单目面阵相机距标识线端点的距离L₁和后端单目面阵相机距标识线端点的距离L₂用下式得到：

前端单目面阵相机的横摆角和后端单目面阵相机的横摆角依次用下式表示：

式中，x₁和x₂分别为前端单目面阵相机和后端单目面阵相机距标识线的垂直距离，隧道检测车的横摆角用下式表示：

优选的，S1将得到的横摆角和编码器的位置量采用递推算法进行融合处理，具体公式如下；

式中，τ∈(t_k,t_k+1)，v(τ)为编码器在不同时刻τ输出的隧道检测车行驶方向上的速度，v_x代表隧道检测车沿地球坐标系x方向的速度分量，v_y代表隧道检测车沿地球坐标系y方向的速度分量，x(t_k+1)和y(t_k+1)分别代表隧道检测车在k+1时刻的位置，为隧道检测车在不同时刻τ下的横摆角，不同时刻下隧道检测车的坐标值点集为隧道检测车在隧道内的运动轨迹，具体表示为下式：s＝{(x(t₀),y(t₀)),(x(t₁),y(t₁)),…,(x(t_k),y(t_k)),(x(t_k+1),y(t_k+1))}。

进一步，当k＝0时，x(t₀),y(t₀)分别为隧道检测车进入隧道的初始时刻位置，所述的初始时刻位置由GPS提供。

优选的，S2在将隧道检测车在地球表面的纬度、经度转化成地球坐标系下的数值时，采用如下公式：

其中，地球坐标系记为坐标系e；

式中：θ、γ、ψ分别为隧道检测车的俯仰角、横滚角和横摆角，λ和L分别为隧道检测车的经度和纬度。

进一步，所述隧道检测车的俯仰角、横滚角和横摆角按如下过程得到：

先定义坐标系n，其中以隧道检测车的质心为原点，取向东为x₂轴正方向，取向北为y₂轴正方向，取垂直向上的方向为z₂轴正方向，之后定义坐标系b中的矢量r^b，r^b＝xi+yi+zk，转换到坐标系n中的矢量记为rⁿ，再定义四元数用四元数运算法则，rⁿ可以表示为下式：

进一步可得到下式：

坐标系b到坐标系n的姿态位置矩阵表示为下式：

表示为q₀、q₁、q₂、q₃均为实数，i、j和k均为方向矢量，/>是/>的共轭矩阵。

与/>的关系用矩阵形式表示为下式：

式中，为MEMS陀螺测得的隧道检测车在坐标系b下的角速度，/>分别为/>绕x₁轴、y₁轴、z₁轴的角速度，垂直x₁轴向上为z₁轴正方向；其中/>为MEMS陀螺输出隧道检测车的角动量，/>为地球的自转角速度，/>为地理坐标系相对地球的角速率，/>是/>的转置矩阵；

对与/>的关系式用四元数毕卡算法计算，得到下式的结果：

其中：进一步进行泰勒级数展开，得到下式所示的姿态位置矩阵/>

进一步用含有不同双下标的T来表示姿态位置矩阵/>记为：

最后求得：

优选的，S2用卡尔曼滤波器对M和N，以及S1所述运动轨迹中的X和Y进行融合时，采用如下过程：

先求解M和N的噪声方差以及S1所述运动轨迹中X和Y的噪声方差/>隧道检测车当前位置Pos(k)与M和N对应的位置Pos₁(k)，以及S1所述运动轨迹中X和Y对应的位置Pos₂(k)的关系如下：

利用隧道检测车当前位置Pos(k)得到融合后的数值。

与现有技术相比，本发明具有以下有益的技术效果：

本发明一种基于捷联惯导***和图像定位的隧道检测车定位方法，在隧道内由于GPS几乎接收不到信号，因此无法依靠GPS实现对隧道检测车的定位，因此可利用加速度计得到隧道检测车的位置更新微分方程，进一步即可求解得到隧道检测车在地球表面的纬度和经度，实现对隧道检测车位置的追踪；前端单目面阵相机和后端单目面阵相机可对隧道内各车道间的相同间断虚线先后进行拍摄，之后以标识线端点和单目面阵相机底部中心点为特征点对隧道检测车的横摆角进行解算，将得到的横摆角和编码器的位置量可进行融合处理，再结合隧道检测车进入隧道的初始时刻位置，便可得到隧道检测车在地球坐标系下隧道内的运动轨迹，该运动轨迹为X和Y的点集，其中X对应隧道检测车在地球坐标系下x方向的数值，Y对应隧道检测车在地球坐标系下y方向的数值，实现隧道检测车的实时定位。理论上讲这两种方式应该能保持完全的轨迹重合，但前者误差累积较为严重，后者由于单目面阵相机易受到光照的影响及隧道检测车自身的抖动使定位精度降低。所以将隧道检测车在地球表面的纬度、经度转化成地球坐标系下的数值，之后分别与单目面阵相机所得运动轨迹中的X和Y进行比较，当差值均在设定误差值以内，定位误差较小，用转化后的纬度、经度定位隧道检测车，否则将转化后的纬度数值、经度数值、单目面阵相机所得运动轨迹中的X和Y用卡尔曼滤波器进行融合，用融合后的数值定位隧道检测车，最终实现高精度的综合定位，完成在隧道内缺陷检测时隧道检测车的定位。

附图说明

图1为本发明所述单目面阵相机和LED灯在隧道检测车上的安装位置示意图。

图2为本发明所述捷联惯导***在隧道检测车底盘上的安装位置示意图。

图3为本发明的两台单目面阵相机拍摄同一路段示意图。

图4为本发明的隧道检测车横摆角解算示意图。

图5为本发明的总流程图。

图6为本发明的惯导定位流程图。

图7为本发明的图像定位流程图。

图8为本发明的融合定位流程图。

图中：隧道检测车1、前端单目面阵相机2、前端LED灯3、后端单目面阵相机4、后端LED灯5和捷联惯导***6。

具体实施方式

下面结合具体的实施例对本发明做进一步的详细说明，所述是对本发明的解释而不是限定。

本发明一种基于捷联惯导***(简写为SINS)和图像定位的隧道检测车线形定位方法，如图1所示，包括安装在隧道检测车1前端顶部的前端单目面阵相机2、前端LED灯3、后端单目面阵相机4和后端LED灯5，以及如图2所示安装在隧道检测车1底盘中部横梁上的捷联惯导***6，隧道检测车1为客车底盘改装，隧道内缺陷检测时隧道检测车1保持稳定车速前进。前端单目面阵相机2、后端单目面阵相机4用于路面标识线的图像采集。前端LED灯3、后端LED灯5用来实现照明，具有亮度高的效果，能保证隧道内图像采集的光照。捷联惯导***7包括MEMS陀螺、加速度计和微型计算机，结合加速度计求得的位置更新微分方程求解得到隧道检测车1的位置，实现隧道检测车1在隧道内的定位。MEMS陀螺可对隧道检测车1的姿态进行实时更新，得到相应的姿态角，即之后提到的俯仰角、横滚角和横摆角。如图5所示，在隧道检测车1进入隧道之前，利用车载GPS对隧道检测车1进行定位，隧道内图像采集时，隧道检测车1保持稳定车速行驶，进入隧道后，利用单目面阵相机采集隧道内标识线实现定位，通过对比轨迹图中基于隧道内标识线的图像定位轨迹与基于捷联惯导***7的定位轨迹，视情况判断是否需要通过卡尔曼滤波器将二者融合，输出隧道检测车1的定位位置。整套定位***运用图像和惯导的融合技术实现高精度的定位。

下面具体介绍本发明的算法部分。本发明的算法包括捷联惯导***定位、图像定位和融合定位三部分。

第一，如图6所示，利用姿态位置解算，实现捷联惯导***定位；

首先建立两个坐标系：坐标系b和坐标系n。以隧道检测车1的质心为原点o，取沿隧道检测车1轴线向右为x₁轴正方向，取垂直x₁轴向前为y₁轴正方向，取垂直x₁轴向上为z₁轴正方向，将上述确定的坐标系定义为坐标系b。以隧道检测车1的质心为原点o，取向东为x₂轴正方向，取向北为y₂轴正方向，取垂直向天的方向为z₂轴正方向，将此坐标系定义为坐标系n。

姿态位置解算是求出隧道检测车1当前的坐标系b相对于坐标系n的变化。存在这样一个由坐标系b到坐标系n的姿态位置矩阵，对于坐标系b中的一个向量，当它左乘这个姿态位置矩阵之后，就能得出这个向量对应于坐标系n中向量的大小。而姿态位置解算的目标就是根据隧道检测车1上MEMS陀螺输出的数据实时计算出这个姿态位置矩阵。本发明采用四元数法更新算法进行隧道检测车1的姿态位置解算，四元数法更新算法是目前捷联惯导***常用的姿态解算算法，除此之外还有欧拉角法更新算法、方向余弦法更新算法和等效旋转矢量法更新算法。

当隧道检测车1的姿态变化时，坐标系b原点和坐标系n原点的位置及其相应的坐标均会随之变化。安装在隧道检测车1上的MEMS陀螺可间隔地感测隧道检测车1在坐标系b下的角动量，进而可以得到隧道检测车1在坐标系b下的角速度加速度计可感测隧道检测车1在坐标系b下的线速度。二者的感测数据会实时反馈到微型计算机中，微型计算机利用四元数毕卡算法可解算出由坐标系b到坐标系n的姿态位置矩阵/>进而得到关于隧道检测车1姿态的3个角度，即在坐标系b中，取顺时针为正，隧道检测车1绕x₁轴旋转的俯仰角θ，绕y₁轴旋转的横滚角γ和绕z₁轴旋转的横摆角ψ。MEMS陀螺输出的一般是采样时间间隔内的角增量，为了避免噪声的微分放大，应直接用角增量来确定四元数。四元数毕卡算法是由角增量计算四元数法更新算法的常用算法。

定义坐标系b中的矢量r^b＝xi+yi+zk，转换到坐标系n中的矢量记为rⁿ。定义四元数用四元数运算法则，rⁿ可以表示为：/>展开有

可以得到姿态位置矩阵为

q₀,q₁,q₂,q₃均为实数，i、j和k均为方向矢量，是/>的共轭矩阵。

为求得姿态位置矩阵就要先求得四元数/>将四元数/>与角速度/>的关系用矩阵形式表示为下式(1):

式中，分别为MEMS陀螺测得的隧道检测车1绕x₁轴、y₁轴、z₁轴的角速度。/>其中/>为MEMS陀螺输出隧道检测车1的角动量，/>为地球的自转角速度，/>为地理坐标系相对地球的角速率，/>是/>的转置矩阵，以上均可求得，之后将该式代入式(1)。

对于式(1)的求解，用四元数毕卡算法计算，可以得到结果，

其中：

再对其结果进行泰勒级数展开可得到四元数进而确定出实时姿态位置矩阵

将式(2)中第二行的矩阵分别用含有不同双下标的T来表示，则式(2)可记为：

由于式(3)中的9个矩阵值均可求出，所以进一步求得：

式(4)中，θ、γ、ψ分别为之前提到的隧道检测车1的俯仰角、横滚角和横摆角，称为隧道检测车1的三个姿态角，可用于后面融合部分中地球坐标系与坐标系b的转换。

隧道检测车1的位置更新微分方程为：

式(5)中，v_x、v_y、v_z分别为坐标系b中三个坐标轴的速度，加速度计测量的三个坐标轴的加速度为a＝[a_x,a_y,a_z]^T，对加速度进行一次积分即可得到v_x、v_y、v_z。R_M、R_N分别为隧道检测车1所在点对应地球子午圈的半径和对应卯酉圈的曲率半径，L为隧道检测车1在地球表面的纬度，λ为隧道检测车1在地球表面的经度，h为海拔高度。对式(5)进行一次积分便可得到隧道检测车1所在位置的纬度、经度和海拔高度。

第二，如图7所示的图像定位

图像定位是基于隧道路面标识线完成的，通过前端单目面阵相机2和后端单目面阵相机4采集路面标识线的图像，如图3所示，两条弯曲实线表示道路边界，中间虚点划线是道路标志线，图3示意性地画出了弯线，在实际的隧道中弯度较小，v所指的是隧道检测车1的前进方向，t_k表示在t_k时刻，之后的算法通过对时间段积分来定位隧道检测车1的位置。对采集到的图像进行处理，利用针孔成像模型对提取到的特征点进行坐标解算，具体地，前端单目面阵相机2和后端单目面阵相机4将空间点投影到二维照片上，投影的过程能够用对应单目面阵相机的针孔成像模型表示，也可以利用针孔成像模型获得图片坐标与空间坐标的关系，针孔成像模型是现在普遍使用的相机成像模型。之后利用前端单目面阵相机2和后端单目面阵相机4的姿态角来解算隧道检测车1的横摆角，此处的横摆角不同于之前得到的横摆角，捷联惯导***定位与图像定位需要计算各自的横摆角。最后将隧道检测车1的横摆角和编码器的位置量融合，便可解算出基于隧道路面标识线的隧道检测车1的线形定位。

(1)对图像进行预处理

单目面阵相机的拍摄环境在户外的公路上，户外环境会对拍摄的成像造成干扰，因此需对图像进行滤波处理。这里采用中值滤波法，首先从采样窗口中取出奇数个数据，再将这些数据的中值作为当前像素点的灰度值：

再对滤波后的图像进行二值化处理，设定一个阈值T，当图像像素值大于阈值T，则认定为白色；反之，当图像像素值小于阈值T，则认定为黑色。

用Roberts算子对图像进行边缘提取，用对角线方向相邻两像素之差近似梯度幅值的方法来检测边缘，运算公式如下：

式中，f(x,y)为输入图像，g(x,y)为输出图像，x和y分别为图像的像素坐标点。

(2)提取与匹配图像特征点

隧道内各车道间的线是白色的间断虚线，因此可基于隧道内各车道间的间断虚线选取特征点。本发明图像定位是基于隧道检测车1顶部的前端单目面阵相机2和后端单目面阵相机4分别对相同标识线图像段拍摄，进而推算出隧道检测车1的位置，因此需对相同图像段进行立体匹配。

首先对图像中一点在σ尺度上计算Hessian矩阵，其表达式为：

式中，L_xx(x,σ)表示图像点与高斯二阶微分的卷积，L_xy(x,σ)表示图像点与高斯二阶微分/>的卷积，L_yx(x,σ)表示图像点与高斯二阶微分/>的卷积，L_yy(x,σ)表示图像点与高斯二阶微分/>的卷积。

利用盒式滤波结构近似代替高斯二阶微分，计算待选特征点及其周围点的Hessian值，结合积分图可以进行快速计算。判别式为：

Det(Hessian(x,σ))＝D_xx×D_yy-(0.9D_xy)² (9)

式中，Det(Hessian(x,σ))代表Hessian矩阵的行列式，D_xx、D_yy、D_xy分别为用盒式滤波器得到的L_xx(x,σ)、L_yy(x,σ)、L_xy(x,σ)。若Hessian矩阵的行列式值为正，且该像素点在附近像素点中的特征值最大，则认定为特征点。加权系数0.9是为了平衡盒式滤波器的近似误差。

前端单目面阵相机2和后端单目面阵相机4提取到的两幅图像的特征点集记为N₁、N₂。找出N₁中特征点a与N₂中欧氏距离最小和次最小的特征点b和特征点c，特征点a和特征点b的欧氏距离，特征点a和特征点c的欧氏距离分别为d₁、d₂。若它们的比值小于阈值则判定特征点a和特征点b为匹配点对，记为(a,b)，判定公式如下：

按照相同的过程得到两幅图像的匹配点对集合。

(3)改进RANSAC算法，剔除误匹配

首先，采用目前现有的改进RANSAC算法，从上述匹配点对集合中找出任意一个特征点与其他匹配的特征点相交点数目大于3的匹配点对，判定这些匹配点对为错误匹配对，将错误匹配对剔除。计算剩余匹配点对的欧氏距离比值r_i，将r_i按照递增排序组成新的匹配点对集合。再将该集合平均分成四组，可以保证第一组中有不少于五对匹配点对，从第一组中任意选择四对匹配点对，作为一个最优模型的判定。如果选取的四对匹配点对全不是内点，则这组模型作为最优模型的概率很小，这时舍弃此组且不对其他匹配点对进行判定；若发现第一组中有两对匹配点对是内点，再对本组内剩余匹配点对进行内点的判定。对剩余组进行相同操作，选择内点数最多的组作为最优模型。

(4)图像定位

利用张正友标定法对单目面阵相机标定，得到面阵相机的内部参数、外部参数，进而得到特征点的像素坐标系与世界坐标系的转换关系为：

式中，f_x＝f/dx,f_y＝f/dy，f_x和f_y分别表示图像水平轴和垂直轴的尺度因子，c_x和c_y分别图像坐标系原点在像素坐标系中的坐标，Z_c为常数，u、v分别表示特征点在像素坐标系下的横坐标值、纵坐标值。R、t为外部参数，X_w、Y_w和Z_w分别代表世界坐标系里的三维坐标，M为投影矩阵，M₁、M₂由面阵相机标定所得的内、外参确定，为特征点在世界坐标系下的坐标值。M₁中，f_x、f_y、c_x和c_y仅与单目面阵相机内部结构有关系，称这些参数为单目面阵相机内部参数；M₂仅由单目面阵相机对于世界坐标系的方位决定，称为单目面阵相机外部参数，M₁、M₂由单目面阵相机标定所得的内部参数、外部参数确定，M为投影矩阵，/>为特征点在世界坐标系下的坐标值。

选取标识线端点和单目相机底部中心点为特征点，经过坐标变换后，前端单目面阵相机2提取到的标识线端点和单目相机底部中心点的世界坐标分别为(X₁,Y₁)，(X₂,Y₂)，后端单目面阵相机4提取到的标识线端点和单目相机底部中心点的世界坐标分别为(X₁′,Y₁′)，(X₂′,Y₂′)。则可得到前端单目面阵相机2拍摄的隧道检测车1距标识线的距离，记为L₁。同理，可得到后端单目面阵相机4拍摄的隧道检测车1距标识线的距离，记为L₂。

在对隧道检测车1进行二维定位时，利用装在车顶部前端单目面阵相机2对标识线拍摄，对前端单目面阵相机2横摆角进行解算。隧道检测车1往前行驶，再利用安装于车顶后端单目面阵相机4对原路段再次拍摄，对后端单目面阵相机2横摆角进行解算，如图4所示。

式中，x₁，x₂为前端单目面阵相机2和后端单目面阵相机4分别距标志线的垂直距离。若隧道检测车1在该路段未发生偏转，则隧道检测车1前后端的横摆角相等；若隧道检测车1在该路段发生偏转，则隧道检测车前后端的横摆角之差即为车体在该路段的横摆角大小。

式中，分别为隧道检测车1前后端横摆角大小。

安装于隧道检测车1车轮输出轴的编码器可得到隧道检测车1的位置量，将编码器的位置量融合和横摆角进行融合处理，可得到在相应角度下隧道检测车1的位置，实现基于隧道标识线的隧道检测车线形定位。递推算法如下：

式中，τ∈(t_k,t_k+1)，v(τ)为编码器在不同时刻τ输出的隧道检测车1行驶方向上的速度，v_x代表隧道检测车1速度沿地球坐标系x₂方向的分量，v_y代表隧道检测车1速度沿地球坐标系y₂方向的分量，x(t_k+1)和y(t_k+1)分别代表隧道检测车1在k+1时刻的位置，为基于隧道标识线推算的隧道检测车1在不同时刻τ下的横摆角，当k＝0时，x(t₀),y(t₀)分别为进入隧道初始时刻隧道检测车1的初始位置，此位置量信息由GPS提供。

隧道检测车1在隧道内的运动轨迹即为不同时刻隧道检测车1坐标值的点集，即为：

s＝{(x(t₀),y(t₀)),(x(t₁),y(t₁)),…,(x(t_k),y(t_k)),(x(t_k+1),y(t_k+1))} (16)

另外，隧道内标识线由于磨损或路面设计因素会使得部分路段无标识线的情况。因此，本发明在对短距离无标识线路况下无法通过图像定位隧道检测车的横向位置，高速公路上的白色虚实线称之为车行道分界线，其标准长度为长度6米，间隔9米。道路中心虚实线连续两条缺失，短距离即15米时，故默认为上一时刻的横向位置，若超出此距离，本发明的技术方案则不适用。

以隧道检测车1进隧道前一刻t₀为例，隧道检测车1此时坐标位置为(x(t₀),y(t₀))，若隧检车在该路段未发生偏移，带入(15)式，有

若在t₁-t₂时刻之间隧道检测车1发生偏移，则有

第三，融合定位

图像定位和捷联惯导***定位是两套独立的***，在实现融合前，首先应解决坐标系的统一，即将坐标系b与地球坐标系进行坐标转换，转换关系如下：

式中，θ、γ、ψ分别为隧道检测车1的俯仰角、横滚角和横摆角；(λ，L)分别为隧道检测车1的经度、纬度。

将基于隧道标识线的图像定位轨迹与基于捷联惯导***的定位轨迹(即转化后的纬度数值、经度数值)显示在一张轨迹图中。在轨迹图中可看出在各时刻隧道检测车1行驶的路径以及对应点的实时坐标信息。在理论上，二者应该能保持完全的轨迹重合，但捷联惯导误差累积较为严重。而单纯依靠图像定位可具有较高精度，但由于单目面阵相机易受到光照的影响及隧道检测车1自身的抖动使定位精度降低。基于此点可利用图像和捷联惯导的融合实现综合定位。

如图8所示，通过对比轨迹图中隧道检测车1的实时位置数据，利用设定的误差值x判断是否需要融合定位。当两张图中隧道检测车1的任一坐标值(x,y)之差的绝对值超过设定误差值x＝0.2m时，即判定该时刻捷联惯导定位误差较大，此时采用图像的定位值作为补偿进行融合定位，利用卡尔曼滤波器将二者进行融合，首先求解惯导定位(即求解转化后的纬度数值、经度数值)的噪声方差其次求解图像定位的运动轨迹中x和y的噪声方差/>求解隧道检测车1当前位置Pos(k)与转化后的纬度数值、经度数值对应的定位位置Pos₁(k)与图像定位的运动轨迹中x和y对应的定位位置Pos₂(k)的关系进而能够得到融合后的位置参数，进一步定位隧道检测车。若两张图中隧道检测车1的任一坐标值之差均不超过误差值0.2m时，即判定该时刻惯导定位准确，此时输出惯导定位数据来定位隧道检测车。/>

Claims (10)

1.一种基于捷联惯导***和图像定位的隧道检测车定位方法，其特征在于，包括如下步骤：

S1，先利用加速度计得到隧道检测车的位置更新微分方程，之后求解所述的位置更新微分方程，得到隧道检测车在地球表面的纬度和经度；

隧道检测车的前端单目面阵相机先对隧道内各车道间的间断虚线进行拍摄，隧道检测车的后端单目面阵相机之后对隧道内各车道间的间断虚线进行拍摄，两次拍摄的间断虚线相同，然后以标识线端点和单目面阵相机底部中心点为特征点对隧道检测车的横摆角进行解算，将得到的横摆角和编码器的位置量进行融合处理，再结合隧道检测车进入隧道的初始时刻位置，得到隧道检测车在地球坐标系下隧道内的运动轨迹，运动轨迹为X和Y的点集，其中X对应隧道检测车在地球坐标系下x方向的数值，Y对应隧道检测车在地球坐标系下y方向的数值；

S2，将隧道检测车在地球表面的纬度转化成地球坐标系下的数值后记为M，将M与S1所述运动轨迹中的X进行比较，所得的绝对值记为O，将隧道检测车在地球表面的经度转化成地球坐标系下的数值后记为N，将N与S1所述运动轨迹中的Y进行比较，所得的绝对值记为P，当O和P均在设定误差值以内，用M和N定位隧道检测车，当O和P中至少有一个大于设定误差值，将M和N，以及S1所述运动轨迹中的X和Y在卡尔曼滤波器中进行融合，用融合后的数值定位隧道检测车。

2.根据权利要求1所述的基于捷联惯导***和图像定位的隧道检测车定位方法，其特征在于，S1中所述的位置更新微分方程如下所示：

其中，L为隧道检测车在地球表面的纬度，v_y为隧道检测车在坐标系b中y₁方向上的速度分量，R_M为隧道检测车所在点对应地球子午圈的半径，h为海拔高度；λ为隧道检测车在地球表面的经度，v_x为隧道检测车在坐标系b中x₁方向上的速度分量，R_N为隧道检测车所在点对应卯酉圈的曲率半径；

在坐标系b中，以隧道检测车的质心为原点，沿隧道检测车轴线向右为x₁轴正方向，沿垂直x₁轴向前为y₁轴正方向。

3.根据权利要求1所述的基于捷联惯导***和图像定位的隧道检测车定位方法，其特征在于，S1中前端单目面阵相机、后端单目面阵相机提取两幅相同图像的特征点集，分别记为N₁、N₂，之后找出N₁中任意一个特征点a与N₂中欧氏距离最小和次最小的特征点b和特征点c，特征点a和特征点b的欧氏距离、特征点a和特征点c的欧氏距离分别为d₁、d₂；若d₁和d₂的比值小于阈值则判定特征点a和特征点b为匹配点对，记为(a,b)，按照相同的过程得到两幅图像的匹配点对集合，之后剔除所述匹配点对集合中的误匹配，再选取标识线端点为特征点。

4.根据权利要求3所述的基于捷联惯导***和图像定位的隧道检测车定位方法，其特征在于，从所述匹配点对集合中找出任意一个特征点与其他匹配的特征点相交点数目大于3的匹配点对，判定这些匹配点对为错误匹配对，将错误匹配对剔除，之后计算剩余匹配点对的欧氏距离比值r_i，将r_i按照递增排序组成新的匹配点对集合，再将新的匹配点对集合平均分成四组，从第一组中任意选择四对匹配点对，若所述的四对匹配点对全不是内点，舍弃第一组且不对剩余匹配点对进行判定，若第一组中有两对匹配点对是内点，再对第一组内剩余匹配点对进行内点的判定，对剩余组进行相同操作，选择内点数最多的组作为最优模型，完成匹配点对集合中误匹配的剔除。

5.根据权利要求1所述的基于捷联惯导***和图像定位的隧道检测车定位方法，其特征在于，S1将前端单目面阵相机提取到的标识线端点和前端单目面阵相机底部中心点进行世界坐标系变换，分别记为(X₁,Y₁)和(X₂,Y₂)；将后端单目面阵相机提取到的标识线端点和后端单目面阵相机底部中心点进行世界坐标系变换，分别记为(X₁′,Y₁′)，(X₂′,Y₂′)；前端单目面阵相机距标识线端点的距离L₁和后端单目面阵相机距标识线端点的距离L₂用下式得到：

前端单目面阵相机的横摆角和后端单目面阵相机的横摆角依次用下式表示：

式中，x₁和x₂分别为前端单目面阵相机和后端单目面阵相机距标识线的垂直距离，隧道检测车的横摆角用下式表示：

6.根据权利要求1所述的基于捷联惯导***和图像定位的隧道检测车定位方法，其特征在于，S1将得到的横摆角和编码器的位置量采用递推算法进行融合处理，具体公式如下；

式中，τ∈(t_k,t_k+1)，v(τ)为编码器在不同时刻τ输出的隧道检测车行驶方向上的速度，v_x代表隧道检测车沿地球坐标系x方向的速度分量，v_y代表隧道检测车沿地球坐标系y方向的速度分量，x(t_k+1)和y(t_k+1)分别代表隧道检测车在k+1时刻的位置，为隧道检测车在不同时刻τ下的横摆角，不同时刻下隧道检测车的坐标值点集为隧道检测车在隧道内的运动轨迹，具体表示为下式：s＝{(x(t₀),y(t₀)),(x(t₁),y(t₁)),…,(x(t_k),y(t_k)),(x(t_k+1),y(t_k+1))}。

7.根据权利要求6所述的基于捷联惯导***和图像定位的隧道检测车定位方法，其特征在于，当k＝0时，x(t₀),y(t₀)分别为隧道检测车进入隧道的初始时刻位置，所述的初始时刻位置由GPS提供。

8.根据权利要求2所述的基于捷联惯导***和图像定位的隧道检测车定位方法，其特征在于，S2在将隧道检测车在地球表面的纬度、经度转化成地球坐标系下的数值时，采用如下公式：

其中，地球坐标系记为坐标系e；

式中：θ、γ、ψ分别为隧道检测车的俯仰角、横滚角和横摆角，λ和L分别为隧道检测车的经度和纬度。

9.根据权利要求8所述的基于捷联惯导***和图像定位的隧道检测车定位方法，其特征在于，所述隧道检测车的俯仰角、横滚角和横摆角按如下过程得到：

先定义坐标系n，其中以隧道检测车的质心为原点，取向东为x₂轴正方向，取向北为y₂轴正方向，取垂直向上的方向为z₂轴正方向，之后定义坐标系b中的矢量r^b，r^b＝xi+yi+zk，转换到坐标系n中的矢量记为rⁿ，再定义四元数用四元数运算法则，rⁿ可以表示为下式：

进一步可得到下式：

坐标系b到坐标系n的姿态位置矩阵表示为下式： 表示为q₀、q₁、q₂、q₃均为实数，i、j和k均为方向矢量，/>是/>的共轭矩阵；

与/>的关系用矩阵形式表示为下式：

式中，为MEMS陀螺测得的隧道检测车在坐标系b下的角速度，/>分别为/>绕x₁轴、y₁轴、z₁轴的角速度，垂直x₁轴向上为z₁轴正方向；/>其中/>为MEMS陀螺输出隧道检测车的角动量，/>为地球的自转角速度，/>为地理坐标系相对地球的角速率，/>是/>的转置矩阵；

对与/>的关系式用四元数毕卡算法计算，得到下式的结果：

其中：进一步进行泰勒级数展开，得到下式所示的姿态位置矩阵/>

进一步用含有不同双下标的T来表示姿态位置矩阵/>记为：

最后求得：

10.根据权利要求1所述的基于捷联惯导***和图像定位的隧道检测车定位方法，其特征在于，S2用卡尔曼滤波器对M和N，以及S1所述运动轨迹中的X和Y进行融合时，采用如下过程：

先求解M和N的噪声方差以及S1所述运动轨迹中X和Y的噪声方差/>隧道检测车当前位置Pos(k)与M和N对应的位置Pos₁(k)，以及S1所述运动轨迹中X和Y对应的位置Pos₂(k)的关系如下：

利用隧道检测车当前位置Pos(k)得到融合后的数值。