一种基于超宽带的无人机辅助定位方法及系统

摘要

本发明公开了一种基于超宽带的无人机辅助定位方法及系统，该方法包括获取第一超宽带定位模组的姿态数据及所述第一超宽带定位模组与第二超宽带定位模组之间的测距值；通过扩展卡尔曼滤波算法融合所述姿态数据以及所述测距值。在超宽带基站间距受限的情况下，通过对测距和定位两个阶段进行优化，从而提升超宽带定位精度。采用扩展卡尔曼滤波算法融合超宽带测距与姿态数据，动态调整测量噪声统计特性，降低超宽带测距误差，进而提升了超宽带测距定位精度，降低了无人机对卫星信号的依赖性，综合了超宽带测距和惯性导航定位，解决了系留无人机在卫星信号拒止环境下无法定位的问题。

权利要求

1.一种基于超宽带的无人机辅助定位方法，其特征在于，所述方法包括：获取第一超宽带定位模组的姿态数据及所述第一超宽带定位模组与第二超宽带定位模组之间的测距值；所述第一超宽带定位模组设置于无人机上；所述第二超宽带定位模组设置于地面工作站上；

通过扩展卡尔曼滤波算法融合所述姿态数据以及所述测距值；

通过所述姿态数据计算获得所述第一超宽带定位模组与所述地面工作站之间的解算距离；计算获得所述测距值与所述解算距离之间的偏差，通过所述偏差更新所述扩展卡尔曼滤波算法中的测量噪声统计特性，以使所述扩展卡尔曼滤波算法进行更新获得状态变量的后验更新值；所述状态变量包括所述第一超宽带定位模组所在的超宽带坐标系下X、Y、Z轴方向上的位置以及速度；

从所述后验更新值中获取所述第一超宽带定位模组在所述超宽带坐标系下的位置估计值，将所述位置估计值作为所述无人机相对所述地面工作站的位置数据；

所述获取所述第一超宽带定位模组与第二超宽带定位模组之间的测距值，包括：采用双边双向测距法测量获得所述第一超宽带定位模组与第二超宽带定位模组之间的校准后的测距值；

所述双边双向测距法包括：

超宽带定位模组A在Ta1时刻发送出测距信号，超宽带定位模组B在Tb1时刻接收到A的测距信号，并在Tb2时刻发送出测距信号，超宽带定位模组A在Ta2时刻接收到超宽带定位模组B的测距信号，并在Ta3时刻发送出测距信号，超宽带定位模组B在Tb3时刻接收到超宽带定位模组A的测距信号；超宽带定位模组A与超宽带定位模组B之间的测距信号单向传输时间为：其中，Tround1＝Ta2‑Ta1，Tround2＝Tb3‑Tb2、Treply1＝Tb2‑Tb1、Treply2＝Ta3‑Ta2；超宽带定位模组A与超宽带定位模组B之间的距离为rAB＝C.TTOF,其中C为光速；

所述通过扩展卡尔曼滤波算法融合所述姿态数据以及所述测距值；包括：根据所述姿态数据计算出机体坐标系到导航坐标系的旋转矩阵 根据所述第二超宽带定位模组的姿态角、四元数计算出导航坐标系到所述超宽带坐标系的旋转矩阵选取所述状态变量为超宽带坐标系下X、Y、Z轴方向上的位置和速度系统状态方程为：

对应矩阵形式为Xk＝Ak‑1Xk‑1+Bk‑1uk‑1+Qk‑1；

其中，Qk‑1是过程噪声协方差矩阵,Ts是扩展卡尔曼滤波算法的迭代周期； 为第一超宽带定位模组在超宽带坐标系下k时刻的位置X轴分量， 为第一超宽带定位模组在超宽带坐标系下k时刻的位置Y轴分量， 为第一超宽带定位模组在超宽带坐标系下k时刻的位置Z轴分量；

为第一超宽带定位模组在超宽带坐标系下k时刻的速度X轴分量， 为第一超宽带定位模组在超宽带坐标系下k时刻的速度Y轴分量， 为第一超宽带定位模组在超宽带坐标系下k时刻的速度Z轴分量；

为第一超宽带定位模组在超宽带坐标系下k‑1时刻的位置X轴分量， 为第一超宽带定位模组在超宽带坐标系下k‑1时刻的加速度X轴分量，ηpx，k‑1为第一超宽带定位模组在k‑1时刻位置X轴分量的过程噪声；

为第一超宽带定位模组在超宽带坐标系下k‑1时刻的位置Y轴分量， 为第一超宽带定位模组在超宽带坐标系下k‑1时刻的加速度Y轴分量，ηpy，k‑1为第一超宽带定位模组在k‑1时刻位置Y轴分量的过程噪声；

为第一超宽带定位模组在超宽带坐标系下k‑1时刻的位置Z轴分量， 为第一超宽带定位模组在超宽带坐标系下k‑1时刻的加速度Z轴分量，ηpz，k‑1为第一超宽带定位模组在k‑1时刻位置Z轴分量的过程噪声；

ηvx，k‑1为第一超宽带定位模组在k‑1时刻速度X轴分量的过程噪声；ηvy，k‑1为第一超宽带定位模组在k‑1时刻速度Y轴分量的过程噪声；ηvz，k‑1为第一超宽带定位模组在k‑1时刻速度Z轴分量的过程噪声；

Ak‑1为系统在k‑1时刻的状态矩阵，Bk‑1为系统在k‑1时刻的输入矩阵，uk‑1为系统在k‑1时刻的控制向量,Xk为系统在k时刻的状态变量；

结合所述姿态数据，系统输入量为：

其中，g为重力加速度；accx，k‑1为第一超宽带定位模组在机体坐标系下k‑1时刻的加速度X轴分量；

观测方程是第一超宽带定位模组与第二超宽带定位模组i(i＝1,…,n)之间的测距值；

