CN106802669A - 一种基于速度矢量合成的航迹规划算法及其飞行器 - Google Patents

Abstract

本发明提供一种基于速度矢量合成的航迹规划算法及其飞行器，属于控制领域，通过检测环境中的风的大小和方向，在根据遥控装置设定的飞行器本体飞行的速度，飞行器本体飞行根据风的大小和方向与设定的飞行速度从而进一步算出飞行器本体的实际飞行的速度和方向，从而能更好根据人们的需要进行飞行器的目标搜索，提高目标寻找的速度和效率。

Description

一种基于速度矢量合成的航迹规划算法及其飞行器

技术领域

本发明涉及控制领域，特别是涉及一种基于速度矢量合成的航迹规划算法及其飞行器。

背景技术

当前，无人机已经逐渐出现于市场，特别是在跟踪和航拍上的应用，如跟随航拍、电影拍摄、植物保护、规划路线、自然灾害探测等涉及到自动拍摄的场合。常用的gps四轴跟随无人机主要通过跟随目标的状态信息，以及期望的相对距离计算得到无人机的期望位置状态，并与当下状态进行比较，根据算法得出修正量。这样的算法架构可以避免将相对距离看做系统状态所需要的数据转换过程，也实现了一部分的跟随功能。但是在很多情况下跟随状态却受到环境因素的影响很大，在定位跟随下不能为用户取得比较优秀的数据。

另外，无人机不是仅仅只能会是一个被动的牵引分子，而应该是一个完整的自己的思想，能够自我判断取得较好的路径去跟随目标。

发明内容

本发明提供一种基于速度矢量合成的航迹规划算法及其飞行器，解决现有无人机处于跟随状态下被风影响大的问题。

本发明通过以下技术方案解决上述问题：

一种基于速度矢量合成的航迹规划算法，包括如下步骤：

步骤1：用户通过遥控装置控制飞行器本体进入空中悬停模式；

步骤2：遥控装置上的风速风向传感器采集风速的大小v₁和风速的方向θ₁发送给飞行器，其中，v₁为采集的风速的大小，θ₁为风的方向角；

步骤3：飞行器本体接收遥控装置传来的风速的大小v₁和风的方向角θ₁后，飞行器本体把接收的风速的大小v₁与原设定的风速初始值进行比较；

步骤4：飞行器本体根据飞行器气压计采集回来的气压算出飞行器本体相对地面的高度d_f，遥控装置根据遥控气压计采集回来的气压算出遥控装置相对地面的高度d_y，从而可以算出飞行器本体相对遥控装置竖直上的高度为d_j＝d_f+d_y，其中d_y为飞行器本体相对地面的高度，d_y为遥控装置相对地面的高度，d_j为飞行器本体相对遥控装置竖直上的高度；

步骤5：遥控装置通过遥控GPS模块采集遥控装置的经纬度数据(lati1,long1)，飞行器本体通过飞行器GPS模块采集飞行器本体的经纬度数据(lati2,long2)，其中，lati1为遥控装置的经度数据，long1为遥控装置的维度数据，lati2为飞行器本体的经度数据，long2为飞行器本体的维度数据；

步骤6：根据遥控装置纬度数据和飞行器本体经纬度数据解算出飞行器本体与遥控装置的相对距离d_x和相对方向角θ_x，其中，d_x为飞行器本体与遥控装置的相对距离，θ_x为飞行器本体与遥控装置的相对方向角；

步骤7：根据飞行器本体与遥控装置的相对距离d_x和飞行器本体相对遥控装置竖直上的高度d_j算出飞行器本体与遥控装置的绝对距离其中，d_fs为飞行器本体与遥控装置的绝对距离；

步骤8：根据目标点与遥控装置的绝对距离d_fs和飞行器本体与遥控装置的绝对距离d_fj算出飞行器本体与目标点的距离s；

