CN113238577A - 一种基于5g网关的无人机与无人车协作方法 - Google Patents

Abstract

本发明公开了一种基于5G网关的无人机与无人车协作方法，包括云平台、无人车和无人机，所述5G网关终端包括队形保持控制器和轨迹跟踪控制器；基于TDOA和DOA融合定位方式计算5G网关的网关终端坐标；所述云平台根据所述无人车和所述无人机的实际运行状态以及执行任务规划虚拟领航者的轨迹，将期望运行信息发送给所述5G网关终端；所述5G网关终端将编队控制的控制量下发给无人车和无人机两者的控制器，由控制器控制无人车和无人机的运行。该方法提供稳定的精度较高的定位信息，无人机能与地面无人车完成协作任务。

Description

一种基于5G网关的无人机与无人车协作方法

技术领域

本发明涉及5G网络技术领域，尤其涉及一种基于5G网关的无人机与无人车协作方法。

背景技术

目前市面上的无人机多采用2.4G等短程通信方式，仅能在视距范围内进行控制，通信距离短，数据共享能力差，而4G通信在时延、带宽上无法满足部分无人机业务。无人机定位主要基于GPS定位，定位精度不高，GPS信号易受建筑物等遮挡导致定位失效。RTK定位可提供高精度的定位信息，若无人机采用RTK定位，需在无人机上携带两个以上的信号接收机，给无人机带来较大负荷。

而且，现有的大部分无人机不具备协同能力，感知和作业范围小，在有协同作业需求时改造升级成本高、难度大。例如无人机和消防机器人协同侦查灭火的情况。有鉴于此，需要对传统单机飞行的无人机进行定位功能和协同作业能力的快速扩展。

发明内容

本发明的目的在于提出一种基于5G网关的无人机与无人车协作方法，将5G网关作为无人机飞行控制的扩展模块，无需对原有无人机和飞行系统做大规模改动，即可为无人机进行低成本的快速升级，5G网关通过5G无线定位方式提供稳定的精度较高的定位信息，通过5G网关在边缘计算平台为无人机扩展协作功能，使无人机能与地面无人车完成协作任务，有效扩大无人机在侦察巡检范围。

为达此目的，本发明采用以下技术方案：

一种基于5G网关的无人机与无人车协作方法，包括云平台、无人车和无人机，所述无人车和无人机均安装有5G网关终端，所述无人车和无人机均通过5G无线网络与所述云平台通信连接；

所述5G网关包括队形保持控制器和轨迹跟踪控制器；

该方法包括以下步骤：

基于TDOA和DOA融合定位方式计算5G网关终端坐标；

基于5G网关终端坐标，获取所述无人车和所述无人机的实际运行状态，所述实际运行状态包括实际位置、实际速度和实际航向角度；

所述云平台构建有虚拟领航者的无人车和无人机的编队系统模型；

所述5G网关终端将所述无人车和无人机的实际运行状态发送至所述云平台；

所述云平台根据所述无人车和所述无人机的实际运行状态以及执行任务规划虚拟领航者的轨迹，将无人车和无人机的期望运行信息发送给所述5G网关终端，所述期望运行信息包括期望位置、速度和航向角度；

所述5G网关终端的队形保持控制器根据所述期望运行信息计算出无人机和无人车编队保持队形所需的控制量，所述5G网关终端的轨迹跟踪控制器计算出无人车和无人机跟踪虚拟领航者所需的控制量，将两部分控制量加权求和作为编队控制的控制量；

所述5G网关终端将编队控制的控制量分别下发给无人车和无人机两者的控制器，由控制器控制无人车和无人机的运行。

进一步的，基于TDOA和DOA融合定位方式计算5G网关终端坐标的方法为：

TDOA定位：通过5G网关终端到达两个不同基站的间的时间差估计5G网关终端到该两个基站的距离差，再由该距离差与5G网关终端坐标关系解算该5G网关终端的坐标；

DOA定位：通过天线阵列测量两个基站发出的信号到5G网关终端接收的入射角度，建立空间直线方程组，所述入射角度包括俯仰角和偏航角；通过所述的空间直线方程组联解出该5G网关终端的坐标；

将TDOA定位所得5G网关终端的坐标和DOA定位所得5G网关终端的坐标，加权平均得到最终坐标。

进一步的，所述TDOA定位的方法为：

(1)5G网关终端到达第i个基站的时间表示模型：

其中，δ_ue为5G网关终端的时钟误差，δ_i为基站的时钟误差，c为光速，P_ue为网关终端位置，P_i为基站位置，

为测距误差；

(2)作差估计出无人机到达两个基站的距离差D_ij，记D_i＝|P_ue-P_i|为无人机到达基站i的距离，D_ij＝|D_i-D_j|为无人机到基站i,j的距离差；

