CN117119586A - 一种基于uwb和imu室内定位融合算法 - Google Patents

一种基于uwb和imu室内定位融合算法 Download PDF

Info

Publication number
CN117119586A
CN117119586A CN202311090681.0A CN202311090681A CN117119586A CN 117119586 A CN117119586 A CN 117119586A CN 202311090681 A CN202311090681 A CN 202311090681A CN 117119586 A CN117119586 A CN 117119586A
Authority
CN
China
Prior art keywords
data
positioning
uwb
imu
fusion
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.)
Granted
Application number
CN202311090681.0A
Other languages
English (en)
Other versions
CN117119586B (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.)
Changchun University of Science and Technology
Original Assignee
Changchun University of Science and Technology
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 Changchun University of Science and Technology filed Critical Changchun University of Science and Technology
Priority to CN202311090681.0A priority Critical patent/CN117119586B/zh
Publication of CN117119586A publication Critical patent/CN117119586A/zh
Application granted granted Critical
Publication of CN117119586B publication Critical patent/CN117119586B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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/006—Locating users or terminals or network equipment for network management purposes, e.g. mobility management with additional information processing, e.g. for direction or speed determination
    • 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/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • 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/025—Services making use of location information using location based information parameters
    • H04W4/027—Services making use of location information using location based information parameters using movement velocity, acceleration information
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Navigation (AREA)

Abstract

本发明属于室内定位技术领域，尤其为本发明公开了一种基于UWB和IMU室内定位融合算法，包括以下求解步骤：S1，建立IMU测量模型，利用加速度以及角速度的数据，得到运动平台的第一定位估计数据；S2，建立UWB测量模型，利用单片机测量出距离数据，并利用海伦公式求出三角形面积，通过面积得到运动平台的第二定位估计数据；S3，计算两种数据得到的差值，并且使用卡尔曼滤波进行数据更新处理，得到一组新的融合数据：S4，使用第一定位估计数据和S3所述的融合数据得到的差值，作为最终定位估计数据。本发明的算法流程简单，实用性强，且精度较高，计算量小，适用于UWB和IMU融合定位系统中。

Description

