[发明专利]一种无人机飞行控制及定位方法

权利要求书

1.一种无人机飞行控制及定位方法，其特征在于，包括：

一种无人机的基于互补滤波的姿态解算法，用于根据传感器的数据求解出无人机的空中姿态；

姿态控制算法，用于根据当前的姿态和期望的姿态的偏差，对无人机进行控制直到当前的姿态达到期望的姿态；

一种无人机的基于惯性导航位置估算的控制方法，用于根据位置和速度信息对无人机进行控制；

一种无人机的基于高度估算法的控制方法，用于根据高度信息对无人机进行控制；

所述一种无人机的基于惯性导航位置估算的控制方法，包括：

由主控制器根据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为控制系统的控制周期。

2.如权利要求1所述的方法，所述一种无人机的基于互补滤波的姿态解算法，其特征在于，包括：

对传感器进行校准，包括：通过椭球校准、温度校准、当地磁偏角校准方法对加速度计、陀螺仪、三轴磁力计进行校准；

由主控制器根据当前传感器的数据用基于互补滤波的姿态解算法计算得到当前的机体四元数；

由主控制器根据收到的控制信息计算出期望的机体四元数；

由主控制器根据当前的机体四元数和期望的机体四元数以及GPS定位期望四元数通过误差四元数PID控制环向微控制器发送电机调速指令。

3.如权利要求2所述的方法，所述由主控制器根据当前传感器的数据用基于互补滤波的姿态解算法计算得到当前的机体四元数，其特征在于，包括：

由主控制器用加速度计和磁力计的数据校准陀螺仪的漂移，所用方法根据第一公式为对陀螺仪的数据进行补偿，所述第一公式为：

ΔGyro＝K_P·(W×Mag+V×Acc)+∑K_I·(W×Mag+V×Acc)·dt

其中，

其中，ΔGyro为对陀螺仪读数进行补偿的数据；K_P为控制系统的比例系数；Mag为磁力计读数向量；Acc为加速度计读数向量；K_I为控制系统的积分系数；dt为控制系统的控制周期；g为当地的重力加速度常数；为表示地面坐标系到机体坐标系的四元数坐标旋转矩阵；为机体坐标系到地面坐标系的四元数坐标旋转矩阵；V、W、H为中间变量。