CN104714556A - 无人机智能航向控制方法 - Google Patents

Abstract

本发明提供一种无人机智能航向控制方法，包括：无人机开机自检，记录起飞点处的北东地坐标系下的航向和GPS坐标，在普通遥控模式下起飞；选择初始航向锁定模式或返航点锁定模式；若选择初始航向锁定模式，则以起飞点处的飞行器航向作为初始航向，并在以起飞点原点的直角坐标系下实现智能航向控制；若选择返航点锁定模式，则在以起飞点为极点的极坐标下实现智能航向控制。根据本发明实施例的无人机智能航向控制方法，可以有效减少因控制人员操作不熟练、或飞行距离太远而无法判断当前航向引起误操作带来的无人机坠毁或失联事故，可以大大简化无人机的操作并提高其飞行安全性。

Description

无人机智能航向控制方法

技术领域

本发明涉及无人机技术领域，特别涉及一种无人机智能航向控制方法。

背景技术

无人机因其具有费效比低、部署迅速和零伤亡等特点，被广泛应用于军事和民用领域。控制人员通过传统遥控操作控制无人机的飞行动作，可在灾难场景监测、航拍、搜救、基础设施监察等领域发挥重要作用。

传统遥控中的无人机航向控制是指在正常(无智能航向)模式下，操纵杆的前向与无人机头的前向严格对应，操纵杆的前后左右对应着无人机体前后左右的飞行动作。这种传统的控制方式存在着很大的问题。一方面，当控制人员对无人机控制不够熟悉的情况下，容易造成无人机航向混乱导致无人机坠毁；另一方面，当无人机与控制人员距离超出视距范围(如大范围自然场景航拍、搜索等)，或因复杂天气状况(如云、雾、霾等)影响，控制人员无法判断无人机机头方向时，极易造成误操作，导致无人机坠毁甚至失联。因此，在现有无人机控制系统中引入智能航向控制算法非常重要和紧迫。

现有的无人机控制系统中，在飞行距离超过视距范围时一般采取GPS定位返航的方法，即使用GPS记录飞行器的起飞点，通过地面站或遥控器控制，使得无人机根据已记录的起飞点返航。但因无人机所处的自然环境复杂，极易因电磁干扰或高层建筑遮挡造成GPS定位信号丢失，进而导致无人机坠毁。

发明内容

本发明的目的旨在至少解决上述的技术缺陷之一。

为此，本发明的目的在于提出一种无人机智能航向控制方法。该方法可以提升无人机的飞行安全性，并简化操控性。

为了实现上述目的，本发明公开了一种无人机智能航向控制方法，包括以下步骤：无人机开机自检，记录起飞点处的北东地坐标系下的航向和GPS坐标，并在普通遥控模式下起飞；选择智能航向模式，其中，所述智能航向模式包括初始航向锁定模式和返航点锁定模式；如果选择初始航向锁定模式，则以起飞点处的飞行器航向作为初始航向，并在以起飞点原点的直角坐标系下进行无人机的智能航向控制；如果选择返航点锁定模式，则在以起飞点为极点的坐标系下进行无人机的智能航向控制。

根据本发明实施例的无人机智能航向控制方法，可以有效减少因控制人员操作不熟练、或飞行距离太远而无法判断当前航向引起误操作带来的无人机坠毁或失联事故，可以大大简化无人机的操作并提高其飞行安全性。

另外，根据本发明上述实施例的无人机智能航向控制方法还可以具有如下附加的技术特征：

在一些示例中，所述起飞点处的北东地坐标系下的航向指在北东地坐标系下，无人机机头与正北方向的夹角。

在一些示例中，所述起飞点处的GPS坐标指起飞点的经纬度坐标。

在一些示例中，设初始航向为θ₀，切换到初始航向锁定模式时航向为θ_i，则遥控器的输入参数与控制算法施加在无人机上的控制量之间的关系为：