一种基于UWB和IMU室内定位融合算法
技术领域
本发明涉及室内定位技术领域，具体为一种基于UWB和IMU室内定位融合算法。
背景技术
随着科技的不断发展，室内定位技术的种类各式各样，比如红外线定位、超声波定位、Wi-Fi定位、蓝牙定位、UWB定位、IMU定位等。这些定位技术各有优缺点，应对的场景也各不相同。其中，UWB定位技术凭借其定位精度高、传输率高、功耗低等特点，但是会受到环境的影响，使用定位算法估计标签坐标的位置偏差会很大。
IMU定位技术是依靠加速度计、陀螺仪等传感器，测量出加速度以及角速度等的状态信息进行定位。IMU定位技术具有不受环境影响的优点，但是时间长会具有累计误差，定位精度越来越差。
因此，使用单一定位系统进行定位时，本身算法就有一定局限性，且还会受到外界影响导致数据偏差，无法实现复杂的室内环境下的高精度定位。
发明内容
(一)解决的技术问题
针对现有技术的不足，本发明提供了一种基于UWB和IMU室内定位融合算法，解决了UWB受环境影响时定位精度不高以及IMU随时间长会具有累计误差的问题。
(二)技术方案
本发明为了实现上述目的具体采用以下技术方案：
一种基于UWB和IMU室内定位融合算法，包括以下步骤，
S1，建立IMU测量模型，利用IMU中的传感器包括陀螺仪以及加速度计测出标签的加速度以及角速度的数据，通过加速度以及角速度得到运动平台的第一定位估计数据；
S2，建立UWB测量模型，使用3个UWB基站，利用单片机测量每个基站到标签之间的距离数据，并利用海伦公式求出每两个基站和标签为顶点的三角形面积，通过面积得到运动平台的第二定位估计数据；
S3，计算根据第一定位估计数据和第二定位估计数据得到的差值，并且使用卡尔曼滤波进行数据更新处理，得到一组新的融合数据：
S4，使用第一定位估计数据和S3所述的融合数据得到的差值，作为最终定位估计数据。
进一步地，所述S1中，建立IMU测量模型获取位置的估计值包括：
惯性导航系统在载体运动时，通过陀螺仪输出不断改变的角速度信息，进而得到载体的运动姿态及航向角信息；
通过加速度计输出载体坐标系下的加速度信息，并通过一次积分得到载体的运动速度、二次积分得到载体的运动距离。
进一步地，所述S1中，加速度以及角速度的数据为载体坐标系的信息，通过公式转换为导航坐标系的信息即为坐标估计值(x,y)。
进一步地，IMU测量模型获取位置的估计值表示为：
其中，a为加速度计测得的加速度，ω为陀螺仪测得的角速度，t为运动时间；V0，x0，y0为上一时刻的速度以及坐标估计值。
进一步地，所述S2中，使用海伦公式面积法作为UWB的的室内定位算法；设置3个基站，使用单片机测出3个基站到标签的距离，以及这三个基站两两之间的距离；使用海伦公式求出每两个基站和标签为顶点的三角形面积，通过面积求解坐标估计值。
进一步地，所述S3中，数据更新处理步骤如下：
根据第一定位估计数据和第二定位估计数据得到的差值作为观测值，记录每一时刻的差值；
根据k-1时刻得到的Xk-1/k-1和Pk-1/k-1及Qk通过下式计算得到Xk/k-1和Pk/k-1；
设观测向量Yk1＝xhailun和Yk2＝yhailun分别带入下式中进行数据更新，即可得到融合算法，其中xhailun为使用海伦公式面积法解出标签坐标点的x，yhailun为使用海伦公式面积法解出标签坐标点的y；
Pk/k＝[E-KkHk]Pk/k-1
其中P为设置状态转移的矩阵，Q为wk-1的协方差阵，R为vk的协方差阵，Kk为卡尔曼增益，为残差，E为单位阵。
进一步地，所述S4中，融合数据得到更新后，使用第一定位估计数据与更新后的融合数据做差值，对定位数据再次更新，进而得到更新后的最终定位估计数据。
(三)有益效果
与现有技术相比，本发明提供了一种基于UWB和IMU室内定位融合算法，具备以下有益效果：
本发明，利用UWB与IMU定位系统的互补特性，克服了单一定位系统在复杂环境下的局限性；
使用海伦公式面积法作为UWB的定位算法，利用三角形的面积求解载体坐标，克服了使用距离解算坐标的难题；
利用IMU定位数据测量来解决短时间UWB受到环境误差的影响，同时通过UWB定位数据来约束长时间IMU定位数据的累计误差；
采用卡尔曼滤波算法将IMU和UWB定位信息进行融合，减少迭代带来的误差，克服了复杂室内环境下长时间定位误差过大的问题。
附图说明
图1为本发明流程图；
图2为本发明海伦公式面积法示意图；
图3为海伦公式面积法与常规定位算法对比图；
图4为本发明室内定位融合算法与常规室内定位融合算法对比图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例
如图1-4所示，本发明一个实施例提出的一种基于UWB和IMU室内定位融合算法，包括以下步骤。
S1，建立IMU测量模型，利用IMU中的传感器包括陀螺仪以及加速度计测出标签的加速度以及角速度的数据，通过加速度以及角速度得到运动平台的第一定位估计数据；
第一定位估计数据具体计算如下：
其中，a为加速度计测得的加速度，ω为陀螺仪测得的角速度，t为运动时间；V0，x0，y0为上一时刻的速度以及坐标估计值，通过上式即可解出坐标估计值(x,y)。
S2，建立UWB测量模型，使用3个UWB基站，利用单片机测量每个基站到标签之间的距离数据，并利用海伦公式求出每两个基站和标签为顶点的三角形面积，通过面积得到运动平台的第二定位估计数据，第二定位估计数据具体计算如下：
如图2所示，A(x1,y1),B(x2,y2),C(x3,y3)为3个基站，O(x,y)为标签，AB距离为d1，AC距离为d2，BC距离为d3，AO距离为r1，BO距离为r2，CO距离为r3,h1为O到AB直线的距离，h2为O到AC直线的距离，h3为O到BC直线的距离，可得到
坐标点到每两个基站连成的直线距离可以通过点到直线距离公式能得到：
设
根据海伦公式可得：
因为d1,d2,d3为已知量r1,r2,r3,为测量值，所以三角形面积为已知量，将数值带入下面公式：
为了简化计算，将A设为(0,0)B设为(0,y2)C设为(x3,y3)将求出的h1,h2,h3和d1，d2，d3带入(18)式，两边平方可得：
通过上式可以求出x的两个解，跟据路径判断最优x的解，将x的解带入下式
可得出y的两个解，根据路径得到最优y的解，
此式即可作为冗余公式，对精度不高的坐标进行调整修正。
仿真结果定位图如图3所示，可以看出海伦公式面积法的定位估计要比常规定位算法精度要高，常规定位算法的定位平均误差在45.3cm，而海伦公式面积法的定位平均误差在31.8cm，相比常规定位算法定位精度提高了29.8％，由此可以看出海伦公式面积法的定位轨迹更加符合路径。
S3，计算根据第一定位估计数据和第二定位估计数据得到的差值，并且使用卡尔曼滤波进行数据更新处理，得到一组新的融合数据：
根据第一定位估计数据和第二定位估计数据得到的差值作为观测值，记录每一时刻的差值；
根据k-1时刻得到的Xk-1/k-1和Pk-1/k-1及Qk通过下式计算得到Xk/k-1和Pk/k-1；
设观测向量Yk1＝xhailun和Yk2＝yhailun分别带入下式中进行数据更新，即可得到融合算法。其中xhailun为使用海伦公式面积法解出标签坐标点的x，yhailun为使用海伦公式面积法解出标签坐标点的y；
Pk/k＝[E-KkHk]Pk/k-1
其中P为设置状态转移的矩阵，Q为wk-1的协方差阵，R为vk的协方差阵，Kk为卡尔曼增益，为残差，E为单位阵。
S4，使用第一定位估计数据和S3所述的融合数据得到的差值，作为最终定位估计数据。
如图4所示，可以看出本发明的一种基于UWB和IMU室内定位融合算法定位精度要强于常规UWB和IMU室内定位融合算法。常规UWB和IMU室内定位融合算法的定位平均误差在70.1cm，而本发明的一种基于UWB和IMU室内定位融合算法的定位平均误差在28.6cm，相比常规融合定位算法定位精度提高了59.2％，由此可以看出本发明的一种基于UWB和IMU室内定位融合算法定位轨迹更加符合路径。
本发明的一种基于UWB和IMU室内定位融合算法的优点：本发明的算法流程简单，实用性强，且精度较高，计算量小，适用于UWB和IMU融合定位系统中。
最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于UWB和IMU室内定位融合算法，其特征在于：包括以下步骤，
S1，建立IMU测量模型，利用IMU中的传感器包括陀螺仪以及加速度计测出标签的加速度以及角速度的数据，通过加速度以及角速度得到运动平台的第一定位估计数据；
S2，建立UWB测量模型，使用3个UWB基站，利用单片机测量每个基站到标签之间的距离数据，并利用海伦公式求出每两个基站和标签为顶点的三角形面积，通过面积得到运动平台的第二定位估计数据；
S3，计算根据第一定位估计数据和第二定位估计数据得到的差值，并且使用卡尔曼滤波进行数据更新处理，得到一组新的融合数据：
S4，使用第一定位估计数据和S3所述的融合数据得到的差值，作为最终定位估计数据。
2.根据权利要求1所述的一种基于UWB和IMU室内定位融合算法，其特征在于：所述S1中，建立IMU测量模型获取位置的估计值包括：
惯性导航系统在载体运动时，通过陀螺仪输出不断改变的角速度信息，进而得到载体的运动姿态及航向角信息；
通过加速度计输出载体坐标系下的加速度信息，并通过一次积分得到载体的运动速度、二次积分得到载体的运动距离。
3.根据权利要求1所述的一种基于UWB和IMU室内定位融合算法，其特征在于：所述S1中，加速度以及角速度的数据为载体坐标系的信息，通过公式转换为导航坐标系的信息即为坐标估计值(x,y)。
4.根据权利要求3所述的一种基于UWB和IMU室内定位融合算法，其特征在于：IMU测量模型获取位置的估计值表示为：
其中，a为加速度计测得的加速度，ω为陀螺仪测得的角速度，t为运动时间；V0，x0，y0为上一时刻的速度以及坐标估计值。
5.根据权利要求1所述的一种基于UWB和IMU室内定位融合算法，其特征在于：所述S2中，使用海伦公式面积法作为UWB的的室内定位算法；设置3个基站，使用单片机测出3个基站到标签的距离，以及这三个基站两两之间的距离；使用海伦公式求出每两个基站和标签为顶点的三角形面积，通过面积求解坐标估计值。
6.根据权利要求1所述的一种基于UWB和IMU室内定位融合算法，其特征在于：所述S3中，数据更新处理步骤如下：
根据第一定位估计数据和第二定位估计数据得到的差值作为观测值，记录每一时刻的差值；
根据k-1时刻得到的Xk-1/k-1和Pk-1/k-1及Qk通过下式计算得到Xk/k-1和Pk/k-1；
设观测向量Yk1＝xhailun和Yk2＝yhailun分别带入下式中进行数据更新，即可得到融合算法，其中xhailun为使用海伦公式面积法解出标签坐标点的x，yhailun为使用海伦公式面积法解出标签坐标点的y；
Pk/k＝[E-KkHk]Pk/k-1
其中P为设置状态转移的矩阵，Q为wk-1的协方差阵，R为vk的协方差阵，Kk为卡尔曼增益，为残差，E为单位阵。
7.根据权利要求1所述的一种基于UWB和IMU室内定位融合算法，其特征在于：所述S4中，融合数据得到更新后，使用第一定位估计数据与更新后的融合数据做差值，对定位数据再次更新，进而得到更新后的最终定位估计数据。
CN202311090681.0A 2023-08-29 2023-08-29 一种基于uwb和imu室内定位融合方法 Active CN117119586B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311090681.0A CN117119586B (zh) 2023-08-29 2023-08-29 一种基于uwb和imu室内定位融合方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311090681.0A CN117119586B (zh) 2023-08-29 2023-08-29 一种基于uwb和imu室内定位融合方法

