一种基于传感器和相机的姿态和里程的误差纠偏方法

本发明属于隧道检测技术领域，特别涉及一种基于传感器和相机的姿态和里程的误差纠偏方法。

背景技术：

随着城市化进程及城市的大规模发展，我国正加紧地铁建设以解决日益严重的交通堵塞问题。由于隧道规模的快速扩大，隧道检测工作量随之骤增，加上中国幅域辽阔，从东到西、从南到北的地质构造、自然环境差异巨大，周围环境的变化以及列车运行时的震动等各种因素的影响，地铁隧道会出现各种病害，在地铁施工和运营维护过程中，隧道检测车的姿态及里程检测是一项极其重要的工作。随着姿态检测车不断运动，实时姿态检测的误差值不断累积，造成计算结果在很短的时间内迅速发散。因此，对误差的校正是测量系统的关键。

现有技术中，实时姿态检测方法融合陀螺仪和加速度计传感器采集的数据来减小误差，以此来提高精度，但是随着时间的推移，误差值会不断累积，造成结果偏差较大，对最终的隧道测量产生影响。

技术实现要素：

本发明的目的在于，克服现有技术中的不足之处，提供一种基于传感器和相机的姿态和里程的误差纠偏方法，解决了现有技术中测量误差大的技术难题，使用本发明检测误差小，检测更加稳定，检测精度更高。

本发明的目的是这样实现的：一种基于传感器和相机的姿态和里程的误差纠偏方法，包括以下步骤，

(1)对于同一路段设定测量组数m，令m＝1；

(2)将小车置于测量初始位置处，使用加速度计测出小车运行的瞬时加速度，从而算出小车的瞬时速度，得到这个时间段内小车里程理论计算值s1；

(3)启动光电编码器，计算出小车的实际路程测量值s2；

(4)使用陀螺仪测出小车在x、y、z三个方向的瞬时偏转角度a、b、c，即小车姿态；

(5)设小车运动实际距离s＝α1*s1+α2*s2，设定α1和α2的初始值为α10和α20；

(6)根据s1和s2，计算出第m组路程测量误差，δsm＝|s1-s2|；

(7)对δs利用误差反向传播算法，计算出α1、α2的修正值，同时计算出第m组s；

(8)根据s和偏转角度算出小车在x、y、z三个方向的路程分向量为由得到第m组x、y、z三个方向的修正值，分别为δa、δb、δc，设定x、y、z三个方向的初始修正值分别为δa0、δb0、δc0；

(9)当m＝1，判断第m组测量下，若α1与α10和α2与α20的差值均不大于设定的波动幅度阈值一、δa与δa0、δb与δb0以及δc与δc0的差值均不大于设定的波动幅度阈值二，则得到理论计算修正值一，转至步骤(10)；当m大于1，若第m组测量的α1与第m-1组测量的α1、第m组测量的α2与第m-1组测量的α2之间的差值均不大于设定的波动幅值阈值一，第m组测量的δa和第m-1组测量的δa、第m组测量的δb和第m-1组测量的δb、第m组测量的δc和第m-1组测量的δc之间的差值均不大于设定的波动幅度阈值二，转至步骤(10)，否则，将m加1，转至步骤(2)；

(10)利用理论计算修正值一得到小车新的姿态和里程理论计算值s1；

(11)利用相机拍摄到的实际小车运动图像进行相机标定，返回步骤(5)，得到拍摄修正值二；

(12)根据理论计算修正值一和拍摄修正值二计算出二阶精度的修正值；

(13)根据二阶精度的修正值得到小车最终的姿态及里程数据；

其中，α1和α2为待确定的s1和s2的修正权值，δa、δb、δc为待确定的姿态偏转角在x、y、z方向上的修正值，a、b、c分别为陀螺仪测出的小车在x、y、z三个方向的瞬时偏转角度。

为了进一步计算出小车里程理论计算值s1，所述(1)中，求出小车里程理论计算值s1步骤具体为，

(101)根据傅里叶逆变换的公式，加速度信号在任一频率的傅里叶分量可以表达为，a(t)＝ae^jwt；

(102)设初速度分量为0，对加速度信号分量的时间积分可以得出速度信号分量，即

