CN112683268A - 一种基于扩展卡尔曼滤波的巷道实时定位导航方法及*** - Google Patents
一种基于扩展卡尔曼滤波的巷道实时定位导航方法及*** Download PDFInfo
- Publication number
- CN112683268A CN112683268A CN202011424015.2A CN202011424015A CN112683268A CN 112683268 A CN112683268 A CN 112683268A CN 202011424015 A CN202011424015 A CN 202011424015A CN 112683268 A CN112683268 A CN 112683268A
- Authority
- CN
- China
- Prior art keywords
- state
- roadway
- covariance
- current state
- tunneling equipment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 50
- 238000000034 method Methods 0.000 title claims abstract description 32
- 230000005641 tunneling Effects 0.000 claims abstract description 114
- 238000006243 chemical reaction Methods 0.000 claims abstract description 23
- 238000005259 measurement Methods 0.000 claims description 70
- 239000011159 matrix material Substances 0.000 claims description 25
- 238000012937 correction Methods 0.000 claims description 8
- 230000007704 transition Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 238000009434 installation Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 238000009412 basement excavation Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000000428 dust Substances 0.000 description 1
- 230000005611 electricity Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 239000007788 liquid Substances 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
Images
Landscapes
- Navigation (AREA)
Abstract
本发明公开一种基于扩展卡尔曼滤波的巷道实时定位导航方法及***,包括:获取掘进设备的初始状态参数及坐标转换关系;通过基于模型线性近似方法建立里程计行走模型,并利用扩展卡尔曼滤波算法以相关参数作为滤波状态变量对其进行预测更新,以获得掘进设备的状态一步预测值和状态一步预测协方差;获取掘进设备的当前状态参数,并利用扩展卡尔曼滤波算法对状态一步预测值和状态一步预测协方差进行测量更新,以获得掘进设备的当前状态最优估计值和当前状态最优估计协方差;根据当前状态最优估计值及当前状态最优估计协方差控制掘进设备的运动状态。本发明能够提高掘进设备在巷道内的实时定位精度和定位可靠性,提高隧道掘进工作效率和质量。
Description
技术领域
本发明涉及巷道定位技术领域,特别涉及一种基于扩展卡尔曼滤波的巷道实时定位导航方法。本发明还涉及一种基于扩展卡尔曼滤波的巷道实时定位导航***。
背景技术
随着中国机械工业的发展,越来越多的机械设备已得到广泛使用。
机械设备的种类很多,在土木及建筑工程中通常需要使用大型工程设备,比如掘进机、挖掘机、龙门吊机等。以盾构机为例,现代盾构机是集光、机、电、液一体的大型高端隧道掘进设备。
巷道掘进设备(以下简称“掘进设备”)在运行过程中,需要时刻保持精确的掘进方向和运动状态,以防止***误差累积。为满足掘进设备在行走、掘进、支护过程中的自动定位、导向和姿态调整的需求,研究适应于巷道掘进复杂恶劣环境下的定位导向技术是巷道精确化施工的前提。
目前,常用的定位方式主要有超宽带定位、光学定位、超声定位、惯性导航定位等。其中,超宽带定位作为一种较常见的地下工程定位技术,具有功耗低、不易受干扰、定位精度高等优点,但由于井下空间受限,没有足够距离布设三维定位所需基站***,在左右、高度方向的定位误差较大。基于光学的测量方法,如激光、机器视觉等,由于现场工作时,掌子面前方煤尘较大,能见度低,光学通道受阻,在实际运用中存在较多问题。基于超声的定位方式,***通过超声波定位***与数字磁罗盘进行融合,实现简单,但易出现传感器之间串扰的情况,难以保证精度。惯性导航定位的最大优势是不依赖外界环境,且能够提供详细的姿态角和速度信息,但惯性导航定位的累计误差大,不能保持长时间高精度工作。可见,在现有技术中,各种单一的巷道定位方式均难以满足巷道掘进对于实时定位的需求。
因此,如何提高掘进设备在巷道内的实时定位精度和定位可靠性,提高隧道掘进工作效率和质量,是本领技术人员面临的技术问题。
发明内容
本发明的目的是提供一种基于扩展卡尔曼滤波的巷道实时定位导航方法,能够提高掘进设备在巷道内的实时定位精度和定位可靠性,提高隧道掘进工作效率和质量。本发明的另一目的是提供一种基于扩展卡尔曼滤波的巷道实时定位导航***。
为解决上述技术问题,本发明提供一种基于扩展卡尔曼滤波的巷道实时定位导航方法,包括:
获取掘进设备在巷道内的初始状态参数及巷道相对于地理坐标系的转换关系;
利用所述初始状态参数及所述转换关系通过基于模型线性近似方法建立里程计行走模型,并利用扩展卡尔曼滤波算法以所述里程计行走模型中的相关参数作为滤波状态变量对所述里程计行走模型进行预测更新,以获得掘进设备的状态一步预测值和状态一步预测协方差;
获取掘进设备在巷道内的当前状态参数,并利用扩展卡尔曼滤波算法根据各所述当前状态参数对所述状态一步预测值和所述状态一步预测协方差进行测量更新,以获得掘进设备的当前状态最优估计值和当前状态最优估计协方差;
根据所述当前状态最优估计值及所述当前状态最优估计协方差控制掘进设备的运动状态。
优选地,获取掘进设备在巷道内的初始状态参数,具体包括:
获取掘进设备的空间坐标、姿态角度、行走系数、转向系数、行走轨迹相对于车体的俯仰修正角、行走轨迹相对于车体的航向修正角。
优选地,获取巷道相对于地理坐标系的转换关系,具体包括:
通过架设在巷道口的全站仪及安装于掘进设备上的棱镜获取巷道相对于地理坐标系的转换关系。
优选地,获取掘进设备的空间坐标,具体包括:
通过安装于巷道口及掘进设备上的超宽带定位组件获取掘进设备在巷道内的纵向位置;
通过所述全站仪及棱镜获取掘进设备在巷道内的位置。
优选地,获取掘进设备的姿态角度,具体包括:
通过安装于掘进设备上的惯性导航仪获取掘进设备的姿态角度。
优选地,利用扩展卡尔曼滤波算法以所述里程计行走模型中的相关参数作为滤波状态变量对所述里程计行走模型进行预测更新,具体包括:
通过状态转移方程:
计算状态一步预测值;
并通过公式:
计算状态一步预测协方差;
优选地,利用扩展卡尔曼滤波算法根据各所述当前状态参数对所述状态一步预测值和所述状态一步预测协方差进行测量更新,具体包括:
通过公式:
计算卡尔曼滤波增益;
其中,Kt+1为卡尔曼滤波增益,Ht+1为测量矩阵,Ht+1 T为Ht+1的转置,Qt+1为测量误差向量;
并通过公式:
计算掘进设备的当前状态最优估计值;
其中,μt+1为掘进设备的当前状态最优估计值,Dt+1为量测预测误差;
再通过公式:
计算掘进设备的当前状态最优估计协方差;
其中,Σt+1为掘进设备的当前状态最优估计协方差,I为单位阵。
优选地,利用扩展卡尔曼滤波算法根据各所述当前状态参数对所述状态一步预测值和所述状态一步预测协方差进行测量更新,具体包括:
分别通过所述惯性导航仪、所述超宽带定位组件及所述全站仪的测量矩阵、量测预测误差、测量误差向量对所述状态一步预测值和所述状态一步预测协方差进行测量更新。
本发明还提供一种基于扩展卡尔曼滤波的巷道实时定位导航***,包括:
初始获取模块,用于获取掘进设备在巷道内的初始状态参数及巷道相对于地理坐标系的转换关系;
模型建立模块,用于利用所述初始状态参数及所述转换关系通过基于模型线性近似方法建立里程计行走模型;
预测更新模块,用于利用扩展卡尔曼滤波算法以所述里程计行走模型中的相关参数作为滤波状态变量对所述里程计行走模型进行预测更新,以获得掘进设备的状态一步预测值和状态一步预测协方差;
测量更新模块,用于获取掘进设备在巷道内的当前状态参数,并利用扩展卡尔曼滤波算法根据各所述当前状态参数对所述状态一步预测值和所述状态一步预测协方差进行测量更新,以获得掘进设备的当前状态最优估计值和当前状态最优估计协方差;
控制输出模块,用于根据所述当前状态最优估计值及所述当前状态最优估计协方差控制掘进设备的运动状态。
本发明所提供的基于扩展卡尔曼滤波的巷道实时定位导航方法,主要包括四个步骤。在第一步中,主要内容为获取初始时刻(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,α,β,γ,ω,κ,Δα,Δγ]T
其中,空间坐标即空间三坐标,分别为车体相对于巷道的纵向位置x(里程或行走距离)、车体在巷道内的横向位置y(左偏)和车体在巷道内的垂向位置z(上偏);而掘进设备的姿态角度分别为俯仰角α、横滚角β和航向角γ。纵向位置x除以行走编码器的增量为行走系数ω,转向角度除以行走编码器的增量差值(车体两侧各设置一个行走编码器,分别为左侧行走编码器和右侧行走编码器)为转向系数κ,行走轨迹(比如履带轨迹)相对于车体的俯仰修正角Δα,行走轨迹相对于车体的航向修正角Δγ也纳入滤波状态变量中。同时,ω、κ、Δα、Δγ又可统称为行走特性参数。
在获取巷道相对于地理坐标系的转换关系时,具体的,可通过架设在巷道口的全站仪及安装于掘进设备上的棱镜进行获取;在获取掘进设备的空间坐标时,具体的,可通过安装于巷道口及掘进设备上的超宽带定位组件获取掘进设备在巷道内的纵向位置,同时通过全站仪及棱镜获取掘进设备在巷道内的位置(包括纵向位置、横向位置及垂向位置);在获取掘进设备的姿态角度时,具体的,可通过安装于掘进设备上的惯性导航仪获取掘进设备的姿态角度。
此外,扩展卡尔曼滤波算法即为利用前一时刻的状态值做预测,获得当前时刻的状态预测值,再利用当前时刻的测量值修正预测值,获得最小均方误差的状态估计值,共分为两个环节,分别为预测更新和测量更新,其中步骤S2主要实现预测更新,而步骤S3主要实现测量更新。
首先有状态转移方程其中ut+1=[θ1,θ2]T,θ1为左侧行走编码器的增量,θ2为右侧行走编码器的增量,为状态一步预测值,Wt+1是***噪声,常将其取为均值为0且协方差为Rt+1的高斯白噪声,μt为掘进设备处于t时刻的状态值,g为函数关系,T为矩阵的转置。
基于模型线性近似的方法建立里程计行走模型,即为状态转移方程的详细描述:
其中,ε为t到t+1时间内,***预测的不确定性误差,即由***噪声引入的误差。
在步骤S3中,首先有测量误差方程:
为明确权重,需要通过公式①计算卡尔曼滤波增益:
之后,即可通过公式②计算掘进设备的当前状态最优估计值:
同时,还可通过公式③计算掘进设备的当前状态最优估计协方差:
其中,Kt+1为卡尔曼滤波增益,Ht+1为测量矩阵(函数h在处的一阶偏导数),Ht+1 T为Ht+1的转置,Qt+1为测量误差向量,μt+1为掘进设备的当前状态最优估计值,Dt+1为量测预测误差,Σt+1为掘进设备的当前状态最优估计协方差,I为单位阵。
另外,为满足掘进设备在巷道定位的实时高精度需求,本实施例中的测量更新环节将分为通过惯性导航测量更新、超宽带测距测量更新、全站仪测量更新和人工测量更新四种,并使用扩展卡尔曼滤波算法对采集的观测量进行融合计算。
对于通过惯性导航测量更新时,可首先建立惯性导航测量误差模型:
其中,eα为俯仰角的量测预测误差(由上一步的预测输出减去仪器测量值计算得到),eβ为横滚角的量测预测误差,eγ为航向角的量测预测误差,为一步预测的俯仰角(里程计行走模型输出的预测值),为一步预测的横滚角,为一步预测的航向角。
其测量矩阵为:
其量测预测误差为:
其测量误差向量为:
其中,δα,δβ,δγ为惯性导航仪本身引入的测量误差,常取为仪器的精度值。在计算时,只需将上述测量矩阵、量测预测误差、测量误差向量分别代入前述公式①、公式②、公式③中,即可分别计算出通过惯性导航测量更新的卡尔曼滤波增益、当前状态最优估计值、当前状态最优估计协方差。
对于通过超宽带测距测量更新时,为解决由于巷道空间受限而导致的超宽带测距在左右、高度方向的定位误差,基于现有已知的巷道掘进设备工作条件,建立掘进设备相对巷道的距离模型,获取掘进设备在巷道前行的距离。
超宽带测距的距离模型为:
其中,L为超宽带测距结果,为载体相对巷道的旋转矩阵,超宽带定位组件中的接收器在巷道的安装位置为掘进设备的车体上安装的发送器位置为掘进设备相对巷道的位置PB=[x,y,z]T,eL为超宽带测距的量测预测误差,为预测的测距值。
其测量矩阵为:
其量测预测误差为:
D=eL
其测量误差向量为:
Qt+1=[δL 2]
其中,δL为超宽带定位组件本身引入的测量误差,常取为仪器的精度值。
只需将上述测量矩阵、量测预测误差、测量误差向量分别代入前述公式①、公式②、公式③中,即可分别计算出通过超宽带测距测量更新的卡尔曼滤波增益、当前状态最优估计值、当前状态最优估计协方差。
对于通过全站仪测量更新时,可首先建立全站仪测量误差模型:
其测量矩阵为:
其量测预测误差为:
其测量误差向量为:
其中,δx,δy,δz即为全站仪本身引入的测量误差,常取为仪器的精度值。
只需将上述测量矩阵、量测预测误差、测量误差向量分别代入前述公式①、公式②、公式③中,即可分别计算出通过超宽带测距测量更新的卡尔曼滤波增益、当前状态最优估计值、当前状态最优估计协方差。
对于通过人工测量更新时,现场操作人员可通过粗略测量,估计给出掘进设备相对于巷道行驶的x、y、z值作为观测量Zt+1,人工测量误差模型、测量矩阵、量测预测误差及测量误差向量的取值原理与前述相同,将其分别代入前述公式①、公式②、公式③中,即可分别计算出通过人工测量超宽带测距测量更新的卡尔曼滤波增益、当前状态最优估计值、当前状态最优估计协方差。
如图3、图4所示,图3为本发明所提供的一种具体实施方式的模块结构图,图4为本发明所提供的一种具体实施方式的硬件结构图。
本实施例还提供一种基于扩展卡尔曼滤波的巷道实时定位导航***,主要包括初始获取模块1、模型建立模块2、预测更新模块3、测量更新模块4和控制输出模块5。其中,初始获取模块1主要用于获取掘进设备在巷道内的初始状态参数及巷道相对于地理坐标系的转换关系。模型建立模块2主要用于利用初始状态参数及转换关系通过基于模型线性近似方法建立里程计行走模型。预测更新模块3主要用于利用扩展卡尔曼滤波算法以里程计行走模型中的相关参数作为滤波状态变量对里程计行走模型进行预测更新,以获得掘进设备的状态一步预测值和状态一步预测协方差。测量更新模块4主要用于获取掘进设备在巷道内的当前状态参数,并利用扩展卡尔曼滤波算法根据各当前状态参数对状态一步预测值和状态一步预测协方差进行测量更新,以获得掘进设备的当前状态最优估计值和当前状态最优估计协方差。控制输出模块5主要用于根据当前状态最优估计值及当前状态最优估计协方差控制掘进设备的运动状态。
对于硬件结构,本实施例在巷道口位置处设置有全站仪7和超宽带定位组件9的接收端,在车体6上安装有超宽带定位组件9的发送端、与全站仪7配合使用的棱镜8、惯性导航仪10和分布与车身左右两侧的行走编码器11。
对所公开的实施例的上述说明,使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的,本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下,在其它实施例中实现。因此,本发明将不会被限制于本文所示的这些实施例,而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
Claims (9)
1.一种基于扩展卡尔曼滤波的巷道实时定位导航方法,其特征在于,包括:
获取掘进设备在巷道内的初始状态参数及巷道相对于地理坐标系的转换关系;
利用所述初始状态参数及所述转换关系通过基于模型线性近似方法建立里程计行走模型,并利用扩展卡尔曼滤波算法以所述里程计行走模型中的相关参数作为滤波状态变量对所述里程计行走模型进行预测更新,以获得掘进设备的状态一步预测值和状态一步预测协方差;
获取掘进设备在巷道内的当前状态参数,并利用扩展卡尔曼滤波算法根据各所述当前状态参数对所述状态一步预测值和所述状态一步预测协方差进行测量更新,以获得掘进设备的当前状态最优估计值和当前状态最优估计协方差;
根据所述当前状态最优估计值及所述当前状态最优估计协方差控制掘进设备的运动状态。
2.根据权利要求1所述的基于扩展卡尔曼滤波的巷道实时定位导航方法,其特征在于,获取掘进设备在巷道内的初始状态参数,具体包括:
获取掘进设备的空间坐标、姿态角度、行走系数、转向系数、行走轨迹相对于车体的俯仰修正角、行走轨迹相对于车体的航向修正角。
3.根据权利要求2所述的基于扩展卡尔曼滤波的巷道实时定位导航方法,其特征在于,获取巷道相对于地理坐标系的转换关系,具体包括:
通过架设在巷道口的全站仪及安装于掘进设备上的棱镜获取巷道相对于地理坐标系的转换关系。
4.根据权利要求3所述的基于扩展卡尔曼滤波的巷道实时定位导航方法,其特征在于,获取掘进设备的空间坐标,具体包括:
通过安装于巷道口及掘进设备上的超宽带定位组件获取掘进设备在巷道内的纵向位置;
通过所述全站仪及所述棱镜获取掘进设备在巷道内的位置。
5.根据权利要求4所述的基于扩展卡尔曼滤波的巷道实时定位导航方法,其特征在于,获取掘进设备的姿态角度,具体包括:
通过安装于掘进设备上的惯性导航仪获取掘进设备的姿态角度。
6.根据权利要求5所述的基于扩展卡尔曼滤波的巷道实时定位导航方法,其特征在于,利用扩展卡尔曼滤波算法以所述里程计行走模型中的相关参数作为滤波状态变量对所述里程计行走模型进行预测更新,具体包括:
通过状态转移方程:
计算状态一步预测值;
并通过公式:
计算状态一步预测协方差;
8.根据权利要求7所述的基于扩展卡尔曼滤波的巷道实时定位导航方法,其特征在于,利用扩展卡尔曼滤波算法根据各所述当前状态参数对所述状态一步预测值和所述状态一步预测协方差进行测量更新,具体包括:
分别通过所述惯性导航仪、所述超宽带定位组件及所述全站仪的测量矩阵、量测预测误差、测量误差向量对所述状态一步预测值和所述状态一步预测协方差进行测量更新。
9.一种基于扩展卡尔曼滤波的巷道实时定位导航***,其特征在于,包括:
初始获取模块,用于获取掘进设备在巷道内的初始状态参数及巷道相对于地理坐标系的转换关系;
模型建立模块,用于利用所述初始状态参数及所述转换关系通过基于模型线性近似方法建立里程计行走模型;
预测更新模块,用于利用扩展卡尔曼滤波算法以所述里程计行走模型中的相关参数作为滤波状态变量对所述里程计行走模型进行预测更新,以获得掘进设备的状态一步预测值和状态一步预测协方差;
测量更新模块,用于获取掘进设备在巷道内的当前状态参数,并利用扩展卡尔曼滤波算法根据各所述当前状态参数对所述状态一步预测值和所述状态一步预测协方差进行测量更新,以获得掘进设备的当前状态最优估计值和当前状态最优估计协方差;
控制输出模块,用于根据所述当前状态最优估计值及所述当前状态最优估计协方差控制掘进设备的运动状态。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011424015.2A CN112683268A (zh) | 2020-12-08 | 2020-12-08 | 一种基于扩展卡尔曼滤波的巷道实时定位导航方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011424015.2A CN112683268A (zh) | 2020-12-08 | 2020-12-08 | 一种基于扩展卡尔曼滤波的巷道实时定位导航方法及*** |
Publications (1)
Publication Number | Publication Date |
---|---|
CN112683268A true CN112683268A (zh) | 2021-04-20 |
Family
ID=75446339
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011424015.2A Pending CN112683268A (zh) | 2020-12-08 | 2020-12-08 | 一种基于扩展卡尔曼滤波的巷道实时定位导航方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112683268A (zh) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113252044A (zh) * | 2021-05-25 | 2021-08-13 | 中国煤炭科工集团太原研究院有限公司 | 一种掘进装备机身偏差计算方法 |
CN114166508A (zh) * | 2021-12-09 | 2022-03-11 | 中国铁建重工集团股份有限公司 | 一种实时获取多排滚子转盘轴承载荷分布状态的方法 |
CN114323003A (zh) * | 2021-12-27 | 2022-04-12 | 青岛慧拓智能机器有限公司 | 一种基于umb、imu及激光雷达的井工矿融合定位方法 |
CN114543795A (zh) * | 2021-12-31 | 2022-05-27 | 文远苏行(江苏)科技有限公司 | 双天线航向角的安装误差估计方法和调整方法及相关设备 |
CN114777749A (zh) * | 2022-04-26 | 2022-07-22 | 华中科技大学 | 一种掘进机位姿测量方法、***、介质、设备及终端 |
CN116222578A (zh) * | 2023-05-04 | 2023-06-06 | 山东大学 | 基于自适应滤波和最优平滑的水下组合导航方法及*** |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105588566A (zh) * | 2016-01-08 | 2016-05-18 | 重庆邮电大学 | 一种基于蓝牙与mems融合的室内定位***及方法 |
CN108180925A (zh) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | 一种里程计辅助车载动态对准方法 |
CN108871325A (zh) * | 2018-06-26 | 2018-11-23 | 北京航空航天大学 | 一种基于两层扩展卡尔曼滤波的WiFi/MEMS组合室内定位方法 |
CN110296709A (zh) * | 2019-07-23 | 2019-10-01 | 南京邮电大学 | 基于自适应里程计模型的车载定位导航方法 |
CN110375730A (zh) * | 2019-06-12 | 2019-10-25 | 深圳大学 | 基于imu和uwb融合的室内定位导航*** |
CN110849374A (zh) * | 2019-12-03 | 2020-02-28 | 中南大学 | 地下环境定位方法、装置、设备及存储介质 |
US10578743B1 (en) * | 2018-12-27 | 2020-03-03 | Intel Corporation | System and method of location determination using multiple location inputs |
CN111256727A (zh) * | 2020-02-19 | 2020-06-09 | 广州蓝胖子机器人有限公司 | 一种基于Augmented EKF的提高里程计精度的方法 |
-
2020
- 2020-12-08 CN CN202011424015.2A patent/CN112683268A/zh active Pending
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105588566A (zh) * | 2016-01-08 | 2016-05-18 | 重庆邮电大学 | 一种基于蓝牙与mems融合的室内定位***及方法 |
CN108180925A (zh) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | 一种里程计辅助车载动态对准方法 |
CN108871325A (zh) * | 2018-06-26 | 2018-11-23 | 北京航空航天大学 | 一种基于两层扩展卡尔曼滤波的WiFi/MEMS组合室内定位方法 |
US10578743B1 (en) * | 2018-12-27 | 2020-03-03 | Intel Corporation | System and method of location determination using multiple location inputs |
CN110375730A (zh) * | 2019-06-12 | 2019-10-25 | 深圳大学 | 基于imu和uwb融合的室内定位导航*** |
CN110296709A (zh) * | 2019-07-23 | 2019-10-01 | 南京邮电大学 | 基于自适应里程计模型的车载定位导航方法 |
CN110849374A (zh) * | 2019-12-03 | 2020-02-28 | 中南大学 | 地下环境定位方法、装置、设备及存储介质 |
CN111256727A (zh) * | 2020-02-19 | 2020-06-09 | 广州蓝胖子机器人有限公司 | 一种基于Augmented EKF的提高里程计精度的方法 |
Non-Patent Citations (3)
Title |
---|
孟祥萍等: "自适应扩展卡尔曼滤波器在移动机器人定位中的应用", 《计算机***应用》 * |
张胜宾等: "基于卡尔曼滤波的轮式移动机器人定位实验研究", 《机电工程技术》 * |
谢海天: "基于捷联惯导的电缆隧道巡检机器人定位技术研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113252044A (zh) * | 2021-05-25 | 2021-08-13 | 中国煤炭科工集团太原研究院有限公司 | 一种掘进装备机身偏差计算方法 |
CN114166508A (zh) * | 2021-12-09 | 2022-03-11 | 中国铁建重工集团股份有限公司 | 一种实时获取多排滚子转盘轴承载荷分布状态的方法 |
CN114323003A (zh) * | 2021-12-27 | 2022-04-12 | 青岛慧拓智能机器有限公司 | 一种基于umb、imu及激光雷达的井工矿融合定位方法 |
CN114543795A (zh) * | 2021-12-31 | 2022-05-27 | 文远苏行(江苏)科技有限公司 | 双天线航向角的安装误差估计方法和调整方法及相关设备 |
CN114543795B (zh) * | 2021-12-31 | 2024-01-02 | 文远苏行(江苏)科技有限公司 | 双天线航向角的安装误差估计方法和调整方法及相关设备 |
CN114777749A (zh) * | 2022-04-26 | 2022-07-22 | 华中科技大学 | 一种掘进机位姿测量方法、***、介质、设备及终端 |
CN116222578A (zh) * | 2023-05-04 | 2023-06-06 | 山东大学 | 基于自适应滤波和最优平滑的水下组合导航方法及*** |
CN116222578B (zh) * | 2023-05-04 | 2023-08-29 | 山东大学 | 基于自适应滤波和最优平滑的水下组合导航方法及*** |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112683268A (zh) | 一种基于扩展卡尔曼滤波的巷道实时定位导航方法及*** | |
CN109946730B (zh) | 一种车路协同下基于超宽带的车辆高可靠融合定位方法 | |
Milanés et al. | Autonomous vehicle based in cooperative GPS and inertial systems | |
CN105509738B (zh) | 基于惯导/多普勒雷达组合的车载定位定向方法 | |
CN110702091B (zh) | 一种沿地铁轨道移动机器人的高精度定位方法 | |
CN109933056B (zh) | 一种基于slam的机器人导航方法以及机器人 | |
CN103335647B (zh) | 一种盾构机姿态测量***及其测量方法 | |
US5983166A (en) | Structure measurement system | |
CN107132563B (zh) | 一种里程计结合双天线差分gnss的组合导航方法 | |
EP0544403A1 (en) | Vehicle navigation system | |
CN108931244B (zh) | 基于列车运动约束的惯导误差抑制方法及*** | |
CN111637888B (zh) | 基于惯导和激光雷达单点测距的掘进机定位方法及*** | |
CN110542417B (zh) | 基于静态和和动态倾角仪校正的陀螺仪线形测量方法与*** | |
CN114166221B (zh) | 动态复杂矿井环境中辅助运输机器人定位方法及*** | |
CN112014849A (zh) | 一种基于传感器信息融合的无人车定位修正方法 | |
CN112628524A (zh) | 一种基于拐弯角的小径管道机器人高精度定位方法 | |
CN112229422A (zh) | 基于fpga时间同步的里程计快速标定方法及*** | |
CN114838732B (zh) | 一种基于图优化的通信受限环境下协同导航方法 | |
CN110333523B (zh) | 一种用于rtg自动行走***的轨道线三维数据生成方法 | |
CN113236363A (zh) | 开采设备导航定位方法、***、设备及可读存储介质 | |
CN114136275A (zh) | 一种轨道线路状态检测装置及路基沉降检测方法 | |
CN112229392B (zh) | 一种高冗余的室内煤场导航方法及*** | |
CN111811467B (zh) | 高速铁路轨面长波不均匀沉降动态检测方法、装置 | |
CN116242372B (zh) | 一种gnss拒止环境下的uwb-激光雷达-惯导融合定位方法 | |
CN115655268A (zh) | 一种基于图优化算法的地下矿井掘进机导航定位方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20210420 |
|
RJ01 | Rejection of invention patent application after publication |