CN111766561A - 一种基于uwb技术的无人机定位方法 - Google Patents

Abstract

本发明公开一种基于UWB技术的无人机定位方法，无人机上悬挂有UWB定位标签，空间中分布设置有UWB定位基站，还包括有定位服务器；定位方法包括以下步骤：(1)将UWB定位标签作为轻量级目标标识挂载到无人机上，通过特定频率向各个基站间广播带有自身时间戳的时间数据包；(2)UWB定位基站接收UWB定位标签发送的时间数据包，同时记录接收到数据包的时间；(3)UWB定位基站将数据包接收时间与时间数据包重新组合，形成定位数据包，并将定位数据包发送给定位服务器；(4)定位服务器接收定位数据包后解包，计算出时间数据包的到达各个UWB定位基站的时间，进而基于TDOA算法，计算出无人机的空间位置，同时绘制、存储飞行轨迹。

Description

一种基于UWB技术的无人机定位方法

技术领域

本发明涉及无人机定位领域，特别是一种基于UWB技术的无人机定位方法。

背景技术

无人机涉及导航定位与路径规划、机器视觉、智能控制，以及多传感器信息融合等关键 技术。其中，自主导航作为一项核心技术，一直是无人机领域的研究重点，而如何对无人机 飞行进行高精度定位测量则是无人机自助导航技术中的关键与难点问题。

在目前应用的无人机定位技术中，以惯性导航与卫星导航相结合的方式最为普遍。惯性 导航定位方法根据惯性测量单元动态响应特性良好，能够推算载体角度、速度、位置信息， 但长期工作会产生累积误差；卫星导航定位方法(GNSS)能够直接给出载体的速度、位置信 息，但更新频率很慢且速度位置信息的准确度易受遮挡干扰。此外，在应用成本方面，低精 度的定位方法虽然价格低廉，但是定位元器件工作时噪声较大且长期运行累积误差大；高精 度的定位方法，价格高昂难以实际应用。

发明内容

本发明的目的是为了克服现有技术中的不足，提供一种成本低廉，定位精度高的基于 UWB技术的无人机定位方法，该方法与GNSS技术相比，本方法具有更高的精度和实时性。 与传统技术相比，本方法具有更强的抗干扰能力和更好地鲁棒性。

本发明的目的是通过以下技术方案实现的：

一种基于UWB技术的无人机定位方法，无人机上悬挂有UWB定位标签，空间中分布设置有UWB定位基站，还包括有定位服务器；定位方法包括以下步骤：

(1)将UWB定位标签作为轻量级目标标识挂载到无人机上，通过特定频率向各个基站 间广播带有自身时间戳的时间数据包；

(2)UWB定位基站接收UWB定位标签发送的时间数据包，同时记录接收到数据包的时间；

(3)UWB定位基站将数据包接收时间与时间数据包重新组合，形成定位数据包，并将 定位数据包发送给定位服务器；

(4)定位服务器接收定位数据包后解包，计算出时间数据包的到达各个UWB定位基站 的时间，进而基于TDOA算法，计算出无人机的空间位置，同时绘制、存储飞行轨迹。

进一步的，为使步骤(2)至步骤(4)中各个UWB定位基站、UWB定位标签之间时间 同步，具体过程如下：

(101)指定任一UWB定位基站为“主基站”，其他UWB基站为“从基站”；

(102)主基站以时间T为周期，向从基站周期性地发送时钟校验包(CCP)；

(103)从基站记录第i次接收到CCP的时间为tccpr_i；

(104)从基站记录第i次接收到CCP时的时间偏移为τccp_i＝T-(tccpr_i-tccpr_i-1)；

(105)从基站当前时间为t_a，其真实时间应为t_real＝t_a+τccp_i；

(106)各个从基站均以主基站为基准，实现时间同步。

进一步的，步骤(4)中定位服务器基于TDOA算法计算出无人机的空间位置的具体过程 如下：

假设三维空间上任意的分布着N个UWB定位基站，移动标签的估计位置为(x，y，z)，第 i个基站的位置为(X_i，Y_i，Z_i)；

无人机即UWB定位标签到达第i个基站的距离为R_i，得到

令

并带入式(1)中，可得

假定以第m个基站为参考点，无人机即UWB定位标签到第i个基站的距离R_i，与到第m 个基站的距离R_m之间的距离差ΔR_i，m写作：

ΔR_i，m＝c·Δt_i，m＝R_i-R_m (3)

式中，c为电磁波传播速度，Δt为第i个基站与第m个基站接收到UWB定位标签时间戳的时 间差；

为求解方程(3)，可先进行线性化处理，则

依据式(2)，R_m为

式(4)、(5)相减可得

式中，X_i，m＝X_i-X_m，Y_i，m＝Y_i-Y_m，Z_i，m＝Z_i-Z_m；