其中，σk为系统在k时刻的测距值的测量噪声，σi,k是测量噪声， 是第二超宽带定位模组i在超宽带坐标系下的坐标；

根据状态变量预测初始值 估计误差协方差矩阵初始值Pk‑1和系统输入量初始值uk‑1，结合下式，计算出状态变量先验预测值 估计误差协方差矩阵先验预测值所述通过所述偏差更新所述扩展卡尔曼滤波算法中的测量噪声统计特性，以使所述扩展卡尔曼滤波算法进行更新获得状态变量的后验更新值；包括：根据所述测距值与所述解算距离之间的偏差 调整测量噪声协方差矩阵Rσ,k(Rσ,k>0)，修改观测噪声的统计特性；其中 是校准后的测距值；

其中，系数δk的取值范围0≤δk≤1，根据下式计算卡尔曼增益Kk；

式中， 为系统在k时刻的观测矩阵的转置,Hk为系统在k时刻的观测矩阵，按下式对状态变量和估计误差协方差矩阵进行后验更新；

从所述后验更新值 中获取所述第一超宽带定位模组在所述超宽带坐标系下的位置估计值。

2.根据权利要求1所述的基于超宽带的无人机辅助定位方法，其特征在于，所述获取第一超宽带定位模组的姿态数据，包括：通过惯导模块测量获取第一超宽带定位模组的姿态数据；所述姿态数据包括姿态角、四元数以及加速度。

3.根据权利要求1所述的基于超宽带的无人机辅助定位方法，其特征在于，所述第二超宽带定位模组包括多个；

所述获取所述第一超宽带定位模组与第二超宽带定位模组之间的测距值，包括：分别获取所述第一超宽带定位模组与每个所述第二超宽带定位模组之间的测距值形成测距值组；通过扩展卡尔曼滤波算法融合所述姿态数据以及所述测距值组；

所述计算获得所述测距值与所述解算距离之间的偏差，包括：分别计算获得所述测距值组中包含的各个所述测距值与所述解算距离之间的偏差形成偏差组，通过所述偏差组更新所述扩展卡尔曼滤波算法中的测量噪声统计特性。

4.根据权利要求3所述的基于超宽带的无人机辅助定位方法，其特征在于，所述无人机包括系留无人机，所述地面工作站包括地面控制车辆；多个所述第二超宽带定位模组均设置于所述地面控制车辆的顶部。

5.根据权利要求1所述的基于超宽带的无人机辅助定位方法，其特征在于，所述第一超宽带定位模组与所述第二超宽带定位模组均为结合激光测距仪进行线性拟合校准标定后的超宽带定位模组。

6.根据权利要求5所述的基于超宽带的无人机辅助定位方法，其特征在于，所述校准标定的方法包括：在视距环境下，采集n组距离(ri,k,di,k)数据，其中ri,k是第i组第一超宽带定位模组与第二超宽带定位模组的第k次测距值，di,k是对应激光测距仪的测量值；校准后的第i组测距为 其中参数scalei和offseti根据最小二乘法线性拟合求取，计算公式如下：

说明书

一种基于超宽带的无人机辅助定位方法及系统

技术领域

[0001] 本发明涉及导航方法技术领域，特别是涉及一种基于超宽带的无人机辅助定位方法及系统。

背景技术

[0002] 无人驾驶飞机简称“无人机”（“UAV”），是利用无线电遥控设备和自备的程序控制装置操纵的不载人飞行器。与载人飞机相比，它具有体积小、造价低、使用方便、对作战环境要求低、战场生存能力较强等优点。无人机导航是指无人机在飞行过程中确定其位置和方向的方法和过程，导航系统的性能直接关系到航线任务是否可以完成。因为无人机只能依靠飞行控制系统来实现自动飞行，而飞行控制系统的反馈输入来自导航信号，即，机载计算机对于当前位置和速度的估计。

[0003] 随着科技的发展，目前无人机的室外定位导航技术基本成熟。无人机在室外空旷区域飞行时大多采用全球导航卫星系统与惯性导航系统提供位置导航信息，其精度可达到米级。配合在地面部署固定的差分基站，无人机的定位精度可达厘米级。

[0004] 虽然采用全球导航卫星系统与惯性导航系统提供位置导航，具有导航精度高，适合长时间飞行等优点，但上述导航方式对卫星信号强度的依赖性较大。尤其一些特殊形式的无人机，由于其使用场景中卫星信号易受到干扰，经常出现由于卫星信号丢失造成无人机失控或无法起飞等问题。

[0005] 系留无人机又称系留式无人机，为多旋翼无人机的一种特殊形式，使用通过系留线缆传输的地面电源作为动力来源，代替传统的锂电池，最主要的特点是长时间的滞空悬停能力。系留无人机常搭载光电吊舱、通信中继等载荷在山地、丛林、城市街道等地区开展任务，卫星信号容易受到环境影响和人为恶意干扰，引起信号丢失，导致无人机失控或无法起飞，虽然通过惯性导航可解决短时间无卫星信号条件下的定位问题，但无法克服长时间下定位漂移问题。

[0006] 因此，如何提供一种辅助定位方法，可以在卫星信号拒止环境下对无人机进行辅助定位，有效降低无人机对卫星信号的依赖，提高无人机应用能力，是迫切需要本领域技术人员解决的技术问题。

发明内容

[0007] 本发明提供了一种基于超宽带的无人机辅助定位方法及系统。

[0008] 本发明提供了如下方案：

[0009] 一种基于超宽带的无人机辅助定位方法，包括：

[0010] 获取第一超宽带定位模组的姿态数据及所述第一超宽带定位模组与第二超宽带定位模组之间的测距值；所述第一超宽带定位模组设置于无人机上；所述第二超宽带定位模组设置于地面工作站上；

[0011] 通过扩展卡尔曼滤波算法融合所述姿态数据以及所述测距值；

