一种基于扩展卡尔曼滤波的巷道实时定位导航方法及系统

摘要

本发明公开一种基于扩展卡尔曼滤波的巷道实时定位导航方法及系统，包括：获取掘进设备的初始状态参数及坐标转换关系；通过基于模型线性近似方法建立里程计行走模型，并利用扩展卡尔曼滤波算法以相关参数作为滤波状态变量对其进行预测更新，以获得掘进设备的状态一步预测值和状态一步预测协方差；获取掘进设备的当前状态参数，并利用扩展卡尔曼滤波算法对状态一步预测值和状态一步预测协方差进行测量更新，以获得掘进设备的当前状态最优估计值和当前状态最优估计协方差；根据当前状态最优估计值及当前状态最优估计协方差控制掘进设备的运动状态。本发明能够提高掘进设备在巷道内的实时定位精度和定位可靠性，提高隧道掘进工作效率和质量。

说明书

技术领域

本发明涉及巷道定位技术领域，特别涉及一种基于扩展卡尔曼滤波的巷道实时定位导航方法。本发明还涉及一种基于扩展卡尔曼滤波的巷道实时定位导航系统。

背景技术

随着中国机械工业的发展，越来越多的机械设备已得到广泛使用。

机械设备的种类很多，在土木及建筑工程中通常需要使用大型工程设备，比如掘进机、挖掘机、龙门吊机等。以盾构机为例，现代盾构机是集光、机、电、液一体的大型高端隧道掘进设备。

巷道掘进设备(以下简称“掘进设备”)在运行过程中，需要时刻保持精确的掘进方向和运动状态，以防止系统误差累积。为满足掘进设备在行走、掘进、支护过程中的自动定位、导向和姿态调整的需求，研究适应于巷道掘进复杂恶劣环境下的定位导向技术是巷道精确化施工的前提。

目前，常用的定位方式主要有超宽带定位、光学定位、超声定位、惯性导航定位等。其中，超宽带定位作为一种较常见的地下工程定位技术，具有功耗低、不易受干扰、定位精度高等优点，但由于井下空间受限，没有足够距离布设三维定位所需基站系统，在左右、高度方向的定位误差较大。基于光学的测量方法，如激光、机器视觉等，由于现场工作时，掌子面前方煤尘较大，能见度低，光学通道受阻，在实际运用中存在较多问题。基于超声的定位方式，系统通过超声波定位系统与数字磁罗盘进行融合，实现简单，但易出现传感器之间串扰的情况，难以保证精度。惯性导航定位的最大优势是不依赖外界环境，且能够提供详细的姿态角和速度信息，但惯性导航定位的累计误差大，不能保持长时间高精度工作。可见，在现有技术中，各种单一的巷道定位方式均难以满足巷道掘进对于实时定位的需求。

因此，如何提高掘进设备在巷道内的实时定位精度和定位可靠性，提高隧道掘进工作效率和质量，是本领技术人员面临的技术问题。

发明内容

本发明的目的是提供一种基于扩展卡尔曼滤波的巷道实时定位导航方法，能够提高掘进设备在巷道内的实时定位精度和定位可靠性，提高隧道掘进工作效率和质量。本发明的另一目的是提供一种基于扩展卡尔曼滤波的巷道实时定位导航系统。

为解决上述技术问题，本发明提供一种基于扩展卡尔曼滤波的巷道实时定位导航方法，包括：

获取掘进设备在巷道内的初始状态参数及巷道相对于地理坐标系的转换关系；

利用所述初始状态参数及所述转换关系通过基于模型线性近似方法建立里程计行走模型，并利用扩展卡尔曼滤波算法以所述里程计行走模型中的相关参数作为滤波状态变量对所述里程计行走模型进行预测更新，以获得掘进设备的状态一步预测值和状态一步预测协方差；

获取掘进设备在巷道内的当前状态参数，并利用扩展卡尔曼滤波算法根据各所述当前状态参数对所述状态一步预测值和所述状态一步预测协方差进行测量更新，以获得掘进设备的当前状态最优估计值和当前状态最优估计协方差；

根据所述当前状态最优估计值及所述当前状态最优估计协方差控制掘进设备的运动状态。

优选地，获取掘进设备在巷道内的初始状态参数，具体包括：

获取掘进设备的空间坐标、姿态角度、行走系数、转向系数、行走轨迹相对于车体的俯仰修正角、行走轨迹相对于车体的航向修正角。

优选地，获取巷道相对于地理坐标系的转换关系，具体包括：

通过架设在巷道口的全站仪及安装于掘进设备上的棱镜获取巷道相对于地理坐标系的转换关系。