步骤9：当步骤3中风速的大小v₁小于等于原设定的风速初始值时，用户通过遥控器根据飞行器本体与遥控装置的相对方向角θ_x来确定飞行器本体的飞行方向，当风速的大小v₁大于原设定的风速初始值时，进入步骤12；

步骤10：飞行器本体运用PID算法来调控飞行器的输出速度，输出速度大小为v₂，v₂为飞行器本体的输出速度；

步骤11：相隔设定的时间后，返回步骤4，直到飞行到达目标点；

步骤12:根据风速的大小v₁和风速的方向θ₁，既风速为再由设定的飞行器本体的输出速度从而算出飞行器本体的飞行速度其中，为风速的矢量，为飞行器本体的输出速度矢量，为飞行器本体实际飞行速度矢量；

步骤13：飞行器本体运用PID算法来调控飞行器的速度，速度大小为v₂，v₂为飞行器本体的输出速度；

步骤14：相隔设定的时间后，返回步骤4，直到飞行到达目标点。

上述方案中，优选的是步骤4中算出飞行器本体相对地面的高度d_f和遥控装置相对地面的高度d_y的过程均为通过气压计采集其空间位置的气压，把采集的气压减去大地表层的气压，再根据气压与高度的比例关系求出高度。

上述方案中，优选的是步骤6算出飞行器本体与遥控装置的相对距离d_x和相对方向角θ_x的过程包括如下步骤，

步骤6.1：根据步骤5采集的经纬度算出飞行器本体与遥控装置两点的经度弧度长度：long＝(long1-long2)×π/180，其中，π为圆周率，long1为遥控装置点的经度，long2为飞行器本体经度，long为飞行器本体与遥控装置两点的经度弧度长度差；

步骤6.2：根据步骤5采集的经纬度算出飞行器本体与遥控装置两点的纬度弧度长度：lati＝(lati1-lati2)×π/180；π为圆周率，lati1为遥控装置点的维度，lati2为飞行器本体维度，lati为飞行器本体与遥控装置两点的维度弧度长度差；

步骤6.3：算出飞行器本体与遥控装置两点的相对距离为：

，

其中，long飞行器本体与遥控装置两点的经度弧度长度差，lati为为飞行器本体与遥控装置两点的维度弧度长度差，lati1为为遥控装置点的维度，lati2为飞行器本体维度，R为地球半径，π为圆周率；

步骤6.4：算出飞行器本体与遥控装置两点的相对角度为：θ_x＝mod(arctan2(R×cos(lati1×(lon2-lon1))，R×(lati2-lati1))，2×π)，mod为坐标点表示式。

上述方案中，优选的是步骤8算出飞行器本体与目标点的距离s的公式为：

其中π为圆周率。

上述方案中，优选的是步骤10中的PID算法为：

其中，u(t)为控制飞行器本体的输出速度v₂；e(t)为控制器的输入(是初始飞行器与目标点的距离s与当前飞行器与目标点的距离s_s之差，即e(t)＝S-S_s)，Kp为控制器的比例放大系数，T_i为控制器的积分时间，T_d为控制器的微分时间，t为时间。

上述方案中，优选的是步骤12中算出飞行器本体实际飞行速度矢量的过程为：先算出飞行器本体飞行的方向：θ₂为飞行器本体飞行的方向角，飞行器本体实际飞行速度大小为：

上述方案中，优选的是步骤13中的PID算法为：

其中，u(t)为控制飞行器的输出速度v₂；e(t)为控制器的输入(即e(t)＝v-v₂)，Kp为控制器的比例放大系数，T_i为控制器的积分时间，T_d为控制器的微分时间，t为时间。

根据上述的一种基于速度矢量合成的航迹规划算法的飞行器，包括飞行器本体和遥控装置，遥控装置与飞行器本体无线连接；

所述飞行器本体包括机架和控制电路单元，控制电路单元包括电机、电机驱动模块、激光模块、GPS定位模块、气压计、控制器模块、磁力计、显示模块、陀螺仪和控制无线模块；