[0012] 通过所述姿态数据计算获得所述第一超宽带定位模组与所述地面工作站之间的解算距离；计算获得所述测距值与所述解算距离之间的偏差，通过所述偏差更新所述扩展卡尔曼滤波算法中的测量噪声统计特性，以使所述扩展卡尔曼滤波算法进行更新获得状态变量的后验更新值；所述状态变量包括所述第一超宽带定位模组所在的超宽带坐标系下X、Y、Z轴方向上的位置以及速度；

[0013] 从所述后验更新值中获取所述第一超宽带定位模组在所述超宽带坐标系下的位置估计值，将所述位置估计值作为所述无人机相对所述地面工作站的位置数据。

[0014] 优选地：所述获取第一超宽带定位模组的姿态数据，包括：

[0015] 通过惯导模块测量获取第一超宽带定位模组的姿态数据；所述姿态数据包括姿态角、四元数以及加速度。

[0016] 优选地：所述第二超宽带定位模组包括多个；

[0017] 所述获取所述第一超宽带定位模组与第二超宽带定位模组之间的测距值，包括：

[0018] 分别获取所述第一超宽带定位模组与每个所述第二超宽带定位模组之间的测距值形成测距值组；通过扩展卡尔曼滤波算法融合所述姿态数据以及所述测距值组；

[0019] 所述计算获得所述测距值与所述解算距离之间的偏差，包括：

[0020] 分别计算获得所述测距值组中包含的各个所述测距值与所述解算距离之间的偏差形成偏差组，通过所述偏差组更新所述扩展卡尔曼滤波算法中的测量噪声统计特性。

[0021] 优选地：所述无人机包括系留无人机，所述地面工作站包括地面控制车辆；多个所述第二超宽带定位模组均设置于所述地面控制车辆的顶部。

[0022] 优选地：所述第一超宽带定位模组与所述第二超宽带定位模组均为结合激光测距仪进行线性拟合校准标定后的超宽带定位模组。

[0023] 优选地：所述校准标定的方法包括：

[0024] 在视距环境下，采集 组距离 数据，其中 是第 组第一超宽带定位模组与第二超宽带定位模组的第 次测距值， 是对应激光测距仪的测量值；校准后的第组测距为 ，其中参数 和 根据最小二乘法线性拟合
求取，计算公式如下：

[0025] 。

[0026] 优选地：所述获取所述第一超宽带定位模组与第二超宽带定位模组之间的测距值，包括：

[0027] 采用双边双向测距法测量获得所述第一超宽带定位模组与第二超宽带定位模组之间的校准后的测距值；

[0028] 所述双边双向测距法包括：

[0029] 超宽带定位模组A在 时刻发送出测距信号，超宽带定位模组B在 时刻接收到A的测距信号，并在 时刻发送出测距信号，超宽带定位模组A在 时刻接收到超宽带定位模组B的测距信号，并在 时刻发送出测距信号，超宽带定位模组B在 时刻接收到超宽带定位模组A的测距信号；超宽带定位模组A与超宽带定位模组B之间的测距信号单向传输时间为：

[0030]

[0031] 其中， ， 、 、；超宽带定位模组A与超宽带定位模组B之间的距离为 ，其中 为光速。

[0032] 优选地：所述通过扩展卡尔曼滤波算法融合所述姿态数据以及所述测距值；包括：

[0033] 根据所述姿态数据计算出机体坐标系到导航坐标系的旋转矩阵 ；根据所述第二超宽带定位模组的姿态角、四元数计算出导航坐标系到所述超宽带坐标系的旋转矩阵 ；

[0034] 选取所述状态变量为超宽带坐标系下X、Y、Z轴方向上的位置和速度，系统状态方程为：

[0035]                 （3）

[0036] 对应矩阵形式为 ；

[0037] 其中， 是过程噪声协方差矩阵,  是扩展卡尔曼滤波算法的迭代周期；为第一超宽带定位模组在超宽带坐标系下k时刻的位置X轴分量， 为第一超宽带定位模组在超宽带坐标系下k时刻的位置Y轴分量， 为第一超宽带定位模组在超宽带坐标系下k时刻的位置Z轴分量；

[0038] 为第一超宽带定位模组在超宽带坐标系下k时刻的速度X轴分量 ， 为第一超宽带定位模组在超宽带坐标系下k时刻的速度Y轴分量 ， 为第一超宽带定位模组在超宽带坐标系下k时刻的速度Z轴分量 ；

[0039] 为第一超宽带定位模组在超宽带坐标系下k‑1时刻的位置X轴分量，为第一超宽带定位模组在超宽带坐标系下k‑1时刻的加速度X轴分量， 为第一超宽
带定位模组在k‑1时刻位置X轴分量的过程噪声；

[0040] 为第一超宽带定位模组在超宽带坐标系下k‑1时刻的位置Y轴分量，为第一超宽带定位模组在超宽带坐标系下k‑1时刻的加速度Y轴分量， 为第一超宽
带定位模组在k‑1时刻位置Y轴分量的过程噪声；

[0041] 为 第一超宽带定位模组在超宽带坐标系下k‑1时刻的位置Z轴分量，为第一超宽带定位模组在超宽带坐标系下k‑1时刻的加速度Z轴分量， 为第一超宽
带定位模组在k‑1时刻位置Z轴分量的过程噪声；

[0042] 为第一超宽带定位模组在k‑1时刻速度X轴分量的过程噪声； 为第一超宽带定位模组在k‑1时刻速度Y轴分量的过程噪声； 为第一超宽带定位模组
在k‑1时刻速度Z轴分量的过程噪声；

[0043] 为系统在k‑1时刻的状态矩阵， 为系统在k‑1时刻的输入矩阵， 为系统在k‑1时刻的控制向量, 为系统在k时刻的状态变量；

[0044] 结合所述姿态数据，系统输入量为：

[0045]

[0046] 其中，g为重力加速度； 为第一超宽带定位模组在机体坐标系下k‑1时刻的加速度X轴分量；

