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

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

A route planning algorithm based on velocity vector synthesis and its aerocraft

The invention provides a velocity vector synthesis route planning algorithm and based on the aircraft, belonging to the field of control, the magnitude and direction of detection in the environment of the wind, the aircraft flight control device according to the ontology set the speed of flight aircraft body according to the size and direction of the wind with the flight speed so as to further calculate the actual aircraft body flight speed and direction, so as to improve the vehicle according to the need of target search, improve the speed and efficiency of search.

【技术实现步骤摘要】

本专利技术涉及控制领域，特别是涉及一种基于速度矢量合成的航迹规划算法及其飞行器。
技术介绍
当前，无人机已经逐渐出现于市场，特别是在跟踪和航拍上的应用，如跟随航拍、电影拍摄、植物保护、规划路线、自然灾害探测等涉及到自动拍摄的场合。常用的gps四轴跟随无人机主要通过跟随目标的状态信息，以及期望的相对距离计算得到无人机的期望位置状态，并与当下状态进行比较，根据算法得出修正量。这样的算法架构可以避免将相对距离看做系统状态所需要的数据转换过程，也实现了一部分的跟随功能。但是在很多情况下跟随状态却受到环境因素的影响很大，在定位跟随下不能为用户取得比较优秀的数据。另外，无人机不是仅仅只能会是一个被动的牵引分子，而应该是一个完整的自己的思想，能够自我判断取得较好的路径去跟随目标。
技术实现思路
本专利技术提供一种基于速度矢量合成的航迹规划算法及其飞行器，解决现有无人机处于跟随状态下被风影响大的问题。本专利技术通过以下技术方案解决上述问题：一种基于速度矢量合成的航迹规划算法，包括如下步骤：步骤1：用户通过遥控装置控制飞行器本体进入空中悬停模式；步骤2：遥控装置上的风速风向传感器采集风速的大小v1和风速的方向θ1发送给飞行器，其中，v1为采集的风速的大小，θ1为风的方向角；步骤3：飞行器本体接收遥控装置传来的风速的大小v1和风的方向角θ1后，飞行器本体把接收的风速的大小v1与原设定的风速初始值进行比较；步骤4：飞行器本体根据飞行器气压计采集回来的气压算出飞行器本体相对地面的高度df，遥控装置根据遥控气压计采集回来的气压算出遥控装置相对地面的高度dy，从而可以算出飞行器本体相对遥控装置竖直上的高度为dj＝df+dy，其中dy为飞行器本体相对地面的高度，dy为遥控装置相对地面的高度，dj为飞行器本体相对遥控装置竖直上的高度；步骤5：遥控装置通过遥控GPS模块采集遥控装置的经纬度数据(lati1,long1)，飞行器本体通过飞行器GPS模块采集飞行器本体的经纬度数据(lati2,long2)，其中，lati1为遥控装置的经度数据，long1为遥控装置的维度数据，lati2为飞行器本体的经度数据，long2为飞行器本体的维度数据；步骤6：根据遥控装置纬度数据和飞行器本体经纬度数据解算出飞行器本体与遥控装置的相对距离dx和相对方向角θx，其中，dx为飞行器本体与遥控装置的相对距离，θx为飞行器本体与遥控装置的相对方向角；步骤7：根据飞行器本体与遥控装置的相对距离dx和飞行器本体相对遥控装置竖直上的高度dj算出飞行器本体与遥控装置的绝对距离其中，dfs为飞行器本体与遥控装置的绝对距离；步骤8：根据目标点与遥控装置的绝对距离dfs和飞行器本体与遥控装置的绝对距离dfj算出飞行器本体与目标点的距离s；步骤9：当步骤3中风速的大小v1小于等于原设定的风速初始值时，用户通过遥控器根据飞行器本体与遥控装置的相对方向角θx来确定飞行器本体的飞行方向，当风速的大小v1大于原设定的风速初始值时，进入步骤12；步骤10：飞行器本体运用PID算法来调控飞行器的输出速度，输出速度大小为v2，v2为飞行器本体的输出速度；步骤11：相隔设定的时间后，返回步骤4，直到飞行到达目标点；步骤12:根据风速的大小v1和风速的方向θ1，既风速为再由设定的飞行器本体的输出速度从而算出飞行器本体的飞行速度其中，为风速的矢量，为飞行器本体的输出速度矢量，为飞行器本体实际飞行速度矢量；步骤13：飞行器本体运用PID算法来调控飞行器的速度，速度大小为v2，v2为飞行器本体的输出速度；步骤14：相隔设定的时间后，返回步骤4，直到飞行到达目标点。上述方案中，优选的是步骤4中算出飞行器本体相对地面的高度df和遥控装置相对地面的高度dy的过程均为通过气压计采集其空间位置的气压，把采集的气压减去大地表层的气压，再根据气压与高度的比例关系求出高度。上述方案中，优选的是步骤6算出飞行器本体与遥控装置的相对距离dx和相对方向角θ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)为控制飞行器本体的输出速度v2；e(t)为控制器的输入(是初始飞行器与目标点的距离s与当前飞行器与目标点的距离ss之差，即e(t)＝S-Ss)，Kp为控制器的比例放大系数，Ti为控制器的积分时间，Td为控制器的微分时间，t为时间。上述方案中，优选的是步骤12中算出飞行器本体实际飞行速度矢量的过程为：先算出飞行器本体飞行的方向：θ2为飞行器本体飞行的方向角，飞行器本体实际飞行速度大小为：上述方案中，优选的是步骤13中的PID算法为：其中，u(t)为控制飞行器的输出速度v2；e(t)为控制器的输入(即e(t)＝v-v2)，Kp为控制器的比例放大系数，Ti为控制器的积分时间，Td为控制器的微分时间，t为时间。根据上述的一种基于速度矢量合成的航迹规划算法的飞行器，包括飞行器本体和遥控装置，遥控装置与飞行器本体无线连接；所述飞行器本体包括机架和控制电路单元，控制电路单元包括电机、电机驱动模块、激光模块、GPS定位模块、气压计、控制器模块、磁力计、显示模块、陀螺仪和控制无线模块；所述激光模块、GPS定位模块、气压计、磁力计和陀螺仪的输出端与控制器模块连接；所述GPS定位模块用于获取飞行器本体的经度和维度数据；所述气压计用以检测飞行器本体外的气压大小从而算出飞行器本体与地相距高度；所述陀螺仪用于检测飞行器本体的平衡状态；所述显示模块的输入端与控制器模块连接；所述控制器模块的输出端经电机驱动模块与电机连接；所述控制器模块与控制无线模块连接；所述控制无线模块与遥控装置无线连接；所述显示模块用于显示飞行器本体的飞行数据和检测的气压、经纬度数据；所述电机驱动模块接收控制器模块的控制信号控制电机转动使得飞行器本体进行飞行；所述遥控装置包括遥控装置包括遥控无线模块、遥控气压计、风速风向传感器、遥控显示模块、遥控控制器模块、按键和摇杆；所述遥控气压计、风速风向传感器、按键和摇杆的输出端与遥控本文档来自技高网...

