CN115290079A - 基于最小方差无偏有限脉冲响应的机器人定位方法及*** - Google Patents
基于最小方差无偏有限脉冲响应的机器人定位方法及*** Download PDFInfo
- Publication number
- CN115290079A CN115290079A CN202210801388.XA CN202210801388A CN115290079A CN 115290079 A CN115290079 A CN 115290079A CN 202210801388 A CN202210801388 A CN 202210801388A CN 115290079 A CN115290079 A CN 115290079A
- Authority
- CN
- China
- Prior art keywords
- impulse response
- finite impulse
- minimum variance
- time
- error
- 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.)
- Withdrawn
Links
- 238000000034 method Methods 0.000 title claims abstract description 109
- 230000004044 response Effects 0.000 title claims abstract description 71
- 230000008878 coupling Effects 0.000 claims abstract description 10
- 238000010168 coupling process Methods 0.000 claims abstract description 10
- 238000005859 coupling reaction Methods 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 42
- 238000005259 measurement Methods 0.000 claims description 36
- 230000008569 process Effects 0.000 claims description 34
- 238000012545 processing Methods 0.000 claims description 9
- 239000013598 vector Substances 0.000 claims description 9
- 238000006073 displacement reaction Methods 0.000 claims description 7
- 230000007704 transition Effects 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 4
- 239000000126 substance Substances 0.000 claims description 2
- 239000002245 particle Substances 0.000 description 20
- 238000005516 engineering process Methods 0.000 description 16
- 238000010586 diagram Methods 0.000 description 11
- 238000001914 filtration Methods 0.000 description 8
- 238000004590 computer program Methods 0.000 description 7
- 230000007547 defect Effects 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000003860 storage Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000036039 immunity Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000000149 penetrating effect Effects 0.000 description 2
- 230000006399 behavior Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000008054 signal transmission Effects 0.000 description 1
- 230000007480 spreading Effects 0.000 description 1
- 238000003892 spreading Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000033772 system development Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- H—ELECTRICITY
- H03—ELECTRONIC CIRCUITRY
- H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
- H03H17/00—Networks using digital techniques
- H03H17/02—Frequency selective networks
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/024—Guidance services
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
- H04W64/003—Locating users or terminals or network equipment for network management purposes, e.g. mobility management locating network equipment
-
- H—ELECTRICITY
- H03—ELECTRONIC CIRCUITRY
- H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
- H03H17/00—Networks using digital techniques
- H03H2017/0072—Theoretical filter design
- H03H2017/0081—Theoretical filter design of FIR filters
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Computer Hardware Design (AREA)
- Mathematical Physics (AREA)
- Feedback Control In General (AREA)
Abstract
本发明涉及一种基于最小方差无偏有限脉冲响应的机器人定位方法及***，方法包括建立包括超宽带装置和惯性传感器的耦合定位***，根据***状态变量建立误差线性状态方程和观测方程；将最小方差无偏有限脉冲响应滤波器引入***，结合误差线性状态方程和观测方程构建滤波器的状态预测方程；化简滤波器的增益后使用状态预测方程估计机器人的速度误差和位置误差；使用速度误差和位置误差修正惯性传感器测量的机器人的速度和位置得到最终的速度和位置。***包括惯导***、超宽带模块和最小方差无偏有限脉冲响应滤波器，惯导***包括惯性传感器，超宽带模块包括超宽带装置。本发明可以高效稳定地得到准确的室内机器人定位结果。
Description
技术领域
本发明涉及机器人定位技术领域，尤其是指一种基于最小方差无偏有限脉冲响应的机器人定位方法及***。
背景技术
近年来，工业界对室内定位的需求迅速增长，定位***需要满足可靠、快速和高精度等特点。目前常见的室内定位技术中，WiFi定位技术存在覆盖范围较大、精度相对较高等优点，但存在信息收集量大、易受干扰等缺点；射频识别技术精度较高成本低，但存在定位距离太短、不利于整合的缺点；使用惯性传感器(Inertial Navigation Sensor,INS)的惯性导航***(惯导)可以不依赖外界环境，但存在随着时间的增加有累计误差的缺点。同时，在定位***中，常使用超宽带(Ultra Wide Band,UWB)技术来实现位置地点的定位，但是这种方法无法有效降低实际环境的干扰、设备限制以及非视距效应等噪声的影响。
发明内容
为此，本发明所要解决的技术问题在于克服现有技术中的不足，提供一种基于最小方差无偏有限脉冲响应的机器人定位方法及***，可以高效稳定地得到准确的室内机器人定位结果。
为解决上述技术问题，本发明提供了一种基于最小方差无偏有限脉冲响应的机器人定位方法，包括以下步骤：
S1：建立包括超宽带装置和惯性传感器的耦合定位***，根据***状态变量建立所述包括超宽带装置和惯性传感器的耦合定位***的误差线性状态方程和观测方程；
S2：将最小方差无偏有限脉冲响应滤波器引入所述包括超宽带装置和惯性传感器的耦合定位***中，结合误差线性状态方程和观测方程构建最小方差无偏有限脉冲响应滤波器的状态预测方程；
S3：化简最小方差无偏有限脉冲响应滤波器的增益，在此基础上使用所述状态预测方程估计机器人的速度误差和位置误差；
S4：使用所述速度误差和位置误差修正所述惯性传感器测量的机器人的速度和位置，得到机器人最终的速度和位置。
作为优选的，所述误差线性状态方程为：
xk＝Axk-1+wk，
其中，xk为k时刻的所述包括超宽带装置和惯性传感器的耦合定位***的状态变量，xk-1为k-1时刻的所述包括超宽带装置和惯性传感器的耦合定位***的状态变量，xk＝[δPE,k,δVE,k,δPN,k,δVN,k]T；δPE,k＝PE,k-P′E,k表示k时刻的东向位移误差，δVE,k＝VE,k-V′E,k表示k时刻的东向速度误差，δPN,k＝PN,k-P′N,k表示k时刻的北向位移误差，δVN,k＝VN,k-V′N,k表示k时刻的北向速度误差；PE,k表示超宽带装置k时刻在东方向测得的位置，PN,k表示超宽带装置k时刻在北方向测得的位置，VE,k表示超宽带装置k时刻在东方向的速度，VN,k表示超宽带装置k时刻在北方向的速度，P′E,k表示惯性传感器k时刻在东方向测得的位置，P′N,k表示惯性传感器k时刻在北方向测得的位置，V′E,k表示惯性传感器k时刻在东方向测得的速度，V′N,k表示惯性传感器k时刻在北方向测得的速度；wk为过程噪声，A为状态转移矩阵，
A中的θ为采样时间。
作为优选的，所述观测方程为：
yk＝Cxk+vk，
其中，yk＝[δPE,k,δPN,k]T是k时刻的测量值，vk是k时刻的测量噪声，C是测量矩阵，
作为优选的，所述状态预测方程为：
其中，
表示k时刻的状态变量xk的估计值，KN是最小方差无偏有限脉冲响应滤波器的增益；Yk,l是由从时间l＝k-N+1到k的测量向量构造的矩阵，
N是自主选择的批处理窗口的长度，N大于等于所述***状态变量的维数。
作为优选的，所述化简最小方差无偏有限脉冲响应滤波器的增益，在此基础上使用所述状态预测方程估计机器人的速度误差和位置误差，具体为：
S3-1：构建含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的初始增益KN；
S3-2：在过程噪声能使***稳定的情况下取最小值，化简最小方差无偏有限脉冲响应滤波器的初始增益得到K′N；
S3-3：处理测量噪声进一步化简K′N，得到最终的不含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的增益K″N；
S3-4：使用最终的不含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的增益K″N更新状态预测方程得到：
根据更新后的状态预测方程得到的k时刻的状态变量xk的估计值
得到估计的位置误差和估计的速度误差。
作为优选的，所述含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的初始增益KN为：
KN＝KN1+KN2，其中：
KN1和KN2中：
AN-1是所述状态转移矩阵A的N-1次幂，
()T是矩阵或向量的转置，CN-1是i＝N-1时刻的Ci，
Ai)[(Ai)T,(Ai-1)T,...,AT,I]T，I是单位矩阵，
由i+1个C矩阵沿对角拼接而成；
BN-1是i＝N-1时刻的Bi，
B′i是Bi的首行向量；
QN-1是i＝N-1时刻的Qi，Qi由i+1个Q矩阵沿对角拼接而成，Q是过程噪声协方差矩阵；HN-1是i＝N-1时刻的Hi，
ZN-1是i＝N-1时刻的Zi，
Ri由i+1个R矩阵沿对角拼接而成，R是测量噪声协方差矩阵，()-1是矩阵的逆。
作为优选的，所述在过程噪声能使***稳定的情况下取最小值，化简最小方差无偏有限脉冲响应滤波器的初始增益得到K′N，具体为：
令过程噪声协方差矩阵Q取值为能使***稳定的最小值，此时KN2≈0，最小方差无偏有限脉冲响应滤波器的初始增益KN简化为：
其中，RN-1是i＝N-1时刻的Ri。
作为优选的，所述处理测量噪声进一步化简K′N，得到最终的不含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的增益K″N，具体为：
令过程噪声协方差矩阵R为具有相同非零元素的对角阵，继续简化K′N得到最终的不含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的增益K″N为：
本发明还提供了一种基于最小方差无偏有限脉冲响应的机器人定位***，包括惯导***、超宽带模块和最小方差无偏有限脉冲响应滤波器，所述惯导***包括惯性传感器，所述超宽带模块包括超宽带装置；
所述惯性传感器和所述超宽带装置分别测量机器人的速度和位置，
所述最小方差无偏有限脉冲响应滤波器结合所述惯性传感器和所述超宽带装置测量的机器人的速度和位置估计机器人的速度误差和位置误差；
所述惯导***使用所述度误差和位置误差修正所述惯性传感器测量的机器人的速度和位置，得到机器人最终的速度和位置。
作为优选的，所述超宽带装置使用基于到达时间差方法测量机器人的速度和位置。
本发明的上述技术方案相比现有技术具有以下优点：
本发明结合了超宽带技术、惯导技术和最小方差无偏有限脉冲响应滤波器，通过超宽带技术穿透力强的特点增加了定位距离，通过最小方差无偏有限脉冲响应滤波器修正惯性传感器的定位数据减少了误差；同时，超宽带技术与惯导技术耦合可以克服***稳定性差、易受到非视距效应等噪声影响的缺点，最小方差无偏有限脉冲响应滤波器也不受噪声统计特性的影响，可以高效稳定地得到准确的室内机器人定位结果。
附图说明
为了使本发明的内容更容易被清楚的理解，下面根据本发明的具体实施例并结合附图，对本发明作进一步详细的说明，其中
图1是本发明方法的流程图；
图2是本发明实施例中包括超宽带装置和惯性传感器的耦合定位***的结构示意图；
图3是本发明实施例中使用超宽带定位方法得到的跟踪数据图；
图4是本发明实施例中使用惯导导航得到的跟踪数据图；
图5是本发明实施例中使用本发明方法和粒子滤波方法得到的东方向的典型瞬时估计对比图；
图6是本发明实施例中使用本发明方法和粒子滤波方法得到的北方向的典型瞬时估计对比图；
图7是本发明实施例中使用本发明方法和粒子滤波方法的迹对比图；
图8是本发明实施例中使用本发明方法和粒子滤波方法的计算时间对比图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。
参照图1所示，本发明公开了一种最小方差无偏有限脉冲响应滤波器的机器人定位方法，其特征在于，包括以下步骤：
S1：建立包括超宽带装置和惯性传感器的耦合定位***，根据***状态变量建立所述包括超宽带装置和惯性传感器的耦合定位***的误差线性状态方程和观测方程。
所述误差线性状态方程为：
xk＝Axk-1+wk，
其中，xk为k时刻的所述包括超宽带装置和惯性传感器的耦合定位***的状态变量，xk-1为k-1时刻的所述包括超宽带装置和惯性传感器的耦合定位***的状态变量，xk＝[δPE,k,δVE,k,δPN,k,δVN,k]T；δPE,k＝PE,k-P′E,k表示k时刻的东向位移误差，δVE,k＝VE,k-V′E,k表示k时刻的东向速度误差，δPN,k＝PN,k-P′N,k表示k时刻的北向位移误差，δVN,k＝VN,k-V′N,k表示k时刻的北向速度误差；PE,k表示超宽带装置k时刻在东方向测得的位置，PN,k表示超宽带装置k时刻在北方向测得的位置，VE,k表示超宽带装置k时刻在东方向的速度，VN,k表示超宽带装置k时刻在北方向的速度，P′E,k表示惯性传感器k时刻在东方向测得的位置，P′N,k表示惯性传感器k时刻在北方向测得的位置，V′E,k表示惯性传感器k时刻在东方向测得的速度，V′N,k表示惯性传感器k时刻在北方向测得的速度；wk为过程噪声，A为状态转移矩阵，
A中的θ为采样时间。由于室内定位只考虑平面定位问题，因此本实施例中忽略垂直方向的位置和速度。
因为超宽带定位方法仅提供不同方向的位置信息，所以所述观测方程为：
yk＝Cxk+vk，
其中，yk＝[δPE,k,δPN,k]T是k时刻的测量值，vk是k时刻的测量噪声，C是测量矩阵，
S2：将最小方差无偏有限脉冲响应(Minimum Variance Unbiased FiniteImpulse Response,MVU FIR)滤波器引入所述包括超宽带装置和惯性传感器的耦合定位***中，结合误差线性状态方程和观测方程构建最小方差无偏有限脉冲响应滤波器的状态预测方程。
所述状态预测方程为：
其中，
表示k时刻的状态变量xk的估计值，KN是最小方差无偏有限脉冲响应滤波器的增益，Yk,l是由从时间l＝k-N+1到k的测量向量构造的矩阵，
N是自主选择的批处理窗口的长度，N大于等于所述***状态变量的维数。
S3：化简最小方差无偏有限脉冲响应滤波器的增益，在此基础上使用所述状态预测方程估计机器人的速度误差和位置误差。
S3-1：构建含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的初始增益KN：
KN＝KN1+KN2，其中：
与KN相关的扩展矩阵是通过对状态空间模型
在估计范围[l,k]进行逐步计算得到的正向时间解。
KN1和KN2中：
AN-1是所述状态转移矩阵A的N-1次幂，
()T是矩阵或向量的转置，CN-1是i＝N-1时刻的Ci，
Ai＝[(Ai)T,(Ai-1)T,...,AT,I]T，I是单位矩阵，
由i+1个C矩阵沿对角拼接而成，即
BN-1是i＝N-1时刻的Bi，
B′i是Bi的首行向量；
QN-1是i＝N-1时刻的Qi，Qi由i+1个Q矩阵沿对角拼接而成，Q是过程噪声协方差矩阵；HN-1是i＝N-1时刻的Hi，
ZN-1是i＝N-1时刻的Zi，
Ri由i+1个R矩阵沿对角拼接而成，R是测量噪声协方差矩阵，()-1是矩阵的逆。
引入定理：给定由公式
得到的特定增益值KN，同时取能使***稳定的最小的Q、具有非零元素相同的对角阵R，则最小方差无偏有限脉冲响应滤波器在对噪声统计中具有更好的免疫性。因此，令Q取值为能使***稳定的最小值，令R为具有相同非零元素的对角阵。
S3-2：在过程噪声能使***稳定的情况下取最小值，化简最小方差无偏有限脉冲响应滤波器的初始增益得到KN'：
令过程噪声协方差矩阵Q取值为能使***稳定的最小值，此时KN2≈0，最小方差无偏有限脉冲响应滤波器的初始增益KN简化为：
其中，RN-1是i＝N-1时刻的Ri。
S3-3：处理测量噪声进一步化简K′N，得到最终的不含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的增益K″N：
令过程噪声协方差矩阵R为具有相同非零元素的对角阵，即R的每一个对角元素r≠0，继续简化K′N得到最终的不含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的增益K″N为：
最终的最小方差无偏有限脉冲响应滤波器的增益K″N中完全不涉及噪声变量Q和R，因此它不受噪声统计特性的影响，具有更好的噪声免疫性。
S3-4：使用最终的不含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的增益K″N更新状态预测方程得到：
根据更新后的状态预测方程得到的k时刻的状态变量xk的估计值
结合xk＝[δPE,k,δVE,k,δPN,k,δVN,k]T得到估计的位置误差和估计的速度误差。
S4：使用所述速度误差和位置误差修正所述惯性传感器测量的机器人的速度和位置，得到机器人最终的速度和位置。
本发明还公开了一种最小方差无偏有限脉冲响应滤波器的机器人定位***，包括惯导***、超宽带模块和最小方差无偏有限脉冲响应滤波器，所述惯导***包括惯性传感器，所述超宽带模块包括超宽带装置。
所述惯性传感器和所述超宽带装置分别测量机器人的速度和位置，所述超宽带装置使用基于到达时间差(TDOA)方法测量机器人的速度和位置。所述最小方差无偏有限脉冲响应滤波器结合所述惯性传感器和所述超宽带装置测量的机器人的速度和位置估计机器人的速度误差和位置误差；将所述惯性传感器测量的机器人的速度和位置减去所述超宽带装置测量的机器人的速度和位置得到的结果作为测量值yk输入最小方差无偏有限脉冲响应滤波器，得到k时刻的状态变量xk的估计值
结合xk＝[δPE,k,δVE,k,δPN,k,δVN,k]T得到估计的位置误差和估计的速度误差。
所述惯导***使用所述度误差和位置误差修正所述惯性传感器测量的机器人的速度和位置，得到机器人最终的速度和位置。超宽带技术最明显的一点是短期稳定性差，受到多径效应和非视线(NLOS)条件的影响，本发明中将超宽带技术(UWB)与惯性传感器(INS)技术相结合克服了这个缺陷。
本发明结合了超宽带技术、惯导技术和最小方差无偏有限脉冲响应滤波器，通过超宽带技术穿透力强的特点增加了定位距离，通过最小方差无偏有限脉冲响应滤波器修正惯性传感器的定位数据减少了误差；同时，超宽带技术与惯导技术耦合可以克服***稳定性差、易受到非视距效应等噪声影响的缺点，最小方差无偏有限脉冲响应滤波器也不受噪声统计特性的影响，可以高效稳定地得到准确的室内机器人定位结果。
为了进一步说明本发明的有益效果。本实施例中搭建如图2所示的***展开实验，***由移动机器人模块1和超宽带装置2组成，移动机器人模块1包括惯导***11和定位标签12，超宽带装置2包括四个超宽带基站21、22、23和24。惯导***11内部包含了多个加速度计和陀螺仪，同时所有的惯导数据都是在惯导***11自身的微处理器中完成的，通过惯导***11即可得到修正后的速度和位置；定位标签12用于发出脉冲信号实现与四周基站的通信；同时移动机器人上也有一套集成***，用于存储定位数据。四个超宽带基站21、22、23和24用于与移动机器人模块1进行信号传输。
在定位前，首先确定四个固定基站的坐标，移动机器人通过定位标签12发出UWB信号，然后四个基站接收具有同步时钟的信号，并发送到达时间(Time Of Arrival,TOA)信息；同时惯导***11测量出机器人的位置和速度数据进行自我定位。
设定机器人的参考路线，速度设定为Vk＝10cm/s，采样时间设为θ＝0.1s。使用超宽带定位方法得到的跟踪数据图如图3所示，使用惯导导航得到的跟踪数据图如图4所示。超宽带定位方法存在太多的噪声，而惯导导航测量值适中但误差随时间逐渐增加，两种方法所得到的路线图虽然都可以接受但不精确。由于***和测量噪声未知，本实施例中基于常识指定噪声统计量。参考路径由计算机控制，本实施例中假定位置标准差为1cm/s，速度标准差为0.1cm/s。量测量选择为东向和北向的位置误差，其测量噪声的标准偏差均为10cm，因此可设东向和北向两个测量噪声的方差均为δ2 v1＝δ2 v2＝102cm2，路径起点为(1m,1m)，东向速度为10cm/s，北向速度为0cm/s，因此，初始状态的误差为[0,0,0,0]T。
本实施例中取N＝30，对1200个后续点进行滤波得到1200个估计的速度误差和位移误差，将使用本发明(MVU FIR)方法和粒子滤波(Particle filter,PF)方法得到的定位结果进行对比，如图5所示为使用本发明方法和粒子滤波方法得到的东方向的典型瞬时估计对比图，如图6所示为使用本发明方法和粒子滤波方法得到的北方向的典型瞬时估计对比图。同时，计算出两种方法在东方向(E)和北方向(N)上的位置均方根、速度均方根和失败率进行比较，结果如表1所示：
表1 位置均方根、速度均方根以及失误率对比表
| 粒子滤波(PF) | MVU FIR | |
| 位置均方根(E；N) | 15.53；11.27 | 6.93；7.04 |
| 速度均方根(E；N) | 1.22；1.12 | 0.99；0.99 |
| 失误率(E；N) | 0.09；0.09 | 0；0 |
从表1可以看出，本发明的位置均方根、速度均方根和失败率都低于粒子滤波，本发明的定位准确性得到了有效提升。
接着，引入两个系数ε和ξ代替Q和R，并计算0.1≤ε≤3、0.1≤ξ≤3时粒子滤波和本发明方法的迹(用tr()表示)以及均方误差(用Pk表示)，以此来比较噪声方差的大小对定位结果的影响。本发明方法与粒子滤波关于ε和ξ的tr(Pk)平方根即
两种方法随着ε和ξ变化的表现如图7所示。从图7可以看出，当ε＞1且ζ＜1时，粒子滤波的
远大于本发明方法，本发明方法表现出了更好的性能，粒子滤波表现很差；当噪声方差较大时，粒子滤波与本发明方法的
相近且略大于本发明方法，说明粒子滤波表现合格，但表现仍不如本发明方法。
最后，对计算时间进行实验。计算时间也是衡量本发明方法的重要指标，特别在本地化的情况时。本实施例中模拟N＝100共100个数据点，粒子滤波的粒子数β∈[0,300]的模型。使用本发明方法和粒子滤波方法在被跟踪时的计算时间如图8所示。从图8可以看出，随数据量的增长，本发明方法的计算时间并没有太大变化；但是随着粒子数量的增加，粒子滤波的计算时间显著增长。因此，本发明方法在时间复杂度上的性能表现优异。
从实验可以看出，本发明方法的准确性相较于传统方法有效提升，并且本发明方法也具有稳定持续输出性能。因此，对于室内离线定位，本发明方法可以实现较好的应用。
本领域内的技术人员应明白，本申请的实施例可提供为方法、***、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
显然，上述实施例仅仅是为清楚地说明所作的举例，并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引申出的显而易见的变化或变动仍处于本发明创造的保护范围之中。
Claims (10)
1.一种基于最小方差无偏有限脉冲响应的机器人定位方法，其特征在于，包括以下步骤：
S1：建立包括超宽带装置和惯性传感器的耦合定位***，根据***状态变量建立所述包括超宽带装置和惯性传感器的耦合定位***的误差线性状态方程和观测方程；
S2：将最小方差无偏有限脉冲响应滤波器引入所述包括超宽带装置和惯性传感器的耦合定位***中，结合误差线性状态方程和观测方程构建最小方差无偏有限脉冲响应滤波器的状态预测方程；
S3：化简最小方差无偏有限脉冲响应滤波器的增益，在此基础上使用所述状态预测方程估计机器人的速度误差和位置误差；
S4：使用所述速度误差和位置误差修正所述惯性传感器测量的机器人的速度和位置，得到机器人最终的速度和位置。
2.根据权利要求1所述的基于最小方差无偏有限脉冲响应的机器人定位方法，其特征在于：所述误差线性状态方程为：
xk＝Axk-1+wk，
其中，xk为k时刻的所述包括超宽带装置和惯性传感器的耦合定位***的状态变量，xk-1为k-1时刻的所述包括超宽带装置和惯性传感器的耦合定位***的状态变量，xk＝[δPE，k，δVE，k，δPN，k，δVN，k]T；δPE，k＝PE，k-P′E，k表示k时刻的东向位移误差，δVE，k＝VE，k-V′E，k表示k时刻的东向速度误差，δPN，k＝PN，k-P′N，k表示k时刻的北向位移误差，δVN，k＝VN，k-V′N，k表示k时刻的北向速度误差；PE,k表示超宽带装置k时刻在东方向测得的位置，PN,k表示超宽带装置k时刻在北方向测得的位置，VE,k表示超宽带装置k时刻在东方向的速度，VN,k表示超宽带装置k时刻在北方向的速度，P′E,k表示惯性传感器k时刻在东方向测得的位置，P′N,k表示惯性传感器k时刻在北方向测得的位置，V′E,k表示惯性传感器k时刻在东方向测得的速度，V′N,k表示惯性传感器k时刻在北方向测得的速度；wk为过程噪声，A为状态转移矩阵，
A中的θ为采样时间。
3.根据权利要求2所述的基于最小方差无偏有限脉冲响应的机器人定位方法，其特征在于：所述观测方程为：
yk＝Cxk+vk，
其中，yk＝[δPE,k,δPN,k]T是k时刻的测量值，vk是k时刻的测量噪声，C是测量矩阵，
4.根据权利要求3所述的基于最小方差无偏有限脉冲响应的机器人定位方法，其特征在于：所述状态预测方程为：
其中，
表示k时刻的状态变量xk的估计值，KN是最小方差无偏有限脉冲响应滤波器的增益；Yk,l是由从时间l＝k-N+1到k的测量向量构造的矩阵，
N是自主选择的批处理窗口的长度，N大于等于所述***状态变量的维数。
5.根据权利要求4所述的基于最小方差无偏有限脉冲响应的机器人定位方法，其特征在于：所述化简最小方差无偏有限脉冲响应滤波器的增益，在此基础上使用所述状态预测方程估计机器人的速度误差和位置误差，具体为：
S3-1：构建含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的初始增益KN；
S3-2：在过程噪声能使***稳定的情况下取最小值，化简最小方差无偏有限脉冲响应滤波器的初始增益得到K′N；
S3-3：处理测量噪声进一步化简K′N，得到最终的不含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的增益K″N；
S3-4：使用最终的不含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的增益K″N更新状态预测方程得到：
根据更新后的状态预测方程得到的k时刻的状态变量xk的估计值
得到估计的位置误差和估计的速度误差。
6.根据权利要求5所述的基于最小方差无偏有限脉冲响应的机器人定位方法，其特征在于：所述含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的初始增益KN为：
KN＝KN1+KN2，其中：
KN1和KN2中：
AN-1是所述状态转移矩阵A的N-1次幂，
( )T是矩阵或向量的转置，CN-1是i＝N-1时刻的Ci，
Ai＝[(Ai)T,(Ai-1)T,...,AT,I]T，I是单位矩阵，
由i+1个C矩阵沿对角拼接而成；
BN-1是i＝N-1时刻的Bi，
B′i是Bi的首行向量；
QN-1是i＝N-1时刻的Qi，Qi由i+1个Q矩阵沿对角拼接而成，Q是过程噪声协方差矩阵；HN-1是i＝N-1时刻的Hi，
ZN-1是i＝N-1时刻的Zi，
Ri由i+1个R矩阵沿对角拼接而成，R是测量噪声协方差矩阵，( )-1是矩阵的逆。
7.根据权利要求6所述的基于最小方差无偏有限脉冲响应的机器人定位方法，其特征在于：所述在过程噪声能使***稳定的情况下取最小值，化简最小方差无偏有限脉冲响应滤波器的初始增益得到K′N，具体为：
令过程噪声协方差矩阵Q取值为能使***稳定的最小值，此时KN2≈0，最小方差无偏有限脉冲响应滤波器的初始增益KN简化为：
其中，RN-1是i＝N-1时刻的Ri。
8.根据权利要求7所述的基于最小方差无偏有限脉冲响应的机器人定位方法，其特征在于：所述处理测量噪声进一步化简K′N，得到最终的不含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的增益K″N，具体为：
令过程噪声协方差矩阵R为具有相同非零元素的对角阵，继续简化K′N得到最终的不含有过程噪声和测量噪声的最小方差无偏有限脉冲响应滤波器的增益K″N为：
9.一种基于最小方差无偏有限脉冲响应的机器人定位***，其特征在于：包括惯导***、超宽带模块和最小方差无偏有限脉冲响应滤波器，所述惯导***包括惯性传感器，所述超宽带模块包括超宽带装置；
所述惯性传感器和所述超宽带装置分别测量机器人的速度和位置，
所述最小方差无偏有限脉冲响应滤波器结合所述惯性传感器和所述超宽带装置测量的机器人的速度和位置估计机器人的速度误差和位置误差；
所述惯导***使用所述度误差和位置误差修正所述惯性传感器测量的机器人的速度和位置，得到机器人最终的速度和位置。
10.根据权利要求9所述的基于最小方差无偏有限脉冲响应的机器人定位***，其特征在于：所述超宽带装置使用基于到达时间差方法测量机器人的速度和位置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210801388.XA CN115290079A (zh) | 2022-07-08 | 2022-07-08 | 基于最小方差无偏有限脉冲响应的机器人定位方法及*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210801388.XA CN115290079A (zh) | 2022-07-08 | 2022-07-08 | 基于最小方差无偏有限脉冲响应的机器人定位方法及*** |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115290079A true CN115290079A (zh) | 2022-11-04 |
Family
ID=83822842
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210801388.XA Withdrawn CN115290079A (zh) | 2022-07-08 | 2022-07-08 | 基于最小方差无偏有限脉冲响应的机器人定位方法及*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115290079A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116679026A (zh) * | 2023-06-27 | 2023-09-01 | 江南大学 | 自适应无偏有限脉冲响应滤波的污水溶解氧浓度估计方法 |
-
2022
- 2022-07-08 CN CN202210801388.XA patent/CN115290079A/zh not_active Withdrawn
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116679026A (zh) * | 2023-06-27 | 2023-09-01 | 江南大学 | 自适应无偏有限脉冲响应滤波的污水溶解氧浓度估计方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Xiong et al. | A robust single GPS navigation and positioning algorithm based on strong tracking filtering | |
| CN105549049B (zh) | 一种应用于gps导航的自适应卡尔曼滤波算法 | |
| CN107255795B (zh) | 基于ekf/efir混合滤波的室内移动机器人定位方法和装置 | |
| Huang et al. | An observability-constrained sliding window filter for SLAM | |
| CN110657806B (zh) | 一种基于CKF、chan解算和Savitzky-Golay平滑滤波的位置解算方法 | |
| CN105043388A (zh) | 基于惯性/重力匹配组合导航的向量搜索迭代匹配方法 | |
| CN110673089A (zh) | 未知视距和非视距分布情况下基于到达时间的定位方法 | |
| CN115290079A (zh) | 基于最小方差无偏有限脉冲响应的机器人定位方法及*** | |
| CN114509069B (zh) | 基于蓝牙aoa和imu融合的室内导航定位*** | |
| WO2014055845A1 (en) | Speed estimation using delta rtt measurements and area maps | |
| US9645219B2 (en) | Systems and methods for off-line and on-line sensor calibration | |
| Hellmers et al. | Indoor localisation for wheeled platforms based on IMU and artificially generated magnetic field | |
| CN111444467B (zh) | 基于实时定位轨迹数据进行局部线性插值和预测的方法 | |
| CN107270890B (zh) | 一种无人机上tof距离传感器的测距方法及*** | |
| CN112240957A (zh) | 一种卫星导航干扰测向中天线幅相特性校正方法 | |
| Amendolare et al. | WPI precision personnel locator system: Inertial navigation supplementation | |
| Wang et al. | Gaussian filtered rssi-based indoor localization in wlan using bootstrap filter | |
| CN116027266A (zh) | Uwb与视觉紧耦合slam算法的定位方法及*** | |
| CN111665533A (zh) | 基于卫星定位有效性的定位方法/***、介质及设备 | |
| CN113640738B (zh) | 一种结合imu与单组uwb的旋转式目标定位方法 | |
| Kausar et al. | A novel Kalman filter based trilateration approach for indoor localization problem | |
| Ma et al. | A TDOA localization method for complex environment localization | |
| CN112437397A (zh) | 基于交替修正牛顿法的分布式传感器节点定位方法 | |
| Peng et al. | Maneuvering Target Tracking Using Current Statistical Model Based Adaptive UKF for Wireless Sensor Network. | |
| CN114089273B (zh) | 一种基于gps及uwb的运动平台定位方法 |
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 | ||
| WW01 | Invention patent application withdrawn after publication |
Application publication date: 20221104 |