x′_in＝cos(θ_i-θ₀)·x_in-sin(θ_i-θ₀)·y_in

y′_in＝sin(θ_i-θ₀)·x_in+cos(θ_i-θ₀)·y_in，

其中，遥控器的输入参数包括pitch方向输入参数x_in和roll方向上输入参数y_in，控制算法施加在无人机上的控制量为x′_in和y′_in。

在一些示例中，在返航点锁定模式下，设起飞点处的北东地坐标系下的坐标为(x₀,y₀)，GPS坐标的纬度坐标为lat₀，经度坐标为lon₀，则飞行器飞行过程中当前北东地坐标(x_i,y_i)与GPS坐标(lat_i,lon_i)之间的转换关系为：

$x_i=k\×[\cos \left(\frac{{\mathrm{lat}}_0\×\mathrm{\π}}{180}\right)\×\sin \left(\frac{{\mathrm{lat}}_i\×\mathrm{\π}}{180}\right)-\sin \left(\frac{{\mathrm{lat}}_0\×\mathrm{\π}}{180}\right)\×\cos \left(\frac{{\mathrm{lat}}_i\×\mathrm{\π}}{180}\right)\×{\cos }_{d\_\mathrm{lon}}]\×R_{\mathrm{Earth}}$

$y_i=k\×\cos \left(\frac{{\mathrm{lat}}_i\×\mathrm{\π}}{180}\right)\×\sin (\frac{{\mathrm{lon}}_i\×\mathrm{\π}}{180}-\frac{{\mathrm{lon}}_0\×\mathrm{\π}}{180})\×R_{\mathrm{Earth}},$

其中R_Earth为地球半径，π＝3.1415926，k为可调参数，设：

$c=\arccos [\sin \left(\frac{{\mathrm{lat}}_0\×\mathrm{\π}}{180}\right)\×\sin \left(\frac{{\mathrm{lat}}_i\×\mathrm{\π}}{180}\right)+\cos \left(\frac{{\mathrm{lat}}_0\×\mathrm{\π}}{180}\right)\×\cos \left(\frac{{\mathrm{lat}}_i\×\mathrm{\π}}{180}\right)\×{\cos }_{d\_\mathrm{lon}}],$

则k取值方式为：

if(fabs(c)<δ)     k＝1.0

else        k＝c/sin(c)。

在一些示例中，在以起飞点为极点的极坐标系下，切换到返航点锁定模式时，北东地坐标系下的坐标为(x_i,y_i)，无人机当前偏航角为θ_yaw，此时极角θ_i＝atan2(y_i-y₀,x_i-x₀)，则遥控器输入参数与控制算法施加在无人机上的控制量之间关系为：

x′_in＝cos(θ_i-θ_yaw)·x_in-sin(θ_i-θ_yaw)·y_in

y′_in＝sin(θ_i-θ_yaw)·x_in+cos(θ_i-θ_yaw)·y_in，

其中，遥控器的输入参数包括pitch方向输入参数x_in和roll方向上输入参数y_in，控制算法施加在无人机上的控制量为x′_in和y′_in。

在一些示例中，在返航点锁定模式下，设选择飞行的安全半径为R_safe，则当飞行器与起飞点的距离小于安全半径R_safe时，如果x_in<0，则置x_in＝0。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中，

图1为根据本发明一个实施例的无人机智能航向控制方法的流程图；

图2为根据本发明实施例中无人机控制量输入示意图；

图3为根据本发明实施例的初始航向锁定模式示意图；以及

图4为根据本发明实施例的返航点锁定模式示意图。

具体实施方式

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

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

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

以下结合附图描述根据本发明实施例的无人机智能航向控制方法。

图1为根据本发明一个实施例的无人机智能航向控制方法的流程图。如图1所示，根据本发明一个实施例的无人机智能航向控制方法，包括如下步骤：

步骤S101：无人机开机自检，记录起飞点处的北东地坐标系下的航向和GPS坐标，并在普通遥控模式下起飞。

