一种基于制造技术

本发明专利技术公开了一种基于

【技术实现步骤摘要】
一种基于IMU、GPS与UWB的高精度定位算法


[0001]本专利技术属于勘测
、
导航
、
陀螺仪
，具体为一种基于
IMU、GPS
与
UWB
的高精度定位算法
。

技术介绍

[0002]GPS/
惯性导航系统（
GPS/INS
）是最常见的组合导航方案之一，全球定位系统（
GPS
）用于提供位置和速度信息，而惯性导航系统（
INS
）利用加速度计和陀螺仪等传感器测量加速度角速度，从而估计运动姿态
、
速度和位置，通过将两者的数据进行融合，可以实现高精度的位置和姿态估计
。
[0003]视觉惯性导航系统（
Visual
‑
Inertial Navigation System
，简称
VINS
）是一种基于视觉和惯性传感器的组合导航方案，通过结合视觉传感器（如摄像头）和惯性传感器（如加速度计和陀螺仪），实现对位置和姿态的估计，
VINS
的工作原理是将视觉和惯性传感器的数据进行融合，以获取更准确和稳定的导航信息；具体而言，
VINS
利用摄像头获取环境中的视觉特征，并利用计算机视觉技术进行特征提取
、
匹配和跟踪，从而得到相机在三维空间中的运动轨迹，同时，惯性传感器测量加速度和角速度，通过积分得到相机的姿态变化，最后，将视觉和惯性传感器的数据进行优化和融合，通过滤波或优化算法，得到更准确的位置和姿态估计结果
。
[0004]GPS/INS
在
GPS
信号较强
、
没有遮挡的开阔领域，如农田
、
开阔的路面等能够提供较高的精度，通常在几米到十几米的范围内；当
GPS
信号受到遮挡
、
多径效应和信号干扰等因素的影响，导致导航精度下降，尤其在城市峡谷
、
森林覆盖等复杂环境中
。VINS
的导航精度通常取决于视觉传感器的分辨率
、
环境特征的质量和数量，以及传感器标定的准确性；在良好的视觉条件下，
VINS
能够达到较高的导航精度，通常在几厘米到几十厘米的范围内，但
VINS
对于光照变化
、
纹理缺失等环境下可能导致导航精度下降
。
[0005]UWB
定位技术具有低功耗
、
对信道衰落不敏感
、
抗干扰能力强
、
不会对同一环境下的其他设备产生干扰
、
穿透性较强等优点，具有很高的定位精度
。

技术实现思路

[0006]（一）技术方案
[0007]GPS/INS
在开阔空间中具有较高的精度，但在密集城市或复杂环境中可能受到限制；
VINS
在良好的视觉条件下能够提供较高的精度，但在光照变化
、
纹理缺失等情况下可能受到挑战，为了解决上述问题，本专利技术采用的技术方案为：一种基于
IMU、GPS
与
UWB
的高精度定位算法，包括确定状态向量
、
惯导的姿态误差微分方程建模
、
惯导的速度误差微分方程建模
、
惯导的位置误差微分方程建模
、
传感器误差建模
、
惯导的系统状态误差微分方程建模
、GPS
速度误差观测方程建模
、GPS
位置误差观测方程建模
、UWB
位置误差观测方程建模和双分支卡尔曼循环定位；所述确定状态向量，就是确定卡尔曼滤波方程中的状态向量，包括导航状态误差向量和传感器：
[0008][0009]上式中，
δ
x
表示状态误差，
δ
r
n
表示惯导位置误差向量在
n
系（当地水平坐标系）下的投影，
δ
v
n
表示惯导速度误差向量在
n
系下的投影，等号右边第三项表示惯导姿态误差向量，
b
g
表示三轴陀螺仪零偏向量，
b
a
表示三轴加速度计零偏向量，
s
g
表示三轴陀螺仪比例因子误差向量，
s
a
表示三轴加速度计比例因子误差向量，等号右边每一项都是三维的列向量，因此
δ
x
共有
21
维
。
[0010]进一步地，所述惯导的姿态误差微分方程建模为：
[0011][0012]上式中，表示惯导的姿态误差的导数，表示惯导的姿态误差，上标
n
表示各物理量在
n
系（当地水平坐标系）下的投影；上标
b
表示各物理量在
b
系（载体坐标系）下的投影；
ω
in
表示
n
系相对于
i
系（实用惯性系）的角速度；
δω
in
表示
n
系相对于
i
系的角速度误差；
δω
ib
表示
b
系相对于
i
系的角速度误差；表示惯导所在的
b
系相对于
n
系的方向余弦矩阵
。
[0013]作为优选地，所述惯导的速度误差微分方程建模为：
[0014][0015]上式中，等号左边表示当地水平坐标系下的速度向量的一阶导数，
δ
f b
表示加速度计测得的比力误差在
b
系的投影；
f n
表示加速度计测得的比力在
n
系的投影；
ω
ie
表示
e
系（地心地固坐标系）相对于
i
系的角速度；
δω
ie
表示
e
系相对于
i
系的角速度误差；
ω
en
表示
n
系相对于
e
系的角速度；
δω
en
表示
n
系相对于
e
系的角速度误差；
υ
n
表示当地水平坐标系下的速度向量；
δυ
n
表示当地水平坐标系下的速度误差向量；
δ
g
p
的表示当地重力误差；当地重力的展开式为：
[0016][0017]上式中，
g
表示重力矢量；
ω
ie
表示
e
系相对于
本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.
一种基于
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
系的方向余弦矩阵；所述惯导的速度误差微分方程建模为：，上式中，等号左边表示当地水平坐标系下的速度向量的一阶导数，
δ
f b
表示加速度计测得的比力误差在
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
系的投影
。2.
根据权利要求1所述的一种基于
IMU、GPS
与
UWB
的高精度定位算法，其特征在于：所述双分支卡尔曼循环定位：由误差状态外推方程
、
误差状态协方差外推方程
、
卡尔曼增益方程
、GPS
误差状态更新方程
、UWB
误差状态更新方程和误差状态协方差更新方程组成；以上方程均由误差状态外推方程
、GPS
观测方程和
UWB
位置误差观测方程推导得出；本发明为融合
IMU、GPS
和
UWB
多源数据，以
IMU
作为推导误差状态方程的主要传感器，将
GPS
的观测数据和
UWB
的观测数据作为推导观测方程的传感器；当
GPS
观测数据超过给定误差阈值或者
GPS
观
测数据落在
UWB
信号覆盖范围时，使用
UWB
数据作为观测数据
。3.
根据权利要求1所述的一种基于
IMU、GPS
与
UWB
的高精度定位算法，其特征在于：所述
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
标签的位置，带上标“˜”表示
...

【专利技术属性】
技术研发人员：李波，李野，
申请(专利权)人：江苏领创星通卫星通信科技有限公司，
类型：发明
国别省市：