[发明专利]一种无人机飞控多传感器姿态置信解算方法

权利要求书

1.一种无人机飞控多传感器姿态置信解算方法，其特征在于，该方法具体包括如下步骤：

1）对无人机飞控系统中内置的3组传感器进行数据预测，建立数学观测模型：

2）各传感器通过测量科里奥利力来间接得到角速度，通过角速度数学模型阶导求得当前状态角速度与数学模型预测量；

3）将3组传感器最终测得的角速度数据中的其中一组直接输出至无人机飞控系统，其他两组数据分别输出至2个冗余观测器，通过计算IMU中加速度计和陀螺仪的误差来获得角速度预测量；

4）2个冗余观测器分别输出角速度预测量与实际角速度，共同解算与测量差值，在两组传感器数据中进对比；将以上数据进行整合，选择数据正常的传感器。

2.根据权利要求1所述的一种无人机飞控多传感器姿态置信解算方法，其特征在于，所述的步骤1）中建立的数学观测模型具体如下：

将无人机IMU中的加速度计简化为质量块+弹簧+指示计的结构，其具体公式如下

式中a_m为加速度计测量值，f为弹簧拉力，m为质量块质量，a为物体在世界坐标系下的实际加速度。

3.根据权利要求1所述的一种无人机飞控多传感器姿态置信解算方法，其特征在于，所述的步骤2）中的角速度数学模型为：

传感器采用东北天坐标系，考虑一个从原点出发的向量 r 绕单位轴 u 旋转，角速度大小为，角速度矢量表示为，易得向量r末端点P的速度矢量，即r的时间一阶导为：

坐标系{B}绕单位轴u旋转，如上所述，其三个轴的时间一阶导同样为：

[i_B jB k_B]为是坐标系{B}相对于参考坐标系的旋转矩阵R，R的时间一阶导为：

。

4.根据权利要求1所述的一种无人机飞控多传感器姿态置信解算方法，其特征在于，所述的步骤3）中IMU中加速度计和陀螺仪的误差可以分为确定性误差和随机误差。

5.根据权利要求4所述的一种无人机飞控多传感器姿态置信解算方法，其特征在于，所述的确定性误差事先标定确定，包括偏置误差和尺度误差；其中加速度计的偏置误差是假设理论上当没有外部作用时，IMU传感器的输出为 0，但实际输出数据存在的偏置数值；尺度误差是实际数值和传感器输出值之间的比值。

6.根据权利要求4所述的一种无人机飞控多传感器姿态置信解算方法，其特征在于，所述的随机误差假设噪声服从高斯分布，包括高斯白噪声和bias随机游走误差。

7.根据权利要求1所述的一种无人机飞控多传感器姿态置信解算方法，其特征在于，所述的步骤4）中数据对比遵循如下原则：

①若其中2组传感器出现异常，则认为观测误差较大的该2组传感器，而剩下1组传感器与该2组传感器差值较大；

②若单个传感器出现异常，该传感器与另外两组传感器误差较大，而另外两组传感器误差较小。