CN105892487A - 一种无人机8字形航迹控制方法 - Google Patents

Abstract

本发明提供了一种无人机8字形航迹控制方法，首先给定覆盖任务区域的8字形航迹，然后计算无人机到8字形航迹中心交点的距离，得到无人机飞行的期望航向，根据无人机到8字形航迹中心交点的方位角，控制飞机进入顺时针盘旋或逆时针盘旋并不断相互切换，直至收到停止盘旋指令。本发明能够避免无人机频繁的切换状态及调整姿态，使盘旋过程平滑、流畅，在有侧风干扰的情况下也能按照预定的航迹进行盘旋飞行。

Description

一种无人机8字形航迹控制方法

技术领域

本发明涉及无人机领域，尤其涉及一种无人机航迹控制方法。

背景技术

无人机在执行侦察、监视、巡逻或勘察等任务时，经常需要在某一区域上空进行8字形的盘旋飞行，以实现对该区域的长时间、全方位覆盖。目前的区域导航方法是依靠航线实现。在航线构成上一般采用多个航点组成一个航点系列，航点之间利用线段连接形成航线，无人机依照这些航点的地理坐标信息进行飞行，从而实现区域导航。

现有的一种实现8字形盘旋的区域导航方法是将多个航点按照近似8字形的轨迹进行布置，形状见图1。无人机沿由多个航点组成的折线进行飞行，实际并没有进行盘旋动作，而是利用不停地转弯、直飞、再转弯来实现近似的8字形航迹，该方法需要飞机频繁的进行状态切换及姿态调整，不利于稳定的跟踪或监视区域内目标。

还有一种不依赖航点的方法，它是利用给定固定的副翼控制量，先控制无人机进行左盘旋，完成一个逆时针圆形飞行后，再切换为右盘旋，完成另一个顺时针圆形飞行，从而实现8字形盘旋。该方法的缺点是飞行中不对无人机位置进行控制，若飞行中受到侧风干扰，飞机将不会按照期望的区域进行飞行，影响任务的执行精度。

发明内容

为了克服现有技术的不足，本发明提供一种无人机8字形航迹的控制方法，实现无人机在一个指定区域内进行8字形盘旋飞行，并能在侧风干扰的情况下保证无人机按期望的航迹进行飞行。

本发明解决其技术问题所采用的技术方案包括以下步骤：

步骤1、给定8字形航迹中心交点的纬经度坐标(B_t，L_t)、顺时针圆盘旋的直径R_s、逆时针圆盘旋的直径R_n和过8字型两个圆心的连线与正北方向的顺时针夹角θ，使得8字形航迹完全覆盖任务区域；

步骤2、将无人机的纬经度坐标(B_u，L_u)转换为平面直角坐标下的位置(X_u，Y_u)，将8字形航迹中心交点的纬经度坐标(B_t，L_t)转换为平面直角坐标下的位置(X_t，Y_t)，计算顺时针圆盘旋的圆心位置(X_s，Y_s)及逆时针盘旋的圆心位置(X_n，Y_n)，