步骤S102：选择智能航向模式，其中，所述智能航向模式包括初始航向锁定模式和返航点锁定模式。

步骤S103：如果选择初始航向锁定模式，则以起飞点处的飞行器航向作为初始航向，并在以起飞点原点的直角坐标系下进行无人机的智能航向控制。

步骤S104：如果选择返航点锁定模式，则在以起飞点为极点的坐标系下进行无人机的智能航向控制。

根据本发明实施例的无人机智能航向控制方法，可以有效减少因控制人员操作不熟练、或飞行距离太远而无法判断当前航向引起误操作带来的无人机坠毁或失联事故，可以大大简化无人机的操作并提高其飞行安全性。

具体而言，在步骤S101中，无人机开机自检，记录起飞点处的北东地坐标系下的航向和GPS经纬度坐标。其中，起飞点处的北东地坐标系下的航向指在北东地坐标系下，无人机机头与正北方向的夹角。起飞点处的GPS坐标指起飞点的经纬度坐标。

进一步地，在普通遥控模式下，遥控器在俯仰pitch方向输入的控制量为x_in，滚转roll方向上输入的控制量为y_in，如图2所示。

在步骤S102中，可根据无人机操作手具体需求选择智能航向的控制模式，分为初始航向锁定模式和返航点锁定模式。

图3为初始航向锁定模式示意图。初始航向锁定模式下，无人机在以起飞点为原点的直角坐标系下飞行。设初始航向为θ₀，切换到初始航向锁定模式时航向为θ_i，则遥控器的输入参数与控制算法施加在无人机上的控制量之间的关系如以下公式，也就是说：记录飞行器的初始航向为θ₀，飞行器在普通控制模式下从O点飞到A点之后，调整了航向，即图中A点处航向从θ₀调整为θ_i。此时开启初始航向锁定模式，则遥控器输入与控制算法施加在无人机上的控制量关系为：

x′_in＝cos(θ_i-θ₀)·x_in-sin(θ_i-θ₀)·y_in

y′_in＝sin(θ_i-θ₀)·x_in+cos(θ_i-θ₀)·y_in，

其中，遥控器的输入参数包括pitch方向输入参数x_in和roll方向上输入参数y_in，控制算法施加在无人机上的控制量为x′_in和y′_in。

此时通过遥控器输入pitch方向控制量x_in时，飞行器在保持现在的航向θ_i不变的情况下，会按照原始航向θ₀情况下的pitch方向飞行，即从A点向B点的方向飞行；通过遥控器输入roll方向控制量y_in时，飞行器仍保持现在的航向θ_i不变的情况下，会按照原始航向θ₀情况下的roll方向飞行，即从A点向C点的方向飞行。

图4为返航点锁定模式示意图。返航点锁定模式下，无人机在以起飞点为极点的极坐标系下飞行。设起飞点处的北东地坐标系下的坐标为(x₀,y₀)，GPS坐标的纬度坐标为lat₀，经度坐标为lon₀，则飞行器飞行过程中当前北东地坐标(x_i,y_i)与GPS坐标(lat_i,lon_i)之间的转换关系如下，即：记录飞行器起飞位置的GPS坐标(纬度坐标为lat₀，经度坐标为lon₀)，并设起飞点在北东地坐标系下的坐标为(x₀＝0,y₀＝0)。飞行器在普通控制模式下从O点飞到A点之后，读取飞行器在A点的GPS坐标(lat_i,lon_i)，计算A点在北东地坐标(x_i,y_i)：

$x_i=k\&times;[\cos \left(\frac{{\mathrm{lat}}_0\&times;\mathrm{\&pi;}}{180}\right)\&times;\sin \left(\frac{{\mathrm{lat}}_i\&times;\mathrm{\&pi;}}{180}\right)-\sin \left(\frac{{\mathrm{lat}}_0\&times;\mathrm{\&pi;}}{180}\right)\&times;\cos \left(\frac{{\mathrm{lat}}_i\&times;\mathrm{\&pi;}}{180}\right)\&times;{\cos }_{d\_\mathrm{lon}}]\&times;R_{\mathrm{Earth}}$

