基于最小方差无偏有限脉冲响应的机器人定位方法及系统技术方案

本发明专利技术涉及一种基于最小方差无偏有限脉冲响应的机器人定位方法及系统，方法包括建立包括超宽带装置和惯性传感器的耦合定位系统，根据系统状态变量建立误差线性状态方程和观测方程；将最小方差无偏有限脉冲响应滤波器引入系统，结合误差线性状态方程和观测方程构建滤波器的状态预测方程；化简滤波器的增益后使用状态预测方程估计机器人的速度误差和位置误差；使用速度误差和位置误差修正惯性传感器测量的机器人的速度和位置得到最终的速度和位置。系统包括惯导系统、超宽带模块和最小方差无偏有限脉冲响应滤波器，惯导系统包括惯性传感器，超宽带模块包括超宽带装置。本发明专利技术可以高效稳定地得到准确的室内机器人定位结果。以高效稳定地得到准确的室内机器人定位结果。以高效稳定地得到准确的室内机器人定位结果。

【技术实现步骤摘要】
基于最小方差无偏有限脉冲响应的机器人定位方法及系统


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

技术介绍

[0002]近年来，工业界对室内定位的需求迅速增长，定位系统需要满足可靠、快速和高精度等特点。目前常见的室内定位技术中，WiFi定位技术存在覆盖范围较大、精度相对较高等优点，但存在信息收集量大、易受干扰等缺点；射频识别技术精度较高成本低，但存在定位距离太短、不利于整合的缺点；使用惯性传感器(Inertial Navigation Sensor,INS)的惯性导航系统(惯导)可以不依赖外界环境，但存在随着时间的增加有累计误差的缺点。同时，在定位系统中，常使用超宽带(Ultra Wide Band,UWB)技术来实现位置地点的定位，但是这种方法无法有效降低实际环境的干扰、设备限制以及非视距效应等噪声的影响。

技术实现思路

