一种多旋翼无人机悬停方法

技术领域

本发明涉及无人机悬停技术领域，尤其涉及一种多旋翼无人机悬停方法。

背景技术

随着航空与电子技术的发展，多旋翼无人机因其高机动性、安全性、可靠性在军事中的军情侦察、农业植保、工厂巡检、森林灭火、航拍等诸多领域对无人机精准悬停有广泛的应用场景。同时，在逐渐复杂化的场景中，多旋翼无人机的悬停精度也被赋予了更高的要求。多旋翼无人机的悬停精度在很大程度上决定了无人机任务完成的质量。因此，多旋翼无人机悬停方法在实际应用中有着重要的工程意义和价值。

相关技术的多旋翼无人机悬停方法一般采用模糊自适应串级PID控制方法、滑模控制悬停方法、神经网络训练悬停方法和光流定位悬停方法。

然而，模糊自适应串级PID控制方法直接使用系统输入输出之间的误差进行悬停控制，不依赖于系统模型，并且其算法本身易受外界扰动的影响，算法超调过大，当其应用于外界扰动较大或复杂多变的环境时，其很难保证多旋翼无人机在悬停时保持稳定，无人机悬停系统的实时性要求较高，而自适应串级PID控制算法依赖于精准模型和强大的计算能力，模糊自适应串级PID控制方法的系统解决方案的计算量和实时性有待提升。滑模控制悬停方法对于不确定模型系统和外界具有较强的鲁棒性，对控制目标响应速度较快，但是普通滑模控制悬停法存在稳态误差无法在有效的时间内收敛、滑动时沿滑模面抖动等问题，且实现难度较大。神经网络训练悬停方法有自组织自学习的能力，容错能力和稳健性强大，但其稳定性较差，学习速度慢，难以满足无人机实时悬停控制的需求。光流定位悬停方法对于一切可能影响视觉评判结果的因素都可能影响光流定位悬停的准确性，当地面为单一颜色、重复图案、镜面或水面或反光较为强烈的反光面时，会严重影响无人机光流摄像头的判断，即使无人机发生了偏移，其光流定位系统也检测不到。因此光流定位悬停法只适用于环境较为简单的平面，对于图案复杂、凹凸不平的地面环境，光流悬停法的误差较大，不能达到理想的悬停高度和悬停区域，不能满足多旋翼无人机精准悬停的需求。

发明内容

针对以上现有技术的不足，本发明提出一种实用性高、抗扰性能好、控制精度高、参数易于整定和系统运算量小的多旋翼无人机悬停方法。

为了解决上述技术问题，本发明实施例提供了一种多旋翼无人机悬停方法，该方法包括如下步骤：

步骤S1、通过多旋翼无人机的多个传感器获得所述多旋翼无人机的机体的姿态角和高度位置，建立多旋翼无人机基于所述姿态角与所述高度位置的动力学模型；

步骤S2、采用PID改进型互补滤波算法设计；具体为：通过运用PID控制器调整所述多旋翼无人机的滤波器的截止频率、消除静态误差的时间、抑制数据的震动以及增强系统的快速性；

步骤S3、基于四元数的扩展卡尔曼滤波算法设计；具体为：通过所述步骤S2中取得的所述四元数为输入数据，结合所述步骤S1建立的所述动力学建模,通过所述扩展卡尔曼滤波算法分析设计，以实现获得到在每个更新周期中所述多旋翼无人机估计的姿态参数和垂直方向上的加速度、速度或者位置参数；

步骤S4、将所述步骤S3中所述扩展卡尔曼滤波算法分析设计后的数据输入到所述多旋翼无人机的动力系统中，以实现将所述多旋翼无人机进行悬停控制；所述悬停控制具体为：当所述多旋翼无人机接收到外部输入的悬停指令后，若所述多旋翼无人机受到外部因素出现偏移时，通过所述多旋翼无人机的加速度计测量的加速度值进行积分计算出偏移位移，根据所述偏移位移控制所述动力系统向偏移相反的方向移动相同的位移量，以实现所述多旋翼无人机在悬停过程中的快速矫正并回到初始悬停位置。

