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

摘要

本发明提供了一种无人机8字形航迹控制方法，首先给定覆盖任务区域的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 \theta \\ Y_s=Y_t+\frac{1}{2}·R_s·\cos \theta \end{array}\right.& \left\{\begin{array}{c}{X}_n=X_t-\frac{1}{2}·R_n·\sin \theta \\ Y_n=Y_t-\frac{1}{2}·R_n·\cos \theta \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}\\ {\theta }_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}\\ {\theta }_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。