优选地，获取掘进设备的空间坐标，具体包括：

通过安装于巷道口及掘进设备上的超宽带定位组件获取掘进设备在巷道内的纵向位置；

通过所述全站仪及棱镜获取掘进设备在巷道内的位置。

优选地，获取掘进设备的姿态角度，具体包括：

通过安装于掘进设备上的惯性导航仪获取掘进设备的姿态角度。

优选地，利用扩展卡尔曼滤波算法以所述里程计行走模型中的相关参数作为滤波状态变量对所述里程计行走模型进行预测更新，具体包括：

通过状态转移方程：

计算状态一步预测值；

其中，

并通过公式：

计算状态一步预测协方差；

其中，

优选地，利用扩展卡尔曼滤波算法根据各所述当前状态参数对所述状态一步预测值和所述状态一步预测协方差进行测量更新，具体包括：

通过公式：

计算卡尔曼滤波增益；

其中，K

并通过公式：

计算掘进设备的当前状态最优估计值；

其中，μ

再通过公式：

计算掘进设备的当前状态最优估计协方差；

其中，Σ

优选地，利用扩展卡尔曼滤波算法根据各所述当前状态参数对所述状态一步预测值和所述状态一步预测协方差进行测量更新，具体包括：

分别通过所述惯性导航仪、所述超宽带定位组件及所述全站仪的测量矩阵、量测预测误差、测量误差向量对所述状态一步预测值和所述状态一步预测协方差进行测量更新。

本发明还提供一种基于扩展卡尔曼滤波的巷道实时定位导航系统，包括：

初始获取模块，用于获取掘进设备在巷道内的初始状态参数及巷道相对于地理坐标系的转换关系；

模型建立模块，用于利用所述初始状态参数及所述转换关系通过基于模型线性近似方法建立里程计行走模型；

预测更新模块，用于利用扩展卡尔曼滤波算法以所述里程计行走模型中的相关参数作为滤波状态变量对所述里程计行走模型进行预测更新，以获得掘进设备的状态一步预测值和状态一步预测协方差；

测量更新模块，用于获取掘进设备在巷道内的当前状态参数，并利用扩展卡尔曼滤波算法根据各所述当前状态参数对所述状态一步预测值和所述状态一步预测协方差进行测量更新，以获得掘进设备的当前状态最优估计值和当前状态最优估计协方差；

控制输出模块，用于根据所述当前状态最优估计值及所述当前状态最优估计协方差控制掘进设备的运动状态。

本发明所提供的基于扩展卡尔曼滤波的巷道实时定位导航方法，主要包括四个步骤。在第一步中，主要内容为获取初始时刻(t)下的掘进设备在巷道(或隧道)内的初始状态参数，以及巷道相对于地理坐标系的转换关系。第二步中，首先利用在第一步中获取的掘进设备的若干个初始状态参数和转换关系，通过基于模型线性近似方法建立与里程计参数(掘进设备行走时产生变化)相关的里程计行走模型，然后将掘进设备的若干个初始状态参数作为滤波状态变量，利用该滤波状态变量通过扩展卡尔曼滤波算法进行计算，根据计算结果对建立的里程计行走模型进行预测更新，从而获得掘进设备的状态一步预测值和状态一步预测协方差。在第三步中，在掘进设备处于运行状态时，获取掘进设备在巷道内的当前状态参数(观测值)，同样利用扩展卡尔曼滤波算法根据各个当前状态参数进行计算，并利用计算结果对状态一步预测值和状态一步预测协方差进行测量更新和修正，从而获得掘进设备的当前状态最优估计值和当前状态最优估计协方差。在第四步中，即可根据获得的当前状态最优估计值及当前状态最优估计协方差对掘进设备的行走、转向等运动状态进行精确控制。如此，本发明所提供的基于扩展卡尔曼滤波的巷道实时定位导航方法，通过使用卡尔曼滤波增益确定里程计行走模型的递推预测量和融合多渠道检测方式采集到的观测值的权值，输出当前状态最优估计值，实现掘进设备的实时高精度精准定位，能够提高掘进设备在巷道内的实时定位精度和定位可靠性，提高隧道掘进工作效率和质量。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明所提供的一种具体实施方式的流程图。

图2为巷道实时定位导航方法原理框图。

图3为本发明所提供的一种具体实施方式的模块结构图。

图4为本发明所提供的一种具体实施方式的硬件结构图。

其中，图3—图4中：