所述激光模块、GPS定位模块、气压计、磁力计和陀螺仪的输出端与控制器模块连接；所述GPS定位模块用于获取飞行器本体的经度和维度数据；所述气压计用以检测飞行器本体外的气压大小从而算出飞行器本体与地相距高度；所述陀螺仪用于检测飞行器本体的平衡状态；

所述显示模块的输入端与控制器模块连接；所述控制器模块的输出端经电机驱动模块与电机连接；所述控制器模块与控制无线模块连接；所述控制无线模块与遥控装置无线连接；所述显示模块用于显示飞行器本体的飞行数据和检测的气压、经纬度数据；所述电机驱动模块接收控制器模块的控制信号控制电机转动使得飞行器本体进行飞行；

所述遥控装置包括遥控装置包括遥控无线模块、遥控气压计、风速风向传感器、遥控显示模块、遥控控制器模块、按键和摇杆；

所述遥控气压计、风速风向传感器、按键和摇杆的输出端与遥控控制器模块连接；所述遥控气压计用以检测遥控装置外的气压大小从而算出遥控装置与地相距高度；所述风速风向传感器用于检测飞速的大小和方向；所述按键和摇杆用于用户输入控制飞行器本体的飞行；

所述遥控显示模块的输入端与遥控控制器模块连接；所述遥控无线模块与遥控控制器模块连接；所述遥控无线模块与控制无线模块连接；所述遥控显示模块用于显示飞行器本体的飞行的速度、与目标点的距离数据；所述遥控无线模块实现与飞行器本体无线通信。

上述方案中，优选的是飞行器本体和遥控装置上均设置有供能装置，供能装置为电池。

上述方案中，优选的是陀螺仪使用型号为MCU6050的陀螺仪。

本发明的优点与效果是：

本发明通过采集飞行环境中的风的大小和方向，在根据遥控装置设定的飞行器本体飞行的速度，飞行器本体飞行根据风的大小和方向与设定的飞行速度从而进一步算出飞行器本体的实际飞行的速度和方向，从而能更好根据人们的需要进行飞行器的目标搜索，提高目标寻找的速度和效率，从而能够更好的解决跟随状态下被风影响大的问题。

附图说明

图1为本发明飞行结构示意图。

图2为本发明结构框图。

具体实施方式

以下结合实施例对本发明作进一步说明。

一种基于速度矢量合成的航迹规划算法及其飞行器，如图1所示，包括如下步骤：

步骤1：用户通过遥控装置控制飞行器本体进入空中悬停模式，用于完成飞行前期的数据采集和为下一步跟随状态做准备工作。

步骤2：遥控装置上的风速风向传感器采集风速的大小v₁和风速的方向θ₁发送给飞行器，其中，v₁为采集的风速的大小，θ₁为风的方向角，由于飞行器本体处于飞行状态，无法检测风的大小和方向，所以只能使用遥控装置进行检测，遥控装置主要是使用遥控无线模块与飞行器本体的控制无线模块进行无线连接，遥控无线模块和控制无线模块均使用2.4G无线模块。

步骤3：飞行遥控装置传来的风速的大小v₁和风的方向角θ₁后，飞行器本体把接收的风速的大小v₁与原设定的风速初始值进行比较。原设定的风速初始值可以为零，也可以是用户根据飞行器的大小，风速对飞行器的航线产生影响的范围，用户自行设定。

步骤4：飞行器本体根据飞行器气压计采集回来的气压算出飞行器本体相对地面的高度d_f，遥控装置根据遥控气压计采集回来的气压算出遥控装置相对地面的高度d_y，从而可以算出飞行器本体相对遥控装置竖直上的高度为d_j＝d_f+d_y，其中d_y为飞行器本体相对地面的高度，d_y为遥控装置相对地面的高度，d_j为飞行器本体相对遥控装置竖直上的高度。算出飞行器本体相对地面的高度d_f和遥控装置相对地面的高度d_y的过程均为通过气压计采集其空间位置的气压，把采集的气压减去大地表层的气压，再根据气压与高度的比例关系求出高度。

