利索能及
我要发布
收藏
专利号: 2022103857425
申请人: 燕山大学
专利类型:发明专利
专利状态:已下证
更新日期:2026-06-16
缴费截止日期: 暂无
联系人

摘要:

权利要求书:

1.基于RTK/UWB/IMU多单元协同室内外定位方法,其特征在于:定位系统包括基站和多个定位单元,其中基站包括运行定位监控处理器、通信基站和RTK基站,每个定位单元包括RTK、UWB、IMU模块和通讯电台,根据待测单元在室外或室内的各种不同情况,通过多个定位单元相互协同的方式,不间断的给出合适的定位方法;

开始搭载定位单元的定位载体和协同定位载体至少有4个在RTK信号良好的地方,基站和各定位单元上电启动后,根据RTK和UWB信号强弱判断各个定位单元的定位环境,为定位变量赋初值;RTK提供卫星定位信号,UWB运行在测距模型下,提供两两之间的距离信息;

通过RTK的观测卫星数判断单元在室内还是在室外,在室外情况下,通过RTK和IMU松组合定位;在室内情况下,通过UWB之间的精度因子判断UWB之间是否能够通讯;若UWB之间能通讯,先结合RTK给同一单元UWB坐标,然后再通过IMU和UWB进行紧组合定位;若UWB之间不能通讯,则通过IMU导航算法刷新坐标。

2.根据权利要求1所述的基于RTK/UWB/IMU多单元协同室内外定位方法,其特征在于:所述定位方法具体包括以下步骤:

步骤1,通过IMU对待测点进行姿态更新,速度更新,位置更新;

步骤2,判断待测单元RTK的观测卫星数,在待测单元RTK的观测卫星数大于4时,即代表待测单元在室外运动的情况,通过待测单元携带的RTK和IMU进行松组合定位,从而获得待测单元的位置信息;当待测单元RTK的观测卫星数小于4时,如待测单元进入室内,即待测单元RTK信号微弱,进入步骤3;

步骤3,判断UWB设备精度因子是否大于预设的精度阈值,若UWB的精度因子大于预设精度阀值,即进入室内的待测单元与任何其他单元的UWB都不能进行通信,则进入步骤4;若UWB的精度因子小于预设精度阀值,即存在以下两种情况:

3.1当室内待测单元的UWB模块与室外单元UWB模块至少存在一个可通讯时,通过室外RTK给绑在一个模块上的UWB位置信息,再通过该室外UWB与室内待测单元的UWB通讯经TWR算法测得距离信息,然后通过IMU与上述同一室外UWB模块测得距离信息,把两者的测得的距离信息做差后作为卡尔曼滤波器的量测输入,最后用卡尔曼滤波器的输出差值对IMU的位置进行修正从而得到待测单元的位置信息;

3.2当室内待测单元的UWB模块与室外单元UWB模块没有一个可通讯时,若室内待测单元不能和任意室外单元的UWB进行通信,则考虑让多个单元进入室内协同测距,保证该进入室内的协助单元的UWB能够与至少四个室外UWB通信从而得到自身的位置信息,通过该得到位置信息的室内单元的UWB与待测单元的UWB通讯后得到距离信息,再算出IMU与该已知位置的室内单元之间的距离,将两者距离的差值作为卡尔曼滤波的输入,最后把卡尔曼滤波器的输出差值对IMU的位置进行修正从而得到待测单元的位置信息;

步骤4,通过丢失信号前的位置和IMU导航算法刷新坐标,从而在短时间内得到较为精准的定位信息。

3.根据权利要求2所述的基于RTK/UWB/IMU多单元协同室内外定位方法,其特征在于:步骤1中,IMU基于待测点的初始位置、速度和姿态信息,结合加速度计与陀螺仪输出的测量数据,计算每一更新时刻的待测点的位置、速度和姿态信息,计算公式分别如下:设四元数为:

Q=q0+q1i+q2j+q3k

其中,i、j、k为虚数,几何意义理解为一种旋转,q0、q1、q2、q3均为实数;

更新微分方程为:

其中, 为姿态角速率和输出角速率, 为以四元数表示的姿态矩阵,分别为导航系下地球自转角速率的投影和导航系相对于地球的角速率,M′w(t)是姿态角速率 组成的反对称矩阵;

速度更新采用比力方程:

其中, 为加速度计的输出, 为地球自转角速度分量, 为运载体相对于地球的运n动速度,g为重力加速度矢量;

通过对速度积分得到待测单元位置的更新:

其中,λ、L、h分别为待测单元的经度、维度和高度信息,RN、RM分别为当地子午面曲线半n n径和当地卯酉曲面曲率半径,简记 为V, 为V分量。

4.根据权利要求2所述的基于RTK/UWB/IMU多单元协同室内外定位方法,其特征在于:步骤2中,所述待测单元运动到室外情况下,通过IMU和RTK对待测单元进行松组合定位,从而获得待测单元的位置信息;RTK/IMU松组合定位算法包括:卡尔曼滤波的系统状态向量由18个状态组成:

其中,有位置误差δP=[δL,δλ,δh],速度误差δV=[VE,VN,VU],姿态角误差Φ=[ΦE,ΦN,ΦU],陀螺仪偏差 陀螺仪一阶马尔科误差 加速度计偏差卡尔曼滤波的系统观测向量为:

其中, 为IMU测得的位置, 为RTK测得的位置, 为IMU测得的速度, 为RTK测得的速度, 为高斯白噪声;

H(k)是量测矩阵,公式如下:

5.根据权利要求2所述的基于RTK/UWB/IMU多单元协同室内外定位方法,其特征在于:步骤3中,所述待测单元运动到室内情况下,且室内待测单元UWB与室外UWB至少存在一个无遮挡时,通过室外RTK给同一单元的UWB位置信息,从而实现UWB和IMU对待测单元进行组合定位,UWB/IMU紧组合定位算法包括:通过室外RTK给绑在一个模块上的UWB位置信息,再通过该室外UWB与室内待测单元的UWB通讯经TWR算法测得距离信息,然后通过IMU与上述同一室外UWB模块测得距离信息,把两者的测得的距离信息做差后作为卡尔曼滤波器的量测输入,最后用卡尔曼滤波器的输出差值对IMU的位置进行修正从而得到待测单元的位置信息;

卡尔曼滤波的系统状态向量由15个状态组成:

其中,姿态角误差Φ=[ΦE,ΦN,ΦU],位置误差δP=[δL,δλ,δh],速度误差δV=[VE,VN,VU],陀螺仪零偏误差 加速度计零偏误差所述卡尔曼滤波的系统观测向量为:

T

Z=[Z1 Z2 Z3…Zn]

其中, 表示室外RTK测得的坐标即同一单元UWB的坐标, 表示当前的IMU位置估计,代表到第i个UWB测量基站的距离,n代表室内UWB与室外UWB能通讯的个数;

系统量测方程为:

Zk+1=Hk+1Xk+1+Vr,k+1

Hk+1=[0n×3 JF 0n×3 0n×3 0n×3]

其中,Vr代表系统量测噪声,也就是UWB距离测量噪声,(xAi yAi zAi)是RTK测得的同一模块UWB的坐标,(xIMU yIMU zIMU)由IMU解算的标签坐标。

6.根据权利要求2所述的基于RTK/UWB/IMU多单元协同室内外定位方法,其特征在于:步骤4中,所述待测单元运动到室内情况下,且待测单元UWB模块与其他UWB模块完全失去通讯功能时,则通过丢失信号前的位置和IMU导航算法刷新坐标,从而在短时间内得到较为精准的定位信息。