CN109581289A - 车载消防旋翼无人机toa协同定位方法 - Google Patents

Abstract

本发明公开了一种车载消防旋翼无人机TOA协同定位方法，包括以下步骤：UWB模块安装与部署：采用地面三辆机动消防车作为定位平台，在每辆车的车顶上部署并安装四个UWB模块；每辆机动消防车车顶平台部署一架消防旋翼无人机，每架旋翼无人机上安装一个UWB模块；消防旋翼无人机UWB模块坐标解算；消防旋翼无人机UWB模块TOA传感器数据采集；消防旋翼无人机空间位置解算。本发明在城市楼宇间GPS信号不可靠、信号弱或拒止条件下，通过多辆小型机动消防车TOA协同定位方式，实现消防旋翼无人机空间位置的精确可靠解算，从而为消防旋翼无人机自主或半自主飞行提供先决条件，提高消防员作业时的安全性。

Description

车载消防旋翼无人机TOA协同定位方法

技术领域

本发明涉及定位技术领域，具体涉及一种车载消防旋翼无人机TOA协同定位方法。

背景技术

城市消防问题一直是困扰城市经济发展的重大难题，一旦扑救不及时，会造成巨大的财产、人员伤亡损失。据不完全统计，中国现有100米以上高楼数以万计，未来10年，中国摩天大楼的增长速度和数量将傲视全球。超高层建筑的崛起，给城市消防带来巨大压力。目前，高楼消防通常采用搭建云梯使用高压水枪喷射灭火，国内目前最高灭火云梯仅101米，且造价极其昂贵、体型庞大笨重，无法在交通繁忙、道路狭窄的城市消防中大规模普及。而小型化的消防车又达不到对高层建筑灭火的高度。近年来，国内外旋翼无人机技术得到了飞速发展，并逐步向军民应用领域挺近，已有多家企业和科研机构开始研发用于消防的无人机样机。由于消防作业环境的危险性和特殊性，业内通常要求消防旋翼无人机作业时能实现自主或半自主飞行(包括定点悬停)，消防员仅需在消防车内或安全区域监控无人机飞行和作业，从而减轻消防员的工作压力，提高其作业时的安全性。要实现消防无人机作业时的自主或半自主飞行，必须首先解决城市楼宇间消防无人机的精确定位问题。

目前，室外消防无人机均采用GPS进行定位，由于GPS定位本身固有的缺陷，当消防无人机位于城市高层建筑楼宇之间时，GPS信号很容易被阻挡，从而出现信号不可靠、信号弱或无信号的拒止情形。因此，目前传统的基于GPS定位的消防无人机无法有效履行楼宇之间的消防定位问题。

发明内容

本发明的目的在于提供一种车载消防旋翼无人机TOA协同定位方法，解决城市建筑楼宇间GPS不可靠或无信号等拒止条件下的精确定位问题。

实现本发明目的的技术方案为：一种车载消防旋翼无人机TOA协同定位方法，包括以下步骤：

第一步，UWB模块安装与部署：采用地面三辆机动消防车作为定位平台，在每辆车的车顶上部署并安装四个UWB模块；每辆机动消防车车顶平台部署一架消防旋翼无人机，每架旋翼无人机上安装一个UWB模块；

第二步，消防旋翼无人机UWB模块坐标解算；

第三步，消防旋翼无人机UWB模块TOA传感器数据采集；

第四步，消防旋翼无人机空间位置解算。

本发明与现有技术相比，其显著优点为：(1)在城市楼宇间GPS信号不可靠、信号弱或拒止条件下，通过多辆小型机动消防车TOA协同定位方式，实现消防旋翼无人机空间位置的精确可靠解算，从而为消防旋翼无人机自主或半自主飞行提供先决条件，达到减轻消防员工作压力和负担，提高消防员作业时的安全性；(2)本发明的定位方法中消防旋翼无人机以地面小型化消防车为平台，显著提高了城市间消防通行机动能力和应对突发消防事件的反应处理速度；由于消防旋翼无人机仅搭载在原有的消防车上，与之前传统的消防功能并不冲突，可有效实现功能互补。

下面结合附图对本发明做进一步详细的描述。

附图说明

图1为第i辆消防车坐标系及UWB基站部署示意图。

图2为消防旋翼无人机W_i空间位置坐标解算流程图。

图3为典型工况下三辆消防车部署位置示意图。

图4为典型工况下消防旋翼无人机W₁位置坐标解算精度GDOP曲线图。

具体实施方式

本发明提出一种基于超宽带(UWB)的多消防车载旋翼无人机到达时间(TOA)协同定位方法，可解决城市建筑楼宇间GPS不可靠或无信号等拒止条件下的精确定位问题，其定位精度显著优于单点GPS定位精度。

