一种无人机多余度传感器自仲裁方法及飞控板与流程

本发明涉及无人机飞行控制，具体涉及一种无人机多余度传感器自仲裁方法及飞控板。

背景技术：

1、目前，无人机的姿态与航向估计通常依赖惯性测量单元和磁场传感器。惯性测量单元输出三轴角速度和三轴比力，可在较高更新率下支撑姿态递推；磁场传感器输出三轴磁场矢量，可在姿态补偿后提供绝对或相对航向约束。对于数十架乃至更多无人机密集运行的集群表演场景，单机航向异常不仅影响自身轨迹，还可能破坏编队间隔，因此需要飞控系统持续判断进入估计器的数据是否可信。

2、惯性测量单元可能因温漂、强振动、通信中断、输出饱和、寄存器异常或器件失效而产生异常数据。仅采用单一惯性测量单元时，系统缺少实时参照；设置两颗或更多惯性测量单元后，若仅进行简单平均或固定主备切换，异常值仍可能被带入估计器，或者在切换瞬间造成姿态输入跳变。特别是仅有两个单元时，二者不一致本身并不能直接说明哪一方失效，还需要结合每个单元自身的独立健康证据进行归因。

3、磁场传感器容易受到无人机周围环境和自身电气设备的影响，而且不同位置、不同时间受到的干扰可能不同。无人机经过钢梁、音箱磁体或临时电缆附近时，某一路磁场矢量的模长和方向可能同时改变；电机、电调与配电走线的电流变化还会产生时变磁场。多磁场传感器之间的多数表决能够识别孤立异常，但当两个相邻的外置磁场传感器同时处于同一干扰源作用范围内时，二者可能呈现高度相关且彼此一致的错误变化。若先以传感器间一致性作为充分条件，该共同偏差会形成错误多数。另一方面，若全部磁场传感器暂时不可用，直接继续进行磁场量测更新会把污染量测注入航向状态，直接硬恢复又容易造成航向突变。

技术实现思路

1、本发明旨在解决多余度惯性与磁场传感器在异常或共同磁干扰条件下难以确定可信输入、以及传感器排除和恢复过程易造成估计不连续的技术问题，提供一种以可信惯性参考为非磁性约束、以磁场单路有效性检测先于通道间一致性检测、并配合健康状态渐变恢复的自仲裁方法及飞控板。

2、为实现上述目的，本发明提供了一种无人机多余度传感器自仲裁方法。所述无人机飞控板设有至少两个惯性测量单元和至少三个磁场传感器，方法包括：

3、s1，对各惯性测量单元输出的惯性测量数据进行交叉一致性检测，在至少两个惯性测量单元的惯性测量数据满足预设持续不一致条件时，根据各惯性测量单元的独立健康证据确定无效惯性测量单元并将其数据排除，根据通过检测的惯性测量数据形成可信惯性参考；

4、s2，根据所述可信惯性参考确定短时航向变化参考，并根据各磁场传感器输出的磁场矢量相对于当地地磁场的模长偏差以及各磁场传感器对应的磁航向变化与所述短时航向变化参考之间的变化偏差，确定候选磁场传感器；

5、s3，根据候选磁场传感器的数量及各候选磁场传感器之间的航向一致性，确定有效磁场传感器；

6、s4，根据所述交叉一致性检测和对各磁场传感器的仲裁结果，维护各各惯性测量单元和各磁场传感器的健康状态，将所述可信惯性参考以及处于可信状态的有效磁场传感器数据输入姿态与航向估计器，将处于无效状态的传感器数据排除；当处于无效状态的惯性测量单元重新通过交叉一致性检测，或者处于无效状态的磁场传感器重新通过仲裁时，将该传感器转为过渡状态，并使其数据按照渐变权重重新参与姿态与航向估计。

7、为优化上述技术方案，采取的具体措施还包括：

8、进一步地，步骤s1的交叉一致性检测包括：

9、计算不同惯性测量单元对应轴的角速度数据之差和比力数据之差；

10、当所述角速度数据之差或者比力数据之差连续个检测周期超过对应的差异阈值时，确定惯性测量数据持续不一致，其中，为预先设定的正整数；

11、根据各惯性测量单元的自检状态、通信状态、输出饱和状态、输出恒定状态和噪声异常状态，分别形成各惯性测量单元的独立健康证据；当惯性测量数据持续不一致，且其中一个惯性测量单元的独立健康证据异常时，将该惯性测量单元确定为无效惯性测量单元。

12、进一步地，步骤s2中根据各磁场传感器输出的磁场矢量相对于当地地磁场的模长偏差确定候选磁场传感器，包括：

13、根据无人机所在地的位置信息获得对应的地磁场强度，并根据无人机所在场地的磁场标定结果对所述地磁场强度进行校正，得到地磁场基准值；

14、计算各磁场矢量的模长相对于所述地磁场基准值的模长偏差，将所述模长偏差不超过模长容差的磁场传感器保留为候选磁场传感器。

15、进一步地，步骤s2中根据各磁场传感器对应的磁航向变化与所述短时航向变化参考之间的变化偏差确定候选磁场传感器，包括：

16、从通过检测的惯性测量数据中获得偏航轴角速度，并对其在预设时间段内进行积分，得到所述短时航向变化参考；

17、根据各磁场传感器在预设时间段内输出的磁场矢量确定各磁场传感器对应的磁航向变化；