(103)设初位移分量为0，对加速度信号的傅里叶分量两次积分可得出路程分量：

其中，a(t)为加速度信号在频率w的傅里叶分量；a为对应a(t)的系数；j为虚数，v(t)为速度信号在频率w的傅里叶分量；v为对应v(t)的系数，s1(t)为速度信号在频率w的傅里叶分量；x为对应s1(t)的系数。

作为本发明的进一步改进，所述步骤(7)中，小车在x、y、z三个方向的路程分向量分别为，

在x方向的路程分向量在y方向的路程分向量在z方向的路程分向量

为了进一步得到待确定的s1和s2的修正权值以提高检测精度，所述步骤(7)和步骤(8)中，得到修正值的具体步骤为，

(701)正向传播阶段，

一个神经元的输入为

该神经元的输出为oj(p)＝f[ij(p)]；

(702)选取作为网络的激活函数，

反向传播阶段，

每个样本的误差准则函数为

n个样本的总误差为

利用误差梯度下降法，即沿着误差函数梯度的反方向调整连接权值wij使总误差最小，调整公式为t为迭代次数，重复迭代直至总误差e不超过设定的误差阈值，迭代结束；

(703)经过误差反向传播后，可以得到α1、α2的修正值，同时得到多组s的值，通过步骤(8)中的公式，得到δa、δb、δc的修正值；

其中，ij(p)表示第p层第j个神经元的输入；oj(p)表示它的输出；wij是从(p-1)层第i个神经元到p层第j个神经元的连接权值，在本方法中为步骤(4)中的α1和α2，本实施例中，其初始值均设为0.5；np-1表示第(p-1)层神经元数目；f(·)是该网络中神经元的激活函数，d良为输出层第k个神经元的期望输出；ok为该神经元的实际输出；m’为输出层神经元数；ek为每个样本的误差；e为n个样本总误差，在本方法中为步骤(5)中的δs，a为学习效率。

为了进一步得到小车实际路程测量值s2，所述步骤(11)中，得到小车姿态和里程实际路程测量值s2的步骤具体为，

(1101)假设像点p(u，v)，其空间对应点为p(x，y，z)，用下式建立p点二维像坐标与三维空间坐标的对应关系

通过四组样本点对，求出转换矩阵t’，用(u，v)表示像空间一点，对应物空间坐标为(y，z)，则可以用平方回归多项式表示像空间与物空间的对应关系，

对于已知物空间的标定点(yk，zk)，y(u，v)、z(u，v)的误差函数分别为：

误差函数ey、ez达到最小，则：

解此方程组，求出回归系数aij和bij，建立物像对应关系。

为了进一步提高检测精度，所述步骤(12)中，计算二阶精度的修正值的步骤具体为，

对于不同路段，重复步骤(1)-(11)，得到多组修正值一和修正值二，设修正值一α1、α2、δa、δb、δc，修正值二α1、α2、δa、δb、δc。这边以α1为例求解二阶精度，其他同理，

对β0、β1进行偏导，得：

运用克莱姆法则整理得：

求得β0、β1，即可求得最终α1的值，α1i，为修正值一中第i’个α1值，2i’指的是修正值二中第i’个α1值，同理求得最终α2、δa、δb、δc的参数值，n”为测量组数。

本发明与现有技术相比，具有以下技术效果：本发明通过采集加速度计、编码器、陀螺仪数据，得到小车里程及姿态初值s1、s2，通过对里程误差δs＝s1-s2进行误差反传播算法得到修正值一α1、α2、δa、δb、δc，通过修正值一计算新的理论里程计算值s1，再通过采集相机拍摄得到新的实际路程测量值s2，再次对里程误差δs＝s1-s2进行误差反传播算法，得到修正值二α1、α2、δa、δb、δc，对修正值一和修正值二进行二阶精度计算，得到最终α1、α2、δa、δb、δc的值，从而得到最终里程及姿态值；本发明通过实时调整修正值一和修正值二，并经修正值一和修正值二得到精度更高的修正值，从而不断修正小车的里程和咨态，提高小车的检测精度，为构建精确的地铁隧道三维模型提供了有力的保障，提升隧道检测病害的准确率；可应用于隧道检测的工作中。

附图说明

图1为本发明的流程框图。