[0047] 观测方程是第一超宽带定位模组与第二超宽带定位模组 （ =1,…,n）之间的测距值；

[0048]

[0049] 其中， 为系统在k时刻的测距值的测量噪声， 是测量噪声，是第二超宽带定位模组 在超宽带坐标系下的坐标；

[0050] 根据状态变量预测初始值 、估计误差协方差矩阵初始值 和系统输入量初始值 ，结合下式，计算出状态变量先验预测值 ，估计误差协方差矩阵先验预测值 ；

[0051] 。

[0052] 优选地：所述通过所述偏差更新所述扩展卡尔曼滤波算法中的测量噪声统计特性，以使所述扩展卡尔曼滤波算法进行更新获得状态变量的后验更新值；包括：

[0053] 根据所述测距值与所述解算距离之间的偏差 ，调整测量噪声协方差矩阵 （ >0），修改观测噪声的统计特性；其中 是校准后的测距值；

[0054]

[0055] 其中，系数 的取值范围 ，根据下式计算卡尔曼增益 ；

[0056]

[0057] 式中， 为系统在k时刻的观测矩阵的转置,  为系统在k时刻的观测矩阵；

[0058] 按下式对状态变量和估计误差协方差矩阵进行后验更新；

[0059]

[0060] 从状态变量的后验更新值 中获得所述第一超宽带定位模组在所述超宽带坐标系下的位置估计值。

[0061] 一种基于超宽带的无人机辅助定位系统，所述系统包括：

[0062] 数据获取单元，用于获取第一超宽带定位模组的姿态数据及所述第一超宽带定位模组与第二超宽带定位模组之间的测距值；所述第一超宽带定位模组设置于无人机上；所述第二超宽带定位模组设置于地面工作站上；

[0063] 数据融合单元，用于通过扩展卡尔曼滤波算法融合所述姿态数据以及所述测距值；

[0064] 更新单元；用于通过所述姿态数据计算获得所述第一超宽带定位模组与所述地面工作站之间的解算距离；计算获得所述测距值与所述解算距离之间的偏差，通过所述偏差更新所述扩展卡尔曼滤波算法中的测量噪声统计特性，以使所述扩展卡尔曼滤波算法进行更新获得状态变量的后验更新值；所述状态变量包括所述第一超宽带定位模组所在的超宽带坐标系下X、Y、Z轴方向上的位置以及速度；

[0065] 位置数据确定单元，用于从所述后验更新值中获取所述第一超宽带定位模组在所述超宽带坐标系下的位置估计值，将所述位置估计值作为所述无人机相对所述地面工作站的位置数据。

[0066] 根据本发明提供的具体实施例，本发明公开了以下技术效果：

[0067] 本申请实施例提供的一种基于超宽带的无人机辅助定位方法，在超宽带基站间距受限的情况下，通过对测距和定位两个阶段进行优化，从而提升超宽带定位精度。采用扩展卡尔曼滤波算法融合超宽带测距与姿态数据，动态调整测量噪声统计特性，降低超宽带测距误差，进而提升了超宽带测距定位精度，降低了无人机对卫星信号的依赖性，综合了超宽带测距和惯性导航定位，解决了系留无人机在卫星信号拒止环境下无法定位的问题。

[0068] 另外，在优选的实施方式下，还可以根据激光测距仪的测量值，基于最小二乘法线性拟合校准每个超宽带标签‑基站，并采用双边双向测距法，抑制天线延迟、时钟漂移导致的测距误差。

[0069] 当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。

附图说明

[0070] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

[0071] 图1是本发明实施例提供的一种基于超宽带的无人机辅助定位方法的流程图;

[0072] 图2是本发明实施例提供的超宽带定位模组部署示意图；

[0073] 图3是本发明实施例提供的双边双向UWB测距流程示意图；

[0074] 图4是本发明实施例提供的定位解算XY平面轨迹（高度25米）图；

[0075] 图5是本发明实施例提供的定位解算X轴方向位置估计（高度25米）图；

[0076] 图6是本发明实施例提供的定位解算Y轴方向位置估计（高度25米）图；

[0077] 图7是本发明实施例提供的定位解算XY平面轨迹（高度195米）图；

[0078] 图8是本发明实施例提供的定位解算X轴方向位置估计（高度195米）图；

[0079] 图9是本发明实施例提供的定位解算Y轴方向位置估计（高度195米）图；

[0080] 图10是本发明实施例提供的一种基于超宽带的无人机辅助定位系统的示意图。

具体实施方式

[0081] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本发明保护的范围。

[0082] 在无线通信技术中，超宽带(ultra‑wideband, UWB)信号具有高传输、低功耗、时间分辨率高和抗干扰能力强等优点，应用在室内通信、位置定位等领域，可提供高可靠、低复杂度的无人机室内定位解决方案。目前UWB定位技术是解决卫星信号拒止环境下无人机定位问题的常用方法。它利用纳秒至微秒级的非正弦波窄脉冲传输数据。不需常规窄带调制所需的RF频率变换，脉冲成型后可直接送至天线发射。超带宽信号的辐射非常低，通常只有手机辐射的千分之一，因此在工业上应用时，其不存在对其他仪器仪表的干扰问题。使用时，在需要定位的区域安装一定数量的定位基站让定位基站信号覆盖需要定位的区域，无人机携带定位标签就可实现对目标对象的分米级精确定位。

[0083] 采用该方法在室内无人机导航中可以获得良好的效果。但是其中UWB定位基站布置方式有一定的局限性，要求UWB定位基站之间的距离达到一定的要求。如果UWB定位基站之间的距离过小，在无人机飞行过程中会严重影响定位的精度。