【技术保护点】
一种基于速度矢量合成的航迹规划算法，其特征在于，包括如下步骤：步骤1：用户通过遥控装置控制飞行器本体进入空中悬停模式；步骤2：遥控装置上的风速风向传感器采集风速的大小v1和风速的方向θ1发送给飞行器，其中，v1为采集的风速的大小，θ1为风的方向角；步骤3：飞行器本体接收遥控装置传来的风速的大小v1和风的方向角θ1后，飞行器本体把接收的风速的大小v1与原设定的风速初始值进行比较；步骤4：飞行器本体根据飞行器气压计采集回来的气压算出飞行器本体相对地面的高度df，遥控装置根据遥控气压计采集回来的气压算出遥控装置相对地面的高度dy，从而可以算出飞行器本体相对遥控装置竖直上的高度为dj＝df+dy，其中dy为飞行器本体相对地面的高度，dy为遥控装置相对地面的高度，dj为飞行器本体相对遥控装置竖直上的高度；步骤5：遥控装置通过遥控GPS模块采集遥控装置的经纬度数据(lati1,long1)，飞行器本体通过飞行器GPS模块采集飞行器本体的经纬度数据(lati2,long2)，其中，lati1为遥控装置的经度数据，long1为遥控装置的维度数据，lati2为飞行器本体的经度数据，long2为飞行器本体的维度数据；步骤6：根据遥控装置纬度数据和飞行器本体经纬度数据解算出飞行器本体与遥控装置的相对距离dx和相对方向角θx，其中，dx为飞行器本体与遥控装置的相对距离，θx为飞行器本体与遥控装置的相对方向角；步骤7：根据飞行器本体与遥控装置的相对距离dx和飞行器本体相对遥控装置竖直上的高度dj算出飞行器本体与遥控装置的绝对距离其中，dfs为飞行器本体与遥控装置的绝对距离；步骤8：根据目标点与遥控装置的绝对距离dfs和飞行器本体与遥控装置的绝对距离dfj算出飞行器本体与目标点的距离s；步骤9：当步骤3中风速的大小v1小于等于原设定的风速初始值时，用户通过遥控器根据飞行器本体与遥控装置的相对方向角θx来确定飞行器本体的飞行方向，当风速的大小v1大于原设定的风速初始值时，进入步骤12；步骤10：飞行器本体运用PID算法来调控飞行器的输出速度，输出速度大小为v2，v2为飞行器本体的输出速度；步骤11：相隔设定的时间后，返回步骤4，直到飞行到达目标点；步骤12：根据风速的大小v1和风速的方向θ1，既风速为再由设定的飞行器本体的输出速度从而算出飞行器本体的实际飞行速度其中，为风速的矢量，为飞行器本体的输出速度矢量，为飞行器本体实际飞行速度矢量；步骤13：飞行器本体运用PID算法来调控飞行器的输出速度，输出速度大小为v2，v2为飞行器本体的输出速度；步骤14：相隔设定的时间后，返回步骤4，直到飞行到达目标点。...