图2为本发明中两个光电编码器的安装位置分布图。

具体实施方式

下面结合附图对本发明进行进一步说明。

一种基于传感器和相机的姿态和里程的误差纠偏方法，包括以下步骤，

(1)对于同一路段设定测量组数m，令m＝1；

(2)将小车置于测量初始位置处，使用加速度计测出小车运行的瞬时加速度，从而算出小车的瞬时速度，得到这个时间段内小车里程理论计算值s1；

(3)启动光电编码器，计算出小车的实际路程测量值s2；

(4)使用陀螺仪测出小车在x、y、z三个方向的瞬时偏转角度a、b、c，即小车姿态；

(5)设小车运动实际距离s＝α1*s1+α2*s2，设定α1和α2的初始值为α10和α20；

(6)根据s1和s2，计算出第m组路程测量误差，δsm＝|s1-s2|；

(7)对δs利用误差反向传播算法，计算出α1、α2的修正值，同时计算出第m组s；

(8)根据s和偏转角度算出小车在x、y、z三个方向的路程分向量为由得到第m组x、y、z三个方向的修正值，分别为δa、δb、δc，设定x、y、z三个方向的初始修正值分别为δa0、δb0、δc0；

(9)当m＝1，判断第m组测量下，若α1与α10和α2与α20的差值均不大于设定的波动幅度阈值一、δa与δa0、δb与δb0以及δc与δc0的差值均不大于设定的波动幅度阈值二，则得到理论计算修正值一，转至步骤(10)；当m大于1，若第m组测量的α1与第m-1组测量的α1、第m组测量的α2与第m-1组测量的α2之间的差值均不大于设定的波动幅值阈值一，第m组测量的δa和第m-1组测量的δa、第m组测量的δb和第m-1组测量的δb、第m组测量的δc和第m-1组测量的δc之间的差值均不大于设定的波动幅度阈值二，转至步骤(10)，否则，将第m组的修正值作为理论计算修正值一，将m加1，转至步骤(10)；

(10)利用理论计算修正值一得到小车新的姿态和里程理论计算值s1；

(11)利用相机拍摄到的实际小车运动图像进行相机标定，得到新的实际路程测量值s2，返回步骤(5)，得到拍摄修正值二；

(12)根据理论计算修正值一和拍摄修正值二计算出二阶精度的修正值；

(13)根据二阶精度的修正值得到小车最终的姿态及里程数据；

其中，α1和α2为待确定的s1和s2的修正权值，δa、δb、δc为待确定的姿态偏转角在x、y、z方向上的修正值，a、b、c分别为陀螺仪测出的小车在x、y、z三个方向的瞬时偏转角度；x、y、z三个方向的初始修正值分别为δa0、δb0、δc0，实施时，初始修正值均设为0。

为了进一步计算出小车里程理论计算值s1，(1)中，求出小车里程理论计算值s1步骤具体为，

(101)根据傅里叶逆变换的公式，加速度信号在任一频率的傅里叶分量可以表达为，a(t)＝ae^jwt；

(102)设初速度分量为0，对加速度信号分量的时间积分可以得出速度信号分量，即

(103)设初位移分量为0，对加速度信号的傅里叶分量两次积分可得出路程分量：

其中，a(t)为加速度信号在频率w的傅里叶分量；a为对应a(t)的系数；j为虚数，v(t)为速度信号在频率w的傅里叶分量；v为对应v(t)的系数，s1(t)为速度信号在频率w的傅里叶分量；x为对应s1(t)的系数。

其中，步骤(7)中，小车在x、y、z三个方向的路程分向量分别为，在x方向的路程分向量在y方向的路程分向量在z方向的路程分向量

为了进一步得到待确定的s1和s2的修正权值以提高检测精度，步骤(7)和步骤(8)中，得到修正值的具体步骤为，

(701)正向传播阶段，

一个神经元的输入为

该神经元的输出为oj(p)＝f[ij(p)]；

(702)选取作为网络的激活函数，

反向传播阶段，

每个样本的误差准则函数为

n个样本的总误差为

利用误差梯度下降法，即沿着误差函数梯度的反方向调整连接权值wij使总误差最小，调整公式为t为迭代次数，重复迭代直至总误差e不超过设定的误差阈值，迭代结束；