(3)由上述距离差与无人机坐标关系解算无人机的坐标：

其中，(x_i,y_i,z_i)，(x_j,y_j,z_j)表示基站i,j的坐标；

(4)联立多个步骤(1)-(3)的方程式，解出5G网关终端的坐标(x,y,z)。

进一步的，所述DOA定位的方法为：

(一)波达角度θ,

的估计

天线整列为均匀线性阵列，阵元间距均为Δ，选取最左侧阵元作为基准阵元，第i个阵元的坐标为x_i(i＝1,2,…,M)；

(1)对于第k个基站发来的第k个宽带信号(k＝1,2,…,K)，第i个阵元与基准阵元的到达时间差为：

相位差为：

(2)第k个信号的导向矢量：

β_k(θ_k)＝[exp(-jφ_1,k)exp(-jφ_2,k)…exp(-jφ_M,k)]^T；

(3)将均匀线性阵列划分为两个子阵列X₁,X₂，子阵列X₁包含线性阵列的前M-1个阵元，子阵列X₂包含线性阵列的后M-1个阵元；两个子阵的结构完全相同，阵列流型间存在旋转不变关系Φ；建立子阵X₁和子阵X₂接收信号矩阵模型：

X₁(t)＝Bs(t)+n₁(t)，

X₂(t)＝BΦs(t)+n₂(t)；

其中，s(t)为入射信号矩阵，n₁(t)，n₂(t)为输出噪声，X₁、X₂为子阵列1和子阵列2的接收信号矩阵，

X₁(t)＝[x₁(t),x₂(t),…,x_M-1(t)]^T，

X₂(t)＝[x₂(t),x₃(t),…,x_M(t)]^T,s(t)＝[s₁(t),s₂(t),…,s_K(t)]^T为入射信号；

Φ＝diag[exp(-jφ₁),exp(-jφ₂),…,exp(-jφ_K)]；

表示第k个信号下，子阵列X₁阵列流型和子阵列X₂阵列流型之间的旋转不变关系；

(4)离散采样得到t＝nT₀时刻子阵X₁和子阵X₂接收信号矩阵模型：

X₁(n)＝Bs(n)+n₁(n)，

X₂(n)＝BΦs(n)+n₂(n)，

记s(n)，n₁(n)，n₂(n)经FFT运算及小批量窄带化后的值为s(w)，n₁(w)，n₂(w)，信号输出为：

X₁(w)＝Bs(w)+n₁(w)，

X₂(w)＝BΦs(w)+n₂(w)；

将上述公式写成矩阵形式为：

(5)计算输出自协方差矩阵R_yy并做特征值分解：

信号空间和阵列流型空间的秩相等，有且仅有一个非奇异矩阵T使得：

E₁,E₂之间关系为E₂＝BΦT＝E₁T^-1ΦT＝E₁Ψ,Ψ＝T^-1ΦT反映两个阵列接收信号子空间的旋转不变关系，Ψ的特征值矩阵为Φ；

(6)计算Ψ，再由阵列流型旋转不变关系Φ计算角估计值

对R_yy做特征值分解求得特征子空间E₁,E₂，进而求得Ψ，

联立两个不同频点的窄带方程，解算第k个基站信号到达天线阵列的俯仰角θ_k和偏航角

再将多个不同频窄带信号求得的θ_k，

做最小均方误差估计，得到俯仰角θ_k、偏航角

(二)由波达角度计算5G网关终端坐标

(1)由偏航角

与水平坐标关系

结合两座以上的基站坐标，解算网关终端水平坐标；

(2)由俯仰角θ_k与垂直坐标关系

解算5G网关终端在z方向上的坐标，其中

表示基站k与天线阵列接收平面水平距离。

进一步的，将TDOA定位所得5G网关终端的坐标和DOA定位所得5G网关终端的坐标，加权平均得到最终坐标的方法为：

定义TDOA定位和AOA定位下坐标估计值与基站坐标的残差分别为：

其中K_TDOA,K_AOA为两种方法中参与定位的基站数，r_i为网关与低i个基站的距离，

为两种方法下网关的坐标估计值，P_i为基站i的坐标；

记λ为TDOA定位的权值，μ为AOA定位的权值，则权值分别为

所述5G网关坐标的最终估计值

进一步的，以l-γ-θ方法来描述虚拟领航者与无人机的距离和夹角关系，l表示二者的总距离，γ表示二者垂直方向的夹角，θ表示二者水平方向的夹角；

将虚拟领航者和无人机的三维空间坐标投影至XY平面上，在XY平面内对异构无人机系统进行队形保持和轨迹跟踪控制；

在XY平面内，无人机U_a的坐标为(x_a,y_a)、航向角为θ_a，无人车U_g的坐标为(x_g,y_g)、航向角为θ_g；当形成稳定队形时，无人机和无人车之间距离达到期望值

