[發明專利]基于UWB和激光測距的室內航位推算定位方法及系統

有效

• 申請號: 201810466502.1
• 申請日: 2018-05-16
• 公開（公告）號: CN108680167B
• 公開（公告）日: 2020-10-16
• 發明（設計）人: 鄒波;李志懷
• 申請（專利權）人: 深迪半導體（上海）有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01S5/02;G01S17/06;H04W4/024;H04W4/33
• 代理公司: 上海劍秋知識產權代理有限公司 31382
• 代理人: 楊飛

1.一種室內航位推算定位方法，其特征在于，輔以UWB定位信息和激光測距信息；當UWB定位信息有效時，采用UWB定位信息與航位推算信息進行數據融合；當UWB定位信息無效時，采用激光測距信息與航位推算信息進行數據融合；當需要對航位推算信息進行初始化時，采用UWB定位信息對航位推算的位置信息和航向角進行初始化；航位推算的位置和航向初始化包括：

在航位推算的航向角未被初始化前，不進行推算更新，直接使用UWB的位置信息來賦值航位推算的位置信息；

當定位載體開始運動時，使用UWB定位信息來計算定位載體的速度，計算公式如下所示；

Vue(Ku+1)＝(Pue(Ku+1)-Pue(Ku))/Tku (1)

Vun(Ku+1)＝(Pun(Ku+1)-Pun(Ku))/Tku (2)

其中Vue(Ku+1)和Vun(Ku+1)分別為UWB在東北坐標系中Ku+1時刻的東向速度和北向速度，Pue(Ku+1)和Pue(Ku)分別為UWB在東北坐標系中Ku+1和Ku時刻的東向位置，Pun(Ku+1)和Pun(Ku)分別為UWB在東北坐標系中Ku+1和Ku時刻的北向位置，Tku是UWB在Ku時刻到Ku+1時刻的時間間隔；

使用公式(1)和(2)獲得的速度信息來計算航向角ψ0，其公式如下所示：

ψ0＝(Vue/Vun)*180/PI (3)

其中Vue和Vun分別是UWB在東北坐標系中當前的東向速度和北向速度，PI是圓周率，ψ0的單位是度；

當檢測到定位載體在做直線運動后，將由公式(3)計算得到的ψ0作為航位推算的航向角，使用當前UWB的位置信息來賦值航位推算的位置信息，從而完成航位推算信息初始化。

2.如權利要求1所述的室內航位推算定位方法，其特征在于，通過陀螺儀檢測定位載體是否在做直線運動。

3.如權利要求1所述的室內航位推算定位方法，其特征在于，采用陀螺儀和速度傳感器的信息來對航位推算的位置信息和航向角進行遞推更新。

4.如權利要求3所述的室內航位推算定位方法，其特征在于，采用陀螺儀和速度傳感器的信息來對航位推算的位置信息和航向角進行遞推更新，包括：

Kg+1時刻航位推算的航向角更新，更新公式為：

ψ(Kg+1)＝ψ(Kg)+St*(Wt-Wt0)*Tkg (4)

其中ψ(Kg+1)和ψ(Kg)分別為航位推算在Kg+1和Kg時刻的航向角，St為陀螺儀的刻度系數，Wt為陀螺儀的角速率輸出，Wt0為陀螺儀的角速率零偏，Tkg為陀螺儀數據在Kg時刻到Kg+1時刻的時間間隔；

Kv+1時刻航位推算的位置信息更新，更新公式為：

Pe(Kv+1)＝Pe(Kv)+sin(ψ)*Sv*Vp*Tkv (5)

Pn(Kv+1)＝Pn(Kv)+cos(ψ)*Sv*Vp*Tkv (6)

Lat(Kv+1)＝Lat(Kv)+Pn(Kv+1)/Er (7)

Lon(Kv+1)＝Lon(Kv)+Pe(Kv+1)/Er/cos(Lat) (8)

其中，Pe(Kv+1)和Pn(Kv+1)分別是Kv+1時刻航位推算在東北坐標系的東向和北向的位置，Pe(Kv)和Pn(Kv)分別是Kv時刻航位推算在東北坐標系的東向和北向的位置，ψ為航位推算的航向角，ψ＝(ψ(Kv+1)+ψ(Kv))/2，Sv是速度傳感器的速度刻度系數，Vp是速度傳感器的速度輸出，Tkv為速度傳感器數據在Kv時刻到Kv+1時刻的時間間隔，Er為地球半徑，Lat(Kv+1)和Lon(Kv+1)分別為Kv+1時刻航位推算的經度和緯度，Lat(Kv)和Lon(Kv)分別為Kv時刻航位推算的經度和緯度，Lat＝(Lat(Kv+1)+Lat(Kv))/2。

5.如權利要求1所述的室內航位推算定位方法，其特征在于，UWB定位信息與航位推算信息的數據融合采用擴展Kalman濾波算法。

6.如權利要求1所述的室內航位推算定位方法，其特征在于，激光測距信息與航位推算信息的數據融合采用擴展Kalman濾波算法。

7.一種室內航位推算定位系統，其特征在于，定位載體設置有UWB裝置、陀螺儀、速度傳感器和激光測距裝置，并且采用如權利要求1-6任一所述的室內航位推算定位方法。