初始获取模块—1，模型建立模块—2，预测更新模块—3，测量更新模块—4，控制输出模块—5，车体—6，全站仪—7，棱镜—8，超宽带定位组件—9，惯性导航仪—10，行走编码器—11。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参考图1、图2，图1为本发明所提供的一种具体实施方式的流程图，图2为巷道实时定位导航方法原理框图。

在本发明所提供的一种具体实施方式中，基于扩展卡尔曼滤波的巷道实时定位导航方法主要包括四个步骤，分别为：

S1、获取掘进设备在巷道内的初始状态参数及巷道相对于地理坐标系的转换关系；

S2、利用初始状态参数及转换关系通过基于模型线性近似方法建立里程计行走模型，并利用扩展卡尔曼滤波算法以里程计行走模型中的相关参数作为滤波状态变量对里程计行走模型进行预测更新，以获得掘进设备的状态一步预测值和状态一步预测协方差；

S3、获取掘进设备在巷道内的当前状态参数，并利用扩展卡尔曼滤波算法根据各当前状态参数对状态一步预测值和状态一步预测协方差进行测量更新，以获得掘进设备的当前状态最优估计值和当前状态最优估计协方差；

S4、根据当前状态最优估计值及当前状态最优估计协方差控制掘进设备的运动状态。

其中，在步骤S1中，主要内容为获取初始时刻(t)下的掘进设备在巷道(或隧道)内的初始状态参数，以及巷道相对于地理坐标系的转换关系。

在步骤S2中，首先利用在第一步中获取的掘进设备的若干个初始状态参数和转换关系，通过基于模型线性近似方法建立与里程计参数(掘进设备行走时产生变化)相关的里程计行走模型，然后将掘进设备的若干个初始状态参数作为滤波状态变量，利用该滤波状态变量通过扩展卡尔曼滤波算法进行计算，根据计算结果对建立的里程计行走模型进行预测更新，从而获得掘进设备的状态一步预测值和状态一步预测协方差。

在步骤S3中，在掘进设备处于运行状态时，获取掘进设备在巷道内的当前状态参数(观测值)，同样利用扩展卡尔曼滤波算法根据各个当前状态参数进行计算，并利用计算结果对状态一步预测值和状态一步预测协方差进行测量更新和修正，从而获得掘进设备的当前状态最优估计值和当前状态最优估计协方差。

在步骤S4中，即可根据获得的当前状态最优估计值及当前状态最优估计协方差对掘进设备的行走、转向等运动状态进行精确控制。

如此，本实施例所提供的基于扩展卡尔曼滤波的巷道实时定位导航方法，通过使用卡尔曼滤波增益确定里程计行走模型的递推预测量和融合多渠道检测方式采集到的观测值的权值，输出当前状态最优估计值，实现掘进设备的实时高精度精准定位，能够提高掘进设备在巷道内的实时定位精度和定位可靠性，提高隧道掘进工作效率和质量。

在步骤S1中，首先可经全站仪设站测量和惯性导航仪寻北，获取初始时刻(t时刻)的系统参数及掘进设备在巷道的初始状态，其中，系统参数主要包括巷道相对于地理坐标系的转换矩阵、惯性导航仪的安装误差、超宽带定位组件(收发部件)的安装位置、车身上的棱镜坐标及初始协方差阵等，而掘进设备在巷道内的初始状态主要包括掘进设备的空间坐标、姿态角度、行走系数、转向系数、行走轨迹相对于车体的俯仰修正角、行走轨迹相对于车体的航向修正角共10个参数，并利用该10个参数作为滤波状态变量：

μ＝[x,y,z,α,β,γ,ω,κ,Δα,Δγ]

其中，空间坐标即空间三坐标，分别为车体相对于巷道的纵向位置x(里程或行走距离)、车体在巷道内的横向位置y(左偏)和车体在巷道内的垂向位置z(上偏)；而掘进设备的姿态角度分别为俯仰角α、横滚角β和航向角γ。纵向位置x除以行走编码器的增量为行走系数ω，转向角度除以行走编码器的增量差值(车体两侧各设置一个行走编码器，分别为左侧行走编码器和右侧行走编码器)为转向系数κ，行走轨迹(比如履带轨迹)相对于车体的俯仰修正角Δα，行走轨迹相对于车体的航向修正角Δγ也纳入滤波状态变量中。同时，ω、κ、Δα、Δγ又可统称为行走特性参数。