L(t)为t时刻无人机和无人车之间的距离，Lr为期望距离。

进一步的，计算无人机位姿偏差模型U_ae＝(x_e,y_e,θ_e)的方法如下：

(1)在XY平面内建立无人机、无人车运动模型：

其中v_a,v_g分别对应无人机、无人车的速度；

(2)定义L为无人机和无人车之间的距离，Lx＝x_g-x_a、Ly＝y_g-y_a分别表示L在x,y方向上的分量，无人机相对无人车夹角

(3)定义α＝θ_ga+θ_g-θ_a，求解L,θ_ga微分表达式：

(4)定义无人机U_a＝(x_a,y_a,θ_a)相对无人车U_g＝(x_g,y_g,θ_g)的期望位姿为U_ar＝(x_ar,y_ar,θ_ar)，表达式如下：

其中θ_gar表示期望位置相对无人车的夹角；

(5)将世界坐标变换至无人机坐标，并建立无人机位姿偏差模型U_ae：

进一步的，所述网关队形保持控制器包括速度控制器和航向角速度控制器，以Lyapunov方法设计速度控制器的控制量和航向角速度控制器的控制量，方法如下：

(1)选取系统Lyapunov函数：

(2)设计速度控制器的控制量v_a和航向角速度控制器的控制量

(3)验证稳定性：

k₁,k₂,k₃,k₄,k₅均为正实数，且k₁＝k₂，

可以得到：

其中，U_ae有界，

进一步的，所述5G网关终端的轨迹跟踪控制器的控制量的计算方法如下：

(一)计算无人机和无人车位姿偏差模型U_e＝(x_e,y_e,θ_e)：

定义无人和无人车当前位姿U_c＝(x_c,y_c,θ_c)，所述云平台规划的虚拟领航者位姿U_r＝(x_r,y_r,θ_r)，将无人机和无人车所在的世界坐标系变换为无人机和无人车坐标系，计算位姿偏差模型U_e＝(x_e,y_e,θ_e)。

(二)以Lyapunov方法设计轨迹跟踪控制器的速度控制器的控制量和航向角速度控制器的控制量：

(1)选取系统Lyapunov函数：

(2)设计速度控制器的控制量v_c和航向角速度控制器的控制量w_c：

v_c＝v_rcosθ_e+k₁x_e+x_ek₂|y_e|，

(3)验证稳定性：

k₁,k₂,k₃均为正实数，U_e有界，

本发明实施例的有益效果为：

5G网关终端可为无人机提供5G通信与定位服务，使地面控制中心(即云平台)能对其进行远程控制，同时5G网关终端能为无人机提供5G无线定位信息。该定位信息是利用5G高频毫米波和大规模天线阵列的技术带来的显著提高的测距精度和测角精度，融合TDOA和DOA定位方式，在DOA测角过程中通过将5G宽带信号分解窄带信号，提升测角精度。

该5G网关终端扩展了无人机与地面无人车的协作功能，在5G网关终端上分布式地部署队形保持控制器和轨迹跟踪控制器，该队形保持控制器和轨迹跟踪控制器放置在边缘计算平台，无需对原有无人机和飞行控制系统做大规模改动，可为无人机进行低成本的快速升级。

通过控制虚拟领航者与无人机的夹角γ实现对无人机高度的单独控制，将无人机与无人车的协同控制转化为水平面的协同控制，实现异构无人系统的协同控制。队形保持控制器和轨迹跟踪控制器计算队形保持和轨迹跟踪所需的速度及航向角速度，原无人机/车系统控制器根据速度及航向角速度完成底层控制。因此，该5G网关终端在边缘计算平台为无人机扩展协作功能，使无人机能与地面无人车完成协作任务，有效扩大无人机在侦察巡检范围。

附图说明

图1是本发明一个实施例的基于5G网关的无人机与无人车协作方法的系统结构图；

图2是5G网关的网关终端的定位信息计算流程图；

图3是5G网络的天线阵列示意图；

图4是无人机和无人车在水平坐标系下的相对位置关系图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

在本发明的描述中，需要理解的是，术语“纵向”、“横向”“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。

下面结合图1至图4，描述本发明实施例的基于5G网关的无人机与无人车协作方法。

如图1所示，本发明实施例的一种基于5G网关的无人机与无人车协作方法，包括云平台、无人车和无人机，无人车和无人机均安装有5G网关终端，无人车和无人机均通过5G无线网络与所述云平台通信连接；

5G网关包括队形保持控制器和轨迹跟踪控制器；

该方法包括以下步骤：

基于TDOA和DOA融合定位方式计算5G网关终端坐标；

基于5G网关终端坐标，获取无人车和无人机的实际运行状态，实际运行状态包括实际位置、实际速度和实际航向角度；