$y_i=k\&times;\cos \left(\frac{{\mathrm{lat}}_i\&times;\mathrm{\&pi;}}{180}\right)\&times;\sin (\frac{{\mathrm{lon}}_i\&times;\mathrm{\&pi;}}{180}-\frac{{\mathrm{lon}}_0\&times;\mathrm{\&pi;}}{180})\&times;R_{\mathrm{Earth}},$

其中R_Earth为地球半径，π＝3.1415926，k为可调参数，设：

$c=\arccos [\sin \left(\frac{{\mathrm{lat}}_0\&times;\mathrm{\&pi;}}{180}\right)\&times;\sin \left(\frac{{\mathrm{lat}}_i\&times;\mathrm{\&pi;}}{180}\right)+\cos \left(\frac{{\mathrm{lat}}_0\&times;\mathrm{\&pi;}}{180}\right)\&times;\cos \left(\frac{{\mathrm{lat}}_i\&times;\mathrm{\&pi;}}{180}\right)\&times;{\cos }_{d\_\mathrm{lon}}],$

则k取值方式为：

if(fabs(c)<δ)   k＝1.0

else       k＝c/sin(c)

设在A点切换成返航点锁定模式，无人机当前偏航角为θ_yaw，此时极角θ_i＝atan2(y_i-y₀,x_i-x₀)，则遥控器输入(pitch方向输入为x_in，roll方向上输入为y_in)与控制算法施加在无人机上的控制量(设为x′_in和y′_in)关系为：

x′_in＝cos(θ_i-θ_yaw)·x_in-sin(θ_i-θ_yaw)·y_in

y′_in＝sin(θ_i-θ_yaw)·x_in+cos(θ_i-θ_yaw)·y_in

此时通过遥控器输入pitch方向控制量x_in时，飞行器在保持极角θ_i不变的情况下，沿方向飞行。通过遥控器输入roll方向控制量y_in时，飞行器在保持飞行半径R_safe不变的情况下，沿圆弧OC方向飞行，若输入roll方向的控制量为负向y_in时，则在保持飞行半径R_safe不变的情况下，沿圆弧OB方向飞行。

在沿方向飞行时，用户可自行选择飞行的安全半径R_safe，当飞行器与起飞点的距离小于安全半径R_safe时，若x_in<0，则置x_in＝0，防止无人机越过极点向负方向飞行。

根据本发明实施例的无人机智能航向控制方法，可以有效减少因控制人员操作不熟练、或飞行距离太远而无法判断当前航向引起误操作带来的无人机坠毁或失联事故，可以大大简化无人机的操作并提高其飞行安全性。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (7)

1.一种无人机智能航向控制方法，其特征在于，包括以下步骤：

无人机开机自检，记录起飞点处的北东地坐标系下的航向和GPS坐标，并在普通遥控模式下起飞；

选择智能航向模式，其中，所述智能航向模式包括初始航向锁定模式和返航点锁定模式；

如果选择初始航向锁定模式，则以起飞点处的飞行器航向作为初始航向，并在以起飞点原点的直角坐标系下进行无人机的智能航向控制；

如果选择返航点锁定模式，则在以起飞点为极点的坐标系下进行无人机的智能航向控制。

2.根据权利要求1所述的无人机智能航向控制方法，其特征在于，所述起飞点处的北东地坐标系下的航向指在北东地坐标系下，无人机机头与正北方向的夹角。

3.根据权利要求1所述的无人机智能航向控制方法，其特征在于，所述起飞点处的GPS坐标指起飞点的经纬度坐标。

4.根据权利要求1所述的无人机智能航向控制方法，其特征在于，设初始航向为θ₀，切换到初始航向锁定模式时航向为θ_i，则遥控器的输入参数与控制算法施加在无人机上的控制量之间的关系为：