优选的，所述步骤S1中，所述传感器包括加速度计、陀螺仪、磁力计、超声波和气压计。

优选的，所述步骤S1包括：

步骤S11、通过所述传感器获得多个传感器数据；

步骤S12、通过所述加速度计获取的数据、所述陀螺仪获取的数据和所述磁力计获取的数据计算出姿态信息；

步骤S13、通过所述加速度计获得的数据、所述超声波获得的数据和所述气压计获得的数据计算出高度融合估计和最优高度值。

优选的，所述步骤S12中，所述姿态信息包括姿态角x

其中，u

所述步骤S13中，所述加速度计获得的数据和所述超声波获得的数据融合测高，满足如下公式：

a

其中，a

初始时高度值为h

v

最优高度值为h，满足如下公式：

h＝k×h

h

优选的，所述步骤S2包括：

步骤S21、根据所述加速度计获得的数据、所述磁力计获得的数据、所述地球重力加速度、地磁场强度的误差通过所述PID改进型互补滤波算法进行补偿陀螺仪的数据；

步骤S22、将所述补偿陀螺仪的数据通过一阶龙格库塔方程对所述四元数进行更新。

优选的，所述步骤S21中，所述地磁场强度的误差为

为/>

为所述多旋翼无人机的机体坐标系b系重力加速度；

为/>

为b系的地磁强度；

