无人机 PX4 飞控 | 光流计简介与应用范例

无人机 PX4 飞控 | 光流计简介与应用范例

引用

CSDN

1.

https://blog.csdn.net/qq_32761549/article/details/140983526

光流计是无人机实现自主飞行和定位的重要传感器之一，通过分析摄像头捕捉到的图像序列，计算图像中像素点的运动速度，从而推算出无人机的速度和位置。本文将详细介绍光流计的基本概念、工作原理以及在PX4飞控系统中的具体应用。

前言

光流的概念最早由Gibson在1950年提出，它是空间运动物体在观察成像平面上的像素运动的瞬时速度。光流传感器通过下视相机和向下的距离传感器进行速度估计，主要用于无人机上，可以让飞机保持水平位置，在室内实现定点飞行。

一些常见的光流计实物：

• px4FLOW（新版本的PX4固件已经不支持）
  

• ARK Flow
  

• 基于 PMW3901 的 Flow
  

• UP-T2

无人机光流计（Drone Optical Flowmeter）是一种用于无人机上的光学测量设备，它利用光流技术来帮助无人机实现自主飞行和定位。光流计在无人机中的应用主要是通过分析摄像头捕捉到的图像序列，计算图像中像素点的运动速度，从而推算出无人机的速度和位置。

基于PMW3901 光流在PX4 1.14.3上的应用

环境说明

• 光流计模块：MTF-02（微空科技）
• 无人机飞控：Cuav PX4 X7+
• 飞控版本：PX4 1.14.3

光流计模块介绍

MTF-02是微空科技研发设计并生产的一款光流测距一体传感器，采用串口输出数据，并内置多种通信协议，可兼容主流开源飞控：Ardupilot、PX4、INAV、FMT。依靠MTF-02传感器，无人机可以实现室内无GPS环境下的自主悬停飞行。

产品参数：

• 测距范围：0.01-2.5m
• 测距精度：2%
• 测距光源：LED
• 中心波长：940nm
• 测距视场角：20°
• 光流视场角：42°
• 光流最小工作距离：8cm
• 光流环境光需求：>60Lux
• 通信接口：UART/IIC
• 数据频率：50Hz
• 供电电压：4.0-5.5V
• 功耗：200mW

产品尺寸：

• 模块尺寸：25104.5 mm
• 重量：1.5g

光流计模块使用

安装

将光流计固定于飞机底部，PX4 飞控默认安装方向：光流方向可以用微空助手上位机配置修改，PX4飞控也可以配置光流方向参数。

接线

光流计的接口线序定义：飞控的接口用UART4 ， 即下图中的 2号 接口

其引脚定义如下：所以需要 SH1.0-4P转SH1.25-6P 的接头线.

飞控设置

该光流计模块与PX4 飞控估计配合使用，需要将光流计模块的输出协议设置成mav_px4。

MAV_1_CONFIG TELEM n
重启飞控
MAV_1_MODE Normal
SER_TELn_BAUD 115200 8N1
EKF2_OF_CTRL Enabled
EKF2_RNG_CTRL Enabled
EKF2_HGT_REF Range sensor
重启飞控
SENS_FLOW_ROT No rotation
1.14.0版本以后需要设置好所有其它参数，重启飞控后才能在QGC里找到SENS_FLOW_ROT参数
重启飞控，一切正常的话，在QGC的MAVLink Inspector页面中应该能看到DISTANCE_SENSOR和OPTICAL_FLOW_RAD消息。注意，QGC上显示的消息由飞控发出，消息频率并不等同于传感器的数据频率，这个频率由飞控的设置和数据链路的速度决定，主要用于观察，消息频率多少并不重要。
如果地面站是通过某些数传连接飞控时，由于数据链路速率较低，飞控会选择性发出数据，导致在QGC有可能看不到某些消息帧比如OPTICAL_FLOW_RAD，因此这一步建议使用USB连接飞控。

核心代码

满足距离条件，根据光流计数据，计算机体坐标系无人机速度

if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) {
    const float range = _distance_sum / _distance_sum_count;
    vehicle_optical_flow_vel_s flow_vel{};
    flow_vel.timestamp_sample = vehicle_optical_flow.timestamp_sample;
    // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
    // is produced by a RH rotation of the image about the sensor axis.
    const Vector2f flow_xy_rad{-vehicle_optical_flow.pixel_flow[0], -vehicle_optical_flow.pixel_flow[1]};
    const Vector3f gyro_xyz{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]};
    const float flow_dt = 1e-6f * vehicle_optical_flow.integration_timespan_us;
    // compensate for body motion to give a LOS rate
    const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_xyz.xy();
    Vector3f vel_optflow_body;
    vel_optflow_body(0) = - range * flow_compensated_XY_rad(1) / flow_dt;
    vel_optflow_body(1) =   range * flow_compensated_XY_rad(0) / flow_dt;
    vel_optflow_body(2) = 0.f;
    // vel_body
    flow_vel.vel_body[0] = vel_optflow_body(0);
    flow_vel.vel_body[1] = vel_optflow_body(1);
    // vel_ne
    flow_vel.vel_ne[0] = NAN;
    flow_vel.vel_ne[1] = NAN;
    vehicle_attitude_s vehicle_attitude{};
    if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
        const matrix::Dcmf R_to_earth = matrix::Quatf(vehicle_attitude.q);
        const Vector3f flow_vel_ne = R_to_earth * vel_optflow_body;
        flow_vel.vel_ne[0] = flow_vel_ne(0);
        flow_vel.vel_ne[1] = flow_vel_ne(1);
    }
    // flow_uncompensated_integral
    flow_xy_rad.copyTo(flow_vel.flow_uncompensated_integral);
    // flow_compensated_integral
    flow_compensated_XY_rad.copyTo(flow_vel.flow_compensated_integral);
    const Vector3f measured_body_rate(gyro_xyz * (1.f / flow_dt));
    // gyro_rate
    flow_vel.gyro_rate[0] = measured_body_rate(0);
    flow_vel.gyro_rate[1] = measured_body_rate(1);
    // gyro_rate_integral
    flow_vel.gyro_rate_integral[0] = gyro_xyz(0);
    flow_vel.gyro_rate_integral[1] = gyro_xyz(1);
    flow_vel.timestamp = hrt_absolute_time();
    _vehicle_optical_flow_vel_pub.publish(flow_vel);
}

用光流计采集到的数据，进行更新EKF
主要是水平速度

void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
{
    resetEstimatorAidStatus(aid_src);
    aid_src.timestamp_sample = _flow_sample_delayed.time_us;
    const Vector2f vel_body = predictFlowVelBody();
    const float range = predictFlowRange();
    // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
    // correct for gyro bias errors in the data used to do the motion compensation
    // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
    const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt;
    // compute the velocities in body and local frames from corrected optical flow measurement for logging only
    _flow_vel_body(0) = -opt_flow_rate(1) * range;
    _flow_vel_body(1) =  opt_flow_rate(0) * range;
    _flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
    aid_src.observation[0] = opt_flow_rate(0); // flow around the X axis
    aid_src.observation[1] = opt_flow_rate(1); // flow around the Y axis
    aid_src.innovation[0] =  (vel_body(1) / range) - aid_src.observation[0];
    aid_src.innovation[1] = (-vel_body(0) / range) - aid_src.observation[1];
    // calculate the optical flow observation variance
    const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
    aid_src.observation_variance[0] = R_LOS;
    aid_src.observation_variance[1] = R_LOS;
    const Vector24f state_vector = getStateAtFusionHorizonAsVector();
    Vector2f innov_var;
    Vector24f H;
    sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
    innov_var.copyTo(aid_src.innovation_variance);
    // run the innovation consistency check and record result
    setEstimatorAidStatusTestRatio(aid_src, math::max(_params.flow_innov_gate, 1.f));
}