设x，y，z，R_m为未知数，则式(6)能够视为线性方程组，求解该方程组便能够得到无人机即 UWB定位标签的坐标位置。

与现有技术相比，本发明的技术方案所带来的有益效果是：

(1)通过UWB定位基站与UWB定位标签组合系统结构，在减低无人机定位成本的同时，实现了高精度，实时的无人机定位。

(2)区别于传统的TDOA算法，本发明提供了一种基于Chan算法和Kalman滤波算法的高精度TDOA定位算法。

(3)本发明仅需要4个基站即可解算无人机的位置，而当基站数量大于4时，即可获得 多组无人机位置，提升了无人机定位精度的同时，使得本发明方法具有更强的抗干扰能力。

(4)本发明仅需要4个基站即可解算无人机的位置，而当基站数量大于4时，即使某几 个基站损坏，系统依然可以正常工作，因此具有更好地抗干扰能力和鲁棒性。

(5)本发明方法中还涉及高精度空间定位的基站布局方法和空间坐标几何解算模型及其 最小二乘法误差优化算法，进一步提高无人机空间坐标的定位精度。

(6)本发明方法中还涉及一种适用多种无人机机型的飞行参数实时监测软硬件系统及飞 行安全性能和可靠性评价体系。

(7)本发明方法区别于传统的TDOA算法，提供了一种基于Chan算法和Kalman滤波算法的高精度TDOA定位算法。

附图说明

图1是本发明的框架结构示意图。

附图标记：1-UWB定位基站，2-无人机，3-UWB定位标签，4-定位服务器

具体实施方式

以下结合附图和具体实施例对本发明作进一步详细说明。应当理解，此处所描述的具体 实施例仅仅用以解释本发明，并不用于限定本发明。

如图1所示，无人机2上悬挂UWB定位标签3，UWB定位基站1分布于空间中的不同 位置，各个UWB定位基站1的摆放位置可利用相关测绘仪器准确测量，定位服务器4摆放 至远端；

第一步，UWB定位标签3作为轻量级目标标识挂载到无人机2上，以一定频率向各个UWB定位基站1间广播带有自身时间戳的时间数据包；

第二步，UWB定位基站1接收UWB定位标签3发送的时间数据包，同时记录接收到数据包的时间；

第三步，UWB定位基站1将数据包接收时间与时间数据包重新组合，形成定位数据包， 并将定位数据包发送给定位服务器4；

第四步，定位服务器4接收数据包后解包，计算出时间数据包的到达各个UWB定位基 站1的时间，进而基于TDOA算法，计算出无人机2的空间位置，同时绘制、存储飞行轨迹。

在上述过程中，第二步至第四步的三个步骤中，都需要基于UWB定位基站1和UWB定位标签3的时间信息，然而由于每个UWB定位基站1、UWB定位标签3都有独立的时钟源， 而不同的时钟源晶振会有频率差异，同时不同的UWB定位基站1启动时间也不相同，因此 使得各个UWB定位基站1、UWB定位标签3之间时间不同步，而这不符合TDOA算的基本 要求，会产生较大的误差，针对于此，本发明设计了一种时间同步方法，具体过程如下：

首先，指定任一UWB定位基站1为“主基站”，其他UWB定位基站1为“从基站”；

进一步地，主基站以时间T为周期，向从基站周期性地发送时钟校验包(CCP)；

进一步地，从基站记录第i次接收到CCP的时间为tccpr_i；

进一步地，从基站记录第i次接收到CCP时的时间偏移为τccp_i＝T-(tccpr_i-tccpr_i-1)；

进一步地，从基站当前时间为t_a，其真实时间应为t_real＝t_a+τccp_i；

最终，各个从基站均以主基站为基准，实现了时间同步。

在上述过程中，第四步中，定位服务器4基于TDOA算法计算出无人机2的空间位置。传统TDOA算法主要是基于TDOA定位方程组的Fang算法和Chan算法，其二者的定位方 程组具有非线性特点，通常需将其转换为线性方程组才能进行求解。因此，如果想要获得高 精度的定位结果，必须要多次迭代，具有较高的算法复杂度。本发明提出了一种新型的TDOA算法，其采用Chan求解算法，并将常规Chan二位定位算法推导到三维坐标求解，并采用Kalman滤波算法对测量数据进行了处理。该算法在较少迭代次数的条件下，依然可以获得较 高的定位精度，其具体过程如下：

假设三维空间上任意的分布着N个UWB定位基站1，移动标签的估计位置为(x，y，z)， 第i个基站1的位置为(X_i，Y_i，Z_i)；

进一步地，无人机(即UWB定位标签)到达第i个基站1的距离为R_i，可以得到

进一步地，令

并带入式(1)中，可得