一种车载消防旋翼无人机TOA协同定位方法，包括以下步骤：

第一步：UWB模块安装与部署。

1)采用地面三辆小型机动消防车作为定位平台，在每辆车的车顶上部署并安装四个UWB模块，定义三辆消防车的编号分别为i，i＝1，2，3，每辆车上的四个UWB模块分别标识为A_i，B_i，C_i，D_i，i＝1，2，3。

2)每辆小型机动消防车车顶平台部署一架消防旋翼无人机，标识为W_i，每架旋翼无人机上安装一个UWB模块，标识为T_i，i＝1，2，3，并在每架旋翼无人机底座平台上挂载灭火消防弹。

3)旋翼无人机W_i以i号车为基准车，定义并初始化旋翼无人机W_i空间坐标系A_ixyz，初始化每辆车上的四个UWB模块的位置坐标：将其标识为A_i(0，0，0)，B_i(b₀，0，0)，C_i(0，c₀，0)，D_i(b₀，c₀，d₀)，i＝1，2，3，b₀，c₀，d₀为车载UWB模块安装位置参数常量；旋翼无人机W_i的UWB模块空间位置坐标标识为T_i(x，y，z)，i＝1，2，3。每辆车上有三个UWB模块共面，另外一个UWB模块在三个UWB模块所处平面的上方。

第二步：消防旋翼无人机UWB基站坐标解算

1)旋翼无人机W_i对应第i号车上的四个UWB基站坐标为A_i，B_i，C_i，D_i，以此i号车坐标系A_ixyz为基准坐标系，测量另外两辆车上的8个UWB基站分别到达第i号车上的四个UWB基站A_i，B_i，C_i，D_i的TOA，并将其转换为距离测量，标识为：

其中i≠j，i，j∈{1，2，3}。

2)依据(1)式中的距离测量值，分别计算另外两辆车上的8个UWB基站在A_ixyz中的坐标，标识为：

第三步：消防旋翼无人机UWB基站TOA传感器数据采集。

1)定义UWB基站TOA传感器对旋翼无人机W_i的距离测量数据分别为其中k表示传感器数据采样时刻序号。当k＝0时，表示消防旋翼无人机W_i初始化部署位置对应的TOA传感器的输出数据，分别记录为

2)采集k时刻UWB基站TOA传感器的输出数据，分别记录为

第四步：消防旋翼无人机空间位置解算。

1)k＝0：无人机初始位置解算。基于UWB基站传感器的位置A_i，B_i，C_i，D_i和另外两辆车上8个UWB基站坐标及其测量数据计算消防旋翼无人机W_i的空间位置坐标T_i(x₀，y₀，z₀)。

2)k→k+1：基于UWB基站传感器的位置坐标A_i，B_i，C_i，D_i和另外两辆车上8个UWB基站坐标及TOA传感器测量数据计算消防旋翼无人机W_i空间位置坐标T_i(x_k，y_k，z_k)。

3)消防旋翼无人机W_i任务结束且已返回降落到地面第i号小型机动消防车车顶平台，停止其位置解算，退出；否则，返回步骤2)。

下面结合附图对本发明做进一步详细的描述。

第i辆消防车无人机坐标系及UWB基站部署示意图见图1，对应的消防旋翼无人机W_i空间坐标系A_ixyz定义如下：坐标原点取UWB模块TOA传感器所处安装位置A_i，x轴经过TOA传感器B_i指向车头方向；y轴经过传感器A_i与x轴垂直，z轴指向天顶，为标准右手坐标系。无人机W_i底座平台安装UWB模块T_i，并事先加挂消防弹。

作业时，三辆消防车抵达事发地点。以第i号车坐标系A_ixyz为基准坐标系，按下述方法计算另外两辆车8个基站在A_ixyz中的坐标(为表述简便，仅以计算第j辆车上基站A_j的坐标为例，i≠j，i，j∈{1，2，3}，其它基站坐标计算以此类推)：首先，测量得到基站A_j到第i辆车上的四个基站A_i，B_i，C_i，D_i的距离分别为定义基站A_j在A_ixyz中的坐标为满足如下方程

其中||·||表示欧式空间距离范数。然后，联立上述方程(2)-(5)，解之可得

根据三辆车12个基站的TOA测量数据，可实时求解无人机W_i的空间位置坐标。以第i辆消防车上的旋翼无人机W_i为例，定义该无人机空间位置向量为

p_k＝[x_k，y_k，z_k]^T (9)

