关于px4flow光流模块，是不是直接能得到相对地面速度？

wilsonleong

关于px4flow光流模块，是不是直接能得到相对地面速度？机体倾斜需不需要自己去解算实际的相对地面速度？QGC地面站上要做相关设置么？

wilsonleong已获得悬赏 10 阿木币

最佳答案

[md]下面看PX4固件是怎么用PX4FLOW的。 首先通过SENS_EN_PX4FLOW可以启用PX4FLOW模块，先看[wiki](https://docs.px4.io/master/zh/sensor/px4flow.html)的描述： > I2C1: latest Flow value (i2c_frame) and accumu ...

• wilsonleong
• 发表于2020-08-10
• 详细答案 >

AmovlabQYP

首先声明我没用过，说说我的理解。
应该是直出相对速度。
倾斜他肯定没考虑的，你可以去看源码。src/drivers/optical_flow/px4flow文件夹，代码很短的。
QGC所有可以设置的参数，你在源码中也能找到，我看到就一个使能参数。

wilsonleong

AmovlabQYP 发表于 2020-8-5 10:01
首先声明我没用过，说说我的理解。
应该是直出相对速度。
倾斜他肯定没考虑的，你可以去看源码。src/driver ...

但是px4flow光流模块和TFmini定高雷达不一样，光流芯片是有带声纳和imu的，个人感觉芯片内部已经解算好了

amov_msq

wilsonleong 发表于 2020-8-5 10:08
但是px4flow光流模块和TFmini定高雷达不一样，光流芯片是有带声纳和imu的，个人感觉芯片内部已经解算好了 ...

看一下光流模块发出的几个topic就知道了，主要是速度

wilsonleong

本帖最后由 wilsonleong 于 2020-8-10 15:40 编辑

Honegger D, Meier L, Tanskanen P, et al. An open source and open hardware embedded metric optical flow cmos camera for indoor and outdoor applications[C]//2013 IEEE International Conference on Robotics and Automation. IEEE, 2013: 1736-1741.

根据PX4FLOW论文，光流原理如下： $$v_x=\frac{T_zx-T_xf}{Z}-\omega_yf+\omega_zy+\frac{\omega_xxy-\omega_yx^2}{f}$$ $$v_y=\frac{T_zy-T_yf}{Z}+\omega_xf-\omega_zx+\frac{\omega_xy^2-\omega_yxy}{f}$$ 论文中继续假设$Z$变化不大，所以$T_z=0$；并且上面两个式子的最后一项相对小，所以简化为： $$v_x=-\frac{T_xf}{Z}-\omega_yf+\omega_zy$$ $$v_y=-\frac{T_yf}{Z}+\omega_xf-\omega_zx$$ 其中 $-\omega_yf+\omega_zy$ 和 $\omega_xf-\omega_zx$ 可以用内置陀螺仪补偿掉，$Z$可以通过内置声纳测量。

再看代码flow.c补偿角速度：

if (FLOAT_AS_BOOL(global_data.param[PARAM_BOTTOM_FLOW_GYRO_COMPENSATION]))
{
        if(fabsf(y_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD])
        {
                /* calc pixel of gyro */
                float y_rate_pixel = y_rate * (get_time_between_images() / 1000000.0f) * focal_length_px;
                float comp_x = histflowx + y_rate_pixel;

                /* clamp value to maximum search window size plus half pixel from subpixel search */
                if (comp_x < (-SEARCH_SIZE - 0.5f))
                        *pixel_flow_x = (-SEARCH_SIZE - 0.5f);
                else if (comp_x > (SEARCH_SIZE + 0.5f))
                        *pixel_flow_x = (SEARCH_SIZE + 0.5f);
                else
                        *pixel_flow_x = comp_x;
        }
        else
        {
                *pixel_flow_x = histflowx;
        }

        if(fabsf(x_rate) > global_data.param[PARAM_GYRO_COMPENSATION_THRESHOLD])
        {
                /* calc pixel of gyro */
                float x_rate_pixel = x_rate * (get_time_between_images() / 1000000.0f) * focal_length_px;
                float comp_y = histflowy - x_rate_pixel;

                /* clamp value to maximum search window size plus/minus half pixel from subpixel search */
                if (comp_y < (-SEARCH_SIZE - 0.5f))
                        *pixel_flow_y = (-SEARCH_SIZE - 0.5f);
                else if (comp_y > (SEARCH_SIZE + 0.5f))
                        *pixel_flow_y = (SEARCH_SIZE + 0.5f);
                else
                        *pixel_flow_y = comp_y;
        }
        else
        {
                *pixel_flow_y = histflowy;
        }

//                                /* alternative compensation */
//                                /* compensate y rotation */
//                                *pixel_flow_x = histflowx + y_rate_pixel;
//
//                                /* compensate x rotation */
//                                *pixel_flow_y = histflowy - x_rate_pixel;

}
else
{
        /* without gyro compensation */
        *pixel_flow_x = histflowx;
        *pixel_flow_y = histflowy;
}

是否启用陀螺仪补偿由PARAM_BOTTOM_FLOW_GYRO_COMPENSATION确定，对应PX4FLOW的参数BFLOW_GYRO_COM，默认为不开启。假设开启了陀螺仪补偿，代码里面把$\omega_z$忽略了，所以就只剩下： $$v_x=-\frac{T_xf}{Z}-\omega_yf$$ $$v_y=-\frac{T_yf}{Z}+\omega_xf$$

最后看main.c，通过声纳测量到的$Z$算速度：

float new_velocity_x = - flow_compx * sonar_distance_filtered;
float new_velocity_y = - flow_compy * sonar_distance_filtered;

以上所有计算都在PX4芯片里面完成的。

wilsonleong

本帖最后由 wilsonleong 于 2020-8-10 15:39 编辑

下面看PX4固件是怎么用PX4FLOW的。

首先通过SENS_EN_PX4FLOW可以启用PX4FLOW模块，先看wiki的描述：

I2C1: latest Flow value (i2c_frame) and accumulated Flow (i2c_integral_frame) values since last I2C readout available for readout.

再看main.c怎么通过I2C发布出来的，这里把速度传进去了：

update_TX_buffer(pixel_flow_x, pixel_flow_y, velocity_x_sum/valid_frame_count, velocity_y_sum/valid_frame_count, qual,
                ground_distance, x_rate, y_rate, z_rate, gyro_temp, uavcan_use_export(i2c_data));

进去i2c.c看看update_TX_buffer函数，只有在i2c_frame有速度数据，在i2c_integral_frame没有速度数据。

i2c_frame f;
i2c_integral_frame f_integral;

f.frame_count = frame_count;
f.pixel_flow_x_sum = pixel_flow_x * 10.0f;
f.pixel_flow_y_sum = pixel_flow_y * 10.0f;
f.flow_comp_m_x = flow_comp_m_x * 1000;
f.flow_comp_m_y = flow_comp_m_y * 1000;
f.qual = qual;
f.ground_distance = ground_distance * 1000;

f.gyro_x_rate = gyro_x_rate * getGyroScalingFactor();
f.gyro_y_rate = gyro_y_rate * getGyroScalingFactor();
f.gyro_z_rate = gyro_z_rate * getGyroScalingFactor();
f.gyro_range = getGyroRange();

/* ...... */

lasttime = get_boot_time_us();

f_integral.frame_count_since_last_readout = accumulated_framecount;
f_integral.gyro_x_rate_integral = accumulated_gyro_x * 10.0f;        //mrad*10
f_integral.gyro_y_rate_integral = accumulated_gyro_y * 10.0f;        //mrad*10
f_integral.gyro_z_rate_integral = accumulated_gyro_z * 10.0f; //mrad*10
f_integral.pixel_flow_x_integral = accumulated_flow_x * 10.0f; //mrad*10
f_integral.pixel_flow_y_integral = accumulated_flow_y * 10.0f; //mrad*10
f_integral.integration_timespan = integration_timespan;     //microseconds
f_integral.ground_distance = ground_distance * 1000;                    //mmeters
f_integral.sonar_timestamp = time_since_last_sonar_update;  //microseconds
f_integral.qual =
                (uint8_t) (accumulated_quality / accumulated_framecount); //0-255 linear quality measurement 0=bad, 255=best
f_integral.gyro_temperature = gyro_temp;//Temperature * 100 in centi-degrees Celsius

/* ...... */

memcpy(&(txDataFrame1[notpublishedIndexFrame1]),
        &f, I2C_FRAME_SIZE);

memcpy(&(txDataFrame2[notpublishedIndexFrame2]),
        &f_integral, I2C_INTEGRAL_FRAME_SIZE);

再看optical_flow.msg文件，只有光流数据，没有速度：

uint64 timestamp                # time since system start (microseconds)

uint8 sensor_id                        # id of the sensor emitting the flow value
float32 pixel_flow_x_integral        # accumulated optical flow in radians where a positive value is produced by a RH rotation about the X body axis
float32 pixel_flow_y_integral        # accumulated optical flow in radians where a positive value is produced by a RH rotation about the Y body axis
float32 gyro_x_rate_integral        # accumulated gyro value in radians where a positive value is produced by a RH rotation about the X body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
float32 gyro_y_rate_integral        # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Y body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
float32 gyro_z_rate_integral        # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Z body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
float32 ground_distance_m        # Altitude / distance to ground in meters
uint32 integration_timespan        # accumulation timespan in microseconds
uint32 time_since_last_sonar_update        # time since last sonar update in microseconds
uint16 frame_count_since_last_readout        # number of accumulated frames in timespan
int16 gyro_temperature        # Temperature * 100 in centi-degrees Celsius
uint8 quality        # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality

float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably
float32 min_ground_distance # Minimum distance from ground at which the optical flow sensor operates reliably
float32 max_ground_distance # Maximum distance from ground at which the optical flow sensor operates reliably

再看px4flow.cpp，这里只把在i2c_integral_frame的数据发布出来了，所以是没有速度的。另外PX4FLOW不仅可以发布optical_flow.msg，还可以发布distance_sensor.msg（其实optical_flow.msg也有一个ground_distance_m），带声纳的PX4FLOW也可以通过设置SENS_EN_MB12XX来启用声纳。

if (PX4FLOW_REG == 0) {
        memcpy(&_frame, val, I2C_FRAME_SIZE);
        memcpy(&_frame_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
}

if (PX4FLOW_REG == 0x16) {
        memcpy(&_frame_integral, val, I2C_INTEGRAL_FRAME_SIZE);
}


optical_flow_s report{};

report.timestamp = hrt_absolute_time();
report.pixel_flow_x_integral = static_cast<float>(_frame_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
report.pixel_flow_y_integral = static_cast<float>(_frame_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
report.frame_count_since_last_readout = _frame_integral.frame_count_since_last_readout;
report.ground_distance_m = static_cast<float>(_frame_integral.ground_distance) / 1000.0f;//convert to meters
report.quality = _frame_integral.qual; //0:bad ; 255 max quality
report.gyro_x_rate_integral = static_cast<float>(_frame_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
report.gyro_y_rate_integral = static_cast<float>(_frame_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
report.gyro_z_rate_integral = static_cast<float>(_frame_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
report.integration_timespan = _frame_integral.integration_timespan; //microseconds
report.time_since_last_sonar_update = _frame_integral.sonar_timestamp;//microseconds
report.gyro_temperature = _frame_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
report.sensor_id = 0;
report.max_flow_rate = _sensor_max_flow_rate;
report.min_ground_distance = _sensor_min_range;
report.max_ground_distance = _sensor_max_range;

/* rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT */
float zeroval = 0.0f;

rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);

_px4flow_topic.publish(report);

/* publish to the distance_sensor topic as well */
if (_class_instance == CLASS_DEVICE_PRIMARY) {
        distance_sensor_s distance_report{};
        distance_report.timestamp = report.timestamp;
        distance_report.min_distance = PX4FLOW_MIN_DISTANCE;
        distance_report.max_distance = PX4FLOW_MAX_DISTANCE;
        distance_report.current_distance = report.ground_distance_m;
        distance_report.variance = 0.0f;
        distance_report.signal_quality = -1;
        distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
        /* TODO: the ID needs to be properly set */
        distance_report.id = 0;
        distance_report.orientation = _sonar_rotation;

        _distance_sensor_topic.publish(distance_report);
}

最后看看EKF2，这里也是只拿光流数据用，没有拿PX4FLOW的速度：

flow.flow_xy_rad(0) = -optical_flow.pixel_flow_x_integral;
flow.flow_xy_rad(1) = -optical_flow.pixel_flow_y_integral;
flow.gyro_xyz(0) = -optical_flow.gyro_x_rate_integral;
flow.gyro_xyz(1) = -optical_flow.gyro_y_rate_integral;
flow.gyro_xyz(2) = -optical_flow.gyro_z_rate_integral;
flow.quality = optical_flow.quality;
flow.dt = 1e-6f * (float)optical_flow.integration_timespan;
flow.time_us = optical_flow.timestamp;

PX4FLOW在内部已经计算的速度，但是PX4固件只拿PX4FLOW的光流数据来用，默认情况下PX4FLOW内部默认不做角速度补偿(PX4FLOW内部参数BFLOW_GYRO_COM)；另外如果有需求，PX4固件可以把声纳的数据也拿过来用，不过这算是单独的另外一个传感器使用了。

blueboats

wilsonleong 发表于 2020-8-10 10:54
[md]下面看PX4固件是怎么用PX4FLOW的。

首先通过SENS_EN_PX4FLOW可以启用PX4FLOW模块，看[wiki](https:// ...

wilsonleong

欢迎指正！

wilsonleong

EKF2用到的是未修正的原始光流数据；EKF2会使用PX4FLOW内部陀螺仪去做修正，如果没有内部陀螺仪则会用Pixhawk陀螺仪去做修正

control.cpp

// Only fuse optical flow if valid body rate compensation data is available
if (calcOptFlowBodyRateComp()) {

    bool flow_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min);

    if (!flow_quality_good && !_control_status.flags.in_air) {
        // when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate
        _flow_compensated_XY_rad.setZero();
    } else {
        // compensate for body motion to give a LOS rate
        _flow_compensated_XY_rad = _flow_sample_delayed.flow_xy_rad - _flow_sample_delayed.gyro_xyz.xy();
    }
} else {
    // don't use this flow data and wait for the next data to arrive
    _flow_data_ready = false;
}

optflow_fusion.cpp

// calculate optical flow body angular rate compensation
// returns false if bias corrected body rate data is unavailable
bool Ekf::calcOptFlowBodyRateComp()
{
    // reset the accumulators if the time interval is too large
    if (_delta_time_of > 1.0f) {
        _imu_del_ang_of.setZero();
        _delta_time_of = 0.0f;
        return false;
    }

    const bool use_flow_sensor_gyro =  ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && ISFINITE(_flow_sample_delayed.gyro_xyz(2));

    if (use_flow_sensor_gyro) {

        // if accumulation time differences are not excessive and accumulation time is adequate
        // compare the optical flow and and navigation rate data and calculate a bias error
        if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) {

            const Vector3f reference_body_rate(_imu_del_ang_of * (1.0f / _delta_time_of));

            const Vector3f measured_body_rate(_flow_sample_delayed.gyro_xyz * (1.0f / _flow_sample_delayed.dt));

            // calculate the bias estimate using  a combined LPF and spike filter
            _flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f;
        }

    } else {
        // Use the EKF gyro data if optical flow sensor gyro data is not available
        // for clarification of the sign see definition of flowSample and imuSample in common.h
        _flow_sample_delayed.gyro_xyz = -_imu_del_ang_of;
        _flow_gyro_bias.zero();
    }

    // reset the accumulators
    _imu_del_ang_of.setZero();
    _delta_time_of = 0.0f;
    return true;
}

另外，高度值的来源是EKF2算出来的值，EKF2不拿高度值去算速度，而是去构建残差值

optflow_fusion.cpp

// height above ground of the IMU
float heightAboveGndEst = _terrain_vpos - _state.pos(2);

// calculate the sensor position relative to the IMU in earth frame
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;

// calculate the height above the ground of the optical flow camera. Since earth frame is NED
// a positive offset in earth frame leads to a smaller height above the ground.
heightAboveGndEst -= pos_offset_earth(2);

// constrain minimum height above ground
heightAboveGndEst = math::max(heightAboveGndEst, gndclearance);

// calculate range from focal point to centre of image
const float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view

// 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 + Vector2f(_flow_gyro_bias);

if (opt_flow_rate.norm() < _flow_max_rate) {
    _flow_innov(0) =  vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
    _flow_innov(1) = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis

} else {
    return;
}