[发明专利]一种自动跟随信号源的无人机控制方法

摘要

本发明提供了一种自动跟随信号源的无人机控制方法，运用无线电测向技术中的相关干涉法，采用基于互谱密度函数的相位差测量算法来对信号源测向，用天线阵的结构建立相位差的样本库，并附其对应俯仰角和方位角，以缩短测向时间，对测量到的相位差进行数据匹配，得到真实俯仰角和方位角，以实现目标的定位，利用无人机路径制导算法逼近人的运动轨迹。本发明具有工作稳定、步骤简单有效以及可适用实时控制等特点。在实际应用中，使用该控制方法的自动跟随系统可以使警用无人机跟随警察进行执法记录，可以节省大量的警力资源，非常适合在日常警务实战中推广应用。

主权项

1.一种自动跟随信号源的无人机控制方法，其特征在于包括下述步骤：步骤1：分析来波；设两路无线电信号表达式为：其中，s₁(t)和s₂(t)分别为两路宽带信号表达式，频率为f₀，和分别为两个信号的初相位，A为信号的幅度；两路宽带信号的互谱密度函数为式中，“*”表示信号的共轭；s₁(t)和s₂(t)分别为两路宽带信号表达式；公式(2)进行傅里叶变换后得到：其中F₁(f)和F₂(f)分别为两信号的频谱密度，v为无线电波传播速度；由式(3)可知，在频率分量f处的相位差为互谱密度函数在频率f处的相位；将式(3)以复数的形式表示为：E₁₂(f)＝|F₁(f)|²[cos(2πfΔt)+i·sin(2πfΔt)]    (4)记I(f)＝cos(2πfΔt)和Q(f)＝sin(2πfΔt)分别为互谱密度函数的实部和虚部，Δt为采样时间间隔，则频率f处的相位差计算公式为：n元天线阵即产生n组相位差，方位角为俯仰角为θ的无线电波在天线阵的各阵元上相对于选定的参考点产生n组相位差，通过公式(1)～(5)计算得出n组相位差，组成测量值样本库，记作步骤2：计算信号方位角与俯仰角；建立相位差表，生成信号源，进行匹配运算，得到检测结果；步骤2.1：建立信号相位差理论值的总样本库；设天线阵的直径为R，以阵心为参考点，第n个阵元与参考点的相位差为：式(6)中，θ为无人机相对于人的俯仰角，为无人机相对于人的方位角，λ为电磁波的波长，f_c为电磁波的频率，c为电磁波的传播速度；测向设备的方位角的范围为0°～360°，俯仰角范围为0°～90°，以1°划分，每个阵元所接收的无线电信号的方位角和俯仰角有360×90种情况，组成一个360×90的二维矩阵，将该矩阵中的数据通过公式(5)两两进行计算，得到一个360×90的二维矩阵，n元天线阵中共有n个阵元，因此得到一个360×90×n的三维矩阵，三维矩阵中的数据为各阵元与参考点所接收的信号之间的相位差，记该矩阵为理论值总样本库；步骤2.2：进行数据匹配取信号相位差理论值总样本库中方位角为俯仰角为θ的信号的数据，建立理论值子样本库，n元天线阵即产生1×n的矩阵，记作Φ，共得到360×90个子样本库；根据互相关系数进行样本库匹配的相关系数计算公式如下：其中，为测量值样本库；Φ为理论值子样本库；为理论值子样本库和测量值样本库中数据的协方差，D(Φ)和分别为理论值子样本库和测量值样本库中数据的方差；由于共有360×90个子样本库，所得到的相关系数共有360×90个，进行最大值搜索，得到相关系数的最大值，相关系数的最大值所对应的方位角俯仰角θ为所求的信号方位角与俯仰角θ，该方位角与俯仰角θ与真实值的误差在1°内；步骤3：进行导向计算；步骤3.1：计算无人机的初始坐标；设人的初始位置A₀为坐标原点O(0,0)，无人机初始位置为B₀，x轴指向无人机初始运动方向，建立坐标系，设无人机的初始坐标为(x₀,y₀)，为方位角的绝对值，则无人机的初始坐标为：步骤3.2：无人机运动圆心位置与加速度计算无人机的飞行高度为固定值h，则人与无人机连线的在xoy平面上的投影向心加速度在人、无人机、圆周运动圆心组成的三角形中得到其中，v为无人机速度，a为向心加速度，R为圆周运动半径，O为圆周运动圆心，L₁为无人机和人所在位置的连线，η为无人机速度与L1的夹角；无人机运动圆心位置计算步骤如下：以无人机当前坐标为坐标原点(0,0)，x轴指向无人机初始运动方向，建立直角坐标系；以人与无人机连线的在xoy平面上的投影L₁为目标路径，在目标路径上选择一个参考点B，参考点B位于坐标系四个象限内或坐标轴上，将位于x轴正半轴上的点看作位于第一象限，位于y轴正半轴上的点看作位于第二象限，位于x轴负半轴上的点看作位于第三象限，位于y轴负半轴上的点看作位于第四象限，因此，参考点B在坐标轴上的位置共有位于第一、二、三、四象限四种情况；设参考点与无人机连线与无人机夹角为α，当飞行方向指向第一象限时，当0＜α≤180°时，圆心在第二象限；当180°＜α≤360°时，圆心在第四象限；当飞行方向指向第二象限时，当0＜α≤180°时，圆心在第一象限；当180°＜α≤360°时，圆心在第三象限；当飞行方向指向第三象限时，当0＜α≤180°时，圆心在第二象限；当180°＜α≤360°时，圆心在第四象限；当飞行方向指向第四象限时，当0＜α≤180°时，圆心在第一象限；当180°＜α≤360°时，圆心在第三象限；圆心坐标的计算过程为：计算出：Δx＝|Rsinα|，Δy＝|Rcosα|            (9)；当圆心在第一象限时，圆心坐标为(Δx,Δy)；当圆心在第二象限时，圆心坐标为(‑Δx,Δy)；当圆心在第三象限时，圆心坐标为(‑Δx,‑Δy)，当圆心在第四象限时，圆心坐标为(Δx,‑Δy)；步骤3.3：得到圆心坐标后，根据已知的无人机初始坐标，向无人机发出改变加速度的指令，使无人机逼近参考点，计算步骤如下：设无人机飞行速度为v_f，无人机的运动时间为发送信号得间隔Δt，无人机的旋转角度取决于时间间隔的大小，实际旋转角度为旋转后的实际坐标并不一定与参考点重合；设运动后实际坐标为(x₁，y₁)，则：x₁＝(x₀‑O₁)cos(u)‑(y₀‑O₂)sin(u)+O₁,y₁＝(x₀‑O₁)sin(u)+(y₀‑O₂)cos(u)+O₂                 (10)；无人机原坐标是(x0，y0)，O₁是旋转圆心的横坐标，O₂是旋转圆心的纵坐标；以得到的新坐标(x1，y1)为当前的初始坐标，每间隔时间T后重复步骤1、步骤2和步骤3，当L1与无人机速度方向夹角的绝对值|β|≤1°时，圆周运动半径R趋近于无穷大，a趋近于0，无人机做匀速直线运动，此时暂停循环；当L1与无人机速度方向夹角的绝对值|β|＞1°时，再次开始重复步骤1、步骤2、步骤3，不断循环执行步骤1、步骤2和步骤3，即可实现无人机的目标跟随。