分别为地球坐标系N系的重力加速度[0 0 0

b为参数、N为参数；

q

所述步骤S22中，所述PID改进型互补滤波算法的输出量为

K

为修正后的角度值，/>

q为所述四元数，

ω

优选的，所述步骤S3包括：

步骤S31、建立所述扩展卡尔曼滤波算法的非线性模型；

步骤S32、将所述非线性模型在状态点的函数通过泰勒级数展开进行线性化计算，以实现系统的时间更新。

优选的，所述步骤S31中，所述非线性模型包括状态方程和测量方程，

所述状态方程为：X

所述测量方程为：Z

其中，X

Z

所述步骤S32中，

所述非线性模型在状态点的函数满足如下公式：

其中，

为k时刻的协方差矩阵，P

所述系统的时间更新满足如下公式：

其中，K

优选的，所述外部因素包括风力。

与现有技术相比，本发明的多旋翼无人机悬停方法通过步骤S1、通过多旋翼无人机的多个传感器获得所述多旋翼无人机的机体的姿态角和高度位置，建立多旋翼无人机基于所述姿态角与所述高度位置的动力学模型；实施所述步骤S1,通过所述动力学模型解决所述多旋翼无人机在不同高度测量精度低问题，针对所述多旋翼无人机的悬停的高度，通过选用不同的高度测量传感器，不仅提高测量精度和实时性，同时还改善高度测量系统的冗余性。通过步骤S2、采用PID改进型互补滤波算法设计；实施所述步骤S2，采用所述PID改进型互补滤波算法面向实际应用中的所述多旋翼无人机的控制系统的多姿态数据融合过程，通过运用所述PID控制器选取合适的参数来调整截止频率，从所述多旋翼无人机的加速度计、磁力计获取的测量数据与重力加速度、地磁场强度的误差来补偿陀螺仪姿态数据，综合考虑所述多旋翼无人机的静态和动态误差，抑制姿态数据的震动，以及增强所述多旋翼无人机的悬停系统的快速性和稳定性。步骤S3、基于四元数的扩展卡尔曼滤波算法设计；实施所述步骤S3,采用所述扩展卡尔曼滤波算法与自适应高度融合估计配合使用不仅提高了所述多旋翼无人机的稳定性，也优化了系统模型，解决了所述多旋翼无人机的悬停单独使用卡尔曼滤波时矩阵运算量较多，计算量稍大，不能使所述多旋翼无人机在受风干扰稳定悬停、悬停高度受限等问题。通过步骤S4、将所述步骤S3中所述扩展卡尔曼滤波算法分析设计后的数据输入到所述多旋翼无人机的动力系统中，以实现将所述多旋翼无人机进行悬停控制。通过实施所述步骤S4,可以使得所述多旋翼无人机悬停过程中的快速矫正，回到初始悬停位置。

附图说明

下面结合附图详细说明本发明。通过结合以下附图所作的详细描述，本发明的上述或其他方面的内容将变得更清楚和更容易理解。附图中：

图1为本发明多旋翼无人机悬停方法的流程框图；

图2为本发明多旋翼无人机悬停方法中的步骤S1的流程框图；

图3为本发明多旋翼无人机悬停方法中的步骤S2的流程框图；

图4为本发明多旋翼无人机悬停方法中的步骤S3的流程框图。

具体实施方式

下面结合附图详细说明本发明的具体实施方式。

在此记载的具体实施方式/实施例为本发明的特定的具体实施方式，用于说明本发明的构思，均是解释性和示例性的，不应解释为对本发明实施方式及本发明范围的限制。除在此记载的实施例外，本领域技术人员还能够基于本申请权利要求书和说明书所公开的内容采用显而易见的其它技术方案，这些技术方案包括采用对在此记载的实施例的做出任何显而易见的替换和修改的技术方案，都在本发明的保护范围之内。

本发明提供了一种多旋翼无人机悬停方法。

请参考图1所示，图1为本发明多旋翼无人机悬停方法的流程框图。

所述多旋翼无人机悬停方法包括如下步骤：

步骤S1、通过多旋翼无人机的多个传感器获得所述多旋翼无人机的机体的姿态角和高度位置，建立多旋翼无人机基于所述姿态角与所述高度位置的动力学模型。

其中，所述传感器包括加速度计、陀螺仪、磁力计、超声波和气压计。

请参考图2所示，图2为本发明多旋翼无人机悬停方法中的步骤S1的流程框图。具体的，所述步骤S1还包括：

步骤S11、通过所述传感器获得多个传感器数据。

步骤S12、通过所述加速度计获取的数据、所述陀螺仪获取的数据和所述磁力计获取的数据计算出姿态信息。

所述步骤S12中，所述姿态信息包括姿态角x

其中，u

步骤S13、通过所述加速度计获得的数据、所述超声波获得的数据和所述气压计获得的数据计算出高度融合估计和最优高度值。

所述步骤S13中，所述加速度计获得的数据和所述超声波获得的数据融合测高，满足如下公式：

a

其中，a

初始时高度值为h

v

最优高度值为h，考虑到所述超声波的测量范围和精度，所述气压计的低空精度低可靠性差的特点，采用自适应加权滤波算法对分散融合得到的高度估计进行综合处理，得到最优高度值h，满足如下公式：

h＝k×h

h

本实施例中，当所述多旋翼无人机的高度h低于超声波的范围时，k取较大值；当所述多旋翼无人机的高度在8～12m时，k取较小的值；当高度大于12m时，k的取值为0。

实施所述步骤S1,通过所述动力学模型解决所述多旋翼无人机在不同高度测量精度低问题，针对所述多旋翼无人机的悬停的高度，通过选用不同的高度测量传感器，不仅提高测量精度和实时性，同时还改善高度测量系统的冗余性。

步骤S2、采用PID改进型互补滤波算法设计；具体为：通过运用PID控制器调整所述多旋翼无人机的滤波器的截止频率、消除静态误差的时间、抑制数据的震动以及增强系统的快速性。具体的，通过运用PID控制器选取合适的K

请参考图3所示，图3为本发明多旋翼无人机悬停方法中的步骤S2的流程框图。具体的，所述步骤S2包括：

步骤S21、根据所述加速度计获得的数据、所述磁力计获得的数据、所述地球重力加速度、地磁场强度的误差通过所述PID改进型互补滤波算法进行补偿陀螺仪的数据。

所述步骤S21中，所述地磁场强度的误差为

/>

为/>

为所述多旋翼无人机的机体坐标系b系重力加速度；

为/>

为b系的地磁强度；

分别为地球坐标系N系的重力加速度[0 0 0

b为参数、N为参数；

q

步骤S22、将所述补偿陀螺仪的数据通过一阶龙格库塔方程对所述四元数进行更新。

所述步骤S22中，所述PID改进型互补滤波算法的输出量为

K

为修正后的角度值，/>

q为所述四元数，

ω

实施所述步骤S2，采用所述PID改进型互补滤波算法面向实际应用中的所述多旋翼无人机的控制系统的多姿态数据融合过程，通过运用所述PID控制器选取合适的参数来调整截止频率，从所述多旋翼无人机的加速度计、磁力计获取的测量数据与重力加速度、地磁场强度的误差来补偿陀螺仪姿态数据，综合考虑所述多旋翼无人机的静态和动态误差，抑制姿态数据的震动，以及增强所述多旋翼无人机的悬停系统的快速性和稳定性。

步骤S3、基于四元数的扩展卡尔曼滤波算法设计；具体为：通过所述步骤S2中取得的所述四元数为输入数据，结合所述步骤S1建立的所述动力学建模,通过所述扩展卡尔曼滤波算法分析设计，以实现获得到在每个更新周期中所述多旋翼无人机估计的姿态参数和垂直方向上的加速度、速度或者位置参数。

请参考图4所示，图4为本发明多旋翼无人机悬停方法中的步骤S2的流程框图。具体的，所述步骤S3包括：

步骤S31、建立所述扩展卡尔曼滤波算法的非线性模型。

所述步骤S31中，所述非线性模型包括状态方程和测量方程，

所述状态方程为：X

所述测量方程为：Z

其中，X

Z

其中，X

步骤S32、将所述非线性模型在状态点的函数通过泰勒级数展开进行线性化计算，以实现系统的时间更新。

所述步骤S32中，

所述非线性模型在状态点的函数满足如下公式：

其中，

为k时刻的协方差矩阵，P

所述系统的时间更新满足如下公式：

其中，K

实施所述步骤S3,采用所述扩展卡尔曼滤波算法与自适应高度融合估计配合使用不仅提高了所述多旋翼无人机的稳定性，也优化了系统模型，解决了所述多旋翼无人机的悬停单独使用卡尔曼滤波时矩阵运算量较多，计算量稍大，不能使所述多旋翼无人机在受风干扰稳定悬停、悬停高度受限等问题。

步骤S4、将所述步骤S3中所述扩展卡尔曼滤波算法分析设计后的数据输入到所述多旋翼无人机的动力系统中，以实现将所述多旋翼无人机进行悬停控制；所述悬停控制具体为：当所述多旋翼无人机接收到外部输入的悬停指令后，若所述多旋翼无人机受到外部因素出现偏移时，通过所述多旋翼无人机的加速度计测量的加速度值进行积分计算出偏移位移，根据所述偏移位移控制所述动力系统向偏移相反的方向移动相同的位移量，以实现所述多旋翼无人机在悬停过程中的快速矫正并回到初始悬停位置。

其中，所述外部因素包括风力。

通过实施所述步骤S4,可以使得所述多旋翼无人机悬停过程中的快速矫正，回到初始悬停位置。

综上所述，实施所述步骤S1至所述步骤S4，使得本发明的多旋翼无人机悬停方法更具实用性。本发明的多旋翼无人机悬停方法的适用范围更广，解决了相关技术中的光流定位悬停方法无法在复杂平面环境精准悬停的问题。而且使得所述多旋翼无人机所涉及的元件少，更加节约成本，更具有实用性的特点。

还使得本发明的多旋翼无人机悬停方法抗扰性能更强。本本发明的多旋翼无人机悬停方法使用所述陀螺仪、所述磁力计和所述加速度计协同工作，解决了所述多旋翼无人机在室外或者室内GPS信号弱无人机悬停时出现偏移，强磁干扰等问题，多种传感器协同，能够实现对扰动的实时估计与补偿，较大程度的增强了系统的抗扰能力。

还使得本发明的多旋翼无人机悬停方法控制精度更高。本发明的多旋翼无人机悬停方法以自适应高度融合后高度数据为输入，通过多种传感器协同工作，降低了控制器对因模型参数的不稳定而造成的影响，提高了控制精度。

还使得本发明的多旋翼无人机悬停方法参数易整定。本发明的多旋翼无人机悬停方法以经过自适应高度融合、PID改进型互补滤波算法整定得到的高度和姿态角，再经过所述扩展卡尔曼滤波算法进行滤波得到的数据为输入，调高了系统的扰动估计精度，同时简化了自扰控制器的结构，降低了参数整定的难度。

还使得本发明的多旋翼无人机悬停方法系统运算量更小。本发明的多旋翼无人机悬停方法使用所述扩展卡尔曼滤波算法通过一阶或者二阶的泰勒级数展开，忽略微小项的方式减小系统的运算量，从而提高所述多旋翼无人机的控制系统的响应速度。相比于相关技术中的模糊自适应串级PID控制算方法的运算量大、滑模控制悬停方法的系统模型难以确定、神经网络训练悬停方法需要对多旋翼无人机系统进行大量的训练、光流定位悬停法的悬停区域受限等，本发明的多旋翼无人机悬停方法具有更容易建立系统模型，运算量小，更快的响应速度的特点。

与现有技术相比，本发明的多旋翼无人机悬停方法通过步骤S1、通过多旋翼无人机的多个传感器获得所述多旋翼无人机的机体的姿态角和高度位置，建立多旋翼无人机基于所述姿态角与所述高度位置的动力学模型；实施所述步骤S1,通过所述动力学模型解决所述多旋翼无人机在不同高度测量精度低问题，针对所述多旋翼无人机的悬停的高度，通过选用不同的高度测量传感器，不仅提高测量精度和实时性，同时还改善高度测量系统的冗余性。通过步骤S2、采用PID改进型互补滤波算法设计；实施所述步骤S2，采用所述PID改进型互补滤波算法面向实际应用中的所述多旋翼无人机的控制系统的多姿态数据融合过程，通过运用所述PID控制器选取合适的参数来调整截止频率，从所述多旋翼无人机的加速度计、磁力计获取的测量数据与重力加速度、地磁场强度的误差来补偿陀螺仪姿态数据，综合考虑所述多旋翼无人机的静态和动态误差，抑制姿态数据的震动，以及增强所述多旋翼无人机的悬停系统的快速性和稳定性。步骤S3、基于四元数的扩展卡尔曼滤波算法设计；实施所述步骤S3,采用所述扩展卡尔曼滤波算法与自适应高度融合估计配合使用不仅提高了所述多旋翼无人机的稳定性，也优化了系统模型，解决了所述多旋翼无人机的悬停单独使用卡尔曼滤波时矩阵运算量较多，计算量稍大，不能使所述多旋翼无人机在受风干扰稳定悬停、悬停高度受限等问题。通过步骤S4、将所述步骤S3中所述扩展卡尔曼滤波算法分析设计后的数据输入到所述多旋翼无人机的动力系统中，以实现将所述多旋翼无人机进行悬停控制。通过实施所述步骤S4,可以使得所述多旋翼无人机悬停过程中的快速矫正，回到初始悬停位置。

以上内容仅为本发明的较佳实施例，对于本领域的普通技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，本说明书内容不应理解为对本发明的限制。