基于无偏有限脉冲响应滤波器的管道机器人定位方法技术

本发明专利技术涉及一种基于无偏有限脉冲响应滤波器的管道机器人定位方法，包括：建立包括里程计和惯性传感器的导航系统及其状态变量，根据状态变量建立误差线性状态方程和观测方程；将无偏有限脉冲响应滤波器引入包括里程计和惯性传感器的导航系统中，结合误差线性状态方程和观测方程估计位置误差和速度误差；使用位置误差和速度误差对惯性传感器获取的机器人的位置和速度对进行修正，得到修正后的机器人的位置和速度。本发明专利技术可以实现对管道机器人的精确定位、降低滤波器对噪声知识的依赖、提高系统的鲁棒性。系统的鲁棒性。系统的鲁棒性。

【技术实现步骤摘要】
基于无偏有限脉冲响应滤波器的管道机器人定位方法


[0001]本专利技术涉及机器人定位
，尤其是指一种基于无偏有限脉冲响应滤波器的管道机器人定位方法。

技术介绍

[0002]为了满足社会日常生活的需要，人们安装了大量的地下设施，如水、污物处理、电力、天然气、电信等管道，在这些地下管道的安装和维护过程中，都需要精确的探测和定位技术。
[0003]当前，已有无需挖洞即可探测和定位管道的方法。常用的无需挖洞探测和定位管道的方法有使用振动声学、无源磁场、低频电磁场和探地雷达等，这些方法减少了挖掘的次数、程度和成本，更是降低了由于业务中断和环境破坏带来的间接开支。然而，这些方法存在诸如难以探测非金属管道、探测深度有限、易受电磁场干扰等不足之处。
[0004]惯性传感器(Inertial Navigation Sensor，INS)由三组正交安装的陀螺仪和加速度计构成，可以在短期内提供高频率、高精度的输出；但是，惯性传感器具有误差随时间而发散的缺点。应用于管道机器人定位的导航系统需要提供机器人的位置、速度和姿态等运动状态，因此需要依据机器人运动状态的误差模型去除各传感器的噪声，对估计解算得到的运动状态中的误差进行补偿。卡尔曼滤波器可以给出具有最小均方误差的最优估计，可以准确地实现误差的估计，且具有易于理解、便于实现的特点，所以得到了广泛的应用。但是，卡尔曼滤波器对过程噪声和量测噪声的设置较为敏感，同时管道机器人误差模型中的噪声设置通常又需要丰富的实践经验以及反复的调试，调试过程复杂耗时，所以在管道机器人的导航系统中使用卡尔曼滤波器理论效果不好。

技术实现思路