Publications (2)

Publication Number Publication Date
CN117119586A true CN117119586A (zh) 2023-11-24
CN117119586B CN117119586B (zh) 2024-05-24

Family

ID=88794385

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311090681.0A Active CN117119586B (zh) 2023-08-29 2023-08-29 一种基于uwb和imu室内定位融合方法

Country Status (1)

Country Link
CN (1) CN117119586B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117425124A (zh) * 2023-12-08 2024-01-19 长春理工大学 一种基于海伦公式面积法的uwb定位方法及系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107148080A (zh) * 2017-05-19 2017-09-08 浙江工业大学 一种基于海伦公式计算面积残差的二次规划定位方法
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统
CN110568401A (zh) * 2019-09-11 2019-12-13 东北大学 一种基于uwb的三维定位方法
CN112747747A (zh) * 2021-01-20 2021-05-04 重庆邮电大学 一种改进的uwb/imu融合室内行人定位方法
CN115031723A (zh) * 2022-03-10 2022-09-09 高斯机器人(深圳)有限公司 一种uwb定位精度提升办法
CN116380052A (zh) * 2022-12-13 2023-07-04 重庆邮电大学 一种基于uwb融合imu的室内定位方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107148080A (zh) * 2017-05-19 2017-09-08 浙江工业大学 一种基于海伦公式计算面积残差的二次规划定位方法
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统
CN110568401A (zh) * 2019-09-11 2019-12-13 东北大学 一种基于uwb的三维定位方法
CN112747747A (zh) * 2021-01-20 2021-05-04 重庆邮电大学 一种改进的uwb/imu融合室内行人定位方法
CN115031723A (zh) * 2022-03-10 2022-09-09 高斯机器人(深圳)有限公司 一种uwb定位精度提升办法
CN116380052A (zh) * 2022-12-13 2023-07-04 重庆邮电大学 一种基于uwb融合imu的室内定位方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117425124A (zh) * 2023-12-08 2024-01-19 长春理工大学 一种基于海伦公式面积法的uwb定位方法及系统
CN117425124B (zh) * 2023-12-08 2024-03-22 长春理工大学 一种基于海伦公式面积法的uwb定位方法及系统

