一种基于超宽带制造技术

本发明专利技术提供一种基于超宽带

【技术实现步骤摘要】
一种基于超宽带、惯性测量和轮速计的组合导航定位方法


[0001]本专利技术涉及导航
，尤其涉及一种基于超宽带
、
惯性测量和轮速计的组合导航定位方法
。

技术介绍

[0002]无人移动机器人在智能仓储
、
智慧工厂
、
煤矿等领域中的应用越来越广泛
。
为了实现更好的无人化和智能化的管理，实时获得机器人的位置，速度等状态是必不可少的，且对机器人的定位精度有较高的要求
。
[0003]目前的定位方法主要有两种：一种是基于外部信息实现定位；另一种是基于自身状态信息实现定位
。
其中，基于外部信息定位使用的传感器有很多，如超宽带
(UWB)、WIFI
和卫星定位等；使用这些传感器的定位都需要在移动机器人的外部部署一些设备，对应分别问
UWB
锚点
、
路由器和卫星等；基于自身状态信息定位的方法是利用移动机器人自身的一些状态来实现定位；捷联惯性导航使用惯性测量单元
(IMU)
测量在地心坐标系下的角速度和加速度，然后通过惯导算法实现定位；航迹推算是求得移动机器人的速度和航向角，通过实时解算实现机器人的定位
。
[0004]基于
UWB
的定位方法原理是通过高频脉冲来测量多个锚点到机器人上标签的距离，再列出相应的三边测距方程使用最小二乘法来进行定位，但是当移动机器人和锚点之间存在遮挡时，即处在非视距的环境中，
UWB
的测距精度会受到影响，从而导致整体的定位精度有所降低；惯性导航仅依靠自身的状态信息实现导航定位，且可以获得较多的状态信息，但通常高精度的
IMU
价格比较昂贵，低成本的
IMU
精度较差，在进行长时间的导航定位时会出现漂移现象，导致误差积累越来越大；航迹推算的方法最为简单，但通常应用在2维定位中，且由于速度更新较慢，因此误差较大，且同样也存在误差积累的现象
。

技术实现思路

