[发明专利]车载消防旋翼无人机TOA协同定位方法

权利要求书

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

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

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

每辆机动消防车车顶平台部署一架消防旋翼无人机，标识为每架旋翼无人机上安装一个UWB模块，标识为

旋翼无人机以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；

旋翼无人机对应的基准坐标系A_ixyz定义如下：坐标原点取UWB模块TOA传感器所处安装位置A_i，x轴经过TOA传感器B_i指向车头方向；y轴经过传感器A_i与x轴垂直，z轴指向天顶，为标准右手坐标系；

第二步，消防旋翼无人机UWB模块坐标解算，具体为：

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

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

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

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

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

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

第四步，消防旋翼无人机空间位置解算，具体为：

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

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

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

消防旋翼无人机空间位置解算的具体方法为：

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

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

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

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

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

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

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

其中为传感器测量误差，满足分别为TOA传感器的测量精度；对应TOA传感器的测量值满足如下方程

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

其中对数似然函数为

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

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

其中

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

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

其中Fg为Hessian矩阵：

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

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