一种无人机飞行控制及定位方法技术领域
本发明涉及无人机技术领域，特别涉及一种无人机飞行控制及定位方法。
背景技术
无人机全称为无人驾驶飞机，是利用无线电遥控设备和自备的程序控制装置操纵
的不载人飞机。
多旋翼无人机属于新兴技术、高新前沿科技产业。随着科技的发展，无人机除用于
军事用途外，在民用领域的应用范围也越来越广阔。由于无人机具有运行成本低、无伤亡风
险、机动性能好、可进行超视距飞行、使用方便高效等特点，目前已被成功应用于影视航拍、
测绘航测、高压线巡查、远程监控、救灾救援、农药喷洒、商业表演等领域，越来越多的行业
正希望用无人机取代传统的人工作业方式。
无人机上的飞行控制系统，用以控制无人机的飞行，是无人机技术的核心，控制系
统的优劣直接影响无人机的性能甚至安全性，而对无人机的姿态、位置、速度、高度等信息
的准确获取和控制则是一个优秀的无人机控制系统的基础。如何提高无人机的姿态、位置、
速度、高度的控制速度、控制精度是当前需要解决的问题。
发明内容
本发明提供了一种无人机飞行控制及定位方法，用以解决目前无人机的姿态、位
置、速度、高度的控制速度、控制精度不够高的问题，提高无人机的性能和安全性。
本发明提供的一种无人机飞行控制及定位方法，包括：
一种无人机的基于互补滤波的姿态解算法，用于根据传感器(陀螺仪、加速度计、
罗盘等)的数据求解出无人机的空中姿态；
姿态控制算法，用于根据当前的姿态和期望的姿态的偏差，对无人机进行控制直
到当前的姿态达到期望的姿态；
一种无人机的基于惯性导航位置估算的控制方法，用于根据位置和速度信息对无
人机进行控制；
一种无人机的基于高度估算法的控制方法，用于根据高度信息对无人机进行控
制。
优选的，所述一种无人机的基于互补滤波的姿态解算法，包括：
对传感器进行校准，包括：通过椭球校准、温度校准、当地磁偏角校准等方法对加
速度计、陀螺仪、三轴磁力计进行校准；
由主控制器根据当前传感器的数据用基于互补滤波的姿态解算法计算得到当前
的机体四元数；
由主控制器根据收到的控制信息计算出期望的机体四元数；
由主控制器根据当前的机体四元数和期望的机体四元数以及GPS定位期望四元数
通过误差四元数PID控制环向微控制器发送电机调速指令。
优选的，所述由主控制器根据当前传感器的数据用基于互补滤波的姿态解算法计
算得到当前的机体四元数，包括：
由主控制器用加速度计和磁力计的数据校准陀螺仪的漂移，所用方法根据第一公
式为对陀螺仪的数据进行补偿，所述第一公式为：
ΔGyro＝KP·(W×Mag+V×Acc)+∑KI·(W×Mag+V×Acc)·dt
其中，
其中，ΔGyro为对陀螺仪读数进行补偿的数据；KP为控制系统的比例系数；Mag为
磁力计读数向量；Acc为加速度计读数向量；KI为控制系统的积分系数；dt为控制系统的控
制周期；g为当地的重力加速度常数；为表示地面坐标系到机体坐标系的四元数坐标旋转
矩阵；为机体坐标系到地面坐标系的四元数坐标旋转矩阵；V、W、H为中间变量。
优选的，所述一种无人机的基于惯性导航位置估算的控制方法，包括：
由主控制器根据GPS模块输出的位置数据计算出当前的经纬度和NED速度信息；
由主控制器根据遥控输入信息计算出期望的速度信息和期望的位置信息；
由主控制器根据当前的速度信息和位置信息以及期望的速度信息和期望的位置
信息进行加减速控制。
优选的，所述由主控制器根据GPS模块输出的位置数据计算出当前的经纬度和NED
速度信息，包括：
由主控制器通过第二迭代算法计算出当前的位置、速度和加速度数据，所述第二
迭代算法为，通过以下算法利用上一时间点的数据迭代得到当前时间点的数据：
VelAccCorr＝EFAcc+AccCorrGPS+AccCorrGPSV+∑AccCorrGPSV
VelRateCorr＝∑(VelAccCorr·dt+VelCorrGPS+VelCorrGPSV)
VelAccCorr2＝EFAcc+AccCorr2GPSV+∑AccCorrGPSV
VelRateCorr2＝∑(VelAccCorr2·dt+VelCorr2GPSV)
其中，AccCorrGPS、VelCorrGPS、PosCorrGPS分别为通过GPS得到的位置数据对加
速度、速度和位置的修正量；AccCorrGPSV、VelCorrGPSV分别为通过GPS得到的速度数据对
加速度、速度的第一修正量；AccCorr2GPSV、VelCorr2GPSV分别为通过GPS得到的速度数据
对加速度、速度的第二修正量；VelAccCorr、VelRateCorr分别为通过第一修正量修正后得
到的加速度和速度数据；PosEst为计算出的当前的位置数据；VelAccCorr2、VelRateCorr2
分别为通过第二修正量修正后得到的加速度和速度数据；PosGPS为GPS得到的位置数据；
TimeConstGPS、TimeConstGPSV和TimeConstGPSV2为预先设定的常数；VelGPSdata为通过
GPS得到的速度数据；EFAcc为通过加速度计得到的加速度数据；dt为控制系统的控制周期。
优选的，所述由主控制器根据当前的速度信息和位置信息以及期望的速度信息和
期望的位置信息进行加减速控制，包括：
由主控制器根据当前的位置信息以及期望的位置信息得出速度控制信息；
由主控制器根据当前的速度信息以及期望的速度信息得出加速度控制信息；
由主控制器根据速度控制信息和加速度控制信息得出旋转四元数；
由主控制器根据得出旋转四元数将飞行控制参数发送给动力装置进行加减速控
制。
优选的，所述由主控制器根据速度控制信息和加速度控制信息得出旋转四元数，
包括：
所述四元数为通过对第三公式进行归一化处理后得到，所述第三公式为：
其中，
AccTarX＝AccdesirdX+velLoopBrakeAcc×(veltargetX-vellastX)
-RateKP×VdiffX-RateKI×VintegrX
AccTarY＝AccdesirdY+velLoopBrakeAcc×(veltargetY-vellastY)
-RateKP×VdiffY-RateKI×VintegrY
其中，qw、qx、qy、qz为未归一化处理的四元数；AccTarX和AccTarY为向北和向东的
期望加速度；AccdesirdX和AccdesirdY为向北和向东的期望加速度；velLoopBrakeAcc为刹
车时存在的一个线性变化的数值，不刹车时为0；veltargetX和veltargetY为向北和向东的
期望速度；vellastX和vellastY为上一控制周期结束时的向北和向东的速度；RateKP为控
制系统的速度比例系数；RateKI为控制系统的速度积分系数；VdiffX为VdiffY为向北和向
东方向的期望速度与当前速度的差值；VintegrX和VintegrY为向北和向东方向的速度的积
分数值。
优选的，所述一种无人机的基于高度估算法的控制方法，包括：
由气压计模块得到地面气压值和当前气压值；由惯性测量模块得到由地面起飞到
当前状态的加速度数据；由GPS模块得到GPS经纬度信息和NED速度信息；
由主控制器根据地面气压值和当前气压值、由地面起飞到当前状态的加速度数
据、GPS经纬度信息和NED速度信息根据高度估算法估算当前的高度信息；
由主控制器根据遥控输入信息和当前的高度信息对油门进行控制，所用控制方法
为通过高度控制器根据油门控制曲线对油门输出指令。
优选的，所述由主控制器根据地面气压值和当前气压值、由地面起飞到当前状态
的加速度数据、GPS经纬度信息和NED速度信息根据高度估算法估算当前的高度信息，包括：
当前的高度信息由第四公式计算得到，所述第四公式为：
其中，EstAlt为估算的当前的高度；velRateCorrZ为通过气压计和GPS修正后的当
前速度的高度方向分量；velAccCorrZ为通过气压计和GPS修正后的当前加速度的高度方向
分量；dt为控制系统的控制周期；Pgroud为地面上的气压值，P为当前位置的气压值。
优选的，所述由主控制器根据遥控输入信息和当前的高度信息对油门进行控制，
所用控制方法为通过高度控制器根据油门控制曲线对油门输出指令，包括：
所述输入信息为遥控器的期望速度；控制所需要的期望高度由遥控器的期望速度
对时间进行积分得到；油门控制为根据油门控制曲线和加速度数据进行PI控制，所述油门
控制曲线由第五公式表示，所述第五公式为：
其中，Zexpv为高度方向期望速度；a1、a2、a3、a4为阈值的油门系数，其值均为正，且
a1<a2，a3<a4；throOff为遥控器油门的偏移值；EstAlt为估算的当前的高度，hf1、hf2为预设
的高度值，且hf1<hf2；linemap(a1,a2,EstAlt)表示以EstAlt为变量的单调递增函数，其上
限为a2，下限为a1，linemap(a3,a4,EstAlt)为上限为a4，下限为a3的同一函数。
本发明的一些有益效果可以包括：
通过本发明的方法，可以解决目前无人机的姿态、位置、速度、高度的控制速度、控
制精度不够高的问题，提高无人机的性能和安全性。
本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变
得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明
书、权利要求书、以及附图中所特别指出的结构来实现和获得。
下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。
附图说明
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实
施例一起用于解释本发明，并不构成对本发明的限制。在附图中：
图1A为本发明实施例中一种无人机的控制系统示意图；
图1B为本发明实施例中一种无人机飞行控制及定位方法的流程图；
图2A为本发明实施例中一种无人机的基于互补滤波的姿态解算法的控制系统框
图；
图2B为本发明实施例中一种无人机的基于互补滤波的姿态解算法的控制方法流
程图；
图3A为本发明实施例中一种无人机的基于惯性导航位置估算的控制系统框图；
图3B为本发明实施例中一种无人机的基于惯性导航位置估算的控制方法流程图；
图4A为本发明实施例中一种无人机的基于高度估算法的控制系统框图；
图4B为本发明实施例中一种无人机的基于高度估算法的控制方法流程图。
具体实施方式
以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实
施例仅用于说明和解释本发明，并不用于限定本发明。
在本发明的一个实施例中，如图1A所示，为本发明实施例中一种无人机的控制系
统示意图，包括：
电源系统，用于给整个无人机系统提供电力，包括：锂电池组、直流降压稳压模块；
传感器，用于测量无人机的环境信息和自身的状态信息，并将该信息发给主控制
器，传感器包括：磁力计、GPS模块、气压计、惯性测量模块；
遥控系统，用于输入控制信息并将控制信息发送给主控制器，包括：遥控器通道指
令模块、发送模块和接受模块；
主控制器，用于根据收到的控制信息和无人机当前的环境信息和自身的状态信
息，计算出飞行控制参数，并将飞行控制参数发送给动力装置；
动力装置，用于根据飞行控制参数输出相应的动力，包括：微控制器、无刷直流电
机、旋转翼；
上位机系统，用于为无人机提供监视和控制功能，包括数据传输模块、上位机。
本发明实施例提供的以上各部件共同构成了无人机控制系统，相互配合，实现了
对无人机的飞行控制。
在本发明的一个实施例中，如图1B所示，为本发明实施例中一种无人机飞行控制
及定位方法的流程图，包括：
一种无人机的基于互补滤波的姿态解算法，用于根据传感器(陀螺仪、加速度计、
罗盘等)的数据求解出无人机的空中姿态；
姿态控制算法，用于根据当前的姿态和期望的姿态的偏差，对无人机进行控制直
到当前的姿态达到期望的姿态；
一种无人机的基于惯性导航位置估算的控制方法，用于根据位置和速度信息对无
人机进行控制；
一种无人机的基于高度估算法的控制方法，用于根据高度信息对无人机进行控
制。
本发明实施例提供的这种无人机飞行控制及定位方法构成了无人机控制系统的
核心算法，相互配合，实现了根据传感器的数据和遥控输入信息通过动力装置对无人机的
飞行控制操作。
在本发明的一个实施例中，图2A所示为一种无人机的基于互补滤波的姿态解算法
的控制系统框图；如图2B所示，为本发明实施例中一种无人机的基于互补滤波的姿态解算
法的控制方法流程图，包括：
步骤S201：对传感器进行校准，包括：通过椭球校准、温度校准、当地磁偏角校准等
方法对加速度计、陀螺仪、三轴磁力计进行校准；
步骤S202：由主控制器根据当前传感器的数据用基于互补滤波的姿态解算法计算
得到当前的机体四元数；
步骤S203：由主控制器根据收到的控制信息计算出期望的机体四元数；
步骤S204：由主控制器根据当前的机体四元数和期望的机体四元数以及GPS定位
期望四元数通过误差四元数PID控制环向微控制器发送电机调速指令。
本发明实施例提供的这种无人机的基于互补滤波的姿态解算法的控制方法，通过
对加速度计、陀螺仪、三轴磁力计同时进行校准、补偿，比常用的对加速度计和三轴磁力计
进行校准的做法精确度更高，可以解决目前无人机的姿态控制精度不够高的问题，并为解
决无人机的位置、速度、高度的控制速度、控制精度不够高的问题提供了更好的基础，提高
了无人机的性能和安全性。
在本发明的一个实施例中，由主控制器根据当前传感器的数据用基于互补滤波的
姿态解算法计算得到当前的机体四元数，包括：
由主控制器用加速度计和磁力计的数据校准陀螺仪的漂移，所用方法根据第一公
式为对陀螺仪的数据进行补偿，所述第一公式为：
ΔGyro＝KP·(W×Mag+V×Acc)+∑KI·(W×Mag+V×Acc)·dt
其中，
其中，ΔGyro为对陀螺仪读数进行补偿的数据；KP为控制系统的比例系数；Mag为
磁力计读数向量；Acc为加速度计读数向量；KI为控制系统的积分系数；dt为控制系统的控
制周期；g为当地的重力加速度常数；为表示地面坐标系到机体坐标系的四元数坐标旋转
矩阵；为机体坐标系到地面坐标系的四元数坐标旋转矩阵；V、W、H为中间变量。
本发明实施例提供的这种对陀螺仪的数据进行补偿的方法，使用了加速度计和磁
力计的数据，能够更准确的校准陀螺仪的漂移，进而为解决无人机的位置、速度、高度的控
制速度、控制精度不够高的问题提供了更好的基础，提高了无人机的性能和安全性。
在本发明的一个实施例中，图3A所示为一种无人机的基于惯性导航位置估算的控
制系统框图；如图3B所示，为本发明实施例中一种无人机的基于惯性导航位置估算的控制
方法流程图，包括步骤：
步骤S301：由主控制器根据GPS模块输出的位置数据计算出当前的经纬度和NED速
度信息；
步骤S302：由主控制器根据遥控输入信息计算出期望的速度信息和期望的位置信
息；
步骤S303：由主控制器根据当前的速度信息和位置信息以及期望的速度信息和期
望的位置信息进行加减速控制。
本发明实施例提供的这种无人机的基于惯性导航位置估算的控制方法，通过主控
制器根据GPS模块的输出数据和遥控输入信息对动力装置进行加减速控制，实现了位置估
算的控制。
在本发明的一个实施例中，所述由主控制器根据GPS模块输出的位置数据计算出
当前的经纬度和NED速度信息，包括：
由主控制器通过第二迭代算法计算出当前的位置、速度和加速度数据，所述第二
迭代算法为，通过以下算法利用上一时间点的数据迭代得到当前时间点的数据：
VelAccCorr＝EFAcc+AccCorrGPS+AccCorrGPSV+∑AccCorrGPSV
VelRateCorr＝∑(VelAccCorr·dt+VelCorrGPS+VelCorrGPSV)
VelAccCorr2＝EFAcc+AccCorr2GPSV+∑AccCorrGPSV
VelRateCorr2＝∑(VelAccCorr2·dt+VelCorr2GPSV)
其中，AccCorrGPS、VelCorrGPS、PosCorrGPS分别为通过GPS得到的位置数据对加
速度、速度和位置的修正量；AccCorrGPSV、VelCorrGPSV分别为通过GPS得到的速度数据对
加速度、速度的第一修正量；AccCorr2GPSV、VelCorr2GPSV分别为通过GPS得到的速度数据
对加速度、速度的第二修正量；VelAccCorr、VelRateCorr分别为通过第一修正量修正后得
到的加速度和速度数据；PosEst为计算出的当前的位置数据；VelAccCorr2、VelRateCorr2
分别为通过第二修正量修正后得到的加速度和速度数据；PosGPS为GPS得到的位置数据；
TimeConstGPS、TimeConstGPSV和TimeConstGPSV2为预先设定的常数；VelGPSdata为通过
GPS得到的速度数据；EFAcc为通过加速度计得到的加速度数据；dt为控制系统的控制周期。
本发明实施例提供的这种位置计算方法，通过考虑GPS模块输出的位置数据、速度
数据并结合了加速度计的加速度数据，对无人机当前的位置、速度和加速度进行了修正，实
现了对位置、速度和加速度的更准确的估计，为无人机的位置的控制提供了更好的基础，提
高了无人机的性能和安全性。
在该实施例中，步骤S303可由以下步骤完成，包括：
步骤S3031：由主控制器根据当前的位置信息以及期望的位置信息得出速度控制
信息；
步骤S3032：由主控制器根据当前的速度信息以及期望的速度信息得出加速度控
制信息；
步骤S3033：由主控制器根据速度控制信息和加速度控制信息得出旋转四元数；
步骤S3034：由主控制器根据得出旋转四元数将飞行控制参数发送给动力装置进
行加减速控制。
本发明实施例提供的这种无人机的基于惯性导航位置估算的控制方法，通过对当
前位置信息、当前的速度信息、期望的位置信息、期望的速度信息得出速度控制信息和加速
度控制信息并进一步得出旋转四元数，由主控制器根据得出旋转四元数将飞行控制参数发
送给动力装置进行加减速控制，实现了基于惯性导航位置估算的控制。该控制方法处理速
度较快，精度更高，能解决对无人机的位置、速度的控制速度、控制精度不够高的问题，提高
了无人机的性能和安全性。
在本发明的一个实施例中，所述由主控制器根据速度控制信息和加速度控制信息
得出旋转四元数，包括：
所述四元数为通过对第三公式进行归一化处理后得到，所述第三公式为：
其中，
AccTarX＝AccdesirdX+velLoopBrakeAcc×(veltargetX-vellastX)
-RateKP×VdiffX-RateKI×VintegrX
AccTarY＝AccdesirdY+velLoopBrakeAcc×(veltargetY-vellastY)
-RateKP×VdiffY-RateKI×VintegrY
其中，qw、qx、qy、qz为未归一化处理的四元数；AccTarX和AccTarY为向北和向东的
期望加速度；AccdesirdX和AccdesirdY为向北和向东的期望加速度；velLoopBrakeAcc为刹
车时存在的一个线性变化的数值，不刹车时为0；veltargetX和veltargetY为向北和向东的
期望速度；vellastX和vellastY为上一控制周期结束时的向北和向东的速度；RateKP为控
制系统的速度比例系数；RateKI为控制系统的速度积分系数；VdiffX为VdiffY为向北和向
东方向的期望速度与当前速度的差值；VintegrX和VintegrY为向北和向东方向的速度的积
分数值。
本发明实施例提供的这种得出旋转四元数的方法，通过PI控制系统进行控制，容
易进行实时控制，控制鲁棒性强。
在本发明的一个实施例中，图4A所示为一种无人机的基于高度估算法的控制系统
框图；如图4B所示，为本发明实施例中一种无人机的基于高度估算法的控制方法流程图，包
括步骤：
步骤S401：由气压计模块得到地面气压值和当前气压值；由惯性测量模块得到由
地面起飞到当前状态的加速度数据；由GPS模块得到GPS经纬度信息和NED速度信息；
步骤S402：由主控制器根据地面气压值和当前气压值、由地面起飞到当前状态的
加速度数据、GPS经纬度信息和NED速度信息根据高度估算法估算当前的高度信息；
步骤S403：由主控制器根据遥控输入信息和当前的高度信息对油门进行控制，所
用控制方法为通过高度控制器根据油门控制曲线对油门输出指令。
本发明实施例提供的无人机的基于高度估算法的控制方法，实现了对无人机的高
度的控制，可以解决目前无人机的高度的控制精度不够高的问题，提高无人机的性能和安
全性。
在本发明的一个实施例中，所述由主控制器根据地面气压值和当前气压值、由地
面起飞到当前状态的加速度数据、GPS经纬度信息和NED速度信息根据高度估算法估算当前
的高度信息，包括：
当前的高度信息由第四公式计算得到，所述第四公式为：
其中，EstAlt为估算的当前的高度；velRateCorrZ为通过气压计和GPS修正后的当
前速度的高度方向分量；velAccCorrZ为通过气压计和GPS修正后的当前加速度的高度方向
分量；dt为控制系统的控制周期；Pgroud为地面上的气压值，P为当前位置的气压值。
本发明实施例提供的这种高度估算法，考虑了气压的影响，并通过GPS数据进行修
正，比单纯的使用加速度进行多次积分得到的结果更准确，稳定性更好。
在本发明的一个实施例中，所述由主控制器根据遥控输入信息和当前的高度信息
对油门进行控制，所用控制方法为通过高度控制器根据油门控制曲线对油门输出指令，包
括：
所述输入信息为遥控器的期望速度；控制所需要的期望高度由遥控器的期望速度
对时间进行积分得到；油门控制为根据油门控制曲线和加速度数据进行PI控制，油门控制
曲线由第五公式表示，所述第五公式为：
其中，Zexpv为高度方向期望速度；a1、a2、a3、a4为阈值的油门系数，其值均为正，且
a1<a2，a3<a4；throOff为遥控器油门的偏移值；EstAlt为估算的当前的高度，hf1、hf2为预设
的高度值，且hf1<hf2；linemap(a1,a2,EstAlt)表示以EstAlt为变量的单调递增函数，其上
限为a2，下限为a1，linemap(a3,a4,EstAlt)为上限为a4，下限为a3的同一函数。
本发明实施例提供的这种控制方法，通过PI控制系统进行控制，容易进行实时控
制，控制鲁棒性强。
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精
神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围
之内，则本发明也意图包含这些改动和变型在内。