为表述方便，将三辆消防车上的12个TOA传感器在坐标系A_ixyz中的位置坐标A_i，B_i，C_i，D_i，按顺序统一记为

s_l＝[x_l，y_l，z_l]^T，l＝1，2，…，12 (10)

假定TOA传感器的测量误差相互独立且服从正态分布，可建立如下传感器测量方程

η_l＝h_l(p_k)+w_l，l＝1，2，…，12 (11)

其中w_l为传感器测量误差，满足σ_l分别为TOA传感器的测量精度；η_l对应TOA传感器的测量值h_l(p_k)满足如下方程

定义测量值集合空间无人机坐标的最大似然估计为

其中对数似然函数为

式(14)中为常量。采用牛顿-拉弗森方法求解(13)式，解出无人机空间位置坐标。具体步骤如下：

步骤1，令待求无人机坐标x＝p_k，将式(13)等价转换为

其中

步骤2，定义g为迭代变量。初始化g＝0时的坐标估计值

步骤3，按下式进行迭代求解：

其中F_g为Hessian矩阵：

步骤4，判断是否到达最大迭代次数或收敛至预定误差范围，若是，停止迭代，退出；否则转步骤3。

消防旋翼无人机空间位置解算流程图见图2。

现对该技术方案的定位精度进行理论分析。定义对应的费歇尔信息矩阵(FIM)为J，有

相应的几何精度因子(GDOP)定义为

其中tr{·}表示求括弧中矩阵的迹。最终求得GDOP的解析表达式为

其中S₁＝{(m，n，l)|1≤m＜n＜l≤12}，S₂＝{(m，n)|1≤m＜n≤12}，c_l＝1/σ_l，而

v_l＝(p_k-s_l)/||p_k-s_l|| (22)

式(21)中，“×”表示向量叉乘，<·，·>表示标准向量内积。

下面结合实施例对该技术方案进行定位精度分析。

实施例

假定四个TOA传感器分别安装在三辆小型消防车的车顶平台上，相应部署参数为：b₀＝3.5m，c₀＝1.5m，d₀＝0.5m；TOA传感器的距离测量精度为：σ_l＝σ＝0.01m，三辆车部署位置示意图见图3。考察第1辆上的无人机定位精度：无人机W₁初始位于平台中央始位置，其坐标为：p₀＝[x₀，y₀，z₀]^T＝[1.75，0.75，0.5]^T，假定典型工况下无人机W₁作业时的升降轨迹为直线，其真值轨迹用下述参数化方程表示：

p_k＝p₀+n_ckΔh，k＝0，1，2，…

其中Δh为步长，这里取值10m，n_c＝[0，0，1]^T为无人机运动直线轨迹单位方向向量，无人机高度方向上运动范围z_k∈[0.5，290.5]。

依据精度分析公式(21)，求得该典型工况下的GDOP随无人机飞行高度z_k的变化曲线见图4。由图4可知，在该典型工况下，高度200米以下定位精度优于0.1m。本发明定位方法中无人机W₁的位置解算精度显著优于单点GPS定位精度，其定位方法简单、可靠，可满足旋翼无人机消防作业要求。

Claims (8)

1.一种车载消防旋翼无人机TOA协同定位方法，其特征在于，包括以下步骤：

第一步，UWB模块安装与部署：采用地面三辆机动消防车作为定位平台，在每辆车的车顶上部署并安装四个UWB模块；每辆机动消防车车顶平台部署一架消防旋翼无人机，每架旋翼无人机上安装一个UWB模块；

第二步，消防旋翼无人机UWB模块坐标解算；

第三步，消防旋翼无人机UWB模块TOA传感器数据采集；

第四步，消防旋翼无人机空间位置解算。

2.根据权利要求1所述的车载消防旋翼无人机TOA协同定位方法，其特征在于，第一步具体为：

采用地面三辆机动消防车作为定位平台，在每辆车的车顶上部署并安装四个UWB模块，定义三辆消防车的编号分别为i，i＝1，2，3，每辆车上的四个UWB模块分别标识为A_i，B_i，C_i，D_i，i＝1，2，3；

每辆机动消防车车顶平台部署一架消防旋翼无人机，标识为W_i，每架旋翼无人机上安装一个UWB模块，标识为T_i，i＝1，2，3；

旋翼无人机W_i以i号车为基准车，定义并初始化旋翼无人机W_i空间坐标系A_ixyz，初始化每辆车上的四个UWB模块的位置坐标：将其标识为A_i(0，0，0)，B_i(b₀，0，0)，C_i(0，c₀，0)，D_i(b₀，c₀，d₀)，i＝1，2，3，b₀，c₀，d₀为车载UWB模块安装位置参数常量；旋翼无人机W_i的UWB模块空间位置坐标标识为T_i(x，y，z)，i＝1，2，3。