Also Published As

Publication number Publication date
CN117119586B (zh) 2024-05-24

Similar Documents

Publication Publication Date Title
CN110160542B (zh) 车道线的定位方法和装置、存储介质、电子装置
CN105300410B (zh) 采煤机惯性导航定位误差校准装置及方法
CN113311411B (zh) 一种用于移动机器人的激光雷达点云运动畸变校正方法
Kim et al. High-speed train navigation system based on multi-sensor data fusion and map matching algorithm
CN105509739A (zh) 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN113654555A (zh) 一种基于多传感器数据融合的自动驾驶车辆高精定位方法
CN117119586B (zh) 一种基于uwb和imu室内定位融合方法
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN110702091A (zh) 一种沿地铁轨道移动机器人的高精度定位方法
CN109855621A (zh) 一种基于uwb与sins的组合室内行人导航系统及方法
CN110672099A (zh) 一种用于室内机器人导航的航向校正方法和系统
CN114018248B (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
CN107270896A (zh) 一种行人定位与轨迹跟踪方法和系统
CN111982102B (zh) 一种复杂环境下基于bp-ekf的uwb-imu定位方法
CN110412596A (zh) 一种基于图像信息和激光点云的机器人定位方法
CN111174791A (zh) 一种基于双向长短期记忆网络的定位修正方法
CN114739400A (zh) 一种基于uwb和imu信息融合的室内定位方法
CN105352502A (zh) 一种微惯性航姿参考系统的姿态获取方法
Zheng et al. Train integrated positioning method based on GPS/INS/RFID
CN114111802A (zh) 一种行人航迹推算辅助uwb的定位方法
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN116482735A (zh) 一种受限空间内外高精度定位方法
CN115326068B (zh) 激光雷达-惯性测量单元融合里程计设计方法及系统
CN111679297A (zh) 一种gps定位轨迹的噪声点漂移去除方法

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