[发明专利]一种双基站UWB/INS组合的密林环境人员定位方法

权利要求书

1.一种双基站UWB/INS组合的密林环境人员定位方法，其特征在于，包括如下步骤：

(1)建立密林环境空天地协同定位基准与系统，包括无人机平台、车辆平台和进入密林人员，其中，无人机平台和车辆平台均搭建卫星导航和UWB基站，无人机平台飞行在密林上空，车辆平台在林区边缘，进入密林人员搭载惯性导航IMU和UWB标签；进入密林人员通过UWB标签分别与无人机平台UWB基站和车辆平台UWB基站进行TWR双向测距，建立解析方程，得到不同时刻密林人员位置；

(2)进入密林人员通过惯性导航IMU进行航迹推算，获取密林人员在不同时刻的位置变化；

(3)将通过TWR双向测距得到的位置变化作为观测值，将通过惯性导航IMU进行航迹推算得到的位置变化作为状态值，建立误差方程，将观测值为边，状态值为点，利用图优化进行全局约束，获得密林人员准确位置。

2.根据权利要求1所述的一种双基站UWB/INS组合的密林环境人员定位方法，其特征在于，步骤(1)中进入密林人员通过UWB标签分别与无人机平台UWB基站和车辆平台UWB基站进行TWR双向测距，建立解析方程，具体过程如下：

车辆平台通过卫星导航获取当前绝对位置L₀(b₀,l₀,h₀)，并定义为空天地协同定位系统基准局部坐标原点L₀(0,0,0)；无人机平台通过卫星导航获取当前绝对位置L₁(b₁,l₁,h₁)，并计算无人机在空天地协同定位系统基准局部坐标L₁(x₁,y₁,z₁)：

x₁＝(b₁-b₀)λ_b

y₁＝(l₁-l₀)λ_l

z₁＝(h₁-h₀)

式中，λ_b，λ_l分别表示大地坐标系纬度和经度测量值转换为空间直角坐标系系数；

进入密林人员通过UWB标签分别与无人机平台UWB基站和车辆平台UWB基站进行TWR双向测距，建立如下解析方程：

式中，W₀(x⁰,y⁰,z⁰)为t₀时刻密林人员位置，为t₀时刻密林人员与车辆平台UWB基站的测距距离，为t₀时刻密林人员与无人机平台UWB基站测距距离；W₁(x¹,y¹,z¹)为t₁时刻密林人员位置，为t₁时刻密林人员与车辆平台UWB基站的测距距离，为t₁时刻密林人员与无人机平台UWB基站测距距离。

3.根据权利要求1所述的一种双基站UWB/INS组合的密林环境人员定位方法，其特征在于，步骤(2)具体过程如下：

密林人员通过惯性导航IMU进行航迹推算，估计运动状态速度v_t+Δt、位置r_t+Δt以及旋转q_t+Δt；

式中，a_t,ω_t分别为t时刻的IMU原始加速度和角速度量测；分别为加速度量测偏差和随机白噪声，分别为角速度量测偏差和随机白噪声；g为世界坐标系W的重力加速度，为惯性本体坐标系到世界坐标系的旋转；

通过航迹推算结果获取密林人员在t₀时刻到t₁时刻的位置变化为Δr(Δx,Δy,Δz)。