CN112525197B - 基于图优化算法超宽带惯性导航融合位姿估计方法 - Google Patents

基于图优化算法超宽带惯性导航融合位姿估计方法 Download PDF

Info

Publication number
CN112525197B
CN112525197B CN202011320423.3A CN202011320423A CN112525197B CN 112525197 B CN112525197 B CN 112525197B CN 202011320423 A CN202011320423 A CN 202011320423A CN 112525197 B CN112525197 B CN 112525197B
Authority
CN
China
Prior art keywords
data
ultra
wideband
inertial navigation
frame
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
CN202011320423.3A
Other languages
English (en)
Other versions
CN112525197A (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.)
Aerospace Information Research Institute of CAS
Original Assignee
Aerospace Information Research Institute of CAS
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 Aerospace Information Research Institute of CAS filed Critical Aerospace Information Research Institute of CAS
Priority to CN202011320423.3A priority Critical patent/CN112525197B/zh
Publication of CN112525197A publication Critical patent/CN112525197A/zh
Application granted granted Critical
Publication of CN112525197B publication Critical patent/CN112525197B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/20—Instruments for performing navigational calculations
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • 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/0247—Determining attitude
    • 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/0257—Hybrid positioning
    • 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/14—Determining absolute distances from a plurality of spaced points of known location

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种基于图优化算法超宽带和惯性导航融合位姿估计方法，包括：S1：获取超宽带数据帧和惯性导航数据帧，并组成超宽带‑惯性导航数据帧；S2：判断移动设备是否进行过初始化，若否则在静止状态下初始化位置和姿态；S3：对惯性导航数据帧的数据进行积分计算得到观测值以及计算得到第一当前位姿状态数据；S4：将所有的测距数据进行预处理得到第二当前位姿状态数据，剔除不符合条件的测距数据；S5：将观测值和剔除后剩余所有的测距数据作为约束边加入到图优化中进行优化，判断是否为关键帧，若是则将关键帧加入到优化窗口中；S6：当优化窗口中增加新的关键帧后，判断当前所有关键帧的数量是否满足预设条件，若满足则进行窗口优化。

Description