步骤5：遥控装置通过遥控GPS模块采集遥控装置的经纬度数据(lati1,long1)，飞行器本体通过飞行器GPS模块采集飞行器本体的经纬度数据(lati2,long2)，其中，lati1为遥控装置的经度数据，long1为遥控装置的维度数据，lati2为飞行器本体的经度数据，long2为飞行器本体的维度数据。GPS模块采集定位经纬度已经为现有技术。

步骤6：根据遥控装置纬度数据和飞行器本体经纬度数据解算出飞行器本体与遥控装置的相对距离d_x和相对方向角θ_x，其中，d_x为飞行器本体与遥控装置的相对距离，θ_x为飞行器本体与遥控装置的相对方向角。

飞行器本体与遥控装置的相对距离d_x和相对方向角θ_x的过程包括如下步骤，

步骤6.1：根据步骤5采集的经纬度算出飞行器本体与遥控装置两点的经度弧度长度：long＝(long1-long2)×π/180，其中，π为圆周率，long1为遥控装置点的经度，long2为飞行器本体经度，long为飞行器本体与遥控装置两点的经度弧度长度差。

步骤6.2：根据步骤5采集的经纬度算出飞行器本体与遥控装置两点的纬度弧度长度：lati＝(lati1-lati2)×π/180；π为圆周率，lati1为遥控装置点的维度，lati2为飞行器本体维度，lati为飞行器本体与遥控装置两点的维度弧度长度差。

步骤6.3：算出飞行器本体与遥控装置两点的相对距离为：

，

其中，long飞行器本体与遥控装置两点的经度弧度长度差，lati为飞行器本体与遥控装置两点的维度弧度长度差，lati1为遥控装置点的维度，lati2为飞行器本体维度，R为地球半径，π为圆周率。

步骤6.4：算出飞行器本体与遥控装置两点的相对角度为：θ_x＝mod(arctan2(R×cos(lati1×(lon2-lon1))，R×(lati2-lati1))，2×π)，mod为坐标点表示式，lati1为遥控装置点的维度，lati2为飞行器本体维度，long1为遥控装置点的经度，long2为飞行器本体经度，R为地球半径，π为圆周率。

步骤7：如图1所示，根据飞行器本体与遥控装置的相对距离d_x和飞行器本体相对遥控装置竖直上的高度d_j算出飞行器本体与遥控装置的绝对距离其中，d_fs为飞行器本体与遥控装置的绝对距离。

步骤8：根据目标点与遥控装置的绝对距离d_fs和飞行器本体与遥控装置的绝对距离d_fj算出飞行器本体与目标点的距离s。算出飞行器本体与目标点的距离s的公式为：

其中π为圆周率，d_y为飞行器本体相对地面的高度，d_y为遥控装置相对地面的高度，d_j为飞行器本体相对遥控装置竖直上的高度，d_fs为飞行器本体与遥控装置的绝对距离，飞行器本体与遥控装置的绝对距离d_fj。

步骤9：当步骤3中风速的大小v₁小于等于原设定的风速初始值时，用户通过遥控器根据飞行器本体与遥控装置的相对方向角θ_x来确定飞行器本体的飞行方向，当风速的大小v₁大于原设定的风速初始值时，进入步骤12。主要是选择飞行的模式，更有采集的风速的大小与用户设定的值进行来比较。

步骤10：飞行器本体运用PID算法来调控飞行器的速度，速度大小为v₂，v₂为飞行器的飞行速度。PID算法为：