[0084] 由于一些特殊形式的无人机使用环境以及使用方法的限制，使得可以布置定位基站的区域的面积有限，进而造成布置的定位基站彼此之间的距离难以扩大。例如，系留无人机需要通过线缆与地面工作站相连，通过地面工作站为其供电，其中地面工作站虽然有多种形式，例如，可以是控制车辆以及其他形式的起降平台。无论是控制车辆还是起降平台，可以用于布置定位基站的面积都是有限的。受限于车辆尺寸，车载系留无人机的UWB定位基站只能部署在区域较小的地面控制车上。与无人机的UWB室内定位不同，车辆上的UWB定位基站间距相对较小且系留无人机的飞行高度较高，导致UWB定位精度低，甚至无法定位。

[0085] 本申请实施例提供的方法，采用扩展卡尔曼滤波算法融合UWB测距值与IMU（Inertial Measurement Unit）数据，即惯性测量单元输出的姿态数据，动态调整测量噪声统计特性，降低UWB测距误差，进而提升了UWB测距定位精度，降低了无人机对卫星信号的依赖性，解决了定位基站布置受限的无人机在卫星信号拒止环境下无法定位的问题。

[0086] 参见图1，为本发明实施例提供的一种基于超宽带的无人机辅助定位方法，如图1所示，该方法可以包括：

[0087] S101：获取第一超宽带定位模组的姿态数据及所述第一超宽带定位模组与第二超宽带定位模组之间的测距值；所述第一超宽带定位模组设置于无人机上；所述第二超宽带定位模组设置于地面工作站上；在实际应用中，该姿态数据可以采用多种方式获得，例如，在一种实现方式下，本申请实施例可以提供通过惯导模块测量获取第一超宽带定位模组的姿态数据；所述姿态数据包括姿态角、四元数以及加速度。

[0088] S102：通过扩展卡尔曼滤波算法融合所述姿态数据以及所述测距值；

[0089] S103：通过所述姿态数据计算获得所述第一超宽带定位模组与所述地面工作站之间的解算距离；计算获得所述测距值与所述解算距离之间的偏差，通过所述偏差更新所述扩展卡尔曼滤波算法中的测量噪声统计特性，以使所述扩展卡尔曼滤波算法进行更新获得状态变量的后验更新值；所述状态变量包括所述第一超宽带定位模组所在的超宽带坐标系下X、Y、Z轴方向上的位置以及速度；该超宽带坐标系可以是以第二超宽带定位模组布置位置为坐标中心，以地面工作站上的第二超宽带定位模组的安装面为基准面建立。

[0090] S104：从所述后验更新值中获取所述第一超宽带定位模组在所述超宽带坐标系下的位置估计值，将所述位置估计值作为所述无人机相对所述地面工作站的位置数据。

[0091] 由于UWB测距是相对独立的，不存在累积误差，但受外界环境影响，会恶化测距精度；基于IMU定位不受外界环境影响，但存在误差累积。本申请实施例提供的方法，采用扩展卡尔曼滤波算法融合第一超宽带定位模组的姿态数据以及第一超宽带定位模组与第二超宽带定位模组之间的测距值融合二者数据，可以实现优势互补。

[0092] 可以理解的是，本申请实施例提供的第一超宽带定位模组布置与无人机上，用以形成标签模式，第二超宽带定位模组布置于地面工作站上，用以形成基站模式。为了进一步提高定位效果，可以采用设置多个第二超宽带定位模组的方式，即形成多个基站模式，以消除测距存在的误差。具体的，所述第二超宽带定位模组包括多个；

[0093] 所述获取所述第一超宽带定位模组与第二超宽带定位模组之间的测距值，包括：

[0094] 分别获取所述第一超宽带定位模组与每个所述第二超宽带定位模组之间的测距值形成测距值组；通过扩展卡尔曼滤波算法融合所述姿态数据以及所述测距值组；

[0095] 所述计算获得所述测距值与所述解算距离之间的偏差，包括：

[0096] 分别计算获得所述测距值组中包含的各个所述测距值与所述解算距离之间的偏差形成偏差组，通过所述偏差组更新所述扩展卡尔曼滤波算法中的测量噪声统计特性。

[0097] 在实际应用中，本申请实施例提供的方法适用于多种可能出现卫星信号拒止环境下辅助无人机进行导航。尤其适用于系留无人机使用。具体的，所述无人机包括系留无人机，所述地面工作站包括地面控制车辆；多个所述第二超宽带定位模组均设置于所述地面控制车辆的顶部。在具体选择第二超宽带定位模组的数量时，通常可以不少于4个，并尽量增大彼此之间的距离。

[0098] 可以理解的是，本申请实施例提供的方法还适用于具有跟随功能的非系留无人机使用。在具体操作时，可以将第一超宽带定位模组布置在无人机上，将第二超宽带定位模组布置在其跟随的车辆上。例如，用户可以根据需求在私家车的车顶布置多个第二超宽带定位模组，设置的第二超宽带定位模组即可形成基站模式，采用本申请实施例提供的方法，可以有效的提高无人机的跟随位置精度。

[0099] 在实际应用中，为了可以抑制超宽带定位模组天线等硬件差异导致的测距误差。本申请实施例还可以提供所述第一超宽带定位模组与所述第二超宽带定位模组均为结合激光测距仪进行线性拟合校准标定后的超宽带定位模组。第一超宽带定位模组与所述第二超宽带定位模组校准前需要规划出多个基站和一个标签，以激光测距仪的测量值为参考标准，基于最小二乘法线性拟合校准每个标签‑基站，抑制天线等硬件差异产生的测距误差。

[0100] 所述校准标定的方法包括：

[0101] 在视距环境下，采集 组距离 数据，其中 是第 组第一超宽带定位模组与第二超宽带定位模组的第 次测距值， 是对应激光测距仪的测量值；校准后的第组测距为 ，其中参数 和 根据最小二乘法线性拟合
求取，计算公式如下：

[0102] 。