3.根据权利要求2所述的车载消防旋翼无人机TOA协同定位方法，其特征在于，旋翼无人机W_i空间坐标系A_ixyz定义如下：坐标原点取UWB模块TOA传感器所处安装位置A_i，x轴经过TOA传感器B_i指向车头方向；y轴经过传感器A_i与x轴垂直，z轴指向天顶，为标准右手坐标系。

4.根据权利要求2所述的车载消防旋翼无人机TOA协同定位方法，其特征在于，每辆车上有三个UWB模块共面，另外一个UWB模块在三个UWB模块所处平面的上方。

5.根据权利要求2所述的车载消防旋翼无人机TOA协同定位方法，其特征在于，第二步具体为：

旋翼无人机W_i对应第i号车上的四个UWB基站坐标为A_i，B_i，C_i，D_i，以此i号车坐标系A_ixyz为基准坐标系，测量另外两辆车上的8个UWB基站分别到达第i号车上的四个UWB基站A_i，B_i，C_i，D_i的TOA，并将其转换为距离测量，标识为：

其中i≠j，i，j∈{1，2，3}；

依据(1)式中的距离测量值，分别计算另外两辆车上的8个UWB基站在A_ixyz中的坐标，标识为：

6.根据权利要求5所述的车载消防旋翼无人机TOA协同定位方法，其特征在于，第三步具体为：

定义UWB基站TOA传感器对旋翼无人机W_i的距离测量数据分别为其中k表示传感器数据采样时刻序号；当k＝0时，表示消防旋翼无人机W_i初始化部署位置对应的TOA传感器的输出数据，分别记录为

采集k时刻UWB基站TOA传感器的输出数据，分别记录为

7.根据权利要求6所述的车载消防旋翼无人机TOA协同定位方法，其特征在于，第四步具体为：

1)k＝0：无人机初始位置解算；基于UWB基站传感器的位置A_i，B_i，C_i，D_i和另外两辆车上8个UWB基站坐标及其测量数据计算消防旋翼无人机W_i的空间位置坐标T_i(x₀，y₀，z₀)；

2)k→k+1：基于UWB基站传感器的位置坐标A_i，B_i，C_i，D_i和另外两辆车上8个UWB基站坐标及TOA传感器测量数据计算消防旋翼无人机W_i空间位置坐标T_i(x_k，y_k，z_k)；

3)消防旋翼无人机W_i任务结束且已返回降落到地面第i号机动消防车车顶平台，停止其位置解算，退出；否则，返回步骤2)。

8.根据权利要求7所述的车载消防旋翼无人机TOA协同定位方法，其特征在于，消防旋翼无人机空间位置解算的具体方法为：

以第i号车坐标系A_ixyz为基准坐标系，按下述方法计算另外两辆车8个基站在A_ixyz中的坐标：

首先，测量得到基站A_j到第i辆车上的四个基站A_i，B_i，C_i，D_i的距离分别为定义基站A_j在A_ixyz中的坐标为满足如下方程

其中||·||表示欧式空间距离范数；然后，联立上述方程(2)-(5)，解之可得

根据三辆车12个基站的TOA测量数据，实时求解无人机W_i的空间位置坐标；定义第i辆消防车上的旋翼无人机W_i的空间位置向量为

p_k＝[x_k，y_k，z_k]^T (9)

将三辆消防车上的12个TOA传感器在坐标系A_ixyz中的位置坐标A_i，B_i，C_i，D_i，按顺序统一记为

s_l＝[x_l，y_l，z_l]^T，l＝1，2，…，12 (10)

假定TOA传感器的测量误差相互独立且服从正态分布，可建立如下传感器测量方程

η_l＝h_l(p_k)+ω_l，l＝1，2，…，12 (11)

其中w_l为传感器测量误差，满足σ_l分别为TOA传感器的测量精度；η_l对应TOA传感器的测量值h_l(p_k)满足如下方程

定义测量值集合空间无人机坐标的最大似然估计为

其中对数似然函数为

式(14)中为常量；采用牛顿-拉弗森方法求解(13)式，解出无人机空间位置坐标，具体步骤如下：

1)令待求无人机坐标x＝P_k，将式(13)等价转换为

其中

2)定义g为迭代变量，初始化g＝0时的坐标估计值

3)按下式进行迭代求解：

其中F_g为Hessian矩阵：

4)判断是否到达最大迭代次数或收敛至预定误差范围，若是，停止迭代；否则转步骤3)。