其中，u(t)为控制飞行器本体的输出速度v₂；e(t)为控制器的输入(是初始飞行器与目标点的距离s与当前飞行器与目标点的距离s_s之差，即e(t)＝S-S_s)，Kp为控制器的比例放大系数，T_i为控制器的积分时间，T_d为控制器的微分时间，t为时间。

步骤11：相隔设定的时间后，返回步骤4，直到飞行到达目标点。设定的时间可以是一分钟或几十秒都可以，由用户来设定，用户可以根据寻找目标的距离长远来进行比较。达到目的点后完成寻找任务。

步骤12：根据风速的大小v₁和风速的方向θ₁，既风速为再由设定的飞行器本体与目标点的相对飞行速度从而算出飞行器本体的飞行速度其中，为风速的矢量，为飞行器本体与目标点的相对飞行速度矢量，为飞行器本体实际飞行速度矢量。算出飞行器本体实际飞行速度矢量的过程为：先算出飞行器本体飞行的方向：θ₂为飞行器本体飞行的方向角，飞行器本体实际飞行速度大小为：其中，v₁为风速大小，v₂为飞行器本体的输出速度大小，v为飞行器本体实际飞行速度，θ₂为飞行器本体飞行的方向角度，为飞行器本体实际飞行的方向与飞行器本体与目标点连线构成的角。

步骤13：飞行器本体运用PID算法来调控飞行器的速度，速度大小为v，v为飞行器的飞行速度。PID算法为：其中，u(t)为控制飞行器的速度v₂；e(t)为控制器的输入(即e(t)＝v-v₂)，Kp为控制器的比例放大系数，T_i为控制器的积分时间，T_d为控制器的微分时间，t为时间。

步骤14：相隔设定的时间后，返回步骤4，直到飞行到达目标点。设定的时间可以是一分钟或几十秒都可以，由用户来设定，用户可以根据寻找目标的距离长远来进行比较。

如图2所示，根据上述一种基于速度矢量合成的航迹规划算法的飞行器，包括飞行器本体和遥控装置，遥控装置与飞行器本体无线连接.

所述飞行器本体包括机架和控制电路单元，控制电路单元包括电机、电机驱动模块、激光模块、GPS定位模块、气压计、控制器模块、磁力计、显示模块、陀螺仪和控制无线模块。

所述激光模块、GPS定位模块、气压计、磁力计和陀螺仪的输出端与控制器模块连接；所述GPS定位模块用于获取飞行器本体的经度和维度数据；所述气压计用以检测飞行器本体外的气压大小从而算出飞行器本体与地相距高度；所述陀螺仪用于检测飞行器本体的平衡状态。

所述显示模块的输入端与控制器模块连接；所述控制器模块的输出端经电机驱动模块与电机连接；所述控制器模块与控制无线模块连接；所述控制无线模块与遥控装置无线连接；所述显示模块用于显示飞行器本体的飞行数据和检测的气压、经纬度数据；所述电机驱动模块接收控制器模块的控制信号控制电机转动使得飞行器本体进行飞行。

飞行器本体的控制器模块的微处理器主要实现的功能有：1、提供电机驱动模块所需要的方波脉冲控制信号，以控制电机的转速。2、驱动激光模块来检测当前所处位置附近的障碍物以进行壁障。3、根据GPS定位模块得到当前飞行器所处的经纬度。4、利用2.4G数传模块与遥控器通信。5、处理气压计得到的数据得到当前飞行器所处的高度。6、将当前经纬度以及高度等数据显示在OLED显示模块上。7、采集陀螺仪的数据，完成姿态，速度等导航信息的滤波解算。8、得到遥控装置的数据进行相应的处理。9、驱动磁力计，定位飞行器的飞行方向。

所述遥控装置包括遥控装置包括遥控无线模块、遥控气压计、风速风向传感器、遥控显示模块、遥控控制器模块、按键和摇杆。