进一步地，假定以第m个基站1为参考点，无人机(即UWB定位标签)到第i个基站1的距离R_i，与到第m个基站1的距离R_m之间的距离差ΔR_i，m可以写作：

ΔR_i，m＝c·Δt_i，m＝R_i-R_m (3)

式中，c为电磁波传播速度，Δt为第i个基站1与第m个基站1接收到UWB定位标签3时间 戳的时间差。

进一步地，为求解方程(3)，可先进行线性化处理，则

进一步地，依据式(2)，R_m为

进一步地，式(4)、(5)相减可得

式中，X_i，m＝X_i-X_m，Y_i，m＝Y_i-Y_m，Z_i，m＝Z_i-Z_m；

进一步地，设x，y，z，R_m为未知数，则式(6)可以视为线性方程组，求解该方程组便可以得 到移动标签的坐标位置。以m＝1为例，即以第1个基站为参考点为例，解过程如下：

当UWB定位基站数N为4时，可得到3个TDOA测量值，先假定R_m为已知，则无 人机(即定位标签)位置(x，y，z)可由式(5)按以下形式解出

其中，

进一步地，将式(7)代入式(2)，令i＝1，得到一个关于R₁的二次方程，将其正根代回式(7)， 就得到无人机2的估计位置

当UWB定位基站数N为5个以上时，该算法可利用网络提供的所有TDOA值来取得 更好的计算结果。此时初试非线性TDOA方程组应首先转换为线性方程组，然后采用加权 最小二乘(WLS)算法得到一初始解，再利用第一次得到的估计位置坐标及附加变量等已知的 约束条件进行第二次WLS估计，最后便可得到改进的估计位置

本发明并不限于上文描述的实施方式。以上对具体实施方式的描述旨在描述和说明本发 明的技术方案，上述的具体实施方式仅仅是示意性的，并不是限制性的。在不脱离本发明宗 旨和权利要求所保护的范围情况下，本领域的普通技术人员在本发明的启示下还可做出很多 形式的具体变换，这些均属于本发明的保护范围之内。

Claims (3)

1.一种基于UWB技术的无人机定位方法，其特征在于，无人机上悬挂有UWB定位标签，空间中分布设置有UWB定位基站，还包括有定位服务器；定位方法包括以下步骤：

(1)将UWB定位标签作为轻量级目标标识挂载到无人机上，通过特定频率向各个基站间广播带有自身时间戳的时间数据包；

(2)UWB定位基站接收UWB定位标签发送的时间数据包，同时记录接收到数据包的时间；

(3)UWB定位基站将数据包接收时间与时间数据包重新组合，形成定位数据包，并将定位数据包发送给定位服务器；

(4)定位服务器接收定位数据包后解包，计算出时间数据包的到达各个UWB定位基站的时间，进而基于TDOA算法，计算出无人机的空间位置，同时绘制、存储飞行轨迹。

2.根据权利要求1所述一种基于UWB技术的无人机定位方法，其特征在于，为使步骤(2)至步骤(4)中各个UWB定位基站、UWB定位标签之间时间同步，具体过程如下：

(101)指定任一UWB定位基站为“主基站”，其他UWB基站为“从基站”；

(102)主基站以时间T为周期，向从基站周期性地发送时钟校验包(CCP)；

(103)从基站记录第i次接收到CCP的时间为tccpr_i；

(104)从基站记录第i次接收到CCP时的时间偏移为τccp_i＝T-(tccpr_i-tccpr_i-1)；

(105)从基站当前时间为t_a，其真实时间应为t_real＝t_a+τccp_i；

(106)各个从基站均以主基站为基准，实现时间同步。

3.根据权利要求1所述一种基于UWB技术的无人机定位方法，其特征在于，步骤(4)中定位服务器基于TDOA算法计算出无人机的空间位置的具体过程如下：

假设三维空间上任意的分布着N个UWB定位基站，移动标签的估计位置为(x,y,z)，第i个基站的位置为(X_i，Y_i，Z_i)；

无人机即UWB定位标签到达第i个基站的距离为R_i，得到

令

并带入式(1)中，可得

假定以第m个基站为参考点，无人机即UWB定位标签到第i个基站的距离R_i，与到第m个基站的距离R_m之间的距离差ΔR_i，m写作：

ΔR_i,m＝c·Δt_i,m＝R_i-R_m (3)

式中，c为电磁波传播速度，Δt为第i个基站与第m个基站接收到UWB定位标签时间戳的时间差；

为求解方程(3)，可先进行线性化处理，则

依据式(2)，R_m为

式(4)、(5)相减可得

式中，X_i，m＝X_i-X_m，Y_i，m＝Y_i-Y_m，Z_i，m＝Z_i-Z_m；

设x，y，z，R_m为未知数，则式(6)能够视为线性方程组，求解该方程组便能够得到无人机即UWB定位标签的坐标位置。

Images