x′_in＝cos(θ_i-θ₀)·x_in-sin(θ_i-θ₀)·y_in

y′_in＝sin(θ_i-θ₀)·x_in+cos(θ_i-θ₀)·y_in，

其中，遥控器的输入参数包括pitch方向输入参数x_in和roll方向上输入参数y_in，控制算法施加在无人机上的控制量为x′_in和y′_in。

5.根据权利要求1所述的无人机智能航向控制方法，其特征在于，在返航点锁定模式下，设起飞点处的北东地坐标系下的坐标为(x₀,y₀)，GPS坐标的纬度坐标为lat₀，经度坐标为lon₀，则飞行器飞行过程中当前北东地坐标(x_i,y_i)与GPS坐标(lat_i,lon_i)之间的转换关系为：

$x_i=k\&times;[\cos \left(\frac{{\mathrm{lat}}_0\&times;\mathrm{\&pi;}}{180}\right)\&times;\sin \left(\frac{{\mathrm{lat}}_i\&times;\mathrm{\&pi;}}{180}\right)-\sin \left(\frac{{\mathrm{lat}}_0\&times;\mathrm{\&pi;}}{180}\right)\&times;\cos \left(\frac{{\mathrm{lat}}_i\&times;\mathrm{\&pi;}}{180}\right)\&times;{\cos }_{d\_\mathrm{lon}}]\&times;R_{\mathrm{Earth}}$

$y_i=k\&times;\cos \left(\frac{{\mathrm{lat}}_i\&times;\mathrm{\&pi;}}{180}\right)\&times;\sin (\frac{{\mathrm{lon}}_i\&times;\mathrm{\&pi;}}{180}-\frac{{\mathrm{lon}}_0\&times;\mathrm{\&pi;}}{180})\&times;R_{\mathrm{Earth}}$

其中R_Earth为地球半径，π＝3.1415926，k为可调参数，设：

$c=\arccos [\sin \left(\frac{{\mathrm{lat}}_0\&times;\mathrm{\&pi;}}{180}\right)\&times;\sin \left(\frac{{\mathrm{lat}}_i\&times;\mathrm{\&pi;}}{180}\right)+\cos \left(\frac{{\mathrm{lat}}_0\&times;\mathrm{\&pi;}}{180}\right)\&times;\cos \left(\frac{{\mathrm{lat}}_i\&times;\mathrm{\&pi;}}{180}\right)\&times;{\cos }_{d\_\mathrm{lon}}],$

则k取值方式为：

$\begin{array}{cc}\mathrm{if}(\mathrm{fabs}\left(c\right)<\mathrm{\&delta;})& k=1.0\\ \mathrm{else}& k=c/\sin \left(c\right)\end{array}.$

6.根据权利要求1所述的无人机智能航向控制方法，其特征在于，在以起飞点为极点的极坐标系下，切换到返航点锁定模式时，北东地坐标系下的坐标为(x_i,y_i)，无人机当前偏航角为θ_yaw，此时极角θ_i＝atan2(y_i-y₀,x_i-x₀)，则遥控器输入参数与控制算法施加在无人机上的控制量之间关系为：

x′_in＝cos(θ_i-θ_yaw)·x_in-sin(θ_i-θ_yaw)·y_in

y′_in＝sin(θ_i-θ_yaw)·x_in+cos(θ_i-θ_yaw)·y_in，

其中，遥控器的输入参数包括pitch方向输入参数x_in和roll方向上输入参数y_in，控制算法施加在无人机上的控制量为x′_in和y′_in。

7.根据权利要求1-6任一项所述的无人机智能航向控制方法，其特征在于，在返航点锁定模式下，设选择飞行的安全半径为R_safe，则当飞行器与起飞点的距离小于安全半径R_safe时，如果x_in<0，则置x_in＝0。