所述遥控气压计、风速风向传感器、按键和摇杆的输出端与遥控控制器模块连接；所述遥控气压计用以检测遥控装置外的气压大小从而算出遥控装置与地相距高度；所述风速风向传感器用于检测飞速的大小和方向；所述按键和摇杆用于用户输入控制飞行器本体的飞行。

所述遥控显示模块的输入端与遥控控制器模块连接；所述遥控无线模块与遥控控制器模块连接；所述遥控无线模块与控制无线模块连接；所述遥控显示模块用于显示飞行器本体的飞行的速度、与目标点的距离数据；所述遥控无线模块实现与飞行器本体无线通信。

遥控装置的遥控控制器模块内的微处理器主要实现的功能有：1、根据GPS定位模块得到当前遥控器所处的经纬度2、按键控制得到当前飞行器的模式3、采集当前位置信息4、处理气压计得到的数据得到当前遥控所处的高度5、OLED显示当前位置，模式等信息。

遥控装置通过2.4G数传模块发送相应的控制指令控制飞行器，飞行器既可工作。控制命令包括：1、自稳/悬停/基本跟随模式/有风跟随模式四个模式；2、控制脉冲频率，即电机的转动速度；3、遥控飞行方向；4、GPS数据传输5、无人机状态查询，当电压过低时，发送警报给遥控装置6、当要飞行器处于跟随模式时，先让飞控处于悬停状态模式，然后再处于跟随模式，飞行器即进入跟随模式自主飞行7、处于跟随模式时无人机结合根据遥控装置传过来的风力、风向和飞行目标点做出飞行航向的决策。

工作时，操作者通过遥控装置上的按键装置启动飞行器进入悬停模式再进入跟随模式，飞行器根据遥控装置传回的GPS数据、风速风向数据、气压值判断目标点的方向，进而规划航线和飞行的方向。这种飞行方式有效的决解了飞行器在有风的情况下也不会导致偏离航迹飞行。

以上已对本发明创造的较佳实施例进行了具体说明，但本发明并不限于实施例，熟悉本领域的技术人员在不违背本发明创造精神的前提下还可作出种种的等同的变型或替换，这些等同的变型或替换均包含在本申请的范围内。

Claims (10)

1.一种基于速度矢量合成的航迹规划算法，其特征在于，包括如下步骤：

步骤1：用户通过遥控装置控制飞行器本体进入空中悬停模式；

步骤2：遥控装置上的风速风向传感器采集风速的大小v₁和风速的方向θ₁发送给飞行器，其中，v₁为采集的风速的大小，θ₁为风的方向角；

步骤3：飞行器本体接收遥控装置传来的风速的大小v₁和风的方向角θ₁后，飞行器本体把接收的风速的大小v₁与原设定的风速初始值进行比较；

步骤4：飞行器本体根据飞行器气压计采集回来的气压算出飞行器本体相对地面的高度d_f，遥控装置根据遥控气压计采集回来的气压算出遥控装置相对地面的高度d_y，从而可以算出飞行器本体相对遥控装置竖直上的高度为d_j＝d_f+d_y，其中d_y为飞行器本体相对地面的高度，d_y为遥控装置相对地面的高度，d_j为飞行器本体相对遥控装置竖直上的高度；

步骤5：遥控装置通过遥控GPS模块采集遥控装置的经纬度数据(lati1,long1)，飞行器本体通过飞行器GPS模块采集飞行器本体的经纬度数据(lati2,long2)，其中，lati1为遥控装置的经度数据，long1为遥控装置的维度数据，lati2为飞行器本体的经度数据，long2为飞行器本体的维度数据；

步骤6：根据遥控装置纬度数据和飞行器本体经纬度数据解算出飞行器本体与遥控装置的相对距离d_x和相对方向角θ_x，其中，d_x为飞行器本体与遥控装置的相对距离，θx为飞行器本体与遥控装置的相对方向角；

步骤7：根据飞行器本体与遥控装置的相对距离d_x和飞行器本体相对遥控装置竖直上的高度d_j算出飞行器本体与遥控装置的绝对距离其中，d_fs为飞行器本体与遥控装置的绝对距离；