18、计算各磁航向变化与所述短时航向变化参考之间的变化偏差，将所述变化偏差不超过变化容差的磁场传感器保留为候选磁场传感器。

19、进一步地，步骤s3包括：候选磁场传感器不少于两个时，计算各候选磁场传感器对应的航向角之间的两两差值；

20、当至少两个候选磁场传感器对应的航向角之间的差值不超过一致性容差，且另一候选磁场传感器对应的航向角与所述至少两个候选磁场传感器对应的航向角之间的差值超过一致性容差时，将所述至少两个候选磁场传感器确定为有效磁场传感器；

21、当仅存在一个候选磁场传感器时，在该候选磁场传感器的模长偏差和变化偏差连续个检测周期分别不超过对应容差后，将该候选磁场传感器确定为有效磁场传感器，其中，为预先设定的正整数；

22、当存在多个有效磁场传感器时，根据各有效磁场传感器的健康状态、模长偏差和变化偏差确定融合权重，并根据多个有效磁场传感器的磁场矢量形成航向基准。

23、进一步地，步骤s4中各传感器的健康状态包括可信状态、无效状态和过渡状态；

24、当处于可信状态的传感器在连续个检测周期内均未通过对应的检测或者仲裁时，将该传感器由可信状态转为无效状态；

25、当处于无效状态的传感器在连续个检测周期内均通过对应的检测或者仲裁时，将该传感器由无效状态转为过渡状态；

26、在所述过渡状态下，按照随时间增加的渐变权重将该传感器数据逐步并入姿态与航向估计，并在所述渐变权重达到目标权重后将该传感器转为可信状态；

27、其中，和分别为预先设定的正整数。

28、进一步地，步骤s4中，所述姿态与航向估计器为状态估计滤波器，所述可信惯性参考用于所述状态估计滤波器的状态预测，所述有效磁场传感器数据用于所述状态估计滤波器的航向量测更新；

29、当不存在处于可信状态或过渡状态的有效磁场传感器时，停止利用磁场传感器数据进行航向量测更新，根据所述可信惯性参考维持短时航向估计，并增大航向估计对应的过程噪声；

30、当至少一个磁场传感器重新通过仲裁时，按该磁场传感器对应的渐变权重重新参与航向量测更新。

31、进一步地，步骤s4中对各磁场传感器进行仲裁时，先分别根据所述模长偏差和变化偏差判断各磁场传感器的单路有效性，再对通过单路有效性判断的候选磁场传感器进行航向一致性判断；

32、未通过所述模长偏差判断或者变化偏差判断的磁场传感器，不因其与其他磁场传感器之间的航向一致而被确定为有效磁场传感器；

33、当至少两个磁场传感器之间的航向一致、但该至少两个磁场传感器对应的磁航向变化均与所述短时航向变化参考不一致时，将该至少两个磁场传感器排除。

34、本发明还提供一种具有多余度传感器自仲裁功能的无人机飞控板，包括：飞控电路板；

35、设置于所述飞控电路板上的主控处理器和至少两个惯性测量单元；

36、包括板载磁场传感器和至少两个外置磁场传感器的磁场传感器组；以及连接所述外置磁场传感器与所述主控处理器的外置传感器接口；

37、其中，所述主控处理器用于：

38、对各惯性测量数据进行交叉一致性检测，根据各惯性测量单元各自的独立健康证据确定无效惯性测量单元，并根据通过检测的惯性测量数据形成可信惯性参考；

39、根据所述可信惯性参考确定短时航向变化参考，并根据各磁场传感器输出的磁场矢量相对于当地地磁场的模长偏差以及各磁场传感器对应的磁航向变化与所述短时航向变化参考之间的变化偏差，确定候选磁场传感器，并根据候选磁场传感器的数量以及各候选磁场传感器之间的航向一致性，确定有效磁场传感器；

40、将所述可信惯性参考和处于可信状态的有效磁场传感器数据输入姿态与航向估计器，将未通过检测或者仲裁的传感器数据排除，转为过渡状态，并按照渐变权重重新参与姿态与航向估计。

41、进一步地，至少两个所述外置磁场传感器分别设置于不同的外置磁罗盘板上，各外置磁罗盘板包括向远离无人机机体电磁干扰源的方向延伸的电路板悬臂，所述外置磁场传感器设置在对应电路板悬臂的远端；

42、至少两个所述外置磁场传感器通过具有通道隔离寻址能力的总线连接结构与所述主控处理器连接，使各外置磁场传感器能够被所述主控处理器分别寻址读取。

43、本发明的有益效果是：通过将惯性测量数据的交叉一致性检测与各惯性测量单元的独立健康证据相结合，能够较为准确地确定无效惯性测量单元，减少异常惯性数据进入姿态与航向估计器的可能；通过依次进行磁场模长检测、磁航向变化与短时航向变化参考的比较以及候选磁场传感器之间的航向一致性判断，能够识别单路磁干扰以及多个磁场传感器受到的共同磁干扰；通过连续检测计数、过渡状态和渐变权重，使传感器在排除和恢复过程中平稳切换；当全部磁场传感器暂时不可用时，停止磁场量测更新并利用可信惯性参考维持短时航向，同时通过增大相应过程噪声反映航向估计可信度的下降，避免受扰磁场数据继续影响航向估计。此外，将外置磁场传感器设置于电路板悬臂远端并分别寻址读取，有利于减小机体电磁干扰，并为各磁场传感器的逐路检测和仲裁提供相互独立的数据。