基于图优化算法超宽带惯性导航融合位姿估计方法
技术领域
本公开涉及定位技术，尤其是涉及一种基于图优化算法超宽带惯性导航融合位姿估计方法。
背景技术
随着当今时代万物互联，智慧城市需求大潮的来临，室内定姿定位与导航的需求也在快速扩大，在极大地促使了蓝牙、超声波、红外线、超宽带(Ultra-Wide Band，UWB)和惯性导航(Inertial Navigation System，INS)等室内定位技术的产生和发展的同时，也对其具体的应用性能提出了更高的要求。
UWB定位技术通过发射纳秒级脉冲信号进行测距，具有高时间分辨率、抗多径、低功耗、测距精度高等优点，在室内视距(Light Of Sight，LOS)环境下UWB定位精度可至厘米级，但仍然受非视距(Non-Light Of Sight，NLOS)误差影响导致定位精度下降，导致UWB定位标准差可达10厘米以上，极大地影响其定位精度与定位鲁棒性。惯性导航系统(InertialNavigation System，INS)具有自主定位，不受NLOS影响的特点，短时间内定位精度高，因此常被应用于UWB融合定位中，以期获得稳定的高精度定位输出。传统的融合方式多为基于卡尔曼滤波技术，但滤波器在先验信息不足时累积误差较大，且在系统非线性程度较强时模型吻合度不足，影响其滤波效能，极大地限制了此方法的应用可能。
除定位精度外，姿态计算也是传统UWB定位技术中的盲区。传统UWB定位方法在定位端只利用一个UWB标签，无法获取姿态信息；若仅依靠INS获取姿态，则存在累积误差的问题，无法获取可靠的偏航角信息。目前也有方案使用多UWB标签联合定位(发明专利CN108981720 A，UWB与惯导融合的室内导航系统)，根据多个标签与移动设备之间的几何约束关系同时得到位置和姿态信息，但由于该方法中采用分步计算各UWB标签位置，最后解算整体位姿的方法，造成每个标签定位误差直接累积到最终的整体解算结果内，定位精度不高；且该方法采用扩展卡尔曼滤波方法将UWB定位与INS融合，存在上文所述的滤波方法固有的问题。
针对现有基于滤波的UWB/INS融合定位方法存在的系统模型难以确定、累积线性误差大、定位误差大、定姿精度低的问题，提出了一种基于图优化算法的UWB/惯性导航融合位姿估计方法。
发明内容
为了解决上述基于滤波的UWB/INS融合定位方法存在的系统模型难以确定、累积线性误差大、定位误差大、定姿精度低的问题，本公开提供一种基于图优化算法超宽带惯性导航融合位姿估计方法，可以在不建立系统滤波模型前提下，利用多个UWB标签信息，提供精准的定位信息，极大地提升整体定位精度，同时具有较强的鲁棒性和通用性，具体方案如下。
一种基于图优化算法超宽带和惯性导航融合位姿估计方法，包括：
S1：获取超宽带数据帧和惯性导航数据帧，并组成超宽带-惯性导航数据帧，所述惯性导航数据帧包括当前时刻至前一超宽带数据帧时刻之间所有惯性导航的加速度计数据和陀螺仪数据，所述超宽带数据帧包括所有的超宽带标签与超宽带基站之间的第一测距数据；
S2：判断移动设备是否进行过初始化，若是，则直接进入下一步，若否，则在静止状态下初始化移动设备位置和姿态；
S3：对所述惯性导航数据帧的数据进行积分计算得到惯性导航观测值，并利用所述惯性导航观测值结合上一帧关键帧的位姿状态数据计算得到第一当前位姿状态数据，其中，所述惯性导航观测值用于表征所述上一帧关键帧到当前惯性导航数据帧之间所述移动设备的位姿变化数据；
S4：将所有的所述超宽带标签与所述超宽带基站之间的测距数据进行预处理得到第二当前位姿状态数据，根据所述第二当前位姿状态数据结合所述第一当前位姿状态数据剔除不符合预设条件的第一测距数据；
S5：将所述S3中的所述惯性导航观测值和所述S4中剔除不符合预设条件的第一测距数据后的剩余所有的第一测距数据作为约束边加入到图优化中进行优化，根据优化结果判断所述超宽带-惯性导航数据帧是否为关键帧，若是，将所述关键帧加入到优化窗口中，若否，返回S1；
S6：当所述优化窗口中增加新的超宽带-惯性导航关键帧数据后，判断所述优化窗口中当前所有超宽带-惯性导航关键帧的数量是否满足预设条件，若满足，则进行窗口优化得到所述移动设备位姿数据，并去除所述优化窗口中时间最早的至少一帧所述超宽带-惯性导航关键帧，若不满足，则等待下一帧超宽带-惯性导航关键帧数据加入后重新判断。
根据本公开的一些实施例，所述S4中所述剔除不符合预设条件的第一测距数据后还包括，根据所有所述超宽带的第一测距数据中符合预设要求的数量判断当前超宽带数据帧质量，若数量大于等于第一预设阈值，则进入下一步，若小于所述第一预设阈值，则返回所述S1。
根据本公开的一些实施例，所述S2中，对所述移动设备初始化还包括获取所述加速度计和所述陀螺仪的零偏，用以提高所述惯性导航数据帧的准确性。
根据本公开的一些实施例，所述超宽带数据帧和所述惯性导航数据帧采用同一套系统时间戳，或将采用不同的系统时间戳转化为同一系统时间戳，用以保证所述超宽带数据帧的数据和所述惯性导航数据帧的数据对齐。
根据本公开的一些实施例，所述S3中的积分计算的公式包括：
Figure BDA0002792701980000031
Figure BDA0002792701980000032
Figure BDA0002792701980000033
其中，Δt为采样间隔，ωB、aB分别为陀螺仪和加速度计的测量数据，Rk+1和Rk分别为时刻k和时刻k+1的姿态角，vk+1和vk分别为时刻k和时刻k+1的速度向量，Pk+1和Pk分别为时刻k和时刻k+1的位置，bg、ba分别为陀螺仪和加速度计的零偏，g表示重力向量，SO(3)表示三维特殊正交群的姿态角，相邻采样时刻k与k+1之间姿态角R∈SO(3)；
利用所述惯性导航数据帧的数据和所述公式(1)、(2)和(3)多次累积获得所述惯性导航观测值。
根据本公开的一些实施例，所述S4中，通过所述第二当前位姿状态数据得到所述移动设备的位置P和姿态角R，根据所述位置P和姿态角R的结果反推计算得到所述超宽带标签与所述超宽带基站之间的第二测距数据，计算所述第二测距数据与所述第一测距数据的误差的公式包括：
euwb＝‖Panchor-(P+R·TBT)‖-dobs (4)
其中，euwb表示第二测距数据与第一测距数据的误差，Panchor表示基站位置，TBT表示超宽带标签相对于惯性导航的位置、dobs表示超宽带测距值；
优化所述超宽带数据帧的数据，使得所有所述超宽带标签与所述基站的测距误差的平方和最小，优化公式包括：
Figure BDA0002792701980000041
其中，ρ为鲁棒核函数、Σx为超宽带测距的数据矩阵，Σx可由实验得到，即超宽带测距方差的倒数。
根据本公开的一些实施例，所述剔除不符合预设条件的测距数据包括，将每一个所述第一测距数据计算得到的所述euwb与第二预设阈值比较判断所述第一测距数据是否为野值，若是，则剔除，若否，则保留所述第一测距数据作为约束边。
根据本公开的一些实施例，所述S5中，所述加入到图优化中进行优化包括，当前超宽带-惯性导航数据帧j的位姿集合为θ＝{Rj,pj,vj}，上一帧关键帧序号为i，所述位姿集合的各元素的误差公式包括：
Figure BDA0002792701980000042
Figure BDA0002792701980000043
Figure BDA0002792701980000044
其中，ΔRij,Δvij,Δpij分别为超宽带-惯性导航数据帧j的姿态角变化量的观测值、速度向量变化量的观测值和位置变化量的观测值；
惯性导航数据帧的总误差公式包括：
Figure BDA0002792701980000045
其中ΣI为积分的数据矩阵，由惯性导航数据帧本身性能决定，ρ为鲁棒核函数，Eimu(i,j)为当前超宽带-惯性导航数据帧j至上一阵关键帧i之间的总误差；
所述位姿集合θ的优化公式包括：
Figure BDA0002792701980000051
其中，Euwb(j)由公式(5)计算得出。
根据本公开的一些实施例，所述位姿集合θ的优化中，固定上一帧关键帧的状态，利用通用图优化中高斯牛顿法优化对当前超宽带-惯性导航数据帧进行优化，根据优化结果判断所述超宽带-惯性导航数据帧是否为关键帧。
根据本公开的一些实施例，所述窗口优化线程中，所述优化窗口为包括最新的关键帧和之前多个连续的关键帧的集合，所述进行窗口优化包括如所述跟踪线程中对姿态角、速度向量和位置的优化。
通过上述技术方案，本公开提供的基于图优化算法超宽带惯性导航融合位姿估计方法，利用多个超宽带标签数据，提供精准的定位数据和姿态数据，通过算法对数据质量加以筛选得到高质量的关键帧的数据，进行窗口化优化，提高融合定位的精度。
附图说明
图1示意性示出了本公开实施例的基于图优化算法超宽带惯性导航融合位姿估计方法的流程图；
图2示意性示出了本公开另一实施例的基于图优化算法超宽带惯性导航融合位姿估计方法的流程图；
图3示意性示出了本公开实施例的基于图优化算法超宽带惯性导航融合位姿估计方法的原理图；
图4示意性示出了本公开实施例的基于图优化算法超宽带惯性导航融合位姿估计方法的效果轨迹图；
图5示意性示出了本公开实施例的基于图优化算法超宽带惯性导航融合位姿估计方法的实物图；
图6示意性示出了本公开实施例一的实验结果图；
图7示意性示出了本公开实施例二的仿真数据图；
图8示意性示出了本公开实施例的基于图优化算法超宽带惯性导航融合位姿估计方法与现有技术的仿真实验对比图；
图9为图8中仿真结果误差分析图；
其中，1表示移动设备；2表示上位机；3表示UWB标签；4表示UWB控制器；5表示INS感应器。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明作进一步的详细说明。
但是应该理解，这些描述只是示例性的，而并非要限制本发明的范围。在下面的详细描述中，为便于解释，阐述了许多具体的细节以提供对本发明实施例的全面理解。然而，明显地，一个或多个实施例在没有这些具体细节的情况下也可以被实施。此外，在以下说明中，省略了对公知技术的描述，以避免不必要地混淆本发明的概念。
在此使用的术语仅仅是为了描述具体实施例，而并非意在限制本发明。在此使用的术语“包括”表明了特征、步骤、操作的存在，但是并不排除存在或添加一个或多个其他特征。
在此使用的所有术语(包括技术和科学术语)具有本领域技术人员通常所理解的含义，除非另外定义。应注意，这里使用的术语应解释为具有与本说明书的上下文相一致的含义，而不应以理想化或过于刻板的方式来解释，例如，通用图优化(General GraphOptimization,G2O)；超宽带(Ultra-Wide Band，UWB)；惯性导航(Inertial NavigationSystem，INS)。
图1示意性示出了本公开实施例的基于图优化算法超宽带惯性导航融合位姿估计方法的流程图。
如图1所示，本公开提供了一种基于图优化算法超宽带和惯性导航融合位姿估计方法，包括如下步骤。
S1：获取UWB数据帧和INS数据帧，并组成UWB-INS数据帧，INS数据帧包括当前时刻至前一UWB数据帧时刻之间所有INS的加速度计数据和陀螺仪数据，UWB数据帧包括所有的UWB标签3与UWB基站之间的第一测距数据。
根据本公开的一些实施例，INS数据帧的采样频率大于UWB数据帧的采样频率，因此，将某时刻UWB数据帧和该时刻至前一UWB数据帧时刻之间的多组INS数据共同组成为UWB-INS数据帧。
根据本公开的一些实施例，UWB标签3可以是2到多个。
根据本公开的一些实施例，当UWB标签3数量为多个时，多个UWB标签3的分布应当避免共线，用以获取更可靠数据。
S2：判断移动设备1是否进行过初始化，若是，则直接进入下一步，若否，则在静止状态下初始化移动设备1位置和姿态。
S3：对INS数据帧的数据进行积分计算得到惯性导航观测值，并利用惯性导航观测值结合上一帧关键帧的位姿状态数据计算得到第一当前位姿状态数据，其中，惯性导航观测值用于表征上一帧关键帧到当前INS数据帧之间移动设备1的位姿变化数据。
S4：将所有的UWB标签3与UWB基站之间的第一测距数据进行预处理得到第二当前位姿状态数据，根据第二当前位姿状态数据结合第一当前位姿状态数据剔除不符合预设条件的第一测距数据。
S5：将S3中的惯性导航观测值和S4中剔除不符合预设条件的第一测距数据后的剩余所有的第一测距数据作为约束边加入到图优化中进行优化，根据优化结果判断UWB-INS数据帧是否为关键帧，若是，将关键帧加入到优化窗口中，若否，返回S1。
图3示意性示出了本公开实施例的基于图优化算法超宽带惯性导航融合位姿估计方法的原理图；图4示意性示出了本公开实施例的基于图优化算法超宽带惯性导航融合位姿估计方法的效果轨迹图。
根据本公开的一些实施例，如图3、图4所示，在图优化中，INS数据积分约束边是某UWB-INS数据帧(包括关键帧)时刻到上一关键帧时刻之间所有的INS数据进行积分的结果，也即观测值。
根据本公开的一些实施例，如图3、图4所示，在图优化中，每条UWB测距作为独立的边加入，这样方便选取数据质量高的测距信息以获得更加精确的定位结果。
根据本公开的一些实施例，关键帧即为数据质量高并且与其他关键帧具有一定距离的UWB-INS数据帧，是在运行过程中不断选择出的具有代表性的数据帧。
根据本公开的一些实施例，如图3、图4所示，利用UWB测距约束边和INS数据积分约束边以及耦合的方式进行联合估算移动设备1状态。另外将多帧关键帧及其对应的约束边加入优化窗口进行窗口优化，这样可以充分利用更多的可靠数据从而提升整体定位的精度。
根据本公开的一些实施例，数据帧以及关键帧等，会在运行过程中附加上解得的该时刻的设备位姿速度等状态结果，其中位姿包括位置数据以及姿态数据。
S6：当优化窗口中增加新的UWB-INS关键帧数据后，判断优化窗口中当前所有UWB-INS关键帧的数量是否满足预设条件，若满足，则进行窗口优化得到移动设备1位姿数据，并去除优化窗口中时间最早的至少一帧UWB-INS关键帧，若不满足，则等待下一帧UWB-INS关键帧数据加入后重新判断。
根据本公开的一些实施例，如图2所示，本公开还提供了另一实施例的流程图，其中，在S1之前还包括判断是否存在UWB数据帧和INS数据帧，如若存在，则进行UWB数据帧和INS数据帧的获取，如若不存在，则结束。
根据本公开的一些实施例，如图2所示S4中剔除不符合预设条件的第一测距数据后还包括，根据所有UWB的第一测距数据中符合预设要求的数量判断当前UWB数据帧质量，若符合预设要求的第一测距数据的数量大于等于第一预设阈值，则进入下一步，若小于第一预设阈值，则返回S1。
根据本公开的一些实施例，第一预设阈值设置为50％，也即，所有UWB的测距数据中符合预设要求的数量超过一半，则认定所述UWB数据帧质量较好，可以进入下一步以进一步判定其是否为关键帧。
根据本公开的一些实施例，S2中，对移动设备1初始化还包括获取加速度计和陀螺仪的零偏，用以提高INS数据帧的准确性。
根据本公开的一些实施例，初始化需要获取多个UWB-INS数据帧，以保证初始化结果的有效性。若检测到移动设备1已通过其他方式得到系统初始状态和加速度计和陀螺仪零偏，可以忽略此步骤，直接进入下一步。
根据本公开的一些实施例，UWB数据帧和INS数据帧采用同一套系统时间戳，或将采用不同的系统时间戳转化为同一系统时间戳，用以保证UWB数据帧的数据和INS数据帧的数据对齐。
根据本公开的一些实施例，S3中的积分计算的公式包括：
Figure BDA0002792701980000091
Figure BDA0002792701980000092
Figure BDA0002792701980000093
其中，Δt为采样间隔，ωB、aB分别为陀螺仪和加速度计的测量数据，Rk+1和Rk分别为时刻k和时刻k+1的姿态角，vk+1和vk分别为时刻k和时刻k+1的速度向量，Pk+1和Pk分别为时刻k和时刻k+1的位置，bg、ba分别为陀螺仪和加速度计的零偏，g表示重力向量，SO(3)表示三维特殊正交群的姿态角，相邻采样时刻k与k+1之间姿态角R∈SO(3)；
利用INS数据帧的数据和公式(1)、(2)和(3)多次累积获得惯性导航观测值。
根据本公开的一些实施例，S4中，通过第二当前位姿状态数据得到移动设备1的位置P和姿态角R，根据位置P和姿态角R的结果反推计算得到UWB标签3与UWB基站之间的第二测距数据，计算第二测距数据与第一测距数据的误差的公式包括：
euwb＝‖Panchor-(P+R·TBT)‖-dobs (4)
其中，euwb表示第二测距数据与第一测距数据的误差，Panchor表示基站位置，TBT表示UWB标签3相对于INS的位置、dobs表示UWB测距值；
优化UWB数据帧的数据，使得所有UWB标签3与基站的测距误差的平方和最小，优化公式包括：
Figure BDA0002792701980000094
其中，ρ为鲁棒核函数、Σx为UWB测距的数据矩阵，Σx可由实验得到，即UWB测距方差的倒数。
根据本公开的一些实施例，剔除不符合预设条件的测距数据包括，将每一个第一测距数据计算得到的euwb与第二预设阈值比较判断第一测距数据是否为野值，若是，则剔除，若否，则保留第一测距数据作为约束边。
根据本公开的一些实施例，第二预设阈值设置为允许UWB测距最大误差，也可以根据实际需要设置第二预设阈值的大小。
根据本公开的一些实施例，S5中，加入到图优化中进行优化包括，当前UWB-INS数据帧j的位姿集合为θ＝{Rj,pj,vj}，上一帧关键帧序号为i，位姿集合的各元素的误差公式包括：
Figure BDA0002792701980000101
Figure BDA0002792701980000102
Figure BDA0002792701980000103
其中，ΔRij,Δvij,Δpij分别为UWB-INS数据帧j的姿态角变化量的观测值、速度向量变化量的观测值和位置变化量的观测值；
INS数据帧的总误差公式包括：
Figure BDA0002792701980000104
其中ΣI为积分的数据矩阵，由INS数据帧本身性能决定，ρ为鲁棒核函数，Eimu(i,j)为当前UWB-INS数据帧j至上一阵关键帧i之间的总误差；
位姿集合θ的优化公式包括：
Figure BDA0002792701980000105
其中，Euwb(j)由公式(5)计算得出。
根据本公开的一些实施例，图优化的本质是最小二乘法优化，通过将残差项的误差优化到最小，得到优化后的状态。
根据本公开的一些实施例，残差项包括UWB实际测距和根据待估计状态(第二当前位姿状态数据)反推得到的距离之间的误差、INS实际数据积分值和根据待估计状态反推得到的积分值之间的误差。
根据本公开的一些实施例，根据INS数据积分预测结果设定待估计状态即位姿速度的初始值，通过不断优化迭代，将各残差降到最小，即得到优化后的状态。
根据本公开的一些实施例，S5是只优化当前帧的状态，其残差项即为当前UWB-INS数据帧状态所关联的残差。
根据本公开的一些实施例，S6为窗口优化，其待估计状态为一定大小窗口内所有关键帧的状态，残差项为这些关键帧所关联的残差。
根据本公开的一些实施例，位姿集合θ的优化中，固定上一帧关键帧的状态，利用G20中高斯牛顿法优化对当前UWB-INS数据帧进行优化，根据优化结果判断UWB-INS数据帧是否为关键帧。
根据本公开的一些实施例，根据优化结果判断当前状态是否有效且具有一定的变化，具体的，根据当前UWB-INS数据帧优化后的总体误差大小判断该结果是否有效，如果有效则根据该结果与上一关键帧的之间位姿变化大小去判断是否有一定的变化。
根据本公开的一些实施例，窗口优化线程中，优化窗口为包括最新的关键帧和之前多个连续的关键帧的集合，进行窗口优化包括如跟踪线程中对姿态角、速度向量和位置的优化。根据本公开的一些实施例，当前窗口优化完成后将窗口内较早的关键帧移出优化窗口并固定其位姿信息，继续等待新的关键帧的到来，直到没有新的数据进入系统。
根据本公开的一些实施例，优化窗口的窗口大小即所含关键帧数量，阈值可根据需要设置。
根据本公开的一些实施例，窗口优化为将优化窗口中所有关键帧的待估计状态，即其姿态、速度和位置同时进行优化，而优化窗口的前一关键帧作为固定状态提供基准。
根据本公开的一些实施例，去掉优化窗口最早的1帧或多帧，也即边缘化处理，只保留其状态在系统内以提供状态基准。
下面结合具体实施例对本公开的技术方案做进一步的介绍，应当理解的是，这些具体实施例仅仅是为了便于更清楚的理解本公开的技术方案，而不是对本公开的保护范围的限定。
具体实施例一：
图5为本公开实施例提供的所搭载的系统实物图。
根据本公开的一些实施例，如图5所示，其中包括UWB标签3、UWB控制器4、INS感应器5、上位机2、移动设备1等。
根据本公开的一些实施例，图5中未体现出UWB基站，本公开实施例中基站采用4个UWB节点呈方形共面分布。
根据本公开的一些实施例，上位机2可以为普通笔记本电脑，负责运行本发明算法。
根据本公开的一些实施例，INS感应器5包括加速度计和陀螺仪。
根据本公开的一些实施例，UWB控制器4负责收集所有UWB标签3和基站之间的测距信息，统一发送到上位机2。
根据本公开的一些实施例，本实例使用一台遥控车为移动设备1，以图中从左到右方向为车体正方向。
根据本公开的一些实施例，本实例中使用了3个UWB标签3和4个UWB基站，故UWB数据帧共包含12条测距信息。3个标签呈直角三角形分布，其中标签0和1距离0.57m，标签1和2距离0.60m。3个UWB标签3相对于INS参考系的位姿变换分别为：
Figure BDA0002792701980000124
Figure BDA0002792701980000121
Figure BDA0002792701980000122
其中，4个UWB基站的位置分别为：
Figure BDA0002792701980000123
根据本公开的一些实施例，UWB测距频率为50Hz，即在0.02s内完成12条UWB测距。INS采集频率为200Hz，即每个UWB数据帧与4组INS数据组成UWB-INS数据帧同时传入到系统处理。
根据本公开的一些实施例，在本实例中融合定位主要包括以下步骤：
步骤A：静置状态下估计INS偏置和车体初始位置姿态，并设置第一帧关键帧。
步骤B：对于每UWB-INS数据帧，计算INS从上一关键帧到此帧的积分变化量{ΔRij,Δvij,Δpij}以及对应信息矩阵，并预测当前状态{Rj,pj,vj}。
步骤C：对UWB数据帧进行预处理，即在图优化中添加12个
Figure BDA0002792701980000131
此Σx为先验方差的倒数。利用预处理结果和预测结果剔除UWB测距野值并设置各测距信息矩阵，若保留的有效测距多于总数一半，则认为UWB数据帧质量较好满足关键帧候选条件，进入下一步骤。
步骤D：融合UWB和INS，即将Eimu(i,j)和保留的Euwb(j)共同优化得到结果。如果整体误差较小则认为结果有效，进一步判断此时状态距上一关键帧状态是否有一定变化，有则将此帧作为关键帧插入到优化窗口。
步骤E：在窗口优化线程中，当前优化窗口内关键帧数量大于20时则将窗口内所有待估计状态全部加入图优化中，进行窗口优化，剔除最早的一帧或者几帧关键帧并固定其位姿信息，结束后继续等待新的关键帧。
循环执行以上步骤直到任务结束。
以一次执行结果为例，实验结果如图6所示，其中，NavPx、NavPy、Tag1Px、Tag1Py分别为多UWB标签3和INS融合优化后的结果以及只利用单个UWB标签3得到的X、Y方向的定位结果，可以看出融合后的结果更加稳定和平滑，更加符合车体的运动。
具体实施例二：
本实施例针对本公开提供的方法通过仿真定量分析定位定姿的精度并与其他方法作对比(在20×20m的场景内圆形运动的定姿定位做仿真分析)。其中INS仿真参数为，陀螺仪噪声方差1.5e-4rad/s，随机游走方差2.0e-5rad/s^2，加速度计噪声方差1.5e-3m/s^2，随机游走方差2.0e-4m/s^3。模拟实际运动生成仿真轨迹，包括加速和匀速直线前进5m，逆时针以2m/s速度绕半径5m的圆匀速圆周运动一周。理论轨迹和含噪声的INS数据积分得到的轨迹如INS仿真数据图(如图7所示)。设置UWB测距标准差为0.1m，4个UWB基站呈20m*20m正方形分布。其他参数、UWB标签3和INS分布以及实验步骤与具体实施例一实施例一致，在此不进行过多的描述。基于传统UWB三边定位、本发明利用UWB测距的图优化、本发明融合UWB/INS的窗口优化等三种层次的算法得到的轨迹如仿真实验结果，具体如图8、图9所示。此三种层次的算法对应的位置均方根误差(单位：m)分别为：0.0524、0.0237、0.0082，角度均方根(单位：rad)误差为：0.1406、0.0841、0.0255。可以看出，相对于基于三边定位算法将各UWB标签3计算结果利用几何信息整合的方法，基于图优化的方法可使位置和角度精度皆提高一倍左右，联合INS进行窗口优化的方法可以显著提高位置和角度精度。该仿真说明了本发明算法联合UWB和INS可以获取移动目标全局无漂移局部精确的位置和姿态信息。
通过上述技术方案，本公开提供的基于图优化算法UWBINS融合位姿估计方法，利用多个UWB标签3数据，提供精准的定位数据和姿态数据，通过算法对数据质量加以筛选得到高质量的关键帧的数据，进行窗口化优化，提高融合定位的精度。
现有技术相比，本公开的技术方案具有如下有益效果。
本公开所提出的基于图优化和关键帧的UWB和INS融合方法系统无需建立系统滤波模型，不存在模型不匹配问题，具有较强的鲁棒性和通用性，可以有效提高融合定位的精度。
本公开同时利用多个UWB标签的信息，在精准提供定位信息的同时还可以判断姿态信息，并通过算法对数据质量加以筛选，显著提高了定位定姿数据的稳定性和可靠性。
本公开通过选择关键帧作为INS约束的基准，同时进行窗口优化，充分利用了INS与UWB的综合可靠数据，极大地提升了整体定位精度。
本公开方法稳定、可靠、易用，成本低廉，适用于各种工况下高精度定位定姿，可应用于消费电子、工业设备等各场景在线应用，市场前景更为广阔。
至此，已经结合附图对本公开实施例进行了详细描述。需要说明的是，在附图或说明书正文中，未绘示或描述的实现方式，均为所属技术领域中普通技术人员所知的形式，并未进行详细说明。此外，上述对各零部件的定义并不仅限于实施例中提到的各种具体结构、形状或方式，本领域普通技术人员可对其进行简单地更改或替换。
还需要说明的是，在本公开的具体实施例中，除非有所知名为相反之意，本说明书及所附权利要求中的数值参数是近似值，能够根据通过本公开的内容所得的所需特性改变。具体而言，所有使用于说明书及权利要求中表示组成的尺寸、范围条件等等的数字，应理解为在所有情况中是受到“约”的用语所修饰。一般情况下，其表达的含义是指包含由特定数量在一些实施例中±10％的变化、在一些实施例中±5％的变化、在一些实施例中±1％的变化、在一些实施例中±0.5％的变化。
本领域技术人员可以理解，本公开的各个实施例和/或权利要求中记载的特征可以进行多种组合或/或结合，即使这样的组合或结合没有明确记载于本公开中。特别地，在不脱离本公开精神和教导的情况下，本公开的各个实施例和/或权利要求中记载的特征可以进行多种组合和/或结合。所有这些组合和/或结合均落入本公开的范围。
以上所述的具体实施例，对本公开的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于图优化算法超宽带惯性导航融合位姿估计方法，其特征在于，包括：
S1：获取超宽带数据帧和惯性导航数据帧，并组成超宽带-惯性导航数据帧，所述惯性导航数据帧包括当前时刻至前一超宽带数据帧时刻之间所有惯性导航的加速度计数据和陀螺仪数据，所述超宽带数据帧包括所有的超宽带标签与超宽带基站之间的第一测距数据；
S2：判断移动设备是否进行过初始化，若是，则直接进入下一步，若否，则在静止状态下初始化移动设备位置和姿态；
S3：对所述惯性导航数据帧的数据进行积分计算得到惯性导航观测值，所述惯性导航观测值包括姿态角、速度向量和位置，并利用所述惯性导航观测值结合上一帧关键帧的位姿状态数据计算得到第一当前位姿状态数据，其中，所述惯性导航观测值用于表征所述上一帧关键帧到当前惯性导航数据帧之间所述移动设备的位姿变化数据；
S4：将所有的所述超宽带标签与所述超宽带基站之间的第一测距数据进行预处理得到第二当前位姿状态数据，所述预处理包括优化所述超宽带数据帧的数据，使得所有所述超宽带标签与所述基站的测距误差的平方和最小，根据所述第二当前位姿状态数据结合所述第一当前位姿状态数据剔除不符合预设条件的第一测距数据；
S5：将所述S3中的所述惯性导航观测值和所述S4中剔除不符合预设条件的第一测距数据后的剩余所有的第一测距数据作为约束边加入到图优化中根据最小二乘法进行优化至残差项的误差优化到最小，根据优化结果判断所述超宽带-惯性导航数据帧是否为关键帧，若是，将所述关键帧加入到优化窗口中，若否，返回S1；
S6：当所述优化窗口中增加新的超宽带-惯性导航关键帧数据后，判断所述优化窗口中当前所有超宽带-惯性导航关键帧的数量是否满足预设条件，若满足，则进行窗口优化得到所述移动设备位姿数据，并去除所述优化窗口中时间最早的至少一帧所述超宽带-惯性导航关键帧，若不满足，则等待下一帧超宽带-惯性导航关键帧数据加入后重新判断。
2.根据权利要求1所述的基于图优化算法超宽带惯性导航融合位姿估计方法，其特征在于，所述S4中所述剔除不符合预设条件的第一测距数据后还包括，根据所有所述超宽带的第一测距数据中符合预设要求的数量判断当前超宽带数据帧质量，若数量大于等于第一预设阈值，则进入下一步，若小于所述第一预设阈值，则返回所述S1。
3.根据权利要求2所述的基于图优化算法超宽带惯性导航融合位姿估计方法，其特征在于，所述S2中，对所述移动设备初始化还包括获取所述加速度计和所述陀螺仪的零偏，用以提高所述惯性导航数据帧的准确性。
4.根据权利要求3所述的基于图优化算法超宽带惯性导航融合位姿估计方法，其特征在于，所述超宽带数据帧和所述惯性导航数据帧采用同一套系统时间戳，或将采用不同的系统时间戳转化为同一系统时间戳，用以保证所述超宽带数据帧的数据和所述惯性导航数据帧的数据对齐。
5.根据权利要求4所述的基于图优化算法超宽带惯性导航融合位姿估计方法，其特征在于，所述S3中的积分计算的公式包括：
Figure FDA0003851586920000021
Figure FDA0003851586920000022
Figure FDA0003851586920000023
其中，Δt为采样间隔，ωB、aB分别为陀螺仪和加速度计的测量数据，
Rk+1和Rk分别为时刻k和时刻k+1的姿态角，vk+1和vk分别为时刻k和时刻k+1的速度向量，Pk+1和Pk分别为时刻k和时刻k+1的位置，bg、ba分别为陀螺仪和加速度计的零偏，g表示重力向量，SO(3)表示三维特殊正交群的姿态角，相邻采样时刻k与k+1之间姿态角R∈SO(3)；
利用所述惯性导航数据帧的数据和所述公式(1)、(2)和(3)多次累积获得所述惯性导航观测值。
6.根据权利要求5所述的基于图优化算法超宽带惯性导航融合位姿估计方法，其特征在于，所述S4中，通过所述第二当前位姿状态数据得到所述移动设备的位置P和姿态角R，根据所述位置P和姿态角R的结果反推计算得到所述超宽带标签与所述超宽带基站之间的第二测距数据，计算所述第二测距数据与所述第一测距数据的误差的公式包括：
euwb＝‖Panchor-(P+R·TBT)‖-dobs (4)
其中，euwb表示第二测距数据与第一测距数据的误差，Panchor表示基站位置，TBT表示超宽带标签相对于惯性导航的位置、dobs表示超宽带测距值；
优化所述超宽带数据帧的数据，使得所有所述超宽带标签与所述基站的测距误差的平方和最小，优化公式包括：
Figure FDA0003851586920000031
其中，ρ为鲁棒核函数、Σx为超宽带测距的数据矩阵，Σx可由实验得到，即超宽带测距方差的倒数。
7.根据权利要求6所述的基于图优化算法超宽带惯性导航融合位姿估计方法，其特征在于，所述剔除不符合预设条件的测距数据包括，将每一个所述第一测距数据计算得到的所述euwb与第二预设阈值比较判断所述第一测距数据是否为野值，若是，则剔除，若否，则保留所述第一测距数据作为约束边。
8.根据权利要求7所述的基于图优化算法超宽带惯性导航融合位姿估计方法，其特征在于，所述S5中，所述加入到图优化中进行优化包括，当前超宽带-惯性导航数据帧j的位姿集合为θ＝{Rj,pj,vj}，上一帧关键帧序号为i，所述位姿集合的各元素的误差公式包括：
Figure FDA0003851586920000032
Figure FDA0003851586920000033
Figure FDA0003851586920000034
其中，ΔRij,Δvij,Δpij分别为超宽带-惯性导航数据帧j的姿态角变化量的观测值、速度向量变化量的观测值和位置变化量的观测值；
惯性导航数据帧的总误差公式包括：
Figure FDA0003851586920000035
其中ΣI为积分的数据矩阵，由惯性导航数据帧本身性能决定，ρ为鲁棒核函数，Eimu(i,j)为当前超宽带-惯性导航数据帧j至上一阵关键帧i之间的总误差；
所述位姿集合θ的优化公式包括：
Figure FDA0003851586920000041
其中，Euwb(j)由公式(5)计算得出。
9.根据权利要求8所述的基于图优化算法超宽带惯性导航融合位姿估计方法，其特征在于，所述位姿集合θ的优化中，固定上一帧关键帧的状态，利用通用图优化中高斯牛顿法优化对当前超宽带-惯性导航数据帧进行优化，根据优化结果判断所述超宽带-惯性导航数据帧是否为关键帧。
10.根据权利要求9所述的基于图优化算法超宽带惯性导航融合位姿估计方法，其特征在于，所述窗口优化线程中，所述优化窗口为包括最新的关键帧和之前多个连续的关键帧的集合，所述进行窗口优化包括如跟踪线程中对姿态角、速度向量和位置的优化。
CN202011320423.3A 2020-11-23 2020-11-23 基于图优化算法超宽带惯性导航融合位姿估计方法 Active CN112525197B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011320423.3A CN112525197B (zh) 2020-11-23 2020-11-23 基于图优化算法超宽带惯性导航融合位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011320423.3A CN112525197B (zh) 2020-11-23 2020-11-23 基于图优化算法超宽带惯性导航融合位姿估计方法