云平台构建有虚拟领航者的无人车和无人机的编队系统模型；

5G网关终端将无人车和无人机的实际运行状态发送至云平台；

云平台根据无人车和无人机的实际运行状态以及执行任务规划虚拟领航者的轨迹，将无人车和无人机以的期望运行信息发送给5G网关终端，期望运行信息包括期望位置、速度和航向角度；

5G网关终端的队形保持控制器根据期望运行信息计算出无人机和无人车编队保持队形所需的控制量，5G网关终端的轨迹跟踪控制器计算出无人车和无人机跟踪虚拟领航者所需的控制量，将两部分控制量加权求和作为编队控制的控制量；

5G网关终端将编队控制的控制量分别下发给无人车和无人机两者的控制器，由控制器控制无人车和无人机的运行。

本发明实施例的方法中，该5G网关终端可为无人机提供5G通信与定位服务，使地面控制中心(云平台)能对其进行远程控制，同时5G网关终端为无人机提供5G无线定位信息，融合TDOA和DOA定位方式，计算5G网关的网关终端坐标，显著提高的测距精度和测角精度。

无人机可借助5G网关与地面控制中心的低时延通信，该5G网关终端扩展了无人机与地面无人车的协作功能，在5G网关终端上分布式地部署队形保持控制器和轨迹跟踪控制器，该队形保持控制器和轨迹跟踪控制器放置在边缘计算平台。通过控制无人机与虚拟领航者的夹角γ实现对无人机高度的单独控制，同时，将无人机与无人车的协同控制转化为水平面的协同控制，实现异构无人系统的协同控制。队形保持控制器和轨迹跟踪控制器用于计算队形保持和轨迹跟踪所需的速度及航向角速度，原有的无人机/车系统控制器根据速度及航向角速度完成底层控制。

具体的，5G网关终端搭载SIM8200 5G模组，为无人机/车提供大带宽、低时延通信。进一步的，如图2所示，基于TDOA和DOA融合定位方式计算5G网关终端坐标的方法为：

TDOA定位：通过5G网关终端到达两个不同基站的间的时间差估计5G网关终端到该两个基站的距离差，再由该距离差与5G网关终端坐标关系解算该5G网关终端的坐标；

DOA定位：通过天线阵列测量两个基站发出的信号到5G网关终端接收的入射角度，建立空间直线方程组，入射角度包括俯仰角和偏航角；通过的空间直线方程组联解出该5G网关终端的坐标；

将TDOA定位所得5G网关终端的坐标和DOA定位所得5G网关终端的坐标加权平均得到最终坐标。

本发明实施例利用FFT(一种DFT的高效算法)将5G大宽带的OFDM(OrthogonalFrequency Division Multiplexing即正交频分复用技术)信号分解为多个相互正交的子载波，以小批量方式将频率相近的子载波归入同一窄带信号，再利用ESPRIT算法对不同频点的窄带信号做波达角度的估计。新一代的5G通讯技术采用高频毫米波和大规模天线阵列，毫米波的高频率和大带宽特性能够显著提高TDOA的测距精度，毫米波的定向波束赋形可以精确获取波达方向，同时大规模天线阵列带来的更高分辨率的波束也能大幅提升DOA的测角精度。

具体的，TDOA定位的方法为：

(1)5G网关终端到达第i个基站的时间表示模型：

其中，δ_ue为5G网关终端的时钟误差，δ_i为基站的时钟误差，c为光速，P_ue为5G网关终端位置，P_i为基站位置，

为测距误差。上述的时间差方程可以消除网关终端的钟差δ_ue，同时依赖于5G毫米波的高频和大带宽特性，测距误差

会得到削减。

(2)作差估计出无人机到达两个基站的距离差D_ij，记D_i＝|P_ue-P_i|为无人机到达基站i的距离，D_ij＝|D_i-D_j|为无人机到基站i,j的距离差。

(3)由上述距离差与无人机坐标关系解算无人机的坐标：

其中，(x_i,y_i,z_i)，(x_j,y_j,z_j)表示基站i,j的坐标。

(4)联立多个步骤(1)-(3)的方程式，解出5G网关终端的坐标(x,y,z)。

通过天线阵列测量两个基站发出的信号到网关接收终端的入射角度，包括俯仰角θ(信号到达天线阵列的方向与z轴的夹角)和偏航角

(信号到达天线阵列的方向与x轴的夹角)，建立空间直线方程组，联解出网关终端的坐标，5G毫米波的定向波束赋形和大规模的天线阵列带来的高分辨率波束能够提高测角精度，进而获得更精确的位置信息。具体的，DOA定位的方法为：

(一)波达角度θ,

的估计