[0103] 为抑制超宽带定位模组中标签、基站的时钟漂移所产生的测距误差，本申请实施例可以提供采用双边双向测距法测量获得所述第一超宽带定位模组与第二超宽带定位模组之间的校准后的测距值；

[0104] 所述双边双向测距法包括：

[0105] 超宽带定位模组A在 时刻发送出测距信号，超宽带定位模组B在 时刻接收到A的测距信号，并在 时刻发送出测距信号，超宽带定位模组A在 时刻接收到超宽带定位模组B的测距信号，并在 时刻发送出测距信号，超宽带定位模组B在 时刻接收到超宽带定位模组A的测距信号；超宽带定位模组A与超宽带定位模组B之间的测距信号单向传输时间为：

[0106]

[0107] 其中， ， 、 、；超宽带定位模组A与超宽带定位模组B之间的距离为 ，其中
为光速；计算出校准后的测距值为 。

[0108] 所述通过扩展卡尔曼滤波算法融合所述姿态数据以及所述测距值；包括：

[0109] 根据所述姿态数据计算出机体坐标系到导航坐标系的旋转矩阵 ；根据所述第二超宽带定位模组的姿态角、四元数计算出导航坐标系到所述超宽带坐标系的旋转矩阵 ；

[0110] 选取所述状态变量为超宽带坐标系下X、Y、Z轴方向上的位置和速度，系统状态方程为：

[0111]                    （3）

[0112] 对应矩阵形式为 ，其中 是过程噪声协方差矩阵,  是扩展卡尔曼滤波算法的迭代周期；

[0113] 为 第一超宽带定位模组在超宽带坐标系下k时刻的位置X轴分量， 为 第一超宽带定位模组在超宽带坐标系下k时刻的位置Y轴分量， 为第一超宽带定位模组在超宽带坐标系下k时刻的位置Z轴分量；

[0114] 为第一超宽带定位模组在超宽带坐标系下k时刻的速度X轴分量 ， 为第一超宽带定位模组在超宽带坐标系下k时刻的速度Y轴分量 ， 为第一超宽带定位模组在超宽带坐标系下k时刻的速度Z轴分量 ；

[0115] 为  第一超宽带定位模组在超宽带坐标系下k‑1时刻的位置X轴分量，为第一超宽带定位模组在超宽带坐标系下k‑1时刻的加速度X轴分量， 为第
一超宽带定位模组在k‑1时刻位置X轴分量的过程噪声；

[0116] 为 第一超宽带定位模组在超宽带坐标系下k‑1时刻的位置Y轴分量，为第一超宽带定位模组在超宽带坐标系下k‑1时刻的加速度Y轴分量， 为第
一超宽带定位模组在k‑1时刻位置Y轴分量的过程噪声；

[0117] 为 第一超宽带定位模组在超宽带坐标系下k‑1时刻的位置Z轴分量，为第一超宽带定位模组在超宽带坐标系下k‑1时刻的加速度Z轴分量， 为第一超宽
带定位模组在k‑1时刻位置Z轴分量的过程噪声；

[0118] 为第一超宽带定位模组在k‑1时刻速度X轴分量的过程噪声； 为第一超宽带定位模组在k‑1时刻速度Y轴分量的过程噪声； 为第一超宽带定位模组在
k‑1时刻速度Z轴分量的过程噪声；

[0119] 为系统在k‑1时刻的状态矩阵， 为为系统在k‑1时刻的输入矩阵，为系统在k‑1时刻的控制向量, 为系统在k时刻的状态变量；

[0120] 结合所述姿态数据，系统输入量为：

[0121]

[0122] 其中，g为重力加速度； 为第一超宽带定位模组在机体坐标系下k‑1时刻的加速度X轴分量；

[0123] 观测方程是第一超宽带定位模组与第二超宽带定位模组（ =1,…,n）之间的测距值；

[0124]

[0125] 其中， 为系统在k时刻的测距值的测量噪声， 是测量噪声， 是第二超宽带定位模组 在超宽带坐标系下的坐标；

[0126] 根据状态变量预测初始值 、估计误差协方差矩阵初始值 和系统输入量初始值 ，结合下式，计算出状态变量先验预测值 ，估计误差协方差矩阵先验预测值 ；

[0127] 。

[0128] 所述通过所述偏差更新所述扩展卡尔曼滤波算法中的测量噪声统计特性，以使所述扩展卡尔曼滤波算法进行更新获得状态变量的后验更新值；包括：

[0129] 根据所述测距值与所述解算距离之间的偏差 ，调整测量噪声协方差矩阵 （ >0），修改观测噪声的统计特性；其中 是校准后的测距值；

[0130]

[0131] 其中，系数 的取值范围 ， ；根据下式计算卡尔曼增益 。

[0132]

[0133] 式中， 为系统在k时刻的观测矩阵的转置,  为系统在k时刻的观测矩阵；

[0134] 按下式对状态变量和估计误差协方差矩阵进行后验更新；

[0135]

[0136] 从状态变量的后验更新值 中获得所述第一超宽带定位模组在所述超宽带坐标系下的位置估计值。由于该第一超宽带定位模组安装在无人机上，因此超宽带定位模组的位置估计即可作为无人机的定位数据使用。

[0137] 下面采用地面控制车辆作为地面工作站（工作站上设置6个基站），无人机采用系留无人机为例，对申请实施例提供的方法进行详细说明。

[0138] 在第一超宽带定位模组以及第二超宽带定位模组被分别安装在无人机以及地面工作站之前，可以对第一超宽带定位模组以及第二超宽带定位模组进行校准标定抑制超宽带定位模组自身UWB天线等硬件差异导致的测距误差。具体操作时，在视距环境下，结合激光测距仪，对每个UWB测距模块进行线性拟合校准标定，以抑制UWB天线等硬件差异导致的测距误差。其中，每个UWB测距模块包含两个超宽带定位模组。在UWB测距模块校准前需要规划出6个基站和一个标签，以激光测距仪的测量值为参考标准，基于最小二乘法线性拟合校准每个标签‑基站，抑制天线等硬件差异产生的测距误差。