[0005]为了解决以上技术问题，本专利技术提供了一种基于超宽带
、
惯性测量和轮速计的组合导航定位方法
。
[0006]本专利技术所解决的技术问题可以采用以下技术方案实现：
[0007]一种基于超宽带
、
惯性测量和轮速计的组合导航定位方法，包括：
[0008]步骤
S1
，获取目标对象的惯性测量数据，对所述惯性测量数据进行惯性导航解算，得到解算结果；
[0009]步骤
S2
，获取超宽带基站对所述目标对象进行测距的测距信息；
[0010]步骤
S3
，使用扩展卡尔曼滤波模型对所述解算结果和所述测距信息进行数据融合，得到所述解算结果的误差量，根据所述误差量对所述目标对象的速度和位置进行矫正；
[0011]步骤
S4
，获取轮速计采集得到的轮速计信息，将所述轮速计信息
、
所述解算结果与所述扩展卡尔曼滤波模型输出的误差量进行互补滤波，到优化后的状态估计结果
。
[0012]优选地，所述步骤
S1
中，所述惯性导航采用以下公式进行更新：
[0013][0014]其中，分别表示分别为导航坐标系下
t
m
时刻的四元数
、
速度和位置；分别表示分别为导航坐标系下
t
m
‑1时刻的四元数
、
速度和位置；表示时间段
[t
m
‑1，
t
m
]内的姿态四元数的变化量；
°
表示四元数的乘法；表示时间段
[t
m
‑1，
t
m
]内的等效旋转矢量的变化量；表示时间段
[t
m
‑1，
t
m
]内的陀螺仪输出的角增量；
φ
m
‑1表示
t
m
‑1时刻的等效旋转矢量
。
[0015]优选地，所述步骤
S1
之前包括：进行系统初始化，所述系统初始化的步骤包括：
[0016]将所述目标对象静止放置，根据超宽带基站采集得到的所述目标对象的初始位置信息对位置进行初始化；
[0017]根据惯性测量单元中加速度计采集的数据对姿态进行初始化；
[0018]在所述目标对象开始运动时，通过所述超宽带基站和所述加速度计采集的数据对航向角进行初始化
。
[0019]优选地，所述步骤
S2
还包括：对所述超宽带基站获取得到的所述测距信息进行异常值检测
。
[0020]优选地，所述步骤
S3
包括：
[0021]步骤
S31
，建立基于超宽带
、
惯性测量和轮速计组合导航的扩展卡尔曼滤波模型：
[0022]状态方程为：
[0023][0024]其中，
F
表示系统矩阵；
x
表示误差状态变量；
G
表示噪声分配矩阵；
w
表示系统噪声向量；表示误差状态变量
x
的一阶导数；
[0025]步骤
S32
，将所述解算结果和所述测距信息的距离差值作为所述扩展卡尔曼滤波器的观测值：
[0026]观测方程为：
[0027]z
＝
Hx+n
[0028]其中，
[0029]z
＝
[(
δ
d1，
k
)
T
(
δ
d2，
k
)
T
(
δ
d3，
k
)
T
(
δ
d4，
k
)
T
]T
[0030][0031]其中，
H
表示量测矩阵；
n
表示量测噪声向量；
h(x)
表示量测值和状态变量的函数关系
。
[0032]优选地，所述步骤
S31
中，
[0033]所述系统矩阵
F
为：
[0034][0035]其中，表示在导航坐标系下加速度计的比力输出；表示姿态转换矩阵；
[0036]噪声分配矩阵
G
为：
[0037][0038]系统噪声向量
w
为：
[0039][0040]其中，表示陀螺仪的角速率白噪声；表示加速度计的比力白噪声；
[0041]方程状态变量为：
[0042][0043]其中，
φ
表示
n
本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.
一种基于超宽带
、
惯性测量和轮速计的组合导航定位方法，其特征在于，包括：步骤
S1
，获取目标对象的惯性测量数据，对所述惯性测量数据进行惯性导航解算，得到解算结果；步骤
S2
，获取超宽带基站对所述目标对象进行测距的测距信息；步骤
S3
，使用扩展卡尔曼滤波模型对所述解算结果和所述测距信息进行数据融合，得到所述解算结果的误差量，根据所述误差量对所述目标对象的速度和位置进行矫正；步骤
S4
，获取轮速计采集得到的轮速计信息，将所述轮速计信息
、
所述解算结果与所述扩展卡尔曼滤波模型输出的误差量进行互补滤波，到优化后的状态估计结果
。2.
根据权利要求1所述的基于超宽带
、
惯性测量和轮速计的组合导航定位方法，其特征在于，所述步骤
S1
中，所述惯性导航采用以下公式进行更新：其中，分别表示分别为导航坐标系下
t
m
时刻的四元数
、
速度和位置；分别表示分别为导航坐标系下
t
m
‑1时刻的四元数
、
速度和位置；表示时间段
[t
m
‑1，
t
m
]
内的姿态四元数的变化量；表示四元数的乘法；表示时间段
[t
m
‑1，
t
m
]
内的等效旋转矢量的变化量；表示时间段
[t
m
‑1，
t
m
]
内的陀螺仪输出的角增量；
φ
m
‑1表示
t
m
‑1时刻的等效旋转矢量
。3.
根据权利要求1所述的基于超宽带
、
惯性测量和轮速计的组合导航定位方法，其特征在于，所述步骤
S1
之前包括：进行系统初始化，所述系统初始化的步骤包括：将所述目标对象静止放置，根据超宽带基站采集得到的所述目标对象的初始位置信息对位置进行初始化；根据惯性测量单元中加速度计采集的数据对姿态进行初始化；在所述目标对象开始运动时，通过所述超宽带基站和所述加速度计采集的数据对航向角进行初始化
。4.
根据权利要求1所述的基于超宽带
、
惯性测量和轮速计的组合导航定位方法，其特征在于，所述步骤
S2
还包括：对所述超宽带基站获取得到的所述测距信息进行异常值检测
。
5.
根据权利要求1所述的基于超宽带
、
惯性测量和轮速计的组合导航定位方法，其特征在于，所述步骤
S3
包括：步骤
S31
，建立基于超宽带
、
惯性测量和轮速计组合导航的扩展卡尔曼滤波模型：状态方程为：其中，
F
表示系统矩阵；
x
表示误差状态变量；
G
表示噪声分配矩阵；
w
表示系统噪声向量；表示误差状态变量
x
的一阶导数；步骤
S32
，将所述解算结果和所述测距信息的距离差值作为所述扩展卡尔曼滤波器的观测值：观测方程为：
z
＝
Hx+n
其中，
z
＝
[(
δ
d1，
k

【专利技术属性】
技术研发人员：高同跃，
申请(专利权)人：苏州华米导航科技有限公司，
类型：发明
国别省市：