传统的ESPRIT测角算法可以用于窄带信号的测角，5G通信的大带宽需求使得OFDM调制信号的带宽也急剧增大，ESPRIT测角算法难以获得精准的角估计值。这里通过FFT将OFDM宽带信号分解为多个相互正交的子载波，再对子载波的频域信号做角估计，以此改进ESPRIT算法，使其在大带宽情况下仍能获得较为精准的角估计值。

天线整列为均匀线性阵列，阵元间距均为Δ，选取最左侧阵元作为基准阵元，第i个阵元的坐标为x_i(i＝1,2,…,M)；

(1)对于第k个基站发来的第k个宽带信号(k＝1,2,…,K)，第i个阵元与基准阵元的到达时间差为：

相位差为：

(2)第k个信号的导向矢量：

β_k(θ_k)＝[exp(-jφ_1,k)exp(-jφ_2,k)…exp(-jφ_M,k)]^T；

(3)将均匀线性阵列划分为两个子阵列X₁,X₂，子阵列X₁包含线性阵列的前M-1个阵元，子阵列X₂包含线性阵列的后M-1个阵元；两个子阵的结构完全相同，阵列流型间存在旋转不变关系Φ；建立子阵X₁和子阵X₂接收信号矩阵模型：

X₁(t)＝Bs(t)+n₁(t)，

X₂(t)＝BΦs(t)+n₂(t)；

其中，n₁(t)，n₂(t)为输出噪声，s(t)为入射信号矩阵X₁、X₂为子阵列1和子阵列2的接收信号矩阵，

X₁(t)＝[x₁(t),x₂(t),…,x_M-1(t)]^T，X₂(t)＝[x₂(t),x₃(t),…,x_M(t)]^T,s(t)＝[s₁(t),s₂(t),…,s_K(t)]^T

为入射信号；

B矩阵为导向矢量矩阵，其第k列为第k个信号的导向矢量β_k(θ_k)，即B＝[β₁(θ₁),β₂(θ₂)…,β_K(θ_K)]，

Φ＝diag[exp(-jφ₁),exp(-jφ₂),…,exp(-jφ_K)]；

表示第k个信号下，子阵列X₁阵列流型和子阵列X₂阵列流型之间的旋转不变关系；

假设只有1个基站发来信号，即k＝1时，

子阵X₁(t)＝β₁(θ₁)s₁(t)+n₁(t)，也就是说子阵列X₁接收的信号矩阵等于导向矢量乘以入射信号，再加上噪声。

(4)离散采样得到t＝nT₀时刻子阵X₁和子阵X₂接收信号矩阵模型：

X₁(n)＝Bs(n)+n₁(n)，

X₂(n)＝BΦs(n)+n₂(n)，

经FFT运算将当前时刻的子阵X₁和子阵X₂输出信号分解为不同频子载波下的输出信号。按小批量的方式，将频率接近的信号划分为一个批量，该批量组成的信号近似窄带信号下的输出信号，以该批量中的中心频率作为载波信号的频点。记s(n)，n₁(n)，n₂(n)经FFT运算及小批量窄带化后的值为s(w)，n₁(w)，n₂(w)，信号输出为：

X₁(w)＝Bs(w)+n₁(w)，

X₂(w)＝BΦs(w)+n₂(w)；

将上述公式写成矩阵形式为：

(5)计算输出自协方差矩阵R_yy并做特征值分解：

信号空间和阵列流型空间的秩相等，有且仅有一个非奇异矩阵T使得：

E₁,E₂之间关系为E₂＝BΦT＝E₁T^-1ΦT＝E₁Ψ,Ψ＝T^-1ΦT反映两个阵列接收信号子空间的旋转不变关系，Ψ的特征值矩阵为Φ；

(6)计算Ψ，再由阵列流型旋转不变关系Φ计算角估计值

对R_yy做特征值分解求得特征子空间E₁,E₂，进而求得Ψ，

联立两个不同频点的窄带方程，解算第k个基站信号到达天线阵列的俯仰角θ_k和偏航角

再将多个不同频窄带信号求得的θ_k，

做最小均方误差估计，得到俯仰角θ_k、偏航角

进一步提升测角精度；

(二)由波达角度计算5G网关终端坐标

(1)由偏航角

与水平坐标关系

结合两座以上的基站坐标，解算网关终端水平坐标；

(2)由俯仰角θ_k与垂直坐标关系

解算5G网关终端在z方向上的坐标，其中

表示基站k与天线阵列接收平面水平距离。

具体的，将TDOA定位所得5G网关终端的坐标和DOA定位所得5G网关终端的坐标，加权平均得到最终坐标的方法为：

定义TDOA定位和AOA定位下坐标估计值与基站坐标的残差分别为：

其中K_TDOA,K_AOA为两种方法中参与定位的基站数，r_i为网关与低i个基站的距离，

为两种方法下网关的坐标估计值，P_i为基站i的坐标；