$\begin{array}{cc}\left\{\begin{array}{c}{X}_s=X_t+\frac{1}{2}\·R_s\·\sin \mathrm{\θ}\\ Y_s=Y_t+\frac{1}{2}\·R_s\·\cos \mathrm{\θ}\end{array}\right.& \left\{\begin{array}{c}{X}_n=X_t-\frac{1}{2}\·R_n\·\sin \mathrm{\θ}\\ Y_n=Y_t-\frac{1}{2}\·R_n\·\cos \mathrm{\θ}\end{array}\right.\end{array};$

步骤3、计算无人机距离8字形航迹中心交点的距离若D_t≤max(R_s，R_n)，则进入步骤5，否则执行步骤4；

步骤4、从当前位置沿直线飞往8字形航迹中心交点，无人机飞行的期望航向其中，K_p为比例控制系数，取值为(-0.1)～(-1)，K_i为积分控制系数，取值为(-0.01)～(-0.05)，θ_t为当前位置到8字形航迹中心交点的方位角，D_d为航迹偏差量，根据期望航向得出舵机控制量，控制舵机执行；返回步骤3；

步骤5、计算无人机到8字形航迹中心交点的方位角若(θ)＜θ_s≤(θ+180)，进入步骤6，控制飞机进入逆时针圆盘旋，并设置标志位flag＝1；否则进入步骤8，控制飞机进入顺时针圆盘旋，并设置标志flag＝2；

步骤6、计算无人机到逆时针盘旋圆心的距离D_n和方位角θ_n，

$\left\{\begin{array}{c}{D}_n=\sqrt[2]{{(X_u-X_n)}^2+{(Y_u-Y_n)}^2}\\ {\mathrm{\θ}}_n={\tan }^{-1}\left(\frac{Y_u-Y_n}{X_u-X_n}\right)\end{array}\right.$

无人机飞行的期望航向D_d＝(D_s-R_s)，根据期望航向得出舵机控制量，控制舵机执行；

判断是否收到停止8字盘旋的指令，若收到指令则退出8字形盘旋，否则进入步骤7；

步骤7、计算无人机距离8字形航迹中心交点的距离D_t，若D_t≤L，则设置标志位flag＝2，进入步骤8，否则返回步骤6；其中L为常数，取值为10～200；

步骤8、计算无人机到顺时针盘旋圆心的距离D_s和方位角θ_s，

$\left\{\begin{array}{c}{D}_s=\sqrt[2]{{(X_u-X_s)}^2+{(Y_u-Y_s)}^2}\\ {\mathrm{\θ}}_s={\tan }^{-1}\left(\frac{Y_u-Y_s}{X_u-X_s}\right)\end{array}\right.$

无人机飞行的期望航向D_d＝(D_s-R_s)，根据期望航向得出舵机控制量，控制舵机执行；

判断是否收到停止8字盘旋的指令，若收到指令则退出8字形盘旋，否则进入步骤9；

步骤9、计算无人机距离8字形航迹中心交点的距离D_t，若D_t≤L，则设置标志位flag＝1，进入步骤6，否则返回步骤8。

本发明的有益效果是：

(1)采用了连续变化的控制量计算方法，使得无人机保持基本固定的滚转角进行盘旋飞行，避免无人机频繁的切换状态及调整姿态，使盘旋过程平滑、流畅。

(2)舵机控制量根据无人机相对盘旋圆心的距离进行计算，使得无人机在有侧风干扰的情况下也能按照预定的航迹进行盘旋飞行。

(3)8字形航迹的两个圆形区域大小可调，并且8字形航迹的放置角度可以设置任意角度，区域调整的灵活性比较大，可以利用8字形盘旋飞行完全覆盖不规则的区域。

附图说明

图1是现有8字形盘旋飞行实现示意图；

图2是本发明8字形盘旋飞行航迹选择示意图；

图3是本发明8字形盘旋实现流程图；

图4是控制无人机盘旋的期望航向计算原理图。

具体实施方式

下面结合附图和实施例对本发明进一步说明，本发明包括但不仅限于下述实施例。

本发明包括以下步骤：

步骤1、依据执行任务区域的形状，给定8字形的中心交点纬度、经度坐标(B_t，L_t)，顺时针圆盘旋的直径R_s，逆时针圆盘旋的直径R_n，8字型放置角度θ(过8字型两个圆心的连线与正北方向的顺时针夹角，以正北为零度)，使得8字形轨迹完全覆盖任务区域，从而确定8字形盘旋轨迹。区域选择示意图如附图2所示，图中区域A、B为任务区域。

步骤2：根据高斯坐标转换公式，将由卫星定位所获得的无人机纬度、经度坐标(B_u，L_u)，转换为平面直角坐标下的位置(X_u，Y_u)，并将8字形中心交点坐标转换为平面直角坐标下的位置(X_t，Y_t)，根据两个盘旋直径及放置角度，计算出顺时针圆盘旋的圆心位置(X_s，Y_s)及逆时针盘旋的圆心位置(X_n，Y_n)。

$\left\{\begin{array}{c}{X}_s=X_t+\frac{1}{2}\·R_s\·\sin \mathrm{\θ}\\ Y_s=Y_t+\frac{1}{2}\·R_s\·\cos \mathrm{\θ}\end{array}\right.$

$\left\{\begin{array}{c}{X}_n=X_t-\frac{1}{2}\·R_n\·\sin \mathrm{\θ}\\ Y_n=Y_t-\frac{1}{2}\·R_n\·\cos \mathrm{\θ}\end{array}\right.$

步骤3：计算无人机距离8字形中心交点的距离D_t，

$D_t=\sqrt[2]{{(X_u-X_t)}^2+{(Y_u-Y_t)}^2}$

若D_t≤max(R_s，R_n)，则认为无人机可以进入盘旋轨道，直接进入步骤5执行，否则执行步骤4。

步骤4：引导无人机从当前位置沿直线飞往8字形中心交点。无人机飞行的期望航向根据以下公式计算：

其中

$\left\{\begin{array}{c}{\mathrm{\θ}}_t={\tan }^{-1}\left(\frac{Y_u-Y_t}{X_u-X_t}\right)\\ D_d=(Y_u-Y_t)\·{\mathrm{cos\θ}}_t-(X_u-X_t)\·{\mathrm{sin\θ}}_t\end{array}\right.$

其中，K_p为比例控制系数，该系数经验值范围一般为(-0.1)～(-1)，K_i为积分控制系数，该系数经验值范围一般为(-0.01)～(-0.05)，θ_t为无人机到中心交点的方位角，D_d为航迹偏差量。将计算得出的期望航向输出至横侧向控制器并得出舵机控制量，最后送至舵机进行执行。

此步骤中循环执行步骤3进行判断，直至进入步骤5执行。

步骤5：计算无人机到8字盘旋中心交点的方位角θ_t：

${\mathrm{\θ}}_s={\tan }^{-1}\left(\frac{Y_u-Y_t}{X_u-X_t}\right)$

若(θ)＜θ_s≤(θ+180)，进入步骤6，控制飞机进入逆时针圆盘旋，并设置标志位flag＝1；否则进入步骤8，控制飞机进入顺时针圆盘旋，并设置标志flag＝2。

步骤6：计算无人机到逆时针盘旋圆心的距离D_n，方位角θ_n：

$\left\{\begin{array}{c}{D}_n=\sqrt[2]{{(X_u-X_n)}^2+{(Y_u-Y_n)}^2}\\ {\mathrm{\θ}}_n={\tan }^{-1}\left(\frac{Y_u-Y_n}{X_u-X_n}\right)\end{array}\right.$

无人机飞行的期望航向根据以下公式计算：

将计算得出的期望航向输出至横侧向控制器并得出舵机控制量，最后送至舵机进行执行。

判断是否收到停止8字盘旋的指令，若收到指令，则退出8字形盘旋，否则进入步骤7。

步骤7：计算无人机距离8字形中心交点的距离D_t，若D_t≤L，则设置标志位flag＝2，进入步骤8，否则利用步骤6的方式计算期望航向进行控制。其中L为固定的常数，该常数经验取值范围一般为10～200。

步骤8：计算无人机到顺时针盘旋圆心的距离D_s，方位角θ_s：

$\left\{\begin{array}{c}{D}_s=\sqrt[2]{{(X_u-X_s)}^2+{(Y_u-Y_s)}^2}\\ {\mathrm{\θ}}_s={\tan }^{-1}\left(\frac{Y_u-Y_s}{X_u-X_s}\right)\end{array}\right.$

无人机飞行的期望航向根据以下公式计算：

将计算得出的期望航向输出至横侧向控制器并得出舵机控制量，最后送至舵机进行执行。

判断是否收到停止8字盘旋的指令，若收到指令，则退出8字形盘旋，否则进入步骤9。

步骤9：计算无人机距离8字形中心交点的距离D_t，若D_t≤L，则设置标志位flag＝1，进入步骤6，否则利用步骤8的方式计算期望航向进行控制。

本发明的实施例包括以下步骤：

步骤1：给定8字形的中心交点坐标(B_t，L_t)，顺时针圆盘旋的直径R_s，逆时针圆盘旋的直径R_n，8字型放置角度θ，从而确定8字形盘旋区域。

根据需要盘旋侦察的地形，选取盘旋中心交点坐标、两个圆盘旋的直径以及放置角度，交点坐标取WGS-84坐标系下的经纬度，盘旋直径应大于等于无人机的最小盘旋半径，根据实施的无人机性能，限制盘旋直径最小为1km，放置角度θ规定正北方向为0度，顺时针方向为角度增加方向，最大角度限制为359.9度，该角度θ定义为与正北方向的夹角。

步骤2：根据高斯坐标转换公式，将由卫星定位所获得的无人机位置坐标(B_u，L_u)，转换为平面直角坐标下的位置(X_u，Y_u)，并将8字形中心交点坐标转换为平面直角坐标下的位置(X_t，Y_t)。

将无人机盘旋飞行实施侦察的区域近似看成平面，根据卫星接收机获得的无人机位置信息，对参心空间直角坐标系进行高斯-吕克投影，转为高斯平面直角坐标系，以下的运算均在平面直角坐标系下进行。

根据两个盘旋直径及放置角度，利用三角运算，计算出顺时针圆盘旋的圆心位置(X_s，Y_s)及逆时针盘旋的圆心位置(X_n，Y_n)。

$\left\{\begin{array}{c}{X}_s=X_t+\frac{1}{2}\·R_s\·\sin \mathrm{\θ}\\ Y_s=Y_t+\frac{1}{2}\·R_s\·\cos \mathrm{\θ}\end{array}\right.$

$\left\{\begin{array}{c}{X}_n=X_t-\frac{1}{2}\·R_n\·\sin \mathrm{\θ}\\ Y_n=Y_t-\frac{1}{2}\·R_n\·\cos \mathrm{\θ}\end{array}\right.$

步骤3：计算无人机距离8字形中心交点的距离D_t，

$D_t=\sqrt[2]{{(X_u-X_t)}^2+{(Y_u-Y_t)}^2}$

若D_t≤max(R_s，R_n)，则认为无人机可以进入盘旋轨道，直接进入步骤5执行，否则执行步骤4。

步骤4：引导无人机从当前位置沿直线飞往8字形中心交点。

此阶段无人机沿当前位置为起点，以中心交点为终点进行直线飞行，规定沿给定的航向看去，无人机位置偏离给定航线右方为正向偏差，反之为负向偏差。飞行的期望航向根据以下公式计算：

其中

$\left\{\begin{array}{c}{\mathrm{\θ}}_t={\tan }^{-1}\left(\frac{Y_u-Y_t}{X_u-X_t}\right)\\ D_d=(Y_u-Y_t)\·{\mathrm{cos\θ}}_t-(X_u-X_t)\·{\mathrm{sin\θ}}_t\end{array}\right.$

其中，K_p为比例控制系数，取值为-0.5，K_i为积分控制系数，取值为-0.02，θ_t为无人机到中心交点的方位角，该角度应按照位置关系进行换算，使其符合指向正北方向为0度，顺时针方向为角度增加方向，并且保证0°＜＜θ_t＜360°，D_d为航迹偏差量，当航迹偏差为0时，期望航向即为给定的航向。将计算得出的期望航向输出至横侧向控制器并得出舵机控制量，最后送至舵机进行执行，保证飞机沿给定的航向进行飞行。

此步骤中循环执行步骤3进行判断，直至进入步骤5执行。

步骤5：计算无人机到8字盘旋中心交点的方位角θ_t：

${\mathrm{\θ}}_s={\tan }^{-1}\left(\frac{Y_u-Y_t}{X_u-X_t}\right)$

若(θ)＜θ_s≤(θ+180)，进入步骤6，控制飞机进入逆时针圆盘旋，并设置标志位flag＝1；否则进入步骤8，控制飞机进入顺时针圆盘旋，并设置标志flag＝2。

步骤6：计算无人机到逆时针盘旋圆心的距离D_n，方位角θ_n。

$\left\{\begin{array}{c}{D}_n=\sqrt[2]{{(X_u-X_n)}^2+{(Y_u-Y_n)}^2}\\ {\mathrm{\θ}}_n={\tan }^{-1}\left(\frac{Y_u-Y_n}{X_u-X_n}\right)\end{array}\right.$

计算无人机当前位置到逆时针盘旋圆心的方位角θ_n，由于需要无人机进行逆时针圆盘旋，无人机在该时刻的给定航向应该为(θ_s+90°)，航迹偏差的定义与步骤4中的定义一致，无人机飞行的期望航向根据以下公式计算：

其中，K_p为比例控制系数，取值为-0.4，K_i为积分控制系数，取值为-0.05，D_d为航迹偏差量，将计算得出的期望航向输出至横侧向控制器并得出舵机控制量，最后送至舵机进行执行，控制飞机进行逆时针圆盘旋。

判断是否收到停止8字盘旋的指令，若收到指令，则退出8字形盘旋，否则进入步骤7。

步骤7：计算无人机距离8字形中心交点的距离D_t，计算方式与步骤3相同，若D_t≤100m，则设置标志位flag＝2，标志无人机第二次到达交点，需要进行切换顺时针圆盘旋，进入步骤8，否则利用步骤6的方式计算期望航向进行控制。

步骤8：计算无人机到顺时针盘旋圆心的距离D_s，方位角θ_s。

$\left\{\begin{array}{c}{D}_s=\sqrt[2]{{(X_u-X_s)}^2+{(Y_u-Y_s)}^2}\\ {\mathrm{\θ}}_s={\tan }^{-1}\left(\frac{Y_u-Y_s}{X_u-X_s}\right)\end{array}\right.$

计算无人机当前位置到顺时针盘旋圆心的方位角θ_s，由于需要无人机进行顺时针圆盘旋，无人机在该时刻的给定航向应该为(θ_s-90°)，设定若无人机位置在该期望的圆形轨迹之外为正向偏差，反之则为负向偏差，无人机飞行的期望航向根据以下公式计算：

其中，其中，K_p为比例控制系数，取值为-0.4，K_i为积分控制系数，取值为-0.05，D_d为航迹偏差量。将计算得出的期望航向输出至横侧向控制器并得出舵机控制量，最后送至舵机进行执行，控制飞机进行顺时针圆盘旋。

判断是否收到停止8字盘旋的指令，若收到指令，则退出8字形盘旋，否则进入步骤9。

步骤9：计算无人机距离8字形中心交点的距离D_t，计算方式与步骤3相同，若D_t≤100m，则设置标志位flag＝1，并进入步骤5，重新进行逆时针圆盘旋，否则利用步骤8的方式计算期望航向进行控制。

从而实现无人机8字形航迹的控制。

Claims (1)

1.一种无人机8字形航迹控制方法，其特征在于包括下述步骤：

步骤1、给定8字形航迹中心交点的纬经度坐标(B_t，L_t)、顺时针圆盘旋的直径R_s、逆时针圆盘旋的直径R_n和过8字型两个圆心的连线与正北方向的顺时针夹角θ，使得8字形航迹完全覆盖任务区域；

步骤2、将无人机的纬经度坐标(B_u，L_u)转换为平面直角坐标下的位置(X_u，Y_u)，将8字形航迹中心交点的纬经度坐标(B_t，L_t)转换为平面直角坐标下的位置(X_t，Y_t)，计算顺时针圆盘旋的圆心位置(X_s，Y_s)及逆时针盘旋的圆心位置(X_n，Y_n)，

$\begin{array}{cc}\left\{\begin{array}{c}{X}_s=X_t+\frac{1}{2}\·R_s\·\sin \mathrm{\θ}\\ Y_s=Y_t+\frac{1}{2}\·R_s\·\cos \mathrm{\θ}\end{array}\right.& \left\{\begin{array}{c}{X}_n=X_t-\frac{1}{2}\·R_n\·\sin \mathrm{\θ}\\ Y_n=Y_t-\frac{1}{2}\·R_n\·\cos \mathrm{\θ}\end{array}\right.\end{array};$

步骤3、计算无人机距离8字形航迹中心交点的距离若D_t≤max(R_s，R_n)，则进入步骤5，否则执行步骤4；

步骤4、从当前位置沿直线飞往8字形航迹中心交点，无人机飞行的期望航向其中，K_p为比例控制系数，取值为(-0.1)～(-1)，K_i为积分控制系数，取值为(-0.01)～(-0.05)，θ_t为当前位置到8字形航迹中心交点的方位角，D_d为航迹偏差量，根据期望航向得出舵机控制量，控制舵机执行；返回步骤3；

步骤5、计算无人机到8字形航迹中心交点的方位角若(θ)＜θ_s≤(θ+180)，进入步骤6，控制飞机进入逆时针圆盘旋，并设置标志位flag＝1；否则进入步骤8，控制飞机进入顺时针圆盘旋，并设置标志flag＝2；

步骤6、计算无人机到逆时针盘旋圆心的距离D_n和方位角θ_n，

$\left\{\begin{array}{c}{D}_n=\sqrt[2]{{(X_u-X_n)}^2+{(Y_u-Y_n)}^2}\\ {\mathrm{\θ}}_n={\tan }^{-1}\left(\frac{Y_u-Y_n}{X_u-X_n}\right)\end{array}\right.$

无人机飞行的期望航向根据期望航向得出舵机控制量，控制舵机执行；

判断是否收到停止8字盘旋的指令，若收到指令则退出8字形盘旋，否则进入步骤7；

步骤7、计算无人机距离8字形航迹中心交点的距离D_t，若D_t≤L，则设置标志位flag＝2，进入步骤8，否则返回步骤6；其中L为常数，取值为10～200；

步骤8、计算无人机到顺时针盘旋圆心的距离D_s和方位角θ_s，

$\left\{\begin{array}{c}{D}_s=\sqrt[2]{{(X_u-X_s)}^2+{(Y_u-Y_s)}^2}\\ {\mathrm{\θ}}_s={\tan }^{-1}\left(\frac{Y_u-Y_s}{X_u-X_s}\right)\end{array}\right.$

无人机飞行的期望航向根据期望航向得出舵机控制量，控制舵机执行；

判断是否收到停止8字盘旋的指令，若收到指令则退出8字形盘旋，否则进入步骤9；

步骤9、计算无人机距离8字形航迹中心交点的距离D_t，若D_t≤L，则设置标志位flag＝1，进入步骤6，否则返回步骤8。