[0139] 具体的，把七个UWB定位模组划分为六个定位基站和一个定位标签。在视距环境下，以激光测距仪测量值为标准参考值，使用最小二乘法对每个UWB标签‑基站测距进行线性拟合标定。针对第  ( =1,…,6)组标签‑基站，采集 组距离 测量值数据，其中是第 组标签‑基站的第 次UWB测距值， 是对应激光测距仪的测量值。校准后的第
组UWB测距为 。通过最小化均方误差计算缩放因子 和偏移
量 。均方误差为

[0140]    （1）

[0141] 根据下式

[0142]     （2）

[0143] 计算出 和 为

[0144] （3）。

[0145] 在第一超宽带定位模组与第二超宽带定位模组均校准标定完成后，可以将超宽带定位模组安装至相应的位置。具体的，将6个超宽带定位模组部署到地面控制车辆上并设置为基站模式，一个超宽带定位模组部署到系留无人机上并设置为标签模式。

[0146] 具体的布置方式可以是，如图2所示，在车头驾驶舱顶部两端各部署一个超宽带定位模组并设置为定位基站，在车尾货舱顶部四周各部署一个超宽带定位模组并设置为定位基站，在系留无人机上部署一个超宽带定位模组并设置为标签。六个定位基站的部署平面区域大小为6米*2.4米，其中驾驶舱长为3.3米，货舱长为2.7米。

[0147] 超宽带定位模组安装完成后即可用于对无人机起降以及飞行过程进行辅助定位。具体实现时，每个测距模块（一个基站和标签）采用双边双向测距法测量标签与各基站的距离，以抑制标签、基站时钟漂移导致的测距误差，IMU模块测超宽带定位模组姿态数据。

[0148] 超宽带定位模组的姿态由其IMU模块测量，以四元数和姿态角的形式输出；双边双向测距的具体方法如下：

[0149] 如图3所示，超宽带定位模组A在 时刻发送出测距信号，超宽带定位模组B在 时刻接收到超宽带定位模组A的测距信号，并在 时刻发送出测距信号，超宽带定位模组A在时刻接收到B的测距信号，并在 时刻发送出测距信号，超宽带定位模组B在 时刻接收到超宽带定位模组A的测距信号。超宽带定位模组A、超宽带定位模组B之间的信号单向传输时间为：

[0150]     （4）

[0151] 其中 ， ， 和。超宽带定位模组A、超宽带定位模组B之间的距离为 ，其中
为光速。根据式(3)中的缩放因子和偏移量计算出校准后的测距 。

[0152] 通过扩展卡尔曼滤波算法，融合UWB测距值和IMU数据，根据惯性导航解算距离与UWB测距值的偏差 ，对扩展卡尔曼滤波算法中的测量噪声统计特性进行动态更新。

[0153] UWB测距值与IMU数据融合以及测量噪声统计特性调整的具体方法如下：

[0154] UWB模组中的IMU模块输出姿态角、四元数、加速度等数据。根据标签超宽带定位模组的姿态角、四元数计算出机体坐标系到导航坐标系的旋转矩阵 。根据基站超宽带定位模组的姿态角、四元数计算出导航坐标系到超宽带坐标系的旋转矩阵 。

[0155] 超宽带测距是相对独立的，不存在累积误差，但受外界环境影响，会恶化测距精度；基于IMU定位不受外界环境影响，但存在误差累积。采用扩展卡尔曼滤波器融合二者数据，实现优势互补。扩展卡尔曼滤波器所选取的状态变量为超宽带坐标系下X,Y,Z轴方向上的位置和速度 ，系统状态方程为：

[0156]                     (5)

[0157] 对应矩阵形式 ，其中 （ ）是过程噪声协方差矩阵,  是扩展卡尔曼滤波器的迭代周期。 为 第一超宽带定位模组在超宽带坐标系下k时刻的位置X轴分量， 为 第一超宽带定位模组在超宽带坐标系下k时刻的位置Y轴分量， 为第一超宽带定位模组在超宽带坐标系下k时刻的位置Z轴分量；

[0158] 为第一超宽带定位模组在超宽带坐标系下k时刻的速度X轴分量 ， 为第一超宽带定位模组在超宽带坐标系下k时刻的速度Y轴分量 ， 为第一超宽带定位模组在超宽带坐标系下k时刻的速度Z轴分量 ；

[0159] 为 第一超宽带定位模组在超宽带坐标系下k‑1时刻的位置X轴分量，为第一超宽带定位模组在超宽带坐标系下k‑1时刻的加速度X轴分量， 为第
一超宽带定位模组在k‑1时刻位置X轴分量的过程噪声；

[0160] 为 第一超宽带定位模组在超宽带坐标系下k‑1时刻的位置Y轴分量，为第一超宽带定位模组在超宽带坐标系下k‑1时刻的加速度Y轴分量， 为第
一超宽带定位模组在k‑1时刻位置Y轴分量的过程噪声；

[0161] 为  第一超宽带定位模组在超宽带坐标系下k‑1时刻的位置Z轴分量，为第一超宽带定位模组在超宽带坐标系下k‑1时刻的加速度Z轴分量， 为第
一超宽带定位模组在k‑1时刻位置Z轴分量的过程噪声；

[0162] 为第一超宽带定位模组在k‑1时刻速度X轴分量的过程噪声； 为第一超宽带定位模组在k‑1时刻速度Y轴分量的过程噪声； 为第一超宽带定位模组在
k‑1时刻速度Z轴分量的过程噪声；

[0163] 为系统在k‑1时刻的状态矩阵， 为为系统在k‑1时刻的输入矩阵，为系统在k‑1时刻的控制向量,  为系统在k时刻的状态变量。

[0164] 结合超宽带模组中的IMU模块输出的姿态角、四元数、加速度数据，系统输入量为：

