[发明专利]基于UWB和激光测距的室内航位推算定位方法及系统

权利要求书

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任一所述的室内航位推算定位方法。