CN117289322B - 一种基于imu、gps与uwb的高精度定位算法 - Google Patents

一种基于imu、gps与uwb的高精度定位算法 Download PDF

Info

Publication number
CN117289322B
CN117289322B CN202311575444.3A CN202311575444A CN117289322B CN 117289322 B CN117289322 B CN 117289322B CN 202311575444 A CN202311575444 A CN 202311575444A CN 117289322 B CN117289322 B CN 117289322B
Authority
CN
China
Prior art keywords
error
equation
modeling
gps
vector
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
CN202311575444.3A
Other languages
English (en)
Other versions
CN117289322A (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.)
Jiangsu Lingchuang Xingtong Satellite Communication Technology Co ltd
Original Assignee
Jiangsu Lingchuang Xingtong Satellite Communication Technology Co ltd
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 Jiangsu Lingchuang Xingtong Satellite Communication Technology Co ltd filed Critical Jiangsu Lingchuang Xingtong Satellite Communication Technology Co ltd
Priority to CN202311575444.3A priority Critical patent/CN117289322B/zh
Publication of CN117289322A publication Critical patent/CN117289322A/zh
Application granted granted Critical
Publication of CN117289322B publication Critical patent/CN117289322B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/393—Trajectory determination or predictive tracking, e.g. 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • 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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Signal Processing (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于IMU、GPS与UWB的高精度定位算法，包括确定状态向量、惯导的姿态误差微分方程建模、惯导的速度误差微分方程建模、惯导的位置误差微分方程建模、传感器误差建模、惯导的系统状态误差微分方程建模、GPS速度误差观测方程建模、GPS位置误差观测方程建模、UWB位置误差观测方程建模和双分支卡尔曼循环定位；本发明属于勘测、导航、陀螺仪技术领域，具体为一种基于IMU、GPS与UWB的高精度定位算法，本发明的优点为：本发明结合IMU的高频惯导数据，使用双分支卡尔曼循环定位预测载体位置，使整个算法更加鲁棒和精确，并且本发明使用的传感器价格低廉，是一种低成本的定位方案。

Description

一种基于IMU、GPS与UWB的高精度定位算法
技术领域
本发明属于勘测、导航、陀螺仪技术领域，具体为一种基于IMU、GPS与UWB的高精度定位算法。
背景技术
GPS/惯性导航系统（GPS/INS）是最常见的组合导航方案之一，全球定位系统（GPS）用于提供位置和速度信息，而惯性导航系统（INS）利用加速度计和陀螺仪等传感器测量加速度角速度，从而估计运动姿态、速度和位置，通过将两者的数据进行融合，可以实现高精度的位置和姿态估计。
视觉惯性导航系统（Visual-InertialNavigationSystem，简称VINS）是一种基于视觉和惯性传感器的组合导航方案，通过结合视觉传感器（如摄像头）和惯性传感器（如加速度计和陀螺仪），实现对位置和姿态的估计，VINS的工作原理是将视觉和惯性传感器的数据进行融合，以获取更准确和稳定的导航信息；具体而言，VINS利用摄像头获取环境中的视觉特征，并利用计算机视觉技术进行特征提取、匹配和跟踪，从而得到相机在三维空间中的运动轨迹，同时，惯性传感器测量加速度和角速度，通过积分得到相机的姿态变化，最后，将视觉和惯性传感器的数据进行优化和融合，通过滤波或优化算法，得到更准确的位置和姿态估计结果。
GPS/INS在GPS信号较强、没有遮挡的开阔领域，如农田、开阔的路面等能够提供较高的精度，通常在几米到十几米的范围内；当GPS信号受到遮挡、多径效应和信号干扰等因素的影响，导致导航精度下降，尤其在城市峡谷、森林覆盖等复杂环境中。
VINS的导航精度通常取决于视觉传感器的分辨率、环境特征的质量和数量，以及传感器标定的准确性；在良好的视觉条件下，VINS能够达到较高的导航精度，通常在几厘米到几十厘米的范围内，但VINS对于光照变化、纹理缺失等环境下可能导致导航精度下降。
UWB定位技术具有低功耗、对信道衰落不敏感、抗干扰能力强、不会对同一环境下的其他设备产生干扰、穿透性较强等优点，具有很高的定位精度。
发明内容
（一）技术方案
GPS/INS在开阔空间中具有较高的精度，但在密集城市或复杂环境中可能受到限制；VINS在良好的视觉条件下能够提供较高的精度，但在光照变化、纹理缺失等情况下可能受到挑战，为了解决上述问题，本发明采用的技术方案为：一种基于IMU、GPS与UWB的高精度定位算法，包括确定状态向量、惯导的姿态误差微分方程建模、惯导的速度误差微分方程建模、惯导的位置误差微分方程建模、传感器误差建模、惯导的系统状态误差微分方程建模、GPS速度误差观测方程建模、GPS位置误差观测方程建模、UWB位置误差观测方程建模和双分支卡尔曼循环定位；所述确定状态向量，就是确定卡尔曼滤波方程中的状态向量，包括导航状态误差向量和传感器：
上式中，δx表示状态误差，δr n 表示惯导位置误差向量在n系（当地水平坐标系）下的投影，δv n 表示惯导速度误差向量在n系下的投影，等号右边第三项表示惯导姿态误差向量，b g 表示三轴陀螺仪零偏向量，b a 表示三轴加速度计零偏向量，s g 表示三轴陀螺仪比例因子误差向量，s a 表示三轴加速度计比例因子误差向量，等号右边每一项都是三维的列向量，因此δx共有21维。
进一步地，所述惯导的姿态误差微分方程建模为：
上式中，表示惯导的姿态误差的导数，表示惯导的姿态误差，上标n表示各物理量在n系（当地水平坐标系）下的投影；上标b表示各物理量在b系（载体坐标系）下的投影；ω in 表示n系相对于i系（实用惯性系）的角速度；δω in 表示n系相对于i系的角速度误差；δω ib 表示b系相对于i系的角速度误差；表示惯导所在的b系相对于n系的方向余弦矩阵。
作为优选地，所述惯导的速度误差微分方程建模为：
上式中，等号左边表示当地水平坐标系下的速度向量的一阶导数，表示加速度计测得的比力误差在b系的投影；表示加速度计测得的比力在n系的投影；ω ie 表示e系（地心地固坐标系）相对于i系的角速度；δω ie 表示e系相对于i系的角速度误差；ω en 表示n系相对于e系的角速度；δω en 表示n系相对于e系的角速度误差；υ n 表示当地水平坐标系下的速度向量；δυ n 表示当地水平坐标系下的速度误差向量；δg p 的表示当地重力误差；当地重力的展开式为：
上式中，g表示重力矢量；ω ie 表示e系相对于i系的角速度向量；r表示载体的位置矢量；从而有：
上式中，上标n表示在n系的投影。
进一步地，所述惯导的位置误差微分方程建模为n系下的位置误差微分方程形式：
上式中，φ为载体所在的纬度；h为载体所在的高程；R M 表示子午圈曲率半径、R N 表示卯酉圈曲率半径、υ N 、υ E 和υ D 分别表示n系下的北向速度、东向速度和垂向速度；对纬度的导数做扰动分析有：δυ N 为北向速度误差，δυ E 为东向速度误差，δv D 表示垂向速度误差，等号左边分别表示对δr N 、δr E 和δr D 求导数。
作为优选地，所述传感器误差建模如下：
第一步，加速度计误差建模：
上面1式中，等号左边表示加速度计实际的比力测量输出；表示加速度计的理论输出；I为单位向量；S a 为加速度计的比例因子误差，b a 为加速度计零偏；ωa 表示加速度计测量值的白噪声；在2式中，表示加速度计测量误差；
第二步，陀螺仪误差建模：
式3中，等号左边表示陀螺实际的角速度测量输出，ωib b为b系下的理论角速度；S g 为陀螺仪的比例因子误差，b g为陀螺零偏；w g表示陀螺仪测量值的白噪声；δωib b表示陀螺仪测量误差；加速度计零偏、陀螺零偏、加速度计的比例因子误差和陀螺仪的比例因子误差建模为一阶高斯马尔科夫过程，有：
上式中，T gb、T ab、T gs和T as分别表示陀螺零偏、加速度计零偏、陀螺比例因子和加速度计比例因子的相关时间；ωgb、ωab、ωgs和ωas分别表示陀螺零偏驱动白噪声、加速度计零偏驱动白噪声、陀螺比例因子驱动白噪声和加速度计比例因子驱动白噪声。
进一步地，所述惯导的系统状态误差微分方程建模，具体步骤为：
首先，结合确定状态向量、惯导的姿态误差微分方程建模、惯导的速度误差微分方程建模、惯导的位置误差微分方程建模和传感器误差建模的结果，可以构建出惯导的系统状态误差微分方程，有：
上式中F(t)表示系统矩阵；G(t)表示输入方程；ω(t)表示连续时间的系统高斯白噪声；δx(t)表示t时刻的状态误差；
其次，根据上述微分方程进一步推出误差状态外推方程，有：
其中：
上式中，Φ k,k-1 表示状态转移矩阵，ω k-1 为系统高斯白噪声ω(t)的线性变换，δx k 表示k时刻的状态误差；δx k-1 表示k-1时刻的状态误差；记离散化时间间隔Δt=t k -t k-1 ，当Δt 比较小时，F(t)在时间间隔[t k-1 ，t k ]内变化不大，此时有：
可以证明ω k 是零均值白噪声序列，满足：
上式中，符号“N ”表示高斯分布，符号“~”表示服从，也就是ω k 服从高斯分布，Q k 为系统状态噪声协方差矩阵，计算方法为：
上式中，Q k 的计算使用梯形积分公式。
作为优选地，所述GPS位置误差观测方程建模，GPS的位置误差观测方程可以写成以下形式：
上式中，δz r 表示GPS位置观测误差；H r 表示GPS的位置观测矩阵，δr G表示GPS天线的位置误差，l b 为GPS相位中心的杆臂向量在b系下的投影。
进一步地，所述GPS速度误差观测方程建模为：
上式中，δv G 表示GPS速度误差，H v 表示GPS的速度观测矩阵，具体表达式为：
上式中：
上式中，符号diag表示对角矩阵化操作；GPS速度误差观测方程建模和GPS位置误差观测方程建模共同构成了GPS的观测方程建模。
进一步地，所述UWB位置误差观测方程建模的步骤为：将UWB标签固定在载体中心位置，UWB基站固定在位置已知的地面，所述基站数量设置为4个，记为第1号基站、第2号基站、第3号基站和第4号基站；利用飞行时间原理确定UWB标签到UWB基站的距离，从而确定载体的绝对位置，具体测距方式为：
上式中，T 14表示标签从发送请求到接收到反馈信号的时间间隔；T 23表示基站从接收请求到发送反馈信号的时间间隔；T表示飞行时间间隔；上标表示对应的基站编号；然后计算标签相对于每个基站的距离，计算公式为：
上式中，c为光速，D1、D2、D3和D4分别是载体上的标签到4个基站的距离；计算载体上的标签相对于基站的坐标，设第1号基站的坐标为(x1、y1、z1)，第2号基站的坐标为(x2、y2、z2)，第3号基站的坐标为(x3、y3、z3)，第4号基站的坐标为(x4、y4、z4)，标签坐标记为r UWB=（x、y、z），那么有如下关系式：
利用最小二乘法即可求出标签相对于基站的坐标；基于以上步骤，UWB误差观测方程建模为：
上式中，δr UWB为UWB位置观测误差，带上标“^”表示惯导解算的UWB标签的位置，带上标“˜”表示UWB标签自身解算的带误差的位置。
所述双分支卡尔曼循环定位，由误差状态外推方程、误差状态协方差外推方程、卡尔曼增益方程、GPS误差状态更新方程、UWB误差状态更新方程和误差状态协方差更新方程组成；以上方程均由误差状态外推方程、GPS观测方程和UWB位置误差观测方程推导得出；本发明为融合IMU、GPS和UWB多源数据，以IMU作为推导误差状态方程的主要传感器，将GPS的观测数据和UWB的观测数据作为推导观测方程的传感器；当GPS观测数据超过给定误差阈值或者GPS观测数据落在UWB信号覆盖范围时，使用UWB数据作为观测数据。
（二）有益效果
本发明提供一种基于IMU、GPS与UWB的高精度定位算法，克服了VINS只能在良好的视觉条件下才能提供较高精度的缺陷，本发明利用GPS和UWB的观测数据，使用双分支卡尔曼循环定位预测载体位置，即使在GPS信号不能及时更新的条件下，可以利用UWB的观测数据进行辅助定位，从而使整个算法更加鲁棒和精确，并且本发明使用的传感器价格低廉，是一种低成本的定位方案。
附图说明
图1为本发明提出的双分支卡尔曼循环定位的示意图；
图2为本发明提出的惯导的系统状态误差微分方程建模的流程图；
图3为本发明提出的GPS观测方程建模的流程图。
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。
具体实施方式
下面将结合本方案实施例中的附图，对本方案实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本方案一部分实施例，而不是全部的实施例；基于本方案中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本方案保护的范围。
本发明一种基于IMU、GPS与UWB的高精度定位算法，包括确定状态向量、惯导的姿态误差微分方程建模、惯导的速度误差微分方程建模、惯导的位置误差微分方程建模、传感器误差建模、惯导的系统状态误差微分方程建模、GPS速度误差观测方程建模、GPS位置误差观测方程建模、UWB位置误差观测方程建模和双分支卡尔曼循环定位；上述每个步骤的详细推导过程如下：首先，确定状态向量：
上式中，等号右边第一项表示惯导位置误差向量，等号右边第二项表示惯导速度误差向量，等号右边第三项表示惯导姿态误差向量，等号右边第四项表示三轴陀螺仪零偏向量，等号右边第五项表示三轴加速度计零偏向量，等号右边第六项表示三轴陀螺仪比例因子误差向量，等号右边最后一项表示三轴加速度计比例因子误差向量，上式的每一项都是三维的，因此共有21维。
惯导的姿态误差方程建模：
S1、姿态误差的定义，姿态指示了两个坐标系之间的相对角度关系，当姿态存在偏差时，可将其全部归算到某一个坐标系，有：
上式表示一种定义式，左边的项表示载体坐标系（b系）相对于真实的当地水平坐标系（n系）的姿态估计值，中间的项表示载体坐标系（b系）相对于存在偏差的当地水平坐标系（n'系）的方向余弦矩阵，等号右边向表示载体坐标系（b系）相对于p系的方向余弦矩阵，因此有n'系就是p系；真实的当地水平坐标系（n系）与计算得到的当地水平坐标系（p系）对齐所对应的三个欧拉角定义为姿态误差，有如下定义：
上式中，等号右边第一项为3×3单位矩阵，等号右边第二项为p系相对于n系的三个欧拉角对应的向量所组成的反对称矩阵，由于IMU的更新频率比较高，因此相邻历元之间时间很短，可以不考虑旋转轴的顺序；p系相对于n系的三个欧拉角对应的向量称为姿态误差；姿态误差为小量。
惯导的姿态误差微分方程的目标形式为：
上式表示要将姿态误差表示为一阶线性微分方程的形式，等式右边第一项为位置误差在n系的投影，等式右边第二项为速度误差在n系的投影，等式右边第三项为姿态误差；等式右边第四项为加速度计比力误差在b系的投影，等式右端最后一项为陀螺仪加速度误差在b系的投影；
已知条件：
上式中，1式表示b系相对于n'系的方向余弦矩阵与b系相对于n系的方向余弦矩阵之间的变换关系；2式表示b系相对于n系的方向余弦矩阵的导数与该方向余弦矩阵本身及b系相对于n系的角速度在b系投影向量之间的关系；3式表示b系相对于n系的角速度在b系投影与b系相对于i系的角速度在b系的投影向量和n系相对于i系的角速度在b系投影向量之间的关系；4表示将3式带入到2式后的结果；5表示n系相对于i系的角速度在n系投影向量与n系相对于i系的角速度在b系投影向量之间的关系；6式表示4式和5式推导的结果；对含有姿态误差的1式求导，对6式进行误差扰动，另他们的结果相等，即可得到惯导的姿态误差微分方程，具体步骤如下：
第一步，对1式求导：
第二步，对6式进行误差扰动：
上式的推导用到以下性质，有：
其中，a式表示b系相对于i系的角速度在b系的投影向量的扰动值等于其真值加上其误差项；b式表示n系相对于i系的角速度在n系的投影向量的扰动值等于其真值加上其误差项；c式与1式具有相同含义；d式和e式的等号左侧包含有两个小量，所以他们的乘积约等于0；
因此8式可以化简为：
第三步，另7式和9式相等有：
整理得：
上式两边右乘的转置，由于是单位正交矩阵，因此上式进一步化简为：
.进一步整理得：
由于有：
因此10式进一步化简为：
由于有：
因此11式可化简为：
将12式写成向量形式有：
由于，其中：
上式中，δr N 、δr D 和δr E 分别表示当地水平坐标系下的北向位置误差、当地水平坐标系下的东向位置误差和当地水平坐标系下的垂向位置误差；ωe为地球自转角速度；h为载体所在的高程；φ为载体所在的纬度；R M 表示子午圈曲率半径、R N 表示卯酉圈曲率半径、υ N 、υ D 和υ E 分别表示当地水平坐标系下的北向速度、东向速度和垂向速度。
惯导的速度误差微分方程建模，其目标形式为：
上式表示要将速度误差表示为一阶线性微分方程的形式，已知条件为：
上式中，等号左边表示速度向量的一阶导数在n系下的投影，表示惯导测得的比力在b系的投影；括号里第一项表示e系相对于i系的角速度在n系下的投影的2倍；括号里第二项表示n系相对于e系的角速度在n系下的投影；υ n 表示当地水平坐标系下的速度；g p 的展开式为：
上式中，g表示重力矢量；ω ie 表示e系相对于i系的角速度向量；r 表示载体的位置矢量；从而有：
推导思路为对14式进行扰动分析，逐项展开后取至一阶项，推导过程如下：
上标“^”表示对应物理量的扰动，符号“δ”表示对应物理量的误差，除了姿态扰动外，其余物理量的扰动值等于真值加上其误差；上面17式的推导用到了如下性质：
忽略17式中的二阶小量有：
结合14式，进而有：
上式中，ω en 表示n系相对于e系的角速度；δω en 表示n系相对于e系的角速度误差；υ n 表示当地水平坐标系下的速度向量；δυ n 表示当地水平坐标系下的速度误差向量；δg p 的表示当地重力误差；进一步整理得：
式18即为惯导的速度误差微分方程。
惯导的位置误差方程建模，经纬高意义下的位置误差微分方程的形式为：
上式中，φ为载体所在的纬度；λ为载体所在的经度；h为载体所在的高程；等号左侧每个分量分别为各自的导数；其余各分量的含义前面已做解释，在此不在赘述；
对纬度的导数做扰动分析有：
对经度的导数做扰动分析有：
对高程的导数做扰动分析有：
把惯导的位置误差微分方程建模在n系下时，其形式为：
上式中，φ为载体所在的纬度；h为载体所在的高程；R M 表示子午圈曲率半径、R N 表示卯酉圈曲率半径、υ N 、υ E 和υ D 分别表示n系下的北向速度、东向速度和垂向速度；对纬度的导数做扰动分析有：δυ N 为北向速度误差，δυ E 为东向速度误差，δv D 表示垂向速度误差，等号左边分别表示对δr N 、δr E 和δr D 求导数。
作为优选地，所述传感器误差建模分为以下两个步骤：
第一步，加速度计误差模型：
上面20式中，等号左边表示加速度计实际的比力测量输出；表示加速度计的理论输出；I为单位向量；S a 为加速度计的比例因子误差，b a 为加速度计零偏；ωa 表示加速度计测量值的白噪声；21式中，表示加速度计测量误差；
第二步，陀螺仪误差模型
式23中，等号左边表示陀螺实际的角速度测量输出，ωib b为b系下的理论角速度；S g 为陀螺仪的比例因子误差，b g为陀螺零偏；w g表示陀螺仪测量值的白噪声；δωib b表示陀螺仪测量误差；其中，加速度计零偏、陀螺零偏、加速度计的比例因子误差和陀螺仪的比例因子误差建模为一阶高斯马尔科夫过程，有：
上式中，T gb、T ab、T gs和T as分别表示陀螺零偏、加速度计零偏、陀螺比例因子和加速度计比例因子的相关时间；ωgb、ωab、ωgs和ωas分别表示陀螺零偏驱动白噪声、加速度计零偏驱动白噪声、陀螺比例因子驱动白噪声和加速度计比例因子驱动白噪声。
惯导的系统状态误差微分方程建模方式为：结合确定状态向量、惯导的姿态误差微分方程建模、惯导的速度误差微分方程建模、惯导的位置误差微分方程建模和传感器误差建模的结构，进行惯导的系统状态误差微分方程建模，有：
上式中F(t)表示系统矩阵；G(t)表示输入方程；ω(t)表示连续时间的系统高斯白噪声；根据29式进一步推出误差状态外推方程，有：
其中：
上式中，Φ k,k-1 表示状态转移矩阵，ω k-1 为系统高斯白噪声ω(t)的线性变换，
记离散化时间间隔Δt=t k -t k-1 ，当Δt 比较小时，F(t)在时间间隔[t k-1 ，t k ]内变化不大，此时，有：
可以证明ω k 是零均值白噪声序列，满足：
上式中，Q k 为系统状态噪声协方差矩阵，计算方法为：
上式中q(t)为功率谱密度矩阵，为常值矩阵，与传感器误差模型参数有关；当G(t)在时间间隔[t k-1 ，t k ]内变化不大时，Q k 可以如下计算：
上式使用了梯形积分公式计算。
GPS位置误差观测方程建模：考虑到杆臂效应，有：
上式中，r GPS表示GPS天线测得的实际位置，r IMU表示惯导测得的实际位置，此处的位置用纬度、经度和高程表示；l b 为GPS相位中心的杆臂向量在b系下的投影；D R的逆的作用是将当地水平坐标转化为经纬高形式的坐标，其形式如下：
考虑到误差，有：
式中带上标ˆ的变量表示带有误差的计算值；等号左边表示惯导推算的GPS相位中心的位置向量在n系的投影；等号右边第一项表示惯导推算的IMU中心的位置向量在n系的投影；
将31式子带入到30式可得：
GPS定位解算得到的GPS天线相位中心的位置表示为：
上式中，等号左边表示GPS的位置观测值，等号右边第一项表示GPS的实际位置向量；δr G 表示GPS位置误差；对应的观测向量表示为惯导推算的位置与GPS位置观测之差，有：
因此，GPS的位置观测方程可以写成以下形式：
上式中，H r 表示GPS的位置观测矩阵，具体表达式为：
同理，GPS速度误差观测方程建模（推导过程省略）为：
上式中，δv G 表示GPS速度误差，H v 表示GPS的位置观测矩阵，具体表达式为：
上式中：
上式中，符号diag表示对角矩阵化操作。
UWB位置误差观测方程建模包含以下步骤：
S0、UWB标签固定在载体中心位置，UWB基站固定在位置已知的地面，所述基站数量设置为4个，记为第1号基站、第2号基站、第3号基站和第4号基站；
S1、利用双目测距原理确定人工设置的路标点的距离，从而确定载体的绝对位置，具体测距方式为：
上式中，T 14表示标签从发送请求到接收到反馈信号的时间间隔；T 23表示基站从接收请求到发送反馈信号的时间间隔；T表示飞行时间间隔；上标表示对应的基站编号；
S2、计算标签相对于每个基站的距离，计算公式为：
上式中，c为光速，D1、D2、D3和D4分别是载体上的标签到4个基站的距离；
S3、计算载体上的标签相对于基站的坐标，设第1号基站的坐标为(x1、y1、z1)，第2号基站的坐标为(x2、y2、z2)，第3号基站的坐标为(x3、y3、z3)，第4号基站的坐标为(x4、y4、z4)，标签坐标记为r UWB=（x、y、z），那么有如下关系式：
利用最小二乘法即可求出标签相对于基站的坐标；
S4、UWB误差观测方程建模
上式中，δr UWB为UWB位置观测误差，带上标“^”表示惯导解算的UWB标签的位置，带上标“˜”表示UWB标签自身解算的带误差的位置。
双分支卡尔曼循环定位：由误差状态外推方程、误差状态协方差外推方程、卡尔曼增益方程、GPS误差状态更新方程、UWB误差状态更新方程和误差状态协方差更新方程组成；以上方程均由误差状态外推方程、GPS观测方程和UWB位置误差观测方程推导得出；本发明为融合IMU、GPS和UWB多源数据，以IMU作为推导误差状态方程的主要传感器，将GPS的观测数据和UWB的观测数据作为推导观测方程的传感器；当GPS观测数据超过给定误差阈值或者GPS观测数据落在UWB信号覆盖范围时，使用UWB数据作为观测数据。
实施例一：
这是一个基于IMU、GPS和UWB的高精度定位算法的描述。该算法包括多个步骤和方程，用于确定系统的状态向量、建立惯性导航中姿态误差、速度误差和位置误差的微分方程模型，以及建模传感器误差、系统状态误差、GPS速度误差观测和GPS位置误差观测、UWB位置误差观测等。最后，采用双分支卡尔曼循环定位方法进行位置估计，以下是该算法的实施步骤：
1、确定状态向量：确定卡尔曼滤波方程中的状态向量，包括导航状态误差向量和传感器状态。
2、惯导的速度误差微分方程建模：建立在当地水平坐标系下速度向量的一阶导数与加速度计测得的比力误差之间的关系。该方程描述了速度误差的变化。
3、惯导的位置误差微分方程建模：建立在n系下位置误差的微分方程，其中考虑了载体所在纬度、高程等因素。
4、传感器误差建模：对加速度计和陀螺仪的误差进行建模，包括比例因子误差、零偏和测量噪声等。
5、惯导的系统状态误差微分方程建模：结合前面步骤的结果，建立惯导系统状态误差的微分方程，描述状态误差的演化。
6、GPS速度误差观测方程建模：建立GPS速度观测误差与状态误差之间的关系。
7、GPS位置误差观测方程建模：建立GPS位置观测误差与状态误差之间的关系，考虑了GPS天线位置误差等因素。
8、UWB位置误差观测方程建模：基于UWB标签与基站之间的飞行时间原理，建立UWB位置观测误差与状态误差之间的关系。
9、双分支卡尔曼循环定位：采用卡尔曼滤波方法，包括状态外推、协方差外推、卡尔曼增益计算、状态更新和协方差更新等步骤，将IMU、GPS和UWB的数据融合进行位置估计。
该算法的关键思想是利用多源数据（IMU、GPS和UWB）进行定位，通过卡尔曼滤波进行状态估计和误差校正，以提高定位的精度和鲁棒性，在GPS观测数据超过误差阈值或不可用时，利用UWB数据进行位置估计。
以上便是本发明具体的工作流程，下次使用时重复此步骤即可。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。
以上对本发明及其实施方式进行了描述，这种描述没有限制性，附图中所示的也只是本发明的实施方式之一，实际的结构并不局限于此。总而言之如果本领域的普通技术人员受其启示，在不脱离本发明创造宗旨的情况下，不经创造性的设计出与该技术方案相似的结构方式及实施例，均应属于本发明的保护范围。

Claims (1)

1.一种基于IMU、GPS与UWB的高精度定位算法，包括确定状态向量、惯导的姿态误差微分方程建模、惯导的速度误差微分方程建模、惯导的位置误差微分方程建模、传感器误差建模、惯导的系统状态误差微分方程建模、GPS速度误差观测方程建模、GPS位置误差观测方程建模、UWB位置误差观测方程建模和双分支卡尔曼循环定位；所述确定状态向量，就是确定卡尔曼滤波方程中的状态向量，包括导航状态误差向量和传感器：
，
上式中，δx表示状态误差，δr n 表示惯导位置误差向量在n系下的投影，n系为当地水平坐标系，δv n 表示惯导速度误差向量在n系下的投影，等号右边第三项表示惯导姿态误差向量，b g 表示三轴陀螺仪零偏向量，b a 表示三轴加速度计零偏向量，s g 表示三轴陀螺仪比例因子误差向量，s a 表示三轴加速度计比例因子误差向量，等号右边每一项都是三维的列向量，因此δx共有21维；
所述惯导的姿态误差微分方程建模为：
，
上式中，表示惯导的姿态误差的导数，表示惯导的姿态误差，上标n表示各物理量在n系下的投影；上标b表示各物理量在b系下的投影；ω in 表示n系相对于i系的角速度；δω in 表示n系相对于i系的角速度误差；δω ib 表示b系相对于i系的角速度误差；表示惯导所在的b系相对于n系的方向余弦矩阵；
所述惯导的速度误差微分方程建模为：
，
上式中，等号左边表示当地水平坐标系下的速度向量的一阶导数，表示加速度计测得的比力误差在b系的投影；f n 表示加速度计测得的比力在n系的投影；ω ie 表示e系相对于i系的角速度；δω ie 表示e系相对于i系的角速度误差；ω en 表示n系相对于e系的角速度；δω en 表示n系相对于e系的角速度误差；υ n 表示当地水平坐标系下的速度向量；δυ n 表示当地水平坐标系下的速度误差向量；δg p 的表示当地重力误差；当地重力的展开式为：
，
上式中，g表示重力矢量；ω ie 表示e系相对于i系的角速度向量；r表示载体的位置矢量；从而有：
，
上式中，上标n表示在n系的投影；
所述双分支卡尔曼循环定位由误差状态外推方程、误差状态协方差外推方程、卡尔曼增益方程、GPS误差状态更新方程、UWB误差状态更新方程和误差状态协方差更新方程组成；以上方程均由误差状态外推方程、GPS观测方程和UWB位置误差观测方程推导得出，融合IMU、GPS和UWB多源数据，以IMU作为推导误差状态方程的主要传感器，将GPS的观测数据和UWB的观测数据作为推导观测方程的传感器；当GPS观测数据超过给定误差阈值或者GPS观测数据落在UWB信号覆盖范围时，使用UWB数据作为观测数据；
所述传感器误差建模如下：
第一步，加速度计误差建模：
，
上面1式中，等号左边表示加速度计实际的比力测量输出；表示加速度计的理论输出； I为单位向量；S a 为加速度计的比例因子误差，b a 为加速度计零偏；ω a 表示加速度计测量值的白噪声；在2式中，表示加速度计测量误差；
第二步，陀螺仪误差建模：
，
式3中，等号左边表示陀螺实际的角速度测量输出，ωib b为b系下的理论角速度；S g 为陀螺仪的比例因子误差，b g为陀螺零偏；w g 表示陀螺仪测量值的白噪声；δωib b表示陀螺仪测量误差；加速度计零偏、陀螺零偏、加速度计的比例因子误差和陀螺仪的比例因子误差建模为一阶高斯马尔科夫过程，有：
，
上式中，T gb、T ab、T gs和T as分别表示陀螺零偏、加速度计零偏、陀螺比例因子和加速度计比例因子的相关时间；ωgb、ωab、ωgs和ωas分别表示陀螺零偏驱动白噪声、加速度计零偏驱动白噪声、陀螺比例因子驱动白噪声和加速度计比例因子驱动白噪声；
所述惯导的系统状态误差微分方程建模，具体步骤为：
首先，结合确定状态向量、惯导的姿态误差微分方程建模、惯导的速度误差微分方程建模、惯导的位置误差微分方程建模和传感器误差建模的结果，构建出惯导的系统状态误差微分方程，有：
，
上式中F(t)表示系统矩阵；G(t)表示输入方程；ω(t)表示连续时间的系统高斯白噪声；δx(t)表示t时刻的状态误差；
其次，根据上述微分方程进一步推出误差状态外推方程，有：
，
其中：
，
上式中，Φ k,k-1 表示状态转移矩阵，ω k-1 为系统高斯白噪声ω(t)的线性变换，δx k 表示k时刻的状态误差；δx k-1 表示k-1时刻的状态误差；记离散化时间间隔Δt = t k - t k-1 ，当Δt比较小时，F(t)在时间间隔[t k-1 ，t k ]内变化不大，此时有：
，
能够证明ω k 是零均值白噪声序列，满足：
，
上式中，符号“N”表示高斯分布，符号“~”表示服从，也就是ω k 服从高斯分布，Q k 为系统状态噪声协方差矩阵，计算方法为：
，
上式中，Q k 的计算使用梯形积分公式；
所述GPS位置误差观测方程建模，GPS的位置误差观测方程写成以下形式：
，
上式中，δz r 表示GPS位置观测误差；H r 表示GPS的位置观测矩阵，δr G表示GPS天线的位置误差，所述GPS速度误差观测方程建模为：
，
上式中，δv G 表示GPS速度误差，H v 表示GPS的速度观测矩阵，具体表达式为：
，
上式中， l b 为GPS相位中心的杆臂向量在b系下的投影；
，
上式中，符号diag表示对角矩阵化操作，GPS速度误差观测方程建模和GPS位置误差观测方程建模共同构成了GPS的观测方程建模。
CN202311575444.3A 2023-11-24 2023-11-24 一种基于imu、gps与uwb的高精度定位算法 Active CN117289322B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311575444.3A CN117289322B (zh) 2023-11-24 2023-11-24 一种基于imu、gps与uwb的高精度定位算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311575444.3A CN117289322B (zh) 2023-11-24 2023-11-24 一种基于imu、gps与uwb的高精度定位算法

Publications (2)

Publication Number Publication Date
CN117289322A CN117289322A (zh) 2023-12-26
CN117289322B true CN117289322B (zh) 2024-07-02

Family

ID=89257447

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311575444.3A Active CN117289322B (zh) 2023-11-24 2023-11-24 一种基于imu、gps与uwb的高精度定位算法

Country Status (1)

Country Link
CN (1) CN117289322B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117433517A (zh) * 2023-10-27 2024-01-23 青岛威弗德自动化系统有限公司 一种基于多传感器融合的四向穿梭车高精度定位算法
CN117516549B (zh) * 2024-01-03 2024-03-29 江苏领创星通卫星通信科技有限公司 一种基于惯导与卫星的路径规划算法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108507561A (zh) * 2018-03-05 2018-09-07 华南理工大学 一种基于移动终端的vlc与imu融合定位方法
CN110645979B (zh) * 2019-09-27 2021-09-21 北京交通大学 基于gnss/ins/uwb组合的室内外无缝定位方法
CN113406683A (zh) * 2021-06-15 2021-09-17 北京理工大学 基于多模式立体联合定位的全场景定位系统及方法
CN115096303B (zh) * 2022-08-25 2022-11-22 中南大学 一种gnss多天线与ins紧组合定位定姿方法和设备
CN116817896B (zh) * 2023-04-03 2024-04-16 盐城数智科技有限公司 一种基于扩展卡尔曼滤波的姿态解算方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
基于 GNSS/INS/UWB 组合的室内外无缝定位方法研究;曹琢健;《中国优秀硕士学位论文全文数据库信息科技辑》;摘要、正文第11、14-29、44-45、48-50页 *
多源信息融合无缝定位理论和方法研究;王长强;《中国博士学位论文全文数据库信息科技辑》;摘要、正文第32-40、46-47、101页 *
超宽带/GNSS/SINS 融合定位模型与方法研究;余航;《中国博士学位论文全文数据库信息科技辑》;摘要、正文第15-24、27-30页 *

Also Published As

Publication number Publication date
CN117289322A (zh) 2023-12-26

Similar Documents

Publication Publication Date Title
CN117289322B (zh) 一种基于imu、gps与uwb的高精度定位算法
CN109556632B (zh) 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法
CN107734457B (zh) 智慧停车场导航系统及方法
CN106443746B (zh) 一种低成本双天线gnss/ahrs组合测姿方法
Georgy et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
US20070282565A1 (en) Object locating in restricted environments using personal navigation
CN110686671B (zh) 基于多传感器信息融合的室内3d实时定位方法及装置
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
WO2014060429A1 (en) Surveying system and method
CN111399023B (zh) 基于李群非线性状态误差的惯性基组合导航滤波方法
CN111025366B (zh) 基于ins及gnss的网格slam的导航系统及方法
CN105698822A (zh) 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法
CN116086448B (zh) 基于uwb、imu、gnss融合的无人设备多场景无缝定位方法
CN107289932A (zh) 基于mems传感器和vlc定位融合的单卡尔曼滤波导航装置和方法
CN112797985A (zh) 基于加权扩展卡尔曼滤波的室内定位方法及室内定位系统
Bevermeier et al. Barometric height estimation combined with map-matching in a loosely-coupled Kalman-filter
Klein et al. LiDAR and INS fusion in periods of GPS outages for mobile laser scanning mapping systems
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN116482735A (zh) 一种受限空间内外高精度定位方法
CN115685287A (zh) 基于深度学习的变电站区域弹性融合定位方法及装置
Kim et al. Enhanced outdoor localization of multi-GPS/INS fusion system using Mahalanobis Distance
Vigrahala et al. Attitude, Position and Velocity determination using Low-cost Inertial Measurement Unit for Global Navigation Satellite System Outages
CN117433517A (zh) 一种基于多传感器融合的四向穿梭车高精度定位算法
Fernández et al. Development of a simulation tool for collaborative navigation systems

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