记λ为TDOA定位的权值，μ为AOA定位的权值，则权值分别为

5G网关坐标的最终估计值

进一步的，地面惯性系O-XYZ下，无人车仅在XY平面做二维运动，而无人机做三维运动，为实现异构无人系统的统一协同控制，以l-γ-θ方法来描述虚拟领航者与无人机的距离和夹角关系，l表示二者的总距离，γ表示二者垂直方向的夹角，θ表示二者水平方向的夹角；通过虚拟领航者的高度坐标和无人机与虚拟领航者的夹角γ实现对无人机高度的单独控制。

将虚拟领航者和无人机的三维空间坐标投影至XY平面上，在XY平面内对异构无人机系统进行队形保持和轨迹跟踪控制；

在XY平面内，无人机U_a的坐标为(x_a,y_a)、航向角为θ_a，无人车U_g的坐标为(x_g,y_g)、航向角为θ_g；当形成稳定队形时，无人机和无人车之间距离达到期望值

L(t)为t时刻无人机和无人车之间的距离，Lr为期望距离。

进一步的，计算无人机位姿偏差模型U_ae＝(x_e,y_e,θ_e)的方法如下：

(1)在XY平面内建立无人机、无人车运动模型：

其中v_a,v_g分别对应无人机、无人车的速度；

(2)定义L为无人机和无人车之间的距离，Lx＝x_g-x_a、Ly＝y_g-y_a分别表示L在x,y方向上的分量，无人机相对无人车夹角

(3)定义α＝θ_ga+θ_g-θ_a，求解L,θ_ga微分表达式：

(4)定义无人机U_a＝(x_a,y_a,θ_a)相对无人车U_g＝(x_g,y_g,θ_g)的期望位姿为U_ar＝(x_ar,y_ar,θ_ar)，表达式如下：

其中θ_gar表示期望位置相对无人车的夹角；

(5)将世界坐标变换至无人机坐标，并建立无人机位姿偏差模型U_ae：

进一步的，网关队形保持控制器包括速度控制器和航向角速度控制器，以Lyapunov方法设计速度控制器的控制量和航向角速度控制器的控制量，方法如下：

(1)选取系统Lyapunov函数：

(2)设计速度控制器的控制量v_a和航向角速度控制器的控制量

(3)验证稳定性：

k₁,k₂,k₃,k₄,k₅均为正实数，且k₁＝k₂，

可以得到：

其中，U_ae有界，

速度控制器v_a和航向角速度控制器

可以有效降低无人机位姿与其在编队队形中期望位姿的误差。

进一步的，5G网关终端的轨迹跟踪控制器的控制量的计算方法如下：

(一)计算无人机和无人车位姿偏差模型U_e＝(x_e,y_e,θ_e)：

定义无人和无人车当前位姿U_c＝(x_c,y_c,θ_c)，云平台规划的虚拟领航者位姿U_r＝(x_r,y_r,θ_r)，将无人机和无人车所在的世界坐标系变换为无人机和无人车坐标系，计算位姿偏差模型U_e＝(x_e,y_e,θ_e)。

(二)以Lyapunov方法设计轨迹跟踪控制器的速度控制器的控制量和航向角速度控制器的控制量：

(1)选取系统Lyapunov函数：

(2)设计速度控制器的控制量v_c和航向角速度控制器的控制量w_c：

v_c＝v_rcosθ_e+k₁x_e+x_ek₂|y_e|，

(3)验证稳定性：

k₁,k₂,k₃均为正实数，U_e有界，

速度控制器v_c和航向角速度控制器w_c可以有效降低无人机/车与虚拟领航者的位姿误差。

根据本发明实施例的一种基于5G网关的无人机与无人车协作方法的其他构成等以及操作对于本领域普通技术人员而言都是已知的，这里不再详细描述。

在本说明书的描述中，参考术语“实施例”、“示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (9)

1.一种基于5G网关的无人机与无人车协作方法，其特征在于，包括云平台、无人车和无人机，所述无人车和无人机均安装有5G网关终端，所述无人车和无人机均通过5G无线网络与所述云平台通信连接；

所述5G网关包括队形保持控制器和轨迹跟踪控制器；

该方法包括以下步骤：

基于TDOA和DOA融合定位方式计算5G网关终端坐标；

基于5G网关终端坐标，获取所述无人车和所述无人机的实际运行状态，所述实际运行状态包括实际位置、实际速度和实际航向角度；

所述云平台构建有虚拟领航者的无人车和无人机的编队系统模型；

所述5G网关终端将所述无人车和无人机的实际运行状态发送至所述云平台；

所述云平台根据所述无人车和所述无人机的实际运行状态以及执行任务规划虚拟领航者的轨迹，将无人车和无人机的期望运行信息发送给所述5G网关终端，所述期望运行信息包括期望位置、速度和航向角度；