[0165]                          (6)

[0166] 其中g为重量加速度， 为第一超宽带定位模组在机体坐标系下k‑1时刻的加速度X轴分量；。

[0167] 观测方程是超宽带标签与超宽带基站 （ =1,…,6）的测距

[0168]   (7)

[0169] 其中， 为系统在k时刻的测距值的测量噪声， 是测量噪声，是超宽带基站 在超宽带坐标系下的坐标。

[0170] 在扩展卡尔曼滤波器的预测阶段，根据状态变量预测初始值 、估计误差协方差矩阵初始值 和系统输入量初始值 ，结合下式，计算出状态变量先验预测值 ，估计误差协方差矩阵先验预测值 。

[0171]                                   (8)

[0172] 在扩展卡尔曼滤波器校正阶段，首先根据超宽带测距与惯性导航解算距离的偏差，调整测量噪声协方差矩阵 （ >0），修改观测噪声的统计特性，降低模型误差。其中 是校准后的超宽带测距值。当存在一个或几个（小于6）超宽带测距值无效时（有效与无效通过设定的精度因子确定），则 中对应元素不做更新，不参与运算。

[0173]                                   (9)

[0174] 其中系数 的取值范围 ， 取值越大，新的测距数据对系统的影响越大。根据下式计算卡尔曼增益 。

[0175]                             (10)

[0176] 式中， 为系统在k时刻的观测矩阵的转置,  为系统在k时刻的观测矩阵；

[0177] 按下式对状态变量和估计误差协方差矩阵进行后验更新

[0178]                                     (11)

[0179] 从状态变量后验更新值 中获取标签在超宽带坐标系下的估计位置，作为系留无人机相对地面控制车的位置数据。该方法采用超宽带距离测量值和姿态数据融合的方法，卫星信号拒止环境下，确保在可用测距信号较少的情形下，辅助定位系统仍然可用。

[0180] 为验证本申请实施例提供的方法，在卫星信号拒止环境下车载系留无人机定位的有效性，用以下仿真实验进一步说明。

[0181] 在仿真中，6个定位基站在超宽带坐标系下的坐标分别为（0,0,0），（0,1.85,0.22），（0,4.69,0.22），（‑1.78,0,0），（‑2.3,1.85,0.22），（‑2.3,4.69,0.22）, 超宽带测距精度取10厘米。

[0182] 无人机携带定位标签在地面控制车上方25米高度处飞行，仿真实验结果如图4、图5、图6所示。当扩展卡尔曼滤波算法数据融合算法收敛后，X轴方向的定位误差在 30厘米内，Y轴方向的定位误差在 18厘米内。

[0183] 无人机携带定位标签在地面控制车上方195米高度处飞行，仿真实验结果如图7、图8、图9所示。当扩展卡尔曼滤波算法数据融合算法收敛后，X轴方向的定位误差在 70厘米内，Y轴方向的定位误差在 40厘米内。X轴方向的定位误差更大，是因为X轴方向上基站间的最大间距比Y轴方向上基站间的最大间距小。

[0184] 总之，本申请提供的基于超宽带的无人机辅助定位方法，根据激光测距仪测量值，可以有效的校准超宽带定位模组，使用双边双向测距法测量无人机与各基站的距离，根据扩展卡尔曼滤波算法融合超宽带测距与姿态数据，动态调整测量噪声统计特性，降低超宽带测距误差，进而提升了超宽带测距定位精度，降低了无人机对卫星信号的依赖性，解决了系留无人机在卫星信号拒止环境下无法定位的问题。

[0185] 在超宽带基站间距受限的情况下，通过对测距和定位两个阶段进行优化，从而提升超宽带定位精度。根据激光测距仪的测量值，基于最小二乘法线性拟合校准每个超宽带标签‑基站，并采用双边双向测距法，抑制天线延迟、时钟漂移导致的测距误差。基于扩展卡尔曼滤波算法，融合超宽带测距和姿态数据，可以动态调整测量噪声统计特性，降低定位模型误差。综合了超宽带测距和惯性导航定位，解决了系留无人机在卫星信号拒止环境下无法定位的问题。

[0186] 参见图10，与本申请实施例提供的一种基于超宽带的无人机辅助定位方法相对应，如图10所示，本申请实施例还提供了一种基于超宽带的无人机辅助定位系统，该系统具体可以包括：

[0187] 数据获取单元201，用于获取第一超宽带定位模组的姿态数据及所述第一超宽带定位模组与第二超宽带定位模组之间的测距值；所述第一超宽带定位模组设置于无人机上；所述第二超宽带定位模组设置于地面工作站上；

[0188] 数据融合单元202，用于通过扩展卡尔曼滤波算法融合所述姿态数据以及所述测距值；

[0189] 更新单元203；用于通过所述姿态数据计算获得所述第一超宽带定位模组与所述地面工作站之间的解算距离；计算获得所述测距值与所述解算距离之间的偏差，通过所述偏差更新所述扩展卡尔曼滤波算法中的测量噪声统计特性，以使所述扩展卡尔曼滤波算法进行更新获得状态变量的后验更新值；所述状态变量包括所述第一超宽带定位模组所在的超宽带坐标系下X、Y、Z轴方向上的位置以及速度；

[0190] 位置数据确定单元204，用于从所述后验更新值中获取所述第一超宽带定位模组在所述超宽带坐标系下的位置估计值，将所述位置估计值作为所述无人机相对所述地面工作站的位置数据。

[0191] 需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

[0192] 通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到本申请可借助软件加上必需的通用硬件平台的方式来实现。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）执行本申请各个实施例或者实施例的某些部分所述的方法。

[0193] 本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统或系统实施例而言，由于其基本相似于方法实施例，所以描述得比较简单，相关之处参见方法实施例的部分说明即可。以上所描述的系统及系统实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

[0194] 以上所述仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。