【技术特征摘要】
1.一种基于速度矢量合成的航迹规划算法，其特征在于，包括如下步骤：步骤1：用户通过遥控装置控制飞行器本体进入空中悬停模式；步骤2：遥控装置上的风速风向传感器采集风速的大小v1和风速的方向θ1发送给飞行器，其中，v1为采集的风速的大小，θ1为风的方向角；步骤3：飞行器本体接收遥控装置传来的风速的大小v1和风的方向角θ1后，飞行器本体把接收的风速的大小v1与原设定的风速初始值进行比较；步骤4：飞行器本体根据飞行器气压计采集回来的气压算出飞行器本体相对地面的高度df，遥控装置根据遥控气压计采集回来的气压算出遥控装置相对地面的高度dy，从而可以算出飞行器本体相对遥控装置竖直上的高度为dj＝df+dy，其中dy为飞行器本体相对地面的高度，dy为遥控装置相对地面的高度，dj为飞行器本体相对遥控装置竖直上的高度；步骤5：遥控装置通过遥控GPS模块采集遥控装置的经纬度数据(lati1,long1)，飞行器本体通过飞行器GPS模块采集飞行器本体的经纬度数据(lati2,long2)，其中，lati1为遥控装置的经度数据，long1为遥控装置的维度数据，lati2为飞行器本体的经度数据，long2为飞行器本体的维度数据；步骤6：根据遥控装置纬度数据和飞行器本体经纬度数据解算出飞行器本体与遥控装置的相对距离dx和相对方向角θx，其中，dx为飞行器本体与遥控装置的相对距离，θx为飞行器本体与遥控装置的相对方向角；步骤7：根据飞行器本体与遥控装置的相对距离dx和飞行器本体相对遥控装置竖直上的高度dj算出飞行器本体与遥控装置的绝对距离其中，dfs为飞行器本体与遥控装置的绝对距离；步骤8：根据目标点与遥控装置的绝对距离dfs和飞行器本体与遥控装置的绝对距离dfj算出飞行器本体与目标点的距离s；步骤9：当步骤3中风速的大小v1小于等于原设定的风速初始值时，用户通过遥控器根据飞行器本体与遥控装置的相对方向角θx来确定飞行器本体的飞行方向，当风速的大小v1大于原设定的风速初始值时，进入步骤12；步骤10：飞行器本体运用PID算法来调控飞行器的输出速度，输出速度大小为v2，v2为飞行器本体的输出速度；步骤11：相隔设定的时间后，返回步骤4，直到飞行到达目标点；步骤12：根据风速的大小v1和风速的方向θ1，既风速为再由设定的飞行器本体的输出速度从而算出飞行器本体的实际飞行速度其中，为风速的矢量，为飞行器本体的输出速度矢量，为飞行器本体实际飞行速度矢量；步骤13：飞行器本体运用PID算法来调控飞行器的输出速度，输出速度大小为v2，v2为飞行器本体的输出速度；步骤14：相隔设定的时间后，返回步骤4，直到飞行到达目标点。2.根据权利要求1所述的一种基于速度矢量合成的航迹规划算法，其特征在于：所述步骤4中算出飞行器本体相对地面的高度df和遥控装置相对地面的高度dy的过程均为通过气压计采集其空间位置的气压，把采集的气压减去大地表层的气压，再根据气压与高度的比例关系求出高度。3.根据权利要求1所述的一种基于速度矢量合成的航迹规划算法，其特征在于：所述步骤6算出飞行器本体与遥控装置的相对距离dx和相对方向角θx的过程包括如下步骤，步骤6.1：根据步骤5采集的经纬度算出飞行器本体与遥控装置两点的经度弧度长度：long＝(long1-long2)×π/180，其中，π为圆周率，long1为遥控装置点的经度，long2为飞行器本体经度，long为飞行器本体与遥控装置两点的经度弧度长度差；步骤6.2：根据步骤5采集的经纬度算出飞行器本体与遥控装置两点的纬度弧度长度：lati＝(lati1-lati2)×π/180；π为圆周率，lati1为遥控装置点的维度，lati2为飞行器本体维度，lati为飞行器本体与遥控装置两点的维度弧度长度差；步骤6.3：算出飞行器本体与遥控装置两点的相对距离为：dx=2×arcsin(sin2(lati÷2)+cos(lati1×π÷180)×cos(lati2×π÷180)×sin2(long÷2))×R/1000]]>，其中，long飞行器本体与遥控装置两...

【专利技术属性】
技术研发人员：黄科就，丘森辉，罗阳资，邓键，李德龙，翁艺，李阳，廖彬雄，
申请(专利权)人：广西师范大学，
类型：发明
国别省市：广西;45