[0005]为此，本专利技术所要解决的技术问题在于克服现有技术中的不足，提供一种基于无偏有限脉冲响应滤波器的管道机器人定位方法，可以实现对管道机器人的精确定位、降低滤波器对噪声知识的依赖、提高系统的鲁棒性。
[0006]为解决上述技术问题，本专利技术提供了一种基于无偏有限脉冲响应滤波器的管道机器人定位方法，包括以下步骤：
[0007]S1：建立包括里程计和惯性传感器的导航系统，建立所述包括里程计和惯性传感器的导航系统的状态变量，根据所述包括里程计和惯性传感器的导航系统的状态变量建立误差线性状态方程和观测方程；
[0008]S2：将无偏有限脉冲响应滤波器引入所述包括里程计和惯性传感器的导航系统中，结合所述误差线性状态方程和观测方程估计位置误差和速度误差；
[0009]S3：使用所述位置误差和速度误差对所述惯性传感器获取的机器人的位置和速度对进行修正，得到修正后的机器人的位置和速度。
[0010]作为优选的，所述包括里程计和惯性传感器的导航系统的状态变量x
k
为：
[0011]x
k
＝[δP
E,k
,δV
E,k
,δP
N,k
,δV
N,k
,δP
U,k
,δV
U,k
]T
，
[0012]其中，k表示时间索引，x
k
表示k时刻系统的状态变量；表示k时刻的东向位移误差，表示k时刻的东向速度误差，表示k时刻的北向位移误差，表示k时刻的北向速度误差，表示k时刻的天向位移误差，表示k时刻的天向速度误差；其中，P
E,k
表示里程计在东向测量的位置，P
N,k
表示里程计在北向测量的位置，P
U,k
表示里程计在天向测量的位置，V
E,k
表示里程计在东向测量位置上的速度，V
N,k
表示里程计在北向测量位置上的速度，V
U,k
表示里程计在天向测量位置上的速度；表示惯性传感器在东向测量的位置，表示惯性传感器在北向测量的位置，表示惯性传感器在天向测量的位置，表示惯性传感器在东向测量位置上的速度，表示惯性传感器在北向测量位置上的速度，表示惯性传感器在天向测量位置上的速度。
[0013]作为优选的，所述误差线性状态方程为：
[0014]x
k
＝Fx
k
‑1+w
k
，
[0015]其中，是均值为0，协方差矩阵为Q
k
的过程噪声，F为状态转移矩阵，F中的θ是采样时间。
[0016]作为优选的，所述观测方程为：
[0017]y
k
＝Hx
k
+v
k
，
[0018]其中，y
k
＝[δP
E,k
，δP
N,k
，δP
U,k
]T
是k时刻的测量值，是均值为0，协方差矩阵为R
k
的量测噪声，H为观测矩阵，
[0019]作为优选的，所述将无偏有限脉冲响应滤波器引入所述包括里程计和惯性传感器的导航系统中，结合所述误差线性状态方程和观测方程估计位置误差和速度误差，具体为：
[0020]S2
‑
1：构建无偏有限脉冲响应滤波测量方程；
[0021]S2
‑
2：根据所述无偏有限脉冲响应滤波测量方程计算n时刻的广义噪声功率增益G
n
；
[0022]S2
‑
3：根据所述n时刻的广义噪声功率增益G
n
计算k时刻的广义噪声功率增益G
k
；
[0023]S2
‑
4：根据所述G
k
计算k时刻的迭代无偏有限脉冲响应滤波器增益K
k
；
[0024]S2
‑
5：建立k时刻状态的先验预测方程；
[0025]S2
‑
6：根据所述k时刻状态的先验预测方程、K
k
和观测方程建立k时刻的后验更新方程；
[0026]S2
‑
7：将根据k时刻的后验更新方程得到的k时刻的后验状态估计值作为k时刻的所述包括里程计和惯性传感器的导航系统的状态变量x
k
的估计值，得到估计的位置误差和估计的速度误差。
[0027]作为优选的，所述无偏有限脉冲响应滤波测量方程为：
[0028][0029]其中，x
n
是n时刻的批次无偏有限脉冲响应滤波器的估计值，是从0到n时刻的批次无偏有限脉冲响应滤波器增益，Y
0,n
是用0到n时刻的测量值构造的矩阵；
[0030]所述0到n时刻的批次无偏有限脉冲响应滤波器增益为：
[0031][0032]其中，F
n
表示状态转移矩阵F的n次幂，矩阵()
T
表示矩阵的转置，()
‑1表示矩阵的逆；
[0033]所述0到n时刻的测量值构造的矩阵Y
0,n
为：
[0034][0035]其中，y
m
＝[本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于无偏有限脉冲响应滤波器的管道机器人定位方法，其特征在于，包括以下步骤：S1：建立包括里程计和惯性传感器的导航系统，建立所述包括里程计和惯性传感器的导航系统的状态变量，根据所述包括里程计和惯性传感器的导航系统的状态变量建立误差线性状态方程和观测方程；S2：将无偏有限脉冲响应滤波器引入所述包括里程计和惯性传感器的导航系统中，结合所述误差线性状态方程和观测方程估计位置误差和速度误差；S3：使用所述位置误差和速度误差对所述惯性传感器获取的机器人的位置和速度对进行修正，得到修正后的机器人的位置和速度。2.根据权利要求1所述的基于无偏有限脉冲响应滤波器的管道机器人定位方法，其特征在于：所述包括里程计和惯性传感器的导航系统的状态变量x
k
为：x
k
＝[δP
E,k
,δV
E,k
,δP
N,k
,δV
N,k
,δP
U,k
,δV
U,k
]
T
，其中，k表示时间索引，x
k
表示k时刻系统的状态变量；表示k时刻的东向位移误差，表示k时刻的东向速度误差，表示k时刻的北向位移误差，表示k时刻的北向速度误差，表示k时刻的天向位移误差，表示k时刻的天向速度误差；其中，P
E,k
表示里程计在东向测量的位置，P
N,k
表示里程计在北向测量的位置，P
U,k
表示里程计在天向测量的位置，V
E,k
表示里程计在东向测量位置上的速度，V
N,k
表示里程计在北向测量位置上的速度，V
U,k
表示里程计在天向测量位置上的速度；表示惯性传感器在东向测量的位置，表示惯性传感器在北向测量的位置，表示惯性传感器在天向测量的位置，表示惯性传感器在东向测量位置上的速度，表示惯性传感器在北向测量位置上的速度，表示惯性传感器在天向测量位置上的速度。3.根据权利要求2所述的基于无偏有限脉冲响应滤波器的管道机器人定位方法，其特征在于：所述误差线性状态方程为：x
k
＝Fx
k
‑1+w
k
，其中，是均值为0，协方差矩阵为Q
k
的过程噪声，F为状态转移矩阵，F中的θ是采样时间。4.根据权利要求3所述的基于无偏有限脉冲响应滤波器的管道机器人定位方法，其特征在于：所述观测方程为：y
k
＝Hx
k
+v
k
，
其中，y
k
＝[δP
E,k
，δP
N,k
，δP
U,k
]
T
是k时刻的测量值，是均值为0，协方差矩阵为R
k
的量测噪声，H为观测矩阵，5.根据权利要求4所述的基于无偏有限脉冲响应滤波器的管道机器人定位方法，其特征在于：所述将无偏有限脉冲响应滤波器引入所述包括里程计和惯性传感器的导航系统中，结合所述误差线性状态方程和观测方程估计位置误差和速度误差，具体为：S2
‑
1：构建无偏有限脉冲...

【专利技术属性】
技术研发人员：赵顺毅，周政，
申请(专利权)人：杭州天眼信息咨询有限公司，
类型：发明
国别省市：