[0003]为此，本专利技术所要解决的技术问题在于克服现有技术中的不足，提供一种基于最小方差无偏有限脉冲响应的机器人定位方法及系统，可以高效稳定地得到准确的室内机器人定位结果。
[0004]为解决上述技术问题，本专利技术提供了一种基于最小方差无偏有限脉冲响应的机器人定位方法，包括以下步骤：
[0005]S1：建立包括超宽带装置和惯性传感器的耦合定位系统，根据系统状态变量建立所述包括超宽带装置和惯性传感器的耦合定位系统的误差线性状态方程和观测方程；
[0006]S2：将最小方差无偏有限脉冲响应滤波器引入所述包括超宽带装置和惯性传感器的耦合定位系统中，结合误差线性状态方程和观测方程构建最小方差无偏有限脉冲响应滤波器的状态预测方程；
[0007]S3：化简最小方差无偏有限脉冲响应滤波器的增益，在此基础上使用所述状态预测方程估计机器人的速度误差和位置误差；
[0008]S4：使用所述速度误差和位置误差修正所述惯性传感器测量的机器人的速度和位置，得到机器人最终的速度和位置。
[0009]作为优选的，所述误差线性状态方程为：
[0010]x
k
＝Ax
k
‑1+w
k
，
[0011]其中，x
k
为k时刻的所述包括超宽带装置和惯性传感器的耦合定位系统的状态变量，x
k
‑1为k
‑
1时刻的所述包括超宽带装置和惯性传感器的耦合定位系统的状态变量，x
k
＝[δP
E,k
,δV
E,k
,δP
N,k
,δV
N,k
]T
；δP
E,k
＝P
E,k
‑
P
′
E,k
表示k时刻的东向位移误差，δV
E,k
＝V
E,k
‑
V
′
E,k
表示k时刻的东向速度误差，δP
N,k
＝P
N,k
‑
P
′
N,k
表示k时刻的北向位移误差，δV
N,k
＝V
N,k
‑
V
′
N,k
表示k时刻的北向速度误差；P
E,k
表示超宽带装置k时刻在东方向测得的位置，P
N,k
表示超宽带装置k时刻在北方向测得的位置，V
E,k
表示超宽带装置k时刻在东方向的速度，V
N,k
表示超宽
带装置k时刻在北方向的速度，P
′
E,k
表示惯性传感器k时刻在东方向测得的位置，P
′
N,k
表示惯性传感器k时刻在北方向测得的位置，V
′
E,k
表示惯性传感器k时刻在东方向测得的速度，V
′
N,k
表示惯性传感器k时刻在北方向测得的速度；w
k
为过程噪声，A为状态转移矩阵，A中的θ为采样时间。
[0012]作为优选的，所述观测方程为：
[0013]y
k
＝Cx
k
+v
k
，
[0014]其中，y
k
＝[δP
E,k
,δP
N,k
]T
是k时刻的测量值，v
k
是k时刻的测量噪声，C是测量矩阵，
[0015]作为优选的，所述状态预测方程为：
[0016][0017]其中，表示k时刻的状态变量x
k
的估计值，K
N
是最小方差无偏有限脉冲响应滤波器的增益；Y
k,l
是由从时间l＝k
‑
N+1到k的测量向量构造的矩阵，N是自主选择的批处理窗口的长度，N大于等于所述系统状态变量的维数。
[0018]作为优选的，所述化简最小方差无偏有限脉冲响应滤波器的增益，在此基础上使用所述状态预测方程估计机器人的速度误差和位置误差，具体为：
[0019]S3
‑
1：构建含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的初始增益K
N
；
[0020]S3
‑
2：在过程噪声能使系统稳定的情况下取最小值，化简最小方差无偏有限脉冲响应滤波器的初始增益得到K
′
N
；
[0021]S3
‑
3：处理测量噪声进一步化简K
′
N
，得到最终的不含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的增益K
″
N
；
[0022]S3
‑
4：使用最终的不含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的增益K
″
N
更新状态预测方程得到：
[0023][0024]根据更新后的状态预测方程得到的k时刻的状态变量x
k
的估计值得到估计的位置误差和估计的速度误差。
[0025]作为优选的，所述含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的初始增益K
N
为：
[0026]K
N
＝K
N1
+K
N2
，其中：
[0027][0028][0029]K
N1
和K
N2
中：
[0030]A
N
‑1是所述状态转移矩阵A的N
‑
1次幂，()
T
是矩阵或向量的转置，C
N
‑1是i＝N
‑
1时刻的C
i
，A
i
)[(A
i
)
T
,(A
i...

【技术保护点】

【技术特征摘要】
1.一种基于最小方差无偏有限脉冲响应的机器人定位方法，其特征在于，包括以下步骤：S1：建立包括超宽带装置和惯性传感器的耦合定位系统，根据系统状态变量建立所述包括超宽带装置和惯性传感器的耦合定位系统的误差线性状态方程和观测方程；S2：将最小方差无偏有限脉冲响应滤波器引入所述包括超宽带装置和惯性传感器的耦合定位系统中，结合误差线性状态方程和观测方程构建最小方差无偏有限脉冲响应滤波器的状态预测方程；S3：化简最小方差无偏有限脉冲响应滤波器的增益，在此基础上使用所述状态预测方程估计机器人的速度误差和位置误差；S4：使用所述速度误差和位置误差修正所述惯性传感器测量的机器人的速度和位置，得到机器人最终的速度和位置。2.根据权利要求1所述的基于最小方差无偏有限脉冲响应的机器人定位方法，其特征在于：所述误差线性状态方程为：x
k
＝Ax
k
‑1+w
k
，其中，x
k
为k时刻的所述包括超宽带装置和惯性传感器的耦合定位系统的状态变量，x
k
‑1为k
‑
1时刻的所述包括超宽带装置和惯性传感器的耦合定位系统的状态变量，x
k
＝[δP
E，k
，δV
E，k
，δP
N，k
，δV
N，k
]
T
；δP
E，k
＝P
E，k
‑
P
′
E，k
表示k时刻的东向位移误差，δV
E，k
＝V
E，k
‑
V
′
E，k
表示k时刻的东向速度误差，δP
N，k
＝P
N，k
‑
P
′
N，k
表示k时刻的北向位移误差，δV
N，k
＝V
N，k
‑
V
′
N，k
表示k时刻的北向速度误差；P
E,k
表示超宽带装置k时刻在东方向测得的位置，P
N,k
表示超宽带装置k时刻在北方向测得的位置，V
E,k
表示超宽带装置k时刻在东方向的速度，V
N,k
表示超宽带装置k时刻在北方向的速度，P
′
E,k
表示惯性传感器k时刻在东方向测得的位置，P
′
N,k
表示惯性传感器k时刻在北方向测得的位置，V
′
E,k
表示惯性传感器k时刻在东方向测得的速度，V
′
N,k
表示惯性传感器k时刻在北方向测得的速度；w
k
为过程噪声，A为状态转移矩阵，A中的θ为采样时间。3.根据权利要求2所述的基于最小方差无偏有限脉冲响应的机器人定位方法，其特征在于：所述观测方程为：y
k
＝Cx
k
+v
k
，其中，y
k
＝[δP
E,k
,δP
N,k
]
T
是k时刻的测量值，v
k
是k时刻的测量噪声，C是测量矩阵，4.根据权利要求3所述的基于最小方差无偏有限脉冲响应的机器人定位方法，其特征在于：所述状态预测方程为：其中，表示k时刻的状态变量x
k
的估计值，K
N
是最小方差无偏有限脉冲响应滤波器的
增益；Y
k,l
是由从时间l＝k
‑
N+1到k的测量向量构造的矩阵，N是自主选择的批处理窗口的长度，N大于等于所述系统状态变量的维数。5.根据权利要求4所述的基于最小方差无偏有限脉冲响应的机器人定位方法，其特征在于：所述化简最小方差无偏有限脉冲响应滤波器的增益，在此基础上使用所述状态预测方程估计机器人的速度误差和位置误差，具体为：S3
‑
1：构建含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的初始增益K
N
；S3
‑
2：在过程噪声能使系统稳定的情况下取最小值，化简最小方差无偏有...

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