所述5G网关终端的队形保持控制器根据所述期望运行信息计算出无人机和无人车编队保持队形所需的控制量，所述5G网关终端的轨迹跟踪控制器计算出无人车和无人机跟踪虚拟领航者所需的控制量，将两部分控制量加权求和作为编队控制的控制量；

所述5G网关终端将编队控制的控制量分别下发给无人车和无人机两者的控制器，由控制器控制无人车和无人机的运行。

2.根据权利要求1所述的基于5G网关的无人机与无人车协作方法，其特征在于，基于TDOA和DOA融合定位方式计算5G网关终端坐标的方法为：

TDOA定位：通过5G网关终端到达两个不同基站的间的时间差估计5G网关终端到该两个基站的距离差，再由该距离差与5G网关终端坐标关系解算该5G网关终端的坐标；

DOA定位：通过天线阵列测量两个基站发出的信号到5G网关终端接收的入射角度，建立空间直线方程组，所述入射角度包括俯仰角和偏航角；通过所述的空间直线方程组联解出该5G网关终端的坐标；

将TDOA定位所得5G网关终端的坐标和DOA定位所得5G网关终端的坐标，加权平均得到最终坐标。

3.根据权利要求2所述的基于5G网关的无人机与无人车协作方法，其特征在于，所述TDOA定位的方法为：

(1)5G网关终端到达第i个基站的时间表示模型：

其中，δ_ue为5G网关终端的时钟误差，δ_i为基站的时钟误差，c为光速，P_ue为网关终端位置，P_i为基站位置，

为测距误差；

(2)作差估计出无人机到达两个基站的距离差D_ij，记D_i＝|P_ue-P_i|为无人机到达基站i的距离，D_ij＝|D_i-D_j|为无人机到基站i,j的距离差；

(3)由上述距离差与无人机坐标关系解算无人机的坐标：

其中，(x_i,y_i,z_i)，(x_j,y_j,z_j)表示基站i,j的坐标；

(4)联立多个步骤(1)-(3)的方程式，解出5G网关终端的坐标(x,y,z)。

4.根据权利要求3所述的基于5G网关的无人机与无人车协作方法，其特征在于，所述DOA定位的方法为：

(一)波达角度θ,

的估计：

天线整列为均匀线性阵列，阵元间距均为Δ，选取最左侧阵元作为基准阵元，第i个阵元的坐标为x_i(i＝1,2,…,M)；

(1)对于第k个基站发来的第k个宽带信号(k＝1,2,…,K)，第i个阵元与基准阵元的到达时间差为：

相位差为：

(2)第k个信号的导向矢量：

β_k(θ_k)＝[exp(-jφ_1,k)exp(-jφ_2,k)…exp(-jφ_M,k)]^T；

(3)将均匀线性阵列划分为两个子阵列X₁,X₂，子阵列X₁包含线性阵列的前M-1个阵元，子阵列X₂包含线性阵列的后M-1个阵元；两个子阵的结构完全相同，阵列流型间存在旋转不变关系Φ；建立子阵X₁和子阵X₂接收信号矩阵模型：

X₁(t)＝Bs(t)+n₁(t)，

X₂(t)＝BΦs(t)+n₂(t)；

其中，s(t)为入射信号矩阵，n₁(t)，n₂(t)为输出噪声，X₁、X₂为子阵列1和子阵列2的接收信号矩阵，

X₁(t)＝[x₁(t),x₂(t),…,x_M-1(t)]^T，X₂(t)＝[x₂(t),x₃(t),…,x_M(t)]^T,s(t)＝[s₁(t),s₂(t),…,s_K(t)]^T

为入射信号；

Φ＝diag[exp(-jφ₁),exp(-jφ₂),…,exp(-jφ_K)]；

表示第k个信号下，子阵列X₁阵列流型和子阵列X₂阵列流型之间的旋转不变关系；

(4)离散采样得到t＝nT₀时刻子阵X₁和子阵X₂接收信号矩阵模型：

X₁(n)＝Bs(n)+n₁(n)，

X₂(n)＝BΦs(n)+n₂(n)，

记s(n)，n₁(n)，n₂(n)经FFT运算及小批量窄带化后的值为s(w)，n₁(w)，n₂(w)，信号输出为：

X₁(w)＝Bs(w)+n₁(w)，

X₂(w)＝BΦs(w)+n₂(w)；

将上述公式写成矩阵形式为：

(5)计算输出自协方差矩阵R_yy并做特征值分解：

信号空间和阵列流型空间的秩相等，有且仅有一个非奇异矩阵T使得：

E₁,E₂之间关系为E₂＝BΦT＝E₁T^-1ΦT＝E₁Ψ,Ψ＝T^-1ΦT反映两个阵列接收信号子空间的旋转不变关系，Ψ的特征值矩阵为Φ；

