一种基于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个基站的时间表示模型：

其中，δ

(2)作差估计出无人机到达两个基站的距离差D

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

其中，(x

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

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

(一)波达角度θ,

天线整列为均匀线性阵列，阵元间距均为Δ，选取最左侧阵元作为基准阵元，第i个阵元的坐标为x

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

相位差为：

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

β

(3)将均匀线性阵列划分为两个子阵列X

X

X

其中，s(t)为入射信号矩阵，n

X

X

Φ＝diag[exp(-jφ

(4)离散采样得到t＝nT

X

X

记s(n)，n

X

X

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

(5)计算输出自协方差矩阵R

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

E

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

对R

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

(1)由偏航角

(2)由俯仰角θ

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

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

其中K

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

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

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

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

在XY平面内，无人机U

进一步的，计算无人机位姿偏差模型U

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

其中v

(2)定义L为无人机和无人车之间的距离，Lx＝x

(3)定义α＝θ

(4)定义无人机U

其中θ

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

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

(1)选取系统Lyapunov函数：

(2)设计速度控制器的控制量v

(3)验证稳定性：

可以得到：

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

(一)计算无人机和无人车位姿偏差模型U

定义无人和无人车当前位姿U

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

(1)选取系统Lyapunov函数：

(2)设计速度控制器的控制量v

v

(3)验证稳定性：

k

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

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个基站的时间表示模型：

其中，δ

(2)作差估计出无人机到达两个基站的距离差D

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

其中，(x

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

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

(一)波达角度θ,

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

天线整列为均匀线性阵列，阵元间距均为Δ，选取最左侧阵元作为基准阵元，第i个阵元的坐标为x

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

相位差为：

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

β

(3)将均匀线性阵列划分为两个子阵列X

X

X

其中，n

X

为入射信号；

B矩阵为导向矢量矩阵，其第k列为第k个信号的导向矢量β

Φ＝diag[exp(-jφ

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

子阵X

(4)离散采样得到t＝nT

X

X

经FFT运算将当前时刻的子阵X

X

X

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

(5)计算输出自协方差矩阵R

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

E

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

对R

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

(1)由偏航角

(2)由俯仰角θ

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

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

其中K

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

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

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

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

在XY平面内，无人机U

进一步的，计算无人机位姿偏差模型U

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

其中v

(2)定义L为无人机和无人车之间的距离，Lx＝x

(3)定义α＝θ

(4)定义无人机U

其中θ

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

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

(1)选取系统Lyapunov函数：

(2)设计速度控制器的控制量v

(3)验证稳定性：

可以得到：

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

(一)计算无人机和无人车位姿偏差模型U

定义无人和无人车当前位姿U

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

(1)选取系统Lyapunov函数：

(2)设计速度控制器的控制量v

v

(3)验证稳定性：

k

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

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

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