[发明专利]一种多旋翼无人飞行器悬停控制方法及系统

权利要求书

1.一种多旋翼无人飞行器悬停控制方法，其特征在于，包括如下步骤：

S1、判断多旋翼无人飞行器是否处于近似悬停状态；

S2、当确定多旋翼无人飞行器处于近似悬停状态时，建立水平失准角与水平加速度的关系；

S3、求解水平失准角；

S4、获取水平失准角在载体系的修正量；

S5、根据水平失准角在载体系的修正量对多旋翼无人飞行器的姿态进行修正，以获得悬停最终修正量；

以及S6、将所述悬停最终修正量输入多旋翼无人飞行器的姿态控制器中，使得姿态控制器据此进行卫星失锁状态下对于多旋翼无人飞行器的悬停控制。

2.如权利要求1所述的多旋翼无人飞行器悬停控制方法，其特征在于，步骤S1包括：通过安装在所述多旋翼无人飞行器上的加速度计实时获取多旋翼无人飞行器的加速度测量值f^b，并将其与重力g进行对比，若||f^b|-g|≤β₁，则认为此时多旋翼无人飞行器处于近似悬停状态；其中，所述β₁为预设的加速度判断阈值。

3.如权利要求2所述的多旋翼无人飞行器悬停控制方法，其特征在于，所述多旋翼无人飞行器的加速度测量值f^b为预定时间段内加速度计获取的若干加速度测量值的平均值。

4.如权利要求3所述的多旋翼无人飞行器悬停控制方法，其特征在于，所述预定时间段为5-10s。

5.如权利要求2所述的多旋翼无人飞行器悬停控制方法，其特征在于，β₁为0.2-1m/s²。

6.如权利要求1所述的多旋翼无人飞行器悬停控制方法，其特征在于，步骤S2包括：

根据式(1)-(6)建立水平失准角与水平加速度的关系：

式(1)中，为真实姿态阵，gⁿ为导航系下的重力加速度；

式(2)中，φ×为失准角φ＝[φ_E φ_N φ_U]的反对称阵，φ_E，φ_N，φ_U分别为导航系下东、北、天三轴的失准角分量；

fⁿ×φ＝δfⁿ (4)；

式(4)中，fⁿ为导航系下加速度计的测量值，且δfⁿ为导航系下多旋翼无人飞行器的运动加速度，且δfⁿ＝fⁿ+gⁿ；

式(5)中，“*”表示无需关注的元素；且多旋翼无人飞行器处于近似悬停状态时有fⁿ＝-gⁿ＝[0 0 -g]^T；

-gⁿ×φ^h＝δf^h (6)；

式(6)中，φ^h为多旋翼无人飞行器的水平失准角，且φ^h＝[φ_E φ_N 0]^T；δf^h为旋翼无人飞行器的水平运动加速度，且和分别为导航系下东和北向的多旋翼无人飞行器运动加速度。

7.如权利要求6所述的多旋翼无人飞行器悬停控制方法，其特征在于，步骤S3包括：

令单位矢量e₃＝[0 0 1]^T，根据式(5)和式(6)获得如式(7)所示的多旋翼无人飞行器的水平失准角φ^h：