(6)计算Ψ，再由阵列流型旋转不变关系Φ计算角估计值：

对R_yy做特征值分解求得特征子空间E₁,E₂，进而求得Ψ，

联立两个不同频点的窄带方程，解算第k个基站信号到达天线阵列的俯仰角θ_k和偏航角

再将多个不同频窄带信号求得的θ_k，

做最小均方误差估计，得到俯仰角θ_k、偏航角

(二)由波达角度计算5G网关终端坐标：

(1)由偏航角

与水平坐标关系

结合两座以上的基站坐标，解算网关终端水平坐标；

(2)由俯仰角θ_k与垂直坐标关系

解算5G网关终端在z方向上的坐标，其中

表示基站k与天线阵列接收平面水平距离。

5.根据权利要求4所述的基于5G网关的无人机与无人车协作方法，其特征在于，将TDOA定位所得5G网关终端的坐标和DOA定位所得5G网关终端的坐标，加权平均得到最终坐标的方法为：

定义TDOA定位和AOA定位下坐标估计值与基站坐标的残差分别为：

其中K_TDOA,K_AOA为两种方法中参与定位的基站数，r_i为网关与低i个基站的距离，

为两种方法下网关的坐标估计值，P_i为基站i的坐标；

记λ为TDOA定位的权值，μ为AOA定位的权值，则权值分别为

所述5G网关坐标的最终估计值

6.根据权利要求1所述的基于5G网关的无人机与无人车协作方法，其特征在于，以l-γ-θ方法来描述虚拟领航者与无人机的距离和夹角关系，l表示二者的总距离，γ表示二者垂直方向的夹角，θ表示二者水平方向的夹角；

将虚拟领航者和无人机的三维空间坐标投影至XY平面上，在XY平面内对异构无人机系统进行队形保持和轨迹跟踪控制；

在XY平面内，无人机U_a的坐标为(x_a,y_a)、航向角为θ_a，无人车U_g的坐标为(x_g,y_g)、航向角为θ_g；当形成稳定队形时，无人机和无人车之间距离达到期望值

L(t)为t时刻无人机和无人车之间的距离，Lr为期望距离。

7.根据权利要求6所述的基于5G网关的无人机与无人车协作方法，其特征在于，计算无人机位姿偏差模型U_ae＝(x_e,y_e,θ_e)的方法如下：

(1)在XY平面内建立无人机、无人车运动模型：

其中v_a,v_g分别对应无人机、无人车的速度；

(2)定义L为无人机和无人车之间的距离，Lx＝x_g-x_a、Ly＝y_g-y_a分别表示L在x,y方向上的分量，无人机相对无人车夹角

(3)定义α＝θ_ga+θ_g-θ_a，求解L,θ_ga微分表达式：

(4)定义无人机U_a＝(x_a,y_a,θ_a)相对无人车U_g＝(x_g,y_g,θ_g)的期望位姿为U_ar＝(x_ar,y_ar,θ_ar)，表达式如下：

其中θ_gar表示期望位置相对无人车的夹角；

(5)将世界坐标变换至无人机坐标，并建立无人机位姿偏差模型U_ae：

8.根据权利要求7所述的基于5G网关的无人机与无人车协作方法，其特征在于，所述网关队形保持控制器包括速度控制器和航向角速度控制器，以Lyapunov方法设计速度控制器的控制量和航向角速度控制器的控制量，方法如下：

(1)选取系统Lyapunov函数：

(2)设计速度控制器的控制量v_a和航向角速度控制器的控制量θ_a：

(3)验证稳定性：

k₁,k₂,k₃,k₄,k₅均为正实数，且k₁＝k₂，

可以得到：

其中，U_ae有界，

9.根据权利要求7所述的基于5G网关的无人机与无人车协作方法，其特征在于，所述5G网关终端的轨迹跟踪控制器的控制量的计算方法如下：

(一)计算无人机和无人车位姿偏差模型U_e＝(x_e,y_e,θ_e)：

定义无人和无人车当前位姿U_c＝(x_c,y_c,θ_c)，所述云平台规划的虚拟领航者位姿U_r＝(x_r,y_r,θ_r)，将无人机和无人车所在的世界坐标系变换为无人机和无人车坐标系，计算位姿偏差模型U_e＝(x_e,y_e,θ_e)：

(二)以Lyapunov方法设计轨迹跟踪控制器的速度控制器的控制量和航向角速度控制器的控制量：

(1)选取系统Lyapunov函数：

(2)设计速度控制器的控制量v_c和航向角速度控制器的控制量w_c：

v_c＝v_rcosθ_e+k₁x_e+x_ek₂|y_e|，

(3)验证稳定性：

k₁,k₂,k₃均为正实数，U_e有界，

Images