Publications (2)

Publication Number Publication Date
CN112525197A CN112525197A (zh) 2021-03-19
CN112525197B true CN112525197B (zh) 2022-10-28

Family

ID=74992746

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011320423.3A Active CN112525197B (zh) 2020-11-23 2020-11-23 基于图优化算法超宽带惯性导航融合位姿估计方法

Country Status (1)

Country Link
CN (1) CN112525197B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113324544B (zh) * 2021-04-19 2023-02-28 北京化工大学 一种基于图优化的uwb/imu的室内移动机器人协同定位方法
CN112985392B (zh) * 2021-04-19 2021-07-30 中国人民解放军国防科技大学 基于图优化框架的行人惯性导航方法和装置
CN113124856B (zh) * 2021-05-21 2023-03-14 天津大学 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN113721189B (zh) * 2021-09-26 2023-08-01 中国科学院空天信息创新研究院 一种基于uwb和vio融合的多运动节点定位方法及系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108759825A (zh) * 2018-08-06 2018-11-06 济南大学 面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法及系统
CN111121767A (zh) * 2019-12-18 2020-05-08 南京理工大学 一种融合gps的机器人视觉惯导组合定位方法
CN111882607A (zh) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 一种适用于增强现实应用的视觉惯导融合位姿估计方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105928518B (zh) * 2016-04-14 2018-10-19 济南大学 采用伪距和位置信息的室内行人uwb/ins紧组合导航系统及方法
CN110657803B (zh) * 2018-06-28 2021-10-29 深圳市优必选科技有限公司 机器人定位方法、装置以及存储装置
CN110375730B (zh) * 2019-06-12 2021-07-27 深圳大学 基于imu和uwb融合的室内定位导航系统

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108759825A (zh) * 2018-08-06 2018-11-06 济南大学 面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法及系统
CN111121767A (zh) * 2019-12-18 2020-05-08 南京理工大学 一种融合gps的机器人视觉惯导组合定位方法
CN111882607A (zh) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 一种适用于增强现实应用的视觉惯导融合位姿估计方法