在获取巷道相对于地理坐标系的转换关系时，具体的，可通过架设在巷道口的全站仪及安装于掘进设备上的棱镜进行获取；在获取掘进设备的空间坐标时，具体的，可通过安装于巷道口及掘进设备上的超宽带定位组件获取掘进设备在巷道内的纵向位置，同时通过全站仪及棱镜获取掘进设备在巷道内的位置(包括纵向位置、横向位置及垂向位置)；在获取掘进设备的姿态角度时，具体的，可通过安装于掘进设备上的惯性导航仪获取掘进设备的姿态角度。

此外，扩展卡尔曼滤波算法即为利用前一时刻的状态值做预测，获得当前时刻的状态预测值，再利用当前时刻的测量值修正预测值，获得最小均方误差的状态估计值，共分为两个环节，分别为预测更新和测量更新，其中步骤S2主要实现预测更新，而步骤S3主要实现测量更新。

其中，在步骤S2中，预测更新主要是为了获取状态一步预测值

首先有状态转移方程

基于模型线性近似的方法建立里程计行走模型，即为状态转移方程的详细描述：

其中，ε为t到t+1时间内，系统预测的不确定性误差，即由系统噪声引入的误差。

通过上述状态转移方程即可求得

进一步的，状态一步预测协方差阵

其中，

在步骤S3中，首先有测量误差方程：

其中，Z

为明确权重，需要通过公式①计算卡尔曼滤波增益：

之后，即可通过公式②计算掘进设备的当前状态最优估计值：

同时，还可通过公式③计算掘进设备的当前状态最优估计协方差：

其中，K

另外，为满足掘进设备在巷道定位的实时高精度需求，本实施例中的测量更新环节将分为通过惯性导航测量更新、超宽带测距测量更新、全站仪测量更新和人工测量更新四种，并使用扩展卡尔曼滤波算法对采集的观测量进行融合计算。

对于通过惯性导航测量更新时，可首先建立惯性导航测量误差模型：

其中，e

其测量矩阵为：

其量测预测误差为：

其测量误差向量为：

其中，δ

对于通过超宽带测距测量更新时，为解决由于巷道空间受限而导致的超宽带测距在左右、高度方向的定位误差，基于现有已知的巷道掘进设备工作条件，建立掘进设备相对巷道的距离模型，获取掘进设备在巷道前行的距离。

超宽带测距的距离模型为：

其中，L为超宽带测距结果，

其测量矩阵为：

其量测预测误差为：

D＝e

其测量误差向量为：

Q

其中，δ

只需将上述测量矩阵、量测预测误差、测量误差向量分别代入前述公式①、公式②、公式③中，即可分别计算出通过超宽带测距测量更新的卡尔曼滤波增益、当前状态最优估计值、当前状态最优估计协方差。

对于通过全站仪测量更新时，可首先建立全站仪测量误差模型：

其中，

其测量矩阵为：

其量测预测误差为：

其测量误差向量为：

其中，δ

只需将上述测量矩阵、量测预测误差、测量误差向量分别代入前述公式①、公式②、公式③中，即可分别计算出通过超宽带测距测量更新的卡尔曼滤波增益、当前状态最优估计值、当前状态最优估计协方差。

对于通过人工测量更新时，现场操作人员可通过粗略测量，估计给出掘进设备相对于巷道行驶的x、y、z值作为观测量Z

如图3、图4所示，图3为本发明所提供的一种具体实施方式的模块结构图，图4为本发明所提供的一种具体实施方式的硬件结构图。

本实施例还提供一种基于扩展卡尔曼滤波的巷道实时定位导航系统，主要包括初始获取模块1、模型建立模块2、预测更新模块3、测量更新模块4和控制输出模块5。其中，初始获取模块1主要用于获取掘进设备在巷道内的初始状态参数及巷道相对于地理坐标系的转换关系。模型建立模块2主要用于利用初始状态参数及转换关系通过基于模型线性近似方法建立里程计行走模型。预测更新模块3主要用于利用扩展卡尔曼滤波算法以里程计行走模型中的相关参数作为滤波状态变量对里程计行走模型进行预测更新，以获得掘进设备的状态一步预测值和状态一步预测协方差。测量更新模块4主要用于获取掘进设备在巷道内的当前状态参数，并利用扩展卡尔曼滤波算法根据各当前状态参数对状态一步预测值和状态一步预测协方差进行测量更新，以获得掘进设备的当前状态最优估计值和当前状态最优估计协方差。控制输出模块5主要用于根据当前状态最优估计值及当前状态最优估计协方差控制掘进设备的运动状态。

对于硬件结构，本实施例在巷道口位置处设置有全站仪7和超宽带定位组件9的接收端，在车体6上安装有超宽带定位组件9的发送端、与全站仪7配合使用的棱镜8、惯性导航仪10和分布与车身左右两侧的行走编码器11。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。