CN106774378A - 一种无人机飞行控制及定位方法 - Google Patents
一种无人机飞行控制及定位方法 Download PDFInfo
- Publication number
- CN106774378A CN106774378A CN201710118327.2A CN201710118327A CN106774378A CN 106774378 A CN106774378 A CN 106774378A CN 201710118327 A CN201710118327 A CN 201710118327A CN 106774378 A CN106774378 A CN 106774378A
- Authority
- CN
- China
- Prior art keywords
- information
- control
- current
- speed
- unmanned plane
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Abstract
本发明提供了一种无人机飞行控制及定位方法,包括:一种无人机的基于互补滤波的姿态解算法,用于根据传感器的数据求解出无人机的空中姿态;姿态控制算法,用于根据当前的姿态和期望的姿态的偏差,对无人机进行控制直到当前的姿态达到期望的姿态;一种无人机的基于惯性导航位置估算的控制方法,用于根据位置和速度信息对无人机进行控制;一种无人机的基于高度估算法的控制方法,用于根据高度信息对无人机进行控制。本发明提供的一种无人机飞行控制及定位方法可以解决目前无人机的姿态、位置、速度、高度的控制速度、控制精度不够高的问题,提高了无人机的性能和安全性。
Description
技术领域
本发明涉及无人机技术领域,特别涉及一种无人机飞行控制及定位方法。
背景技术
无人机全称为无人驾驶飞机,是利用无线电遥控设备和自备的程序控制装置操纵的不载人飞机。
多旋翼无人机属于新兴技术、高新前沿科技产业。随着科技的发展,无人机除用于军事用途外,在民用领域的应用范围也越来越广阔。由于无人机具有运行成本低、无伤亡风险、机动性能好、可进行超视距飞行、使用方便高效等特点,目前已被成功应用于影视航拍、测绘航测、高压线巡查、远程监控、救灾救援、农药喷洒、商业表演等领域,越来越多的行业正希望用无人机取代传统的人工作业方式。
无人机上的飞行控制系统,用以控制无人机的飞行,是无人机技术的核心,控制系统的优劣直接影响无人机的性能甚至安全性,而对无人机的姿态、位置、速度、高度等信息的准确获取和控制则是一个优秀的无人机控制系统的基础。如何提高无人机的姿态、位置、速度、高度的控制速度、控制精度是当前需要解决的问题。
发明内容
本发明提供了一种无人机飞行控制及定位方法,用以解决目前无人机的姿态、位置、速度、高度的控制速度、控制精度不够高的问题,提高无人机的性能和安全性。
本发明提供的一种无人机飞行控制及定位方法,包括:
一种无人机的基于互补滤波的姿态解算法,用于根据传感器(陀螺仪、加速度计、罗盘等)的数据求解出无人机的空中姿态;
姿态控制算法,用于根据当前的姿态和期望的姿态的偏差,对无人机进行控制直到当前的姿态达到期望的姿态;
一种无人机的基于惯性导航位置估算的控制方法,用于根据位置和速度信息对无人机进行控制;
一种无人机的基于高度估算法的控制方法,用于根据高度信息对无人机进行控制。
优选的,所述一种无人机的基于互补滤波的姿态解算法,包括:
对传感器进行校准,包括:通过椭球校准、温度校准、当地磁偏角校准等方法对加速度计、陀螺仪、三轴磁力计进行校准;
由主控制器根据当前传感器的数据用基于互补滤波的姿态解算法计算得到当前的机体四元数;
由主控制器根据收到的控制信息计算出期望的机体四元数;
由主控制器根据当前的机体四元数和期望的机体四元数以及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控制系统进行控制,容易进行实时控制,控制鲁棒性强。
显然,本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样,倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内,则本发明也意图包含这些改动和变型在内。
Claims (10)
1.一种无人机飞行控制及定位方法,其特征在于,包括:
一种无人机的基于互补滤波的姿态解算法,用于根据传感器的数据求解出无人机的空中姿态;
姿态控制算法,用于根据当前的姿态和期望的姿态的偏差,对无人机进行控制直到当前的姿态达到期望的姿态;
一种无人机的基于惯性导航位置估算的控制方法,用于根据位置和速度信息对无人机进行控制;
一种无人机的基于高度估算法的控制方法,用于根据高度信息对无人机进行控制。
2.如权利要求1所述的方法,所述一种无人机的基于互补滤波的姿态解算法,其特征在于,包括:
对传感器进行校准,包括:通过椭球校准、温度校准、当地磁偏角校准等方法对加速度计、陀螺仪、三轴磁力计进行校准;
由主控制器根据当前传感器的数据用基于互补滤波的姿态解算法计算得到当前的机体四元数;
由主控制器根据收到的控制信息计算出期望的机体四元数;
由主控制器根据当前的机体四元数和期望的机体四元数以及GPS定位期望四元数通过误差四元数PID控制环向微控制器发送电机调速指令。
3.如权利要求2所述的方法,所述由主控制器根据当前传感器的数据用基于互补滤波的姿态解算法计算得到当前的机体四元数,其特征在于,包括:
由主控制器用加速度计和磁力计的数据校准陀螺仪的漂移,所用方法根据第一公式为对陀螺仪的数据进行补偿,所述第一公式为:
ΔGyro=KP·(W×Mag+V×Acc)+∑KI·(W×Mag+V×Acc)·dt
其中,
其中,ΔGyro为对陀螺仪读数进行补偿的数据;KP为控制系统的比例系数;Mag为磁力计读数向量;Acc为加速度计读数向量;KI为控制系统的积分系数;dt为控制系统的控制周期;g为当地的重力加速度常数;为表示地面坐标系到机体坐标系的四元数坐标旋转矩阵;为机体坐标系到地面坐标系的四元数坐标旋转矩阵;V、W、H为中间变量。
4.如权利要求1所述的方法,所述一种无人机的基于惯性导航位置估算的控制方法,其特征在于,包括:
由主控制器根据GPS模块输出的位置数据计算出当前的经纬度和NED速度信息;
由主控制器根据遥控输入信息计算出期望的速度信息和期望的位置信息;
由主控制器根据当前的速度信息和位置信息以及期望的速度信息和期望的位置信息进行加减速控制。
5.如权利要求4所述的方法,所述由主控制器根据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为控制系统的控制周期。
6.如权利要求4所述的方法,所述由主控制器根据当前的速度信息和位置信息以及期望的速度信息和期望的位置信息进行加减速控制,其特征在于,包括:
由主控制器根据当前的位置信息以及期望的位置信息得出速度控制信息;
由主控制器根据当前的速度信息以及期望的速度信息得出加速度控制信息;
由主控制器根据速度控制信息和加速度控制信息得出旋转四元数;
由主控制器根据得出旋转四元数将飞行控制参数发送给动力装置进行加减速控制。
7.如权利要求6所述的方法,所述由主控制器根据速度控制信息和加速度控制信息得出旋转四元数,其特征在于,包括:
所述四元数为通过对第三公式进行归一化处理后得到,所述第三公式为:
其中,
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为向北和向东方向的速度的积分数值。
8.如权利要求1所述的方法,所述一种无人机的基于高度估算法的控制方法,其特征在于,包括:
由气压计模块得到地面气压值和当前气压值;由惯性测量模块得到由地面起飞到当前状态的加速度数据;由GPS模块得到GPS经纬度信息和NED速度信息;
由主控制器根据地面气压值和当前气压值、由地面起飞到当前状态的加速度数据、GPS经纬度信息和NED速度信息根据高度估算法估算当前的高度信息;
由主控制器根据遥控输入信息和当前的高度信息对油门进行控制,所用控制方法为通过高度控制器根据油门控制曲线对油门输出指令。
9.如权利要求8所述的方法,所述由主控制器根据地面气压值和当前气压值、由地面起飞到当前状态的加速度数据、GPS经纬度信息和NED速度信息根据高度估算法估算当前的高度信息,其特征在于,包括:
当前的高度信息由第四公式计算得到,所述第四公式为:
其中,EstAlt为估算的当前的高度;velRateCorrZ为通过气压计和GPS修正后的当前速度的高度方向分量;velAccCorrZ为通过气压计和GPS修正后的当前加速度的高度方向分量;dt为控制系统的控制周期;Pgroud为地面上的气压值,P为当前位置的气压值。
10.如权利要求9所述的方法,所述由主控制器根据遥控输入信息和当前的高度信息对油门进行控制,所用控制方法为通过高度控制器根据油门控制曲线对油门输出指令,其特征在于,包括:
所述输入信息为遥控器的高度方向期望速度;控制所需要的期望高度由遥控器的高度方向期望速度对时间进行积分得到;油门控制为根据油门控制曲线和加速度数据进行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的同一函数。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710118327.2A CN106774378B (zh) | 2017-03-01 | 2017-03-01 | 一种无人机飞行控制及定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710118327.2A CN106774378B (zh) | 2017-03-01 | 2017-03-01 | 一种无人机飞行控制及定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106774378A true CN106774378A (zh) | 2017-05-31 |
CN106774378B CN106774378B (zh) | 2019-08-09 |
Family
ID=58959167
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710118327.2A Active CN106774378B (zh) | 2017-03-01 | 2017-03-01 | 一种无人机飞行控制及定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106774378B (zh) |
Cited By (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107272740A (zh) * | 2017-07-28 | 2017-10-20 | 北京航天光华电子技术有限公司 | 一种新型四旋翼无人机控制系统 |
CN108702432A (zh) * | 2017-09-11 | 2018-10-23 | 深圳市大疆创新科技有限公司 | 云台相机及具有该云台相机的无人机 |
CN108801322A (zh) * | 2018-07-06 | 2018-11-13 | 哈尔滨工业大学 | 用于无人机飞行控制系统微机电系统传感器的可靠性评估方法 |
CN109917800A (zh) * | 2019-03-19 | 2019-06-21 | 中国科学院深圳先进技术研究院 | 无人机控制方法、装置、计算机设备及存储介质 |
CN111221347A (zh) * | 2020-04-21 | 2020-06-02 | 广东英诺威盛科技有限公司 | 垂直起降固定翼无人机姿态估计中加速度补偿方法及系统 |
CN111580542A (zh) * | 2019-02-15 | 2020-08-25 | 北京京东尚科信息技术有限公司 | 动态无人机编队控制方法、装置及存储介质 |
CN111580553A (zh) * | 2020-05-11 | 2020-08-25 | 桂林电子科技大学 | 一种无人机飞行控制器、无人机防疫监管系统和方法 |
CN112198797A (zh) * | 2020-10-23 | 2021-01-08 | 国网智能科技股份有限公司 | 一种无人机高度多级控制系统及方法 |
CN112731960A (zh) * | 2020-12-02 | 2021-04-30 | 国网辽宁省电力有限公司阜新供电公司 | 一种无人机远程输电线路智能巡检系统和方法 |
CN114679540A (zh) * | 2018-11-19 | 2022-06-28 | 深圳市大疆创新科技有限公司 | 拍摄方法和无人机 |
CN116774734A (zh) * | 2023-08-24 | 2023-09-19 | 北京中景合天科技有限公司 | 一种基于无人机的智慧旅游景区数字孪生巡查方法 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5581250A (en) * | 1995-02-24 | 1996-12-03 | Khvilivitzky; Alexander | Visual collision avoidance system for unmanned aerial vehicles |
CN102809377A (zh) * | 2012-08-15 | 2012-12-05 | 南京航空航天大学 | 飞行器惯性/气动模型组合导航方法 |
CN104898681A (zh) * | 2015-05-04 | 2015-09-09 | 浙江工业大学 | 一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法 |
CN104914874A (zh) * | 2015-06-09 | 2015-09-16 | 长安大学 | 一种基于自适应互补融合的无人机姿态控制系统及方法 |
CN105094138A (zh) * | 2015-07-15 | 2015-11-25 | 东北农业大学 | 一种用于旋翼无人机的低空自主导航系统 |
CN106249745A (zh) * | 2016-07-07 | 2016-12-21 | 苏州大学 | 四轴无人机的控制方法 |
CN106444804A (zh) * | 2016-09-16 | 2017-02-22 | 杭州电子科技大学 | 基于互补滤波算法和串级pid的四旋翼飞行器设计方法 |
-
2017
- 2017-03-01 CN CN201710118327.2A patent/CN106774378B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5581250A (en) * | 1995-02-24 | 1996-12-03 | Khvilivitzky; Alexander | Visual collision avoidance system for unmanned aerial vehicles |
CN102809377A (zh) * | 2012-08-15 | 2012-12-05 | 南京航空航天大学 | 飞行器惯性/气动模型组合导航方法 |
CN104898681A (zh) * | 2015-05-04 | 2015-09-09 | 浙江工业大学 | 一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法 |
CN104914874A (zh) * | 2015-06-09 | 2015-09-16 | 长安大学 | 一种基于自适应互补融合的无人机姿态控制系统及方法 |
CN105094138A (zh) * | 2015-07-15 | 2015-11-25 | 东北农业大学 | 一种用于旋翼无人机的低空自主导航系统 |
CN106249745A (zh) * | 2016-07-07 | 2016-12-21 | 苏州大学 | 四轴无人机的控制方法 |
CN106444804A (zh) * | 2016-09-16 | 2017-02-22 | 杭州电子科技大学 | 基于互补滤波算法和串级pid的四旋翼飞行器设计方法 |
Non-Patent Citations (1)
Title |
---|
李汉博,等: "四旋翼惯性导航姿态解算算法的改进与仿真", 《计算机测量与控制》 * |
Cited By (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107272740A (zh) * | 2017-07-28 | 2017-10-20 | 北京航天光华电子技术有限公司 | 一种新型四旋翼无人机控制系统 |
CN108702432A (zh) * | 2017-09-11 | 2018-10-23 | 深圳市大疆创新科技有限公司 | 云台相机及具有该云台相机的无人机 |
CN108702432B (zh) * | 2017-09-11 | 2020-12-29 | 深圳市大疆创新科技有限公司 | 云台相机及具有该云台相机的无人机 |
CN108801322A (zh) * | 2018-07-06 | 2018-11-13 | 哈尔滨工业大学 | 用于无人机飞行控制系统微机电系统传感器的可靠性评估方法 |
CN114679540A (zh) * | 2018-11-19 | 2022-06-28 | 深圳市大疆创新科技有限公司 | 拍摄方法和无人机 |
CN111580542A (zh) * | 2019-02-15 | 2020-08-25 | 北京京东尚科信息技术有限公司 | 动态无人机编队控制方法、装置及存储介质 |
CN109917800A (zh) * | 2019-03-19 | 2019-06-21 | 中国科学院深圳先进技术研究院 | 无人机控制方法、装置、计算机设备及存储介质 |
CN109917800B (zh) * | 2019-03-19 | 2022-08-12 | 中国科学院深圳先进技术研究院 | 无人机控制方法、装置、计算机设备及存储介质 |
CN111221347A (zh) * | 2020-04-21 | 2020-06-02 | 广东英诺威盛科技有限公司 | 垂直起降固定翼无人机姿态估计中加速度补偿方法及系统 |
CN111580553A (zh) * | 2020-05-11 | 2020-08-25 | 桂林电子科技大学 | 一种无人机飞行控制器、无人机防疫监管系统和方法 |
CN112198797A (zh) * | 2020-10-23 | 2021-01-08 | 国网智能科技股份有限公司 | 一种无人机高度多级控制系统及方法 |
CN112198797B (zh) * | 2020-10-23 | 2023-01-24 | 国网智能科技股份有限公司 | 一种无人机高度多级控制系统及方法 |
CN112731960A (zh) * | 2020-12-02 | 2021-04-30 | 国网辽宁省电力有限公司阜新供电公司 | 一种无人机远程输电线路智能巡检系统和方法 |
CN116774734A (zh) * | 2023-08-24 | 2023-09-19 | 北京中景合天科技有限公司 | 一种基于无人机的智慧旅游景区数字孪生巡查方法 |
CN116774734B (zh) * | 2023-08-24 | 2023-10-24 | 北京中景合天科技有限公司 | 一种基于无人机的智慧旅游景区数字孪生巡查方法 |
Also Published As
Publication number | Publication date |
---|---|
CN106774378B (zh) | 2019-08-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106774378B (zh) | 一种无人机飞行控制及定位方法 | |
US10914590B2 (en) | Methods and systems for determining a state of an unmanned aerial vehicle | |
CN103363992B (zh) | 基于梯度下降的四旋翼无人机姿态航向参考系统解算方法 | |
Lupashin et al. | Stabilization of a flying vehicle on a taut tether using inertial sensing | |
CN104536453B (zh) | 飞行器的控制方法及装置 | |
Kingston et al. | Real-time attitude and position estimation for small UAVs using low-cost sensors | |
Chao et al. | Remote sensing and actuation using unmanned vehicles | |
CN104850127B (zh) | 一种可动感操控四旋翼飞行器的方法 | |
CN205247213U (zh) | 使用在无人机上的高精度定位巡航系统 | |
CN105793792A (zh) | 无人机的飞行辅助方法和系统、无人机和移动终端 | |
CN104115081A (zh) | 利用恒定倾斜角转弯的风计算系统 | |
CN107256030A (zh) | 无人飞行器的遥控终端、飞行辅助系统和方法 | |
CN111207745B (zh) | 一种适用于大机动无人机垂直陀螺仪的惯性测量方法 | |
CN104503466A (zh) | 一种微小型无人机导航装置 | |
CN204302801U (zh) | 飞行器系统 | |
Dorobantu et al. | An airborne experimental test platform: From theory to flight | |
CN109084760B (zh) | 一种楼宇间导航系统 | |
CN103837151A (zh) | 一种四旋翼飞行器的气动模型辅助导航方法 | |
Iwaneczko et al. | A prototype of unmanned aerial vehicle for image acquisition | |
JP2015024705A (ja) | 小型電動ヘリコプタの自動離着陸制御方法 | |
CN105865455A (zh) | 一种利用gps与加速度计计算飞行器姿态角的方法 | |
CN106843275A (zh) | 一种无人机定点绕飞方法、装置以及系统 | |
CN105468010A (zh) | 多自由度惯性传感器四轴无人机自主导航飞行控制器 | |
CN106547275A (zh) | 一种新型旋翼类无人机自动定位操控方法 | |
Stojcsics | Autonomous waypoint-based guidance methods for small size unmanned aerial vehicles |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |