无人机 PX4 飞控 | EKF 使用传感器汇总与添加传感器方法

前言

ECL （Estimation and Control Library，估计和控制库），其中的状态估计使用扩展卡尔曼滤波算法（EKF）处理传感器的测量信息,是一个很成熟很优秀的状态估计模块。

EKF 提供如下状态量的估计值：

• 四元数定义从北东地（NED）局部地球坐标系到 X，Y，Z 机体坐标系的旋转
• IMU 的速度 — 北，东，地 (NED) (m/s)
• IMU 的位置 — 北，东，地 (NED) (m)
• IMU 增量角度偏差估计 — X，Y ，Z (rad)
• IMU 增量速度偏差估计 - X, Y, Z(m/s)
• 地球磁场分量 — 北，东，地 (NED) (gauss)
• 飞行器机体坐标系磁场偏差 — X，Y ，Z (gauss)
• 风速 — 北, 东 (NE) (m/s)

位置及速度状态变量在输出至控制回路之前会根据IMU与机体坐标系之间的偏差量进行修正。 IMU 相对于机体坐标系的位置由 EKF2_IMU_POS_X,Y,Z 参数设置。

EKF 具有不同的操作模式，以允许不同的传感器测量组合。

滤波器在启动时会检查传感器的最小可行组合，并且在完成初始倾斜，偏航和高度对准之后，进入提供旋转，垂直速度，垂直位置，IMU 增量角度偏差和 IMU 增量速度偏差估计的模式。

此模式需要 IMU 数据，一个偏航源（磁力计或外部视觉）和一个高度数据源。 所有EKF操作模式都需要这个最小数据集。

在此基础上可以使用其它传感器数据来估计额外的状态变量。

飞控自带基本传感器

IMU

与机体固连的三轴惯性测量单元 (IMU)，以最小 100Hz 的频率获取增量角度和增量速度数据。

EKF 仅将 IMU 数据用于状态预测

注意：在 EKF 使用它们之前，应该使用圆锥校正算法校正 IMU 增量角度数据。

磁力计

需要以最小 5Hz 的速率的三轴机体固连磁力计数据（或外部视觉系统姿态数据）。

磁力计数据有两种方式参与融合：

• 使用倾角估计和磁偏角将磁力计测量值转换为偏航角。然后将该偏航角用作 EKF 的观察值。该方法精度较低并且不允许学习机体坐标系场偏移，但是它对于磁场异常和大的初始陀螺偏差更有鲁棒性。它是启动期间和在地面时的默认方法
• XY Z 磁力计读数用作单独的观察值。该方法更精确，但需要正确估计磁力计的偏差。
  当无人机旋转时可以观察到偏差，当飞行器加速(线性加速)时可以观察到真实的航向。
  由于偏差可以改变，并且只有在移动时才能观察到，因此在不移动时切换回航向融合更安全。
  它假设地球磁场环境只会缓慢变化，当存在显着的外部磁场异常时表现较差。
  这是飞行器移动时使用的默认方法。

用于选择这些模式的逻辑由 EKF2_MAG_TYPE 参数设置。
这个参数是一个整型变量，用于控制磁力计融合使用的类型，磁航向还是三轴磁力计读数

• 设置为0（Automatic），在地面使用磁力计偏航，在空中使用lost三轴磁力计读数；
• 设置为1（Magnetic heading）时，为一直使用磁力计偏航模式
• 设置为5（None），磁力计将不会在任何情况下使用。
• 设置为6（Init），磁力计仅用于初始化航向

建议使用默认的“Automatic”模式(EKF2_MAG_TYPE=0)，因为它在地面上使用更鲁棒的磁力计偏航，在移动时（参数表中为in-flight）使用更精确的3轴磁力计读数。

一直设置“3-axis”模式(EKF2_MAG_TYPE=2)更容易出错，并且要求所有imu都经过良好校准。[参数列表中没有这个选项，可能取消掉了]

还有有两种方式可在没有磁力计的情况下使用，此时需要将EKF2_MAG_TYPE 设置为 None
1、使用双天线GPS的偏航角替换它，
2、使用IMU测量和GPS速度数据来估计飞行器运动的偏航角。

气压计

EKF2_BARO_CTRL —— 控制是否使用气压计参与融合

需要注意的是，即使飞控中有多个气压计，也只会有一个气压计的数据进行融合。
通过 CAL_BAROx_PRIO 这个参数来设置各个气压计的优先级。
优先级高的首先被选择，如果出现故障，则会切换为下一个优先级的气压计。
如果气压计的优先级设置的相同，那么先收到数据那个被使用。

如果设置CAL_BAROx_PRIO 为0 ，那么x这个气压计就完成被禁止使用。

还是以cuav X7+ 为例，该飞控有两个气压计 MS5611*2

静态气压位置误差校正

气压表示的海拔高度因机体风的相对速度和方向造成的空气动力扰动而发生误差。这就是所谓的静态气压位置误差. 使用ECL/EKF2估计器库的EKF2模块提供了补偿这些误差的方法，只要风速状态估计是激活的。

对应固定翼，风速估计需要空速计或者 Synthetic Sideslip

对于多旋翼飞行器，风速估计可以启用并调整 Drag Specific Forces 的融合，以提供所需风速状态估计。后面有Drag Specific Forces 的详细介绍

EKF2模块将误差建模为与机体固连的椭球体，在将其转换为高度估计之前，它指定了从大气气压中加/减的动态气压的分量。

气压计偏压补偿

气压计在恒定高度时，由于环境压力环境的变化或传感器温度的变化，其测量结果会发生漂移。为了补偿这种测量误差，EKF2使用GNSS高度(如果可用)作为“非漂移”参考来估计偏差。不需要调优。

高度传感器

高度数据源 ，来自 GPS、气压计、测距仪或外部视觉设备，需要最小频率为 5Hz。

如果不存在这些测量值，EKF 将无法启动。

当检测到这些测量值时，EKF 将初始化状态并完成倾角和偏航对准。 当倾角和偏航对齐完成后，EKF 可以转换到其它操作模式，从而可以使用其它传感器数据：

每个高度源可以使用其专用控制参数启用/禁用:

• GNSS： EKF2_GPS_CTRL —— 这个参数是一个通过位来进行功能选择的参数，0: Lon/lat；1: Altitude；2: 3D velocity；3: Dual antenna heading 。 默认为7
• 气压计：EKF2_BARO_CTRL —— 控制是否使用气压计参与融合
• 测距仪：EKF2_RNG_CTRL —— 0：禁止测距仪融合；1使能测距仪融合有条件（在小于设定速度EKF2_RNG_A_VMAX与小于设定高度时EKF2_RNG_A_HMAX）；2使能融合
• 外部视觉设备： EKF2_HGT_REF 这个参数设置为Vision的时候使能，这个参数可以设置0：气压计；1：GPS；2：测距仪；3：视觉设备。存在多个高度源时，高度估计将始终收敛于该参数选择的参考高度源

测距仪

测距仪到地面的距离由一个单状态滤波器来估计地形相对于高度基准面的垂直位置。

控制测距仪是否参与融合由 EKF2_RNG_CTRL 这个参数来决定
EKF2_RNG_CTRL —— 0：禁止测距仪融合；1使能测距仪融合有条件（在小于设定速度EKF2_RNG_A_VMAX与小于设定高度时EKF2_RNG_A_HMAX）；2使能融合

使能测距仪融合有条件

条件测距仪融合(又称条件测距辅助)在低速/低空操作时(除了其他主动高度源之外)激活测距仪融合进行高度估计。

如果将测距仪设置为参考高度源(使用EKF2_HGT_REF)，则其他有效高度源(如气压和GNSS高度)将随着时间的推移调整其测量值以匹配测距仪的读数。

当不满足启动距离辅助的条件时，将自动选择辅助参考。

注意：参考高度之间切换会导致绝对高度估计值随时间漂移。在位置模式下飞行时，这不是问题，但如果需要无人机在特定的GNSS高度飞行任务，则可能会出现问题。如果不想要绝对高度漂移，需要设置GNSS高度作为高度参考，即使使用条件距离辅助。

测距仪主要用于起飞和降落，以防止离地面较近时，过度的螺旋桨下洗气流，造成气压改变，气压计的读数破坏EKF的状态。

测距仪还可以提供飞行器在悬停时的高度保持

地形保持建议在测距仪参与融合的情况下进行地形保持。这是因为地形保持器使用普通的ECL/EKF估计器来确定高度，在大多数情况下，这通常比距离传感器更可靠。

开启条件测距仪融合的参数设置：

• 设置 EKF2_RNG_CTRL 为 Enabled (conditional mode)
• EKF2_RNG_A_VMAX 根据情况设置这个参数，该参数为水平速度阈值，超过该阈值测距仪不参与融合
• EKF2_RNG_A_HMAX 根据情况设置这个参数，该参数为高度阈值，超过该阈值测距仪不参与融合
• EKF2_RNG_A_IGATE 测距辅助EKF 更新一致性 检查阈值(在测距辅助被禁用之前的误差测量)。较低的值意味着HAGL（Height above ground limit）需要更稳定，以便在测距辅助模式下使用测距仪进行高度估计

可额外配置的传感器

光流

如果满足以下条件，将使用Optical flow数据：

• 有效的测距仪数据可用。
• EKF2_OF_CTRL is set.
• 光流传感器返回的质量度量值大于 EKF2_OF_QMIN 参数设置的最低要求。

外部视觉系统

可以使用外部视觉系统测量位置、速度或方向。

通过将EKF2_EV_CTRL的适当位设置为true来配置融合的测量:

EKF2_EV_CTRL （外部视觉系统辅助）通过设置位来选择融合视觉系统的什么数据 ：

• 0: Horizontal position
• 1: Vertical position
• 2: 3D velocity
• 3: Yaw

注意，如果使用偏航数据(位3)，则航向相对于外部视觉框架;否则，航向相对于北方。

EKF 要考虑视觉姿态估计的不确定性。 此不确定性信息可以通过 MAVLink，在 ODOMETRY 消息中的协方差字段发送，也可以通过 EKF2_EVP_NOISE ，EKF2_EVV_NOISE 和 EKF2_EVA_NOISE 参数设置。 你可以通过 EKF2_EV_NOISE_MD 选择不确定性数据源。

新传感器

上述各传感器为PX4 已完成驱动、融合等代码功能的，可配置参数即可将传感器数据融合于EKF2中

在PX4中添加一个新的传感器到EKF（Extended Kalman Filter）会复杂写，涉及以下几个步骤

确定传感器类型和接口

首先，你需要确定你想要添加的传感器类型，输出什么信息数据，并了解该传感器与飞控板的接口（例如，I2C, SPI, UART）。

更新硬件定义

在PX4的代码库中，需要在飞控板的硬件定义文件中添加传感器。这些文件通常位于/boards/your_board_name/hwdef.dat或/boards/your_board_name/board.h。

对于新的I2C设备，需要添加如下行：
I2C_DEV(1, GPIO_SPI_CS_GYRO, I2C_ADDRESS, 100000)
其中I2C_ADDRESS是传感器的I2C地址。
对于新的SPI设备，你可能需要在board.h中定义SPI总线以及相关的GPIO引脚。

添加驱动

如果PX4的代码库中没有你传感器的驱动，需要编写一个新的驱动。驱动通常位于/src/drivers/your_sensor_name。

创建一个新的驱动通常涉及以下步骤：
实现传感器的初始化和配置。
实现读取传感器数据的功能。
将数据发布到PX4的UORB消息系统中。

更新EKF

在添加了传感器驱动后，需要更新EKF以使用新的传感器数据。

修改EKF配置：更新EKF配置参数，以启用新传感器。在/src/modules/ekf2/params目录下的参数定义文件中完成。

修改EKF源码：如果新传感器需要特殊的处理，需要在EKF的源码中进行修改。源码位于/src/modules/ekf2/目录下。
在ekf2_main.cpp中添加对新传感器UORB消息的订阅。
在ekf2.cpp或相应的传感器处理文件中添加新的传感器数据融合逻辑。