步骤8：根据目标点与遥控装置的绝对距离d_fs和飞行器本体与遥控装置的绝对距离d_fj算出飞行器本体与目标点的距离s；

步骤9：当步骤3中风速的大小v₁小于等于原设定的风速初始值时，用户通过遥控器根据飞行器本体与遥控装置的相对方向角θ_x来确定飞行器本体的飞行方向，当风速的大小v₁大于原设定的风速初始值时，进入步骤12；

步骤10：飞行器本体运用PID算法来调控飞行器的输出速度，输出速度大小为v₂，v₂为飞行器本体的输出速度；

步骤11：相隔设定的时间后，返回步骤4，直到飞行到达目标点；

步骤12：根据风速的大小v₁和风速的方向θ₁，既风速为再由设定的飞行器本体的输出速度从而算出飞行器本体的实际飞行速度其中，为风速的矢量，为飞行器本体的输出速度矢量，为飞行器本体实际飞行速度矢量；

步骤13：飞行器本体运用PID算法来调控飞行器的输出速度，输出速度大小为v₂，v₂为飞行器本体的输出速度；

步骤14：相隔设定的时间后，返回步骤4，直到飞行到达目标点。

2.根据权利要求1所述的一种基于速度矢量合成的航迹规划算法，其特征在于：所述步骤4中算出飞行器本体相对地面的高度d_f和遥控装置相对地面的高度d_y的过程均为通过气压计采集其空间位置的气压，把采集的气压减去大地表层的气压，再根据气压与高度的比例关系求出高度。

3.根据权利要求1所述的一种基于速度矢量合成的航迹规划算法，其特征在于：所述步骤6算出飞行器本体与遥控装置的相对距离d_x和相对方向角θ_x的过程包括如下步骤，

步骤6.1：根据步骤5采集的经纬度算出飞行器本体与遥控装置两点的经度弧度长度：long＝(long1-long2)×π/180，其中，π为圆周率，long1为遥控装置点的经度，long2为飞行器本体经度，long为飞行器本体与遥控装置两点的经度弧度长度差；

步骤6.2：根据步骤5采集的经纬度算出飞行器本体与遥控装置两点的纬度弧度长度：lati＝(lati1-lati2)×π/180；π为圆周率，lati1为遥控装置点的维度，lati2为飞行器本体维度，lati为飞行器本体与遥控装置两点的维度弧度长度差；

步骤6.3：算出飞行器本体与遥控装置两点的相对距离为：

$d_x=2\×\arcsin \left(\sqrt{{\sin }^2\left(lati\÷2\right)+\cos \left(lati1\×\mathrm{\π}\÷180\right)\×\cos \left(lati2\×\mathrm{\π}\÷180\right)\×{\sin }^2\left(long\÷2\right)}\right)\×R/1000$ ，

其中，long飞行器本体与遥控装置两点的经度弧度长度差，lati为飞行器本体与遥控装置两点的维度弧度长度差，lati1为遥控装置点的维度，lati2为飞行器本体维度，R为地球半径，π为圆周率；

步骤6.4：算出飞行器本体与遥控装置两点的相对角度为：

θ_x＝mod(arctan2(R×cos(lati1×(lon2-lon1))，R×(lati2-lati1))，2×π)，mod为坐标点表示式，lati1为遥控装置点的维度，lati2为飞行器本体维度，long1为遥控装置点的经度，long2为飞行器本体经度，R为地球半径，π为圆周率。

4.根据权利要求1所述的一种基于速度矢量合成的航迹规划算法，其特征在于：所述步骤8算出飞行器本体与目标点的距离s的公式为：

其中π为圆周率，d_y为飞行器本体相对地面的高度，d_y为遥控装置相对地面的高度，d_j为飞行器本体相对遥控装置竖直上的高度，d_fs为飞行器本体与遥控装置的绝对距离，飞行器本体与遥控装置的绝对距离d_fj。