(703)经过误差反向传播后，可以得到α1、α2的修正值，同时得到多组s的值，通过步骤(8)中的公式，得到δa、δb、δc的修正值；

其中，ij(p)表示第p层第j个神经元的输入；oj(p)表示它的输出；wij是从(p-1)层第i个神经元到p层第j个神经元的连接权值，在本方法中为步骤(4)中的α1和α2，初始值为0.5；np-1表示第(p-1)层神经元数目；f(·)是该网络中神经元的激活函数，dk为输出层第k个神经元的期望输出；ok为该神经元的实际输出；m’为输出层神经元数；ek为每个样本的误差；e为n个样本总误差，在本方法中为步骤(5)中的δs，a为学习效率，实施时，设定误差阈值为0.1。

步骤(3)中，小车的驱动轮上安装有两个光电编码器，两个光电编码器的安装分布图如图2所示，利用光电编码器计算得到小车的实际路程测量值s2的步骤具体为，

光电编码器的红外光由发射器射出，射于地上的黑色条纹上，不间断地反射于接收器上，在接收器的输入端收到与驱动轮转速成正比关系的光脉冲信号，在接收器的输出端形成具有一定频率的电信号，再利用编码器得出小车所走路程测量值s编，记两个编码器所测路程分别为s编1和s编2，又有s编＝θr，θ编1＝θ编2＝θ车轮，从而得到

其中，d为驱动轮的外径，n1和n2分别为两个编码器的码盘上黑白条纹的个数，nx1和nx2分别为两个编码器实际测量时电脉冲正负跳变的次数。

为了进一步得到小车实际路程测量值s2，步骤(11)中，得到小车姿态和里程实际路程测量值s2的步骤具体为，

(1101)假设像点p(u，v)，其空间对应点为p(x，y，z)，用下式建立p点二维像坐标与三维空间坐标的对应关系

通过四组样本点对，求出转换矩阵t’，用(u，v)表示像空间一点，对应物空间坐标为(y，z)，则可以用平方回归多项式表示像空间与物空间的对应关系，

对于已知物空间的标定点(yk，zk)，y(u，v)、z(u，v)的误差函数分别为：

误差函数ey、ez达到最小，则：

解此方程组，求出回归系数aij和bij，建立物像对应关系，即可以通过像空间点计算物空间点，x轴的坐标由光电编码器反馈，从而构建三维模型，得到路程测量值s2。

为了进一步提高检测精度，步骤(12)中，计算二阶精度的修正值的步骤具体为，

取不同路段，重复步骤(1)-(11)，得到多组修正值一和修正值二，设修正值一α1i、α2、δa、δb、δc，修正值二α1、α2、δa、δb、δc，这边以α为例求解二阶精度，其他同理，

对β0、β1进行偏导，得：

运用克莱姆法则整理得：

求得β0、β1，即可求得最终α1的值，α1i，为修止值一中第i’个α1值，2i’指的是修正值二中第i’个α1值，同理求得最终α2、δa、δb、δc的参数值，n”为测量组数。

本发明与现有技术相比，具有以下技术效果：本发明通过采集加速度计、编码器、陀螺仪数据，得到小车里程及姿态初值s1、s2，通过对里程误差δs＝s1-s2进行误差反传播算法得到修正值一α1、α2、δa、δb、δc，通过修正值一计算新的理论里程计算值s1，再通过采集相机拍摄得到新的实际路程测量值s2，再次对里程误差δs＝s1-s2进行误差反传播算法，得到修正值二α1、α2、δa、δb、δc，对修正值一和修正值二进行二阶精度计算，得到最终α1、α2、δa、δb、δc的值，从而得到最终里程及姿态值；本发明通过实时调整修正值一和修正值二，并经修正值一和修正值二得到精度更高的修正值，从而不断修正小车的里程和姿态，提高小车的检测精度，为构建精确的地铁隧道三维模型提供了有力的保障，提升隧道检测病害的准确率；可应用于隧道检测的工作中。

本发明并不局限于上述实施例，在本发明公开的技术方案的基础上，本邻域的技术人员根据所公开的技术内容，不需要创造性的劳动就可以对其中的一些技术特征作出一些替换和变形，这些替换和变形均在本发明的保护范围内。