CN107255795B - 基于ekf/efir混合滤波的室内移动机器人定位方法和装置 - Google Patents

基于ekf/efir混合滤波的室内移动机器人定位方法和装置 Download PDF

Info

Publication number
CN107255795B
CN107255795B CN201710441367.0A CN201710441367A CN107255795B CN 107255795 B CN107255795 B CN 107255795B CN 201710441367 A CN201710441367 A CN 201710441367A CN 107255795 B CN107255795 B CN 107255795B
Authority
CN
China
Prior art keywords
ekf
efir
time
positioning
uwb
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.)
Active
Application number
CN201710441367.0A
Other languages
English (en)
Other versions
CN107255795A (zh
Inventor
田国会
徐元
龚京
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shandong University
Original Assignee
Shandong University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shandong University filed Critical Shandong University
Priority to CN201710441367.0A priority Critical patent/CN107255795B/zh
Publication of CN107255795A publication Critical patent/CN107255795A/zh
Application granted granted Critical
Publication of CN107255795B publication Critical patent/CN107255795B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0252—Radio frequency fingerprinting

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Feedback Control In General (AREA)
  • Mobile Radio Communication Systems (AREA)

Abstract

本发明公开了一种基于EKF/EFIR混合滤波的室内移动机器人定位方法和装置，该方法包括：以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；然后以所述UWB机器人定位模型为基础，利用UWB定位标签测量得到的某参考节点与定位标签之间的距离信息，通过EKF/EFIR混合滤波器估计机器人的位置以及参考节点的位置：首先采用EKF算法进行预估，检测EKF算法性能，若未能达到预设精度，则用EFIR滤波器对EKF算法进行补充，最终得到当前时刻目标机器人最优的位置信息。采用本发明的方法能够提高室内环境下的移动机器人的定位精度。

Description

基于EKF/EFIR混合滤波的室内移动机器人定位方法和装置
技术领域
本发明涉及复杂环境下组合定位技术领域，尤其涉及一种基于EKF/EFIR混合滤波的室内移动机器人定位方法。
背景技术
近年来，随着科学技术的发展和人民生活水平的提高，机器人逐步走进人们的生活。作为机器人为人类提供高质量服务的基础，面向机器人的定位正逐渐成为该领域的研究热点。
在机器人定位技术方面，为了克服全球定位系统(Global Positioning System,GPS)为代表的全球导航卫星系统(GlobalNavigation Satellite System,GNSS)在室内环境下无法克服因信号受遮挡导致定位精度下降甚至失锁的缺点，学者们提出将无线传感器网络(WirelessSensors Network,WSN)技术应用到室内机器人定位领域。例如Sweatt M.等提出基于WiFi的室内移动机器人通信和定位算法；Miah M.S.等对基于射频识别(Radiofrequency identification,RFID)技术的移动机器人定位算法进行了研究。需要指出的是，上述无线定位技术的精度为米级，不能满足室内机器人高精度导航定位的要求。为了获取更高的定位精度，学者们提出将超宽带技术(Ultra Wideband，UWB)应用于移动机器人室内导航定位中[8]。但是需要指出的是，传统的室内定位方法都需要预先获取参考节点的位置信息，这限制了这一技术的灵活性。
在数据融合方法方面，目前应用最为广泛的是卡尔曼滤波算法(Kalman filter,KF)及其相应的改进方法，如扩展卡尔曼滤波(Extended KF,EKF)、无迹卡尔曼滤波(Unscented KF,UKF)等。但需要指出的是KF及其改进算法的精度依赖于滤波模型的精度，特别是噪声特性的描述。然而在实际应用中很难获取机器人所处导航环境的精确环境噪声信息。为了克服这一问题，学者们提出了基于有限脉冲响应结构(finite impulseresponse,FIR)的数据融合算法，以提高滤波算法的鲁棒性。例如Shmaliy Y S提出了一种面向线性系统的FIR滤波器，并将其应用到机器人无线定位领域。但需要指出的是，FIR滤波器虽然具有较高的鲁棒性，但其精度略差于获取到精确噪声描述的KF及其相应改进算法。除此之外，有限脉冲响应结构也使得FIR滤波器的运行效率弱于KF。
目前需要本领域技术人员迫切解决的一个技术问题是：如何在定位过程中既保证精度又兼顾效率。
发明内容
为了解决上述问题，本发明提供了一种基于EKF/EFIR混合滤波的室内移动机器人定位方法，该方法构建UWB定位标签所获取到的距离(即参考节点与目标机器人之间的距离)信息、目标机器人的位置和速度信息之间的模型，以降低室内复杂导航环境对组合导航精度的影响，为滤波器完成对导航信息的高精度预估打下基础。
为了实现上述目的，本发明采用如下技术方案：
一种基于EKF/EFIR混合滤波的室内移动机器人定位方法，包括以下步骤：
(1)以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；
(2)以所述UWB机器人定位模型为基础，利用UWB定位标签测量得到的某参考节点与定位标签之间的距离信息，通过EKF/EFIR混合滤波器估计机器人的位置以及参考节点的位置：首先采用EKF算法进行预估，检测EKF算法性能，若未能达到预设精度，则用EFIR滤波器对EKF算法进行补充，最终得到当前时刻目标机器人最优的位置信息。
作为状态量的目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置的初值根据实际需要自行设定。
所述EKF/EFIR混合滤波器的状态方程为：
Figure BDA0001320063410000022
其中，(xk，yk)分别为移动机器人在k时刻的x和y方向的位置；(Vxk，Vyk)分别为移动机器人在k时刻的x和y方向的速度；T为采样周期；
Figure BDA0001320063410000023
为k时刻参考节点的位置向量，
Figure BDA0001320063410000024
表示参考节点在k时刻在x和y方向的位置，其中，l为参考节点的数目；ωk为满足均值为零正态分布的系统噪声，其协方差矩阵为Q。
所述EKF/EFIR混合滤波器的观测方程为：
其中，yk＝[d1 d2…dl]T为k时刻通过UWB定位系统测量的到的参考节点与未知节点之间的距离；vk为系统的观测噪声矩阵，其协方差矩阵为R，表示参考节点k时刻在x和y方向的位置，l为参考节点的数目。
所述EKF/EFIR滤波器中EKF算法的迭代过程为：
Figure BDA0001320063410000034
Figure BDA0001320063410000036
Figure BDA0001320063410000037
其中，
Figure BDA0001320063410000038
Figure BDA0001320063410000039
表示EKF在k时刻预估的状态向量，
Figure BDA00013200634100000310
表示EKF由k-1时刻到k时刻预估的状态向量，Fk表示表示k时刻的系统矩阵，
Figure BDA00013200634100000311
表示EKF由k-1时刻到k时刻的最小预测均方误差矩阵；
Figure BDA00013200634100000312
表示EKF k时刻的最小预测均方误差矩阵；Kk表示EKF在k时刻的误差增益矩阵；I表示单位阵。
所述EKF/EFIR滤波器中EKF算法的性能采用信道质量评价指标Dk来评价：
Figure BDA00013200634100000313
判断Dk与门限door的关系，如果Dk<door，则信道质量正常，反之，则信道质量差，进入EFIR滤波器。
所述EKF/EFIR滤波器中EFIR算法的迭代过程为：
首先，若当前采样时刻大于EFIR预先设置的窗口N，定义m＝k-N+1，s＝m+M-1，Gs＝I；
然后，从s+1时刻到k时刻，执行以下迭代过程：
Figure BDA00013200634100000314
Figure BDA00013200634100000316
Figure BDA00013200634100000317
Figure BDA00013200634100000318
Figure BDA00013200634100000319
其中，
Figure BDA0001320063410000041
Figure BDA0001320063410000042
表示EFIR在j时刻预估的状态向量，
Figure BDA0001320063410000043
表示EFIR由j-1时刻到j时刻预估的状态向量，Fj表示表示j时刻的系统矩阵，
Figure BDA0001320063410000044
表示EFIR由j-1时刻到j时刻的最小预测均方误差矩阵；
Figure BDA0001320063410000045
表示EFIR j时刻的最小预测均方误差矩阵；Kj表示EKF在j时刻的误差增益矩阵；I表示单位阵；
最后，
Figure BDA0001320063410000046
所述EKF/EFIR混合滤波器的迭代过程为：
首先采用EKF滤波器进行一步预估：
Figure BDA0001320063410000047
Figure BDA0001320063410000048
若Dk＜door，则继续进行EKF滤波器的更新：
Figure BDA0001320063410000049
Figure BDA00013200634100000410
Pk＝(I-KkHk)Pk|k-1
反之，则信道质量差，进入EFIR滤波器；
若当前采样时刻大于EFIR预先设置的窗口N，定义m＝k-N+1，s＝m+M-1，Gs＝1，
从s+1时刻到k时刻，执行以下迭代过程：
Figure BDA00013200634100000412
Figure BDA00013200634100000413
Figure BDA00013200634100000414
Figure BDA00013200634100000415
Figure BDA00013200634100000416
其中，
Figure BDA00013200634100000417
表示EFIR在j时刻预估的状态向量，
Figure BDA00013200634100000419
表示EFIR由j-1时刻到j时刻预估的状态向量，Fj表示表示j时刻的系统矩阵，
Figure BDA00013200634100000420
表示EFIR由j-1时刻到j时刻的最小预测均方误差矩阵；
Figure BDA00013200634100000421
表示EFIR j时刻的最小预测均方误差矩阵；Kj表示EKF在j时刻的误差增益矩阵；I表示单位阵。
最后，
Figure BDA00013200634100000422
本发明还提供了一种基于EKF/EFIR混合滤波的室内移动机器人定位装置，包括：
UWB机器人定位模型构建模块，以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；
机器人定位模块，以所述UWB机器人定位模型为基础，利用UWB定位标签测量得到的某参考节点与定位标签之间的距离信息，通过EKF/EFIR混合滤波器估计机器人的位置以及参考节点的位置：首先采用EKF算法进行预估，检测EKF算法性能，若未能达到预设精度，则用EFIR滤波器对EKF算法进行补充，最终得到当前时刻目标机器人最优的位置信息。
本发明的有益效果：
1、移动机器人定位方法采用改进的UWB定位模型，该模型以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；在此基础上，通过EKF/EFIR滤波将UWB获取的参考节点到未知节点之间的距离信息进行数据融合，最终得到当前时刻最优的目标机器人位置信息和对参考节点位置信息的预估。提高了数据融合滤波器的精度和鲁棒性。
2、可用于室内环境下的移动机器人中高精度定位。
附图说明
图1为一种基于EKF/EFIR混合滤波的室内移动机器人定位方法的示意图。
具体实施例
下面结合附图对本发明进行详细说明：
实施例1：
本实施例提供了一种基于EKF/EFIR混合滤波的室内移动机器人定位方法，示意图如图1，该方法包括：
(1)以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；
其中，作为状态量的目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置的初值根据实际需要自行设定。
参考节点位置可以任意选定，本实施例中要求参考节点静止。
(2)以所述UWB机器人定位模型为基础，利用UWB定位标签测量得到的某参考节点与定位标签之间的距离信息，通过EKF/EFIR混合滤波器估计机器人的位置以及参考节点的位置：首先采用EKF算法进行预估，检测EKF算法性能，若未能达到预设精度，则用EFIR滤波器对EKF算法进行补充，最终得到当前时刻目标机器人最优的位置信息。
其中，所述EKF/EFIR混合滤波器的状态方程为：
Figure BDA0001320063410000061
Figure BDA0001320063410000062
其中，(xk，yk)分别为移动机器人在k时刻的x和y方向的位置；(Vxk，Vyk)分别为移动机器人在k时刻的x和y方向的速度；T为采样周期；为k时刻参考节点的位置向量，
Figure BDA0001320063410000064
表示参考节点在k时刻在x和y方向的位置，其中，l为参考节点的数目；ωk为满足均值为零正态分布的系统噪声，其协方差矩阵为Q。
所述EKF/EFIR混合滤波器的观测方程为：
Figure BDA0001320063410000065
其中，yk＝[d1 d2…dl]T为k时刻通过UWB定位系统测量的到的参考节点与未知节点之间的距离；vk为系统的观测噪声矩阵，其协方差矩阵为R，
Figure BDA0001320063410000066
表示参考节点k时刻在x和y方向的位置，l为参考节点的数目。
所述EKF/EFIR滤波器中EKF算法的迭代过程为：
Figure BDA0001320063410000067
Figure BDA0001320063410000068
Figure BDA0001320063410000069
Figure BDA00013200634100000610
Figure BDA00013200634100000611
其中，
Figure BDA0001320063410000071
Figure BDA0001320063410000072
表示EKF在k时刻预估的状态向量，
Figure BDA0001320063410000073
表示EKF由k-1时刻到k时刻预估的状态向量，Fk表示表示k时刻的系统矩阵，
Figure BDA0001320063410000074
表示EKF由k-1时刻到k时刻的最小预测均方误差矩阵；
Figure BDA0001320063410000075
表示EKF k时刻的最小预测均方误差矩阵；Kk表示EKF在k时刻的误差增益矩阵；I表示单位阵。
所述EKF/EFIR滤波器中EKF算法的性能采用信道质量评价指标Dk来评价：
判断Dk与门限door的关系，如果Dk<door，则信道质量正常，反之，则信道质量差，进入EFIR滤波器。
所述无线通信信道中EKF/EFIR滤波器中EFIR算法的迭代过程为：
首先，若当前采样时刻大于EFIR预先设置的窗口N，定义m＝k-N+1，s＝m+M-1，Gs＝I；
然后，从s+1时刻到k时刻，执行以下迭代过程：
Figure BDA0001320063410000077
Figure BDA0001320063410000078
Figure BDA0001320063410000079
Figure BDA00013200634100000710
Figure BDA00013200634100000711
其中，
Figure BDA00013200634100000713
Figure BDA00013200634100000714
表示EFIR在j时刻预估的状态向量，
Figure BDA00013200634100000715
表示EFIR由j-1时刻到j时刻预估的状态向量，Fj表示表示j时刻的系统矩阵，
Figure BDA00013200634100000716
表示EFIR由j-1时刻到j时刻的最小预测均方误差矩阵；
Figure BDA00013200634100000717
表示EFIR j时刻的最小预测均方误差矩阵；Kj表示EKF在j时刻的误差增益矩阵；I表示单位阵；
最后，
Figure BDA00013200634100000718
所述EKF/EFIR混合滤波器的迭代过程为：
首先采用EKF滤波器进行一步预估：
Figure BDA00013200634100000719
Figure BDA00013200634100000720
若Dk＜door，则继续进行EKF滤波器的更新：
Figure BDA00013200634100000722
Pi＝(l-KkHk)Pk|k-1
反之，则信道质量差，进入EFIR滤波器；
若当前采样时刻大于EFIR预先设置的窗口N，定义m＝k-N+1，s＝m+M-1，Gs＝1，
从s+1时刻到k时刻，执行以下迭代过程：
Figure BDA0001320063410000081
Figure BDA0001320063410000084
Figure BDA0001320063410000085
Figure BDA0001320063410000086
其中，
Figure BDA0001320063410000088
表示EFIR在j时刻预估的状态向量，
Figure BDA0001320063410000089
表示EFIR由j-1时刻到j时刻预估的状态向量，Fj表示表示j时刻的系统矩阵，
Figure BDA00013200634100000810
表示EFIR由j-1时刻到j时刻的最小预测均方误差矩阵；
Figure BDA00013200634100000811
表示EFIR j时刻的最小预测均方误差矩阵；Kj表示EKF在j时刻的误差增益矩阵；I表示单位阵。
最后，
Figure BDA00013200634100000812
为了更清楚地表达EKF/EFIR滤波器的算法运行过程，将运行过程进行如下表示。
EKF/EFIR滤波器中EKF算法的迭代过程为：
其中，
Figure BDA00013200634100000814
EKF/EFIR滤波器中EFIR算法的迭代过程为：
Figure BDA0001320063410000091
EKF/EFIR混合滤波器的迭代过程为：
Figure BDA0001320063410000092
实施例2：
基于实施例1所述的方法，本发明还提供了一种基于EKF/EFIR混合滤波的室内移动机器人定位装置，包括：
UWB机器人定位模型构建模块，以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；
其中，作为状态量的目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置的初值根据实际需要自行设定。
参考节点位置可以任意选定，本实施例中要求参考节点静止。
机器人定位模块，以所述UWB机器人定位模型为基础，利用UWB定位标签测量得到的某参考节点与定位标签之间的距离信息，通过EKF/EFIR混合滤波器估计机器人的位置以及参考节点的位置：首先采用EKF算法进行预估，检测EKF算法性能，若未能达到预设精度，则用EFIR滤波器对EKF算法进行补充，最终得到当前时刻目标机器人最优的位置信息。
本装置基于实施例1中所述的方法，EKF/EFIR混合滤波器具体的算法流程可参见实施例1。
本发明对UWB定位模型进行了改进，以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量，提高了数据融合滤波器的精度和鲁棒性；采用本发明的方法，提高了室内环境下的移动机器人的定位精度。
本领域技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算机装置来实现，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。本发明不限制于任何特定的硬件和软件的结合。
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (8)

1.一种基于EKF/EFIR混合滤波的室内移动机器人定位方法，其特征是，包括以下步骤：
(1)以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；
(2)以所述UWB机器人定位模型为基础，利用UWB定位标签测量得到的某参考节点与定位标签之间的距离信息，通过EKF/EFIR混合滤波器估计机器人的位置以及参考节点的位置：首先采用EKF算法进行预估，检测EKF算法性能，若未能达到预设精度，则用EFIR滤波器对EKF算法进行补充，最终得到当前时刻目标机器人最优的位置信息；
所述EKF/EFIR混合滤波器的状态方程为：
Figure FDA0002265861620000011
其中，(xk,yk)分别为移动机器人在k时刻的x和y方向的位置；(Vxk,Vyk)分别为移动机器人在k时刻的x和y方向的速度；T为采样周期；为k时刻参考节点的位置向量，
Figure FDA0002265861620000014
表示参考节点在k时刻在x和y方向的位置，其中，l为参考节点的数目；ωk为满足均值为零正态分布的系统噪声，其协方差矩阵为Q。
2.如权利要求1所述的一种基于EKF/EFIR混合滤波的室内移动机器人定位方法，其特征是，作为状态量的目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置的初值根据实际需要自行设定。
3.如权利要求1所述的一种基于EKF/EFIR混合滤波的室内移动机器人定位方法，其特征是，所述EKF/EFIR混合滤波器的观测方程为：
Figure FDA0002265861620000021
其中，yk＝[d1 d2 ... dl]T为k时刻通过UWB定位系统测量的到的参考节点与未知节点之间的距离；vk为系统的观测噪声矩阵，其协方差矩阵为R，
Figure FDA0002265861620000022
表示参考节点k时刻在x和y方向的位置，l为参考节点的数目。
4.如权利要求1所述的一种基于EKF/EFIR混合滤波的室内移动机器人定位方法，其特征是，所述EKF/EFIR滤波器中EKF算法的迭代过程为：
Figure FDA0002265861620000023
Figure FDA0002265861620000025
Figure FDA0002265861620000027
其中，
Figure FDA0002265861620000029
表示EKF在k时刻预估的状态向量，
Figure FDA00022658616200000210
表示EKF由k-1时刻到k时刻预估的状态向量，Fk表示表示k时刻的系统矩阵，
Figure FDA00022658616200000211
表示EKF由k-1时刻到k时刻的最小预测均方误差矩阵；
Figure FDA00022658616200000212
表示EKF k时刻的最小预测均方误差矩阵；Kk表示EKF在k时刻的误差增益矩阵；I表示单位阵。
5.如权利要求4所述的一种基于EKF/EFIR混合滤波的室内移动机器人定位方法，其特征是，所述EKF/EFIR滤波器中EKF算法的性能采用信道质量评价指标Dk来评价：
判断Dk与门限door的关系，如果Dk<door，则信道质量正常，反之，则信道质量差，进入EFIR滤波器。
6.如权利要求1所述的一种基于EKF/EFIR混合滤波的室内移动机器人定位方法，其特征是，所述EKF/EFIR滤波器中EFIR算法的迭代过程为：
首先，若当前采样时刻大于EFIR预先设置的窗口N，定义m＝k-N+1，s＝m+M-1，Gs＝I；
然后，从s+1时刻到k时刻，执行以下迭代过程：
Figure FDA0002265861620000032
Figure FDA0002265861620000033
Figure FDA0002265861620000034
Figure FDA0002265861620000035
Figure FDA0002265861620000036
其中，
Figure FDA0002265861620000037
Figure FDA0002265861620000038
表示EFIR在j时刻预估的状态向量，
Figure FDA0002265861620000039
表示EFIR由j-1时刻到j时刻预估的状态向量，Fj表示表示j时刻的系统矩阵，
Figure FDA00022658616200000310
表示EFIR由j-1时刻到j时刻的最小预测均方误差矩阵；
Figure FDA00022658616200000311
表示EFIR j时刻的最小预测均方误差矩阵；Kj表示EKF在j时刻的误差增益矩阵；I表示单位阵；
最后，
Figure FDA00022658616200000312
7.如权利要求1所述的一种基于EKF/EFIR混合滤波的室内移动机器人定位方法，其特征是，所述EKF/EFIR混合滤波器的迭代过程为：
首先采用EKF滤波器进行一步预估：
Figure FDA00022658616200000313
Figure FDA00022658616200000314
若Dk<door，则继续进行EKF滤波器的更新：
Figure FDA00022658616200000315
Figure FDA00022658616200000316
Pk=(I-KkHk)Pk|k-1
反之，则信道质量差，进入EFIR滤波器；
若当前采样时刻大于EFIR预先设置的窗口N，定义m＝k-N+1，s＝m+M-1，Gs＝I，
从s+1时刻到k时刻，执行以下迭代过程：
Figure FDA00022658616200000317
Figure FDA00022658616200000318
Figure FDA00022658616200000319
Figure FDA00022658616200000320
Figure FDA00022658616200000321
其中，
Figure FDA00022658616200000323
Figure FDA00022658616200000324
表示EFIR在j时刻预估的状态向量，
Figure FDA00022658616200000325
表示EFIR由j-1时刻到j时刻预估的状态向量，Fj表示表示j时刻的系统矩阵，
Figure FDA00022658616200000326
表示EFIR由j-1时刻到j时刻的最小预测均方误差矩阵；
Figure FDA00022658616200000327
表示EFIR j时刻的最小预测均方误差矩阵；Kj表示EKF在j时刻的误差增益矩阵；I表示单位阵；
最后，
Figure FDA0002265861620000041
8.一种基于EKF/EFIR混合滤波的室内移动机器人定位装置，其特征是，包括：
UWB机器人定位模型构建模块，以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；
机器人定位模块，以所述UWB机器人定位模型为基础，利用UWB定位标签测量得到的某参考节点与定位标签之间的距离信息，通过EKF/EFIR混合滤波器估计机器人的位置以及参考节点的位置：首先采用EKF算法进行预估，检测EKF算法性能，若未能达到预设精度，则用EFIR滤波器对EKF算法进行补充，最终得到当前时刻目标机器人最优的位置信息；
所述EKF/EFIR混合滤波器的状态方程为：
Figure FDA0002265861620000042
Figure FDA0002265861620000043
其中，(xk,yk)分别为移动机器人在k时刻的x和y方向的位置；(Vxk,Vyk)分别为移动机器人在k时刻的x和y方向的速度；T为采样周期；
Figure FDA0002265861620000044
为k时刻参考节点的位置向量，
Figure FDA0002265861620000045
表示参考节点在k时刻在x和y方向的位置，其中，l为参考节点的数目；ωk为满足均值为零正态分布的系统噪声，其协方差矩阵为Q。
CN201710441367.0A 2017-06-13 2017-06-13 基于ekf/efir混合滤波的室内移动机器人定位方法和装置 Active CN107255795B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710441367.0A CN107255795B (zh) 2017-06-13 2017-06-13 基于ekf/efir混合滤波的室内移动机器人定位方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710441367.0A CN107255795B (zh) 2017-06-13 2017-06-13 基于ekf/efir混合滤波的室内移动机器人定位方法和装置

Publications (2)

Publication Number Publication Date
CN107255795A CN107255795A (zh) 2017-10-17
CN107255795B true CN107255795B (zh) 2020-01-07

Family

ID=60024086

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710441367.0A Active CN107255795B (zh) 2017-06-13 2017-06-13 基于ekf/efir混合滤波的室内移动机器人定位方法和装置

Country Status (1)

Country Link
CN (1) CN107255795B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114683259A (zh) * 2020-12-28 2022-07-01 财团法人工业技术研究院 机械手臂校正系统及机械手臂校正方法

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107907857B (zh) * 2017-10-25 2021-08-17 深圳大学 一种基于uwb的实时定位方法及定位装置
CN107966143A (zh) * 2017-11-14 2018-04-27 济南大学 一种基于多窗口的自适应efir数据融合方法
CN107966142A (zh) * 2017-11-14 2018-04-27 济南大学 一种基于多窗口的室内行人自适应ufir数据融合方法
CN108398704B (zh) * 2018-02-06 2020-11-06 北京科技大学 一种贝叶斯滤波的多车辆协作定位方法
CN108645416B (zh) * 2018-03-30 2020-09-18 北京空间飞行器总体设计部 基于视觉测量系统的非合作目标相对导航仿真验证方法
CN109141413B (zh) * 2018-08-06 2020-07-07 济南大学 具有数据缺失uwb行人定位的efir滤波算法及系统
CN109269498B (zh) * 2018-08-06 2020-09-22 济南大学 面向具有数据缺失uwb行人导航的自适应预估ekf滤波算法及系统
CN109141412B (zh) * 2018-08-06 2020-09-22 济南大学 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统
CN109282819B (zh) * 2018-11-26 2021-07-27 浙江工业大学 基于分布式混合滤波的超宽带定位方法
CN109282820B (zh) * 2018-11-26 2021-05-11 浙江工业大学 基于分布式混合滤波的室内定位方法
CN109655060B (zh) * 2019-02-19 2021-07-06 济南大学 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及系统
CN109884586A (zh) * 2019-03-07 2019-06-14 广东工业大学 基于超带宽的无人机定位方法、装置、系统及存储介质
CN110220513B (zh) * 2019-04-30 2022-10-04 中国科学院深圳先进技术研究院 一种目标定位的方法、系统、无人机及存储介质
CN110879069A (zh) * 2019-12-11 2020-03-13 济南大学 一种面向UWB SLAM的Kalman/R-T-S混合定位方法及系统
TW202224872A (zh) 2020-12-28 2022-07-01 財團法人工業技術研究院 機械手臂校正系統及機械手臂校正系統方法
CN113891245B (zh) * 2021-11-17 2024-04-26 西安邮电大学 一种火场消防员协同接力定位方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103314274A (zh) * 2010-12-01 2013-09-18 原子能及能源替代委员会 活动元件或物体的轨迹的估算方法及系统
CN106680765A (zh) * 2017-03-03 2017-05-17 济南大学 基于分布式组合滤波ins/uwb行人导航系统及方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009281927A (ja) * 2008-05-23 2009-12-03 Nec Electronics Corp 移動端末、並びにその屋内外判定方法及びプログラム
US8344948B2 (en) * 2010-03-29 2013-01-01 Xsens Holding B.V. Positioning system calibration

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103314274A (zh) * 2010-12-01 2013-09-18 原子能及能源替代委员会 活动元件或物体的轨迹的估算方法及系统
CN106680765A (zh) * 2017-03-03 2017-05-17 济南大学 基于分布式组合滤波ins/uwb行人导航系统及方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
"Combined extended FIR Kalman filtering for indoor robot localization via triangulation";Juan Pomarico-Franquiz等;《Measurements 50》;20140110;第236-243页 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114683259A (zh) * 2020-12-28 2022-07-01 财团法人工业技术研究院 机械手臂校正系统及机械手臂校正方法
CN114683259B (zh) * 2020-12-28 2024-08-20 财团法人工业技术研究院 机械手臂校正系统及机械手臂校正方法

Also Published As

Publication number Publication date
CN107255795A (zh) 2017-10-17

Similar Documents

Publication Publication Date Title
CN107255795B (zh) 基于ekf/efir混合滤波的室内移动机器人定位方法和装置
CN105549049B (zh) 一种应用于gps导航的自适应卡尔曼滤波算法
CN110849369B (zh) 机器人跟踪方法、装置、设备及计算机可读存储介质
CN109141413B (zh) 具有数据缺失uwb行人定位的efir滤波算法及系统
CN103471595B (zh) 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法
CN111964667B (zh) 一种基于粒子滤波算法的地磁_ins组合导航方法
CN112887899B (zh) 一种基于单基站软位置信息的定位系统及定位方法
CN106680765A (zh) 基于分布式组合滤波ins/uwb行人导航系统及方法
CN107402375B (zh) 一种带观测时滞的室内行人定位efir数据融合系统及方法
CN103529424B (zh) 一种基于rfid及ukf实现室内目标快速跟踪的方法
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN108120438A (zh) 一种基于imu和rfid信息融合的室内目标快速跟踪方法
Yuanfeng et al. Flexible indoor localization and tracking system based on mobile phone
CN107727097B (zh) 基于机载分布式位置姿态测量系统的信息融合方法和装置
CN107966142A (zh) 一种基于多窗口的室内行人自适应ufir数据融合方法
CN109141412B (zh) 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及系统
CN108759825B (zh) 面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法及系统
CN109655060B (zh) 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及系统
CN109269498B (zh) 面向具有数据缺失uwb行人导航的自适应预估ekf滤波算法及系统
Kong et al. Hybrid indoor positioning method of BLE and monocular VINS based smartphone
CN114339595B (zh) 一种基于多模型预测的超宽带动态反演定位方法
CN114035154B (zh) 一种运动参数辅助的单站射频信号定位方法
CN114705223A (zh) 多移动智能体在目标跟踪中的惯导误差补偿方法及系统
CN109459769A (zh) 一种自主定位方法与系统
CN115574766A (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
GR01 Patent grant
GR01 Patent grant