5.根据权利要求1所述的一种基于速度矢量合成的航迹规划算法，其特征在于：所述步骤10中的PID算法为：

其中，u(t)为控制飞行器本体的输出速度v₂；e(t)为控制器的输入(是初始飞行器与目标点的距离s与当前飞行器与目标点的距离s_s之差，即e(t)＝S-S_s)，Kp为控制器的比例放大系数，T_i为控制器的积分时间，T_d为控制器的微分时间，t为时间。

6.根据权利要求1所述的一种基于速度矢量合成的航迹规划算法，其特征在于：所述步骤12中算出飞行器本体实际飞行速度矢量的过程为：先算出飞行器本体飞行的方向：θ₂为飞行器本体飞行的方向角，飞行器本体实际飞行速度大小为：

其中，v₁为风速大小，v₂为飞行器本体的输出速度大小，v为飞行器本体实际飞行速度，θ₂为飞行器本体飞行的方向角度，为飞行器本体实际飞行的方向与飞行器本体与目标点连线构成的角。

7.根据权利要求1所述的一种基于速度矢量合成的航迹规划算法，其特征在于：所述步骤13中的PID算法为：

其中，u(t)为控制飞行器本体的输出速度v₂；e(t)为控制器的输入(即e(t)＝v-v₂)，Kp为控制器的比例放大系数，T_i为控制器的积分时间，T_d为控制器的微分时间，t为时间。

8.根据上述权利要求1-7任一项所述的一种基于速度矢量合成的航迹规划算法的飞行器，其特征在于：包括飞行器本体和遥控装置，遥控装置与飞行器本体无线连接；

所述飞行器本体包括机架和控制电路单元，控制电路单元包括电机、电机驱动模块、激光模块、GPS定位模块、气压计、控制器模块、磁力计、显示模块、陀螺仪和控制无线模块；

所述激光模块、GPS定位模块、气压计、磁力计和陀螺仪的输出端与控制器模块连接；所述GPS定位模块用于获取飞行器本体的经度和维度数据；所述气压计用以检测飞行器本体外的气压大小从而算出飞行器本体与地相距高度；所述陀螺仪用于检测飞行器本体的平衡状态；

所述显示模块的输入端与控制器模块连接；所述控制器模块的输出端经电机驱动模块与电机连接；所述控制器模块与控制无线模块连接；所述控制无线模块与遥控装置无线连接；所述显示模块用于显示飞行器本体的飞行数据和检测的气压、经纬度数据；所述电机驱动模块接收控制器模块的控制信号控制电机转动使得飞行器本体进行飞行；

所述遥控装置包括遥控装置包括遥控无线模块、遥控气压计、风速风向传感器、遥控显示模块、遥控控制器模块、按键和摇杆；

所述遥控气压计、风速风向传感器、按键和摇杆的输出端与遥控控制器模块连接；所述遥控气压计用以检测遥控装置外的气压大小从而算出遥控装置与地相距高度；所述风速风向传感器用于检测飞速的大小和方向；所述按键和摇杆用于用户输入控制飞行器本体的飞行；

所述遥控显示模块的输入端与遥控控制器模块连接；所述遥控无线模块与遥控控制器模块连接；所述遥控无线模块与控制无线模块连接；所述遥控显示模块用于显示飞行器本体的飞行的速度、与目标点的距离数据；所述遥控无线模块实现与飞行器本体无线通信。

9.根据上述权利要求8所述的一种基于速度矢量合成的航迹规划算法的飞行器，其特征在于：飞行器本体和遥控装置上均设置有供能装置，供能装置为电池。

10.根据上述权利要求8所述的一种基于速度矢量合成的航迹规划算法的飞行器，其特征在于：所述陀螺仪使用型号为MCU6050的陀螺仪。