Also Published As

Publication number Publication date
CN112525197A (zh) 2021-03-19

Similar Documents

Publication Publication Date Title
CN112525197B (zh) 基于图优化算法超宽带惯性导航融合位姿估计方法
US10254118B2 (en) Extrinsic parameter calibration of a vision-aided inertial navigation system
CN104121905B (zh) 一种基于惯性传感器的航向角获取方法
CN108120438B (zh) 一种基于imu和rfid信息融合的室内目标快速跟踪方法
CN112639502A (zh) 机器人位姿估计
CN110375739B (zh) 一种移动端视觉融合定位方法、系统及电子设备
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN102538781A (zh) 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
Cai et al. Mobile robot localization using gps, imu and visual odometry
CN109443348A (zh) 一种基于环视视觉和惯导融合的地下车库库位跟踪方法
CN103776446A (zh) 一种基于双mems-imu的行人自主导航解算算法
CN110412596A (zh) 一种基于图像信息和激光点云的机器人定位方法
CN103994765A (zh) 一种惯性传感器的定位方法
Lin et al. GNSS-denied UAV indoor navigation with UWB incorporated visual inertial odometry
CN113763549A (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
Singh et al. Ubiquitous hybrid tracking techniques for augmented reality applications
CN113865584A (zh) 一种基于视觉惯性里程计的uwb三维寻物方法和装置
Zhang et al. Continuous indoor visual localization using a spatial model and constraint
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
US20230304802A1 (en) Passive combined indoor positioning system and method based on intelligent terminal sensor
CN109769206B (zh) 一种室内定位融合方法、装置、存储介质及终端设备
CN115540854A (zh) 一种基于uwb辅助的主动定位方法、设备和介质
CN109737957A (zh) 一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统
CN109682372A (zh) 一种结合建筑物结构信息与rfid标定的改进型pdr方法

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