Ardupilot光流代码分析

目录

摘要

---

---

本节主要学习Ardupilot光流部分代码，由于无人机在室内无法进行GPS定位，常采用的室内定位方法是光流定位，欢迎批评指正！

---

---

1.光流传感器硬件介绍

---

---

PX4Flow 是一款与Pixhawk飞控配套使用光流传感器。传感器拥有原生 752×480 像素分辨率，计算光学流的过程中采用了4倍分级和剪裁算法，计算速度达到250Hz（白天，室外），具备非常高的感光度。与其他滑鼠传感器不同，它可以以120Hz（黑暗，室内）的计算速度在室内或者室外暗光环境下工作，而无需照明LED。
具体光流原理可以参考：光流原理

主要特征：
MT9V034机器视觉CMOS传感器，全局快门
4×4分级图像算法，光流运算速度从120Hz（室内）至250Hz（室外）
高感光度，24×24 μm高像素
板载16位精度陀螺，最大感应角速率2000°/s，最大数据更新速度780 Hz，默认使用高精度模式时最大角速率500°/s
板载输入输出一体化超声波传感器
USB bootloader
USB数据波特率最高921600（包含地面站软件QGroundControl所使用的摄像机实时视角）
USB供电模式
电路板开孔适合MatrixVision Bluefox MV（摄像机中心作为基准）

---

---

---

---

2.官网资料学习

---

---

参考网址：
官网网址
csdn博客网址

---

1.光流相机板

PX4FLOW Optical Flow Camera Board

---

本文介绍了如何设置可代替GPS的PX4FLOW（光流）传感器。
警告：Px4Flow由无人机固件版本3.3及更高版本提供支持。它还没有在固定翼或无人车上得到支持。

---

2.简介

（Overview）

PX4Flow（光流）传感器是一个专门的高分辨率向下指向摄像头模块和一个使用地面纹理和可见特征来确定飞机地面速度的三轴陀螺仪。尽管传感器可能配备有内置的MaxBotix LZ-EZ4声纳来测量高度，但在测试过程中，在一系列表面上，这种声纳不够可靠，因此不使用其读数。建议购买不带声纳的PX4流量装置。相反，应在车辆上安装一个单独的测距仪，如Lightware SF10B。

警告：为了最好地使用这个光流传感器，您需要购买一个独立的测距仪，如Lightware SF10B。

---

3.初始化驱动仅仅采用

windows（Install Drivers (Windows only)）

在Windows计算机上，任务规划器（和Windows设备管理器）的连接下拉列表中应显示“px4flow”设备，或由QgroundControl自动识别。如果没有，您可能需要下载、解压和手动安装PX4Flow Windows驱动程序，这反过来可能需要允许安装未签名的驱动程序。

---

4.使用 MissionPlanner

1. 载并解压缩px4flow klt固件（此处为源代码）：源码地址
2. 使用Micro USB数据线将PX4光流传感器连接到计算机。
3. 打开初始设置，安装固件屏幕，选择COM端口并单击“加载自定义固件”链接。选择在步骤1中下载的px4flow-klt-06dec2014.px4二进制文件。您可能需要拔下并重新插入传感器才能开始上传。

4. 断开并重新连接传感器/USB数据线
5. 选择适当的COM端口并按Connect。
6. 打开初始设置>可选硬件>PX4Flow屏幕
7. 取下镜头盖，将相机对准至少3米远的高对比度物体。取下阻止透镜转动的小螺丝，调整焦距直到图像清晰显示。
   

5.使用Using QGroundControl

这里不再讲解

6.连接硬件

Connect to the Pixhawk

传感器应通过4针I2c端口连接到自动驾驶仪。在大多数情况下，应使用I2c分路器允许其他I2c设备（如外部RGB LED和GPS/罗盘模块的罗盘）共享同一端口。

7.安装结构

Mounting to the Frame

光流传感器的默认安装方式是直接向下，微型USB端口指向车辆前部。在传感器背面，您应该看到打印的轴，X轴应该指向前方，Y轴应该指向右边。 FLOW_ORIENT_YAW参数可用于说明其他偏航方向。

重要的是，光流传感器应安装在不会出现角振动的地方，角振动会使图像模糊。

默认安装方向与ETH PX4FLOW wiki上显示的不同。如果您按照ETH px4flow wiki所示安装板，则需要将Flow_Orient_Yaw设置为-9000。

---

8.使能传感器

可以通过使用地面站连接到飞行控制器，然后在初始设置可选硬件光流页面上选中启用复选框来启用传感器。或者，应通过完整参数列表将流量启用参数设置为“FLOW_ENABLE=1”。一旦PixHawk板重新启动，传感器将初始化。

9.安装测距传感器

需要注意的是：必须安装测距雷达，光流传感器才能正常工作。。

具体细节可以参考这篇博客：安装测距仪

---

10.测试传感器

传感器连接到飞行控制器的I2c端口后，用地面站连接到飞行控制器，打开飞行数据屏幕的状态选项卡。如果传感器工作，您应该看到非零opt_m_x、opt_m_y和opt_qua值。

---

11.校准传感器

1. 使用地面站连接飞控，启用在锁定状态下的飞行日志（Copter-3.3设置LOG_BITMASK为131071，Copter-3.4及以上设置LOG_DISARMED为1）
2. 找一个有纹理表面和良好照明的地方（自然光或强白炽灯）
3. 拆下螺旋桨（安全第一）
4. 为无人机上电，用手将无人机提起至与视线齐平
5. 在大约一秒钟的时间内，以滚动方式围绕车辆旋转-15到+15度范围，然后再次返回。重复5到10次。通过合上一只眼睛，可以在旋转时使传感器的中心与背景保持静止。
6. 在俯仰（Pitch）方向上重复此动作。
7. 下载数据flash日志并绘制of.flowx、of.bodyx和imu.gyrx数据。它应该是这样的：

8.如果of.flowx大于或小于.bodyx，则可以通过更改flow_fxscaler参数来调整它。

9.如果OF.BODYX与IMU.GYRX不相关或符号相反，则流量定向偏航参数可能设置不正确，或者流量传感器没有指向下方
10.绘制OF.flowY, OF.bodyYband, IMU.GyrY数据。它应该是这样的：

11.如果of.flowy大于或小于bodyy，则可以通过更改Flow fyscaler参数来调整它。
12.如果OF.BODY与IMU.GYRY不相关或符号相反，则流量定向偏航参数可能设置不正确，或者流量传感器没有指向下方

12.传感器范围检查

检查来自流量传感器校准测试的flashlog中的ekf5.mearng消息。检查以下内容：

1. 有连续范围测量。
2. 它在地面上输出的范围为预期值的10厘米（请记住，当您滚动或倾斜车辆时，测量范围将增大，因为激光随后在斜面上测量）。

13.解锁安全检查

要允许在没有GPS的情况下在闲逛时启用和起飞，应关闭GPS启用检查，如下所示。取消选中“全部”和“GPS”，并保留所有其他选项。

由于光流在启用光流时需要良好的声纳/测距仪数据，因此需要执行额外的臂前检查。

当无人机没有解锁时，您应将车辆垂直提升至至少50厘米但不高于2米（如果测距仪发现距离超过2米，则需要重新启动飞行控制器）。

启用失败时的错误消息是“Prearm:Check Range Finder”。
此检查可通过取消选中“参数/声纳”启用检查来禁用。

14.第一次飞行

First Flight (Copter Only)

1. 设置ek2_gps_type=0（我们不希望在这个阶段ekf使用光流）
2. 在50厘米至3米高度范围内，以较小的倾斜角度在稳定或备降场进行短距离试飞。
3. 下载闪存日志并在任务规划器中绘制以下内容
4. ekf5.mearng应与车辆高度的变化相关。
5. of.flowx和of.flowy应不同
6. bodyx和bodyy的应与imu.gyrx和imu.gyry一致

15.第二次飞行

Second Flight (Copter Only)

警告：
您需要在车辆周围至少15米的净空间才能安全飞行。如果光流速度估计不好，你将没有什么警告，直升机可能会很快倾斜到它的最大倾斜角度

---

---

---

3.初始化光流传感器

---

---

void Copter::setup()
{
   init_ardupilot();
}

---

init_ardupilot()  
{
  init_optflow(); //初始化
}

---

void Copter::init_optflow()
{
 void Copter::init_optflow()
{
#if OPTFLOW == ENABLED
    // initialise optical flow sensor
    optflow.init();
#endif      // OPTFLOW == ENABLED
}
}

---

void OpticalFlow::init(void)
{
    //如果没有初始化，立即返回---- return immediately if not enabled
    if (!_enabled) 
    {
        return;
    }

    if (!backend) 
    {
#if AP_FEATURE_BOARD_DETECT
        if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK ||
            AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2 ||
            AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PCNC1) {
            // possibly have pixhart on external SPI
            backend = AP_OpticalFlow_Pixart::detect("pixartflow", *this);
        }
        if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_SP01) {
            backend = AP_OpticalFlow_Pixart::detect("pixartPC15", *this);
        }
        if (backend == nullptr) //默认是PX4的光流
        {
            backend = AP_OpticalFlow_PX4Flow::detect(*this);
        }
        if (backend == nullptr) 
        {
            backend = AP_OpticalFlow_CXOF::detect(*this);
        }
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
        backend = new AP_OpticalFlow_SITL(*this);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
        backend = new AP_OpticalFlow_Onboard(*this);
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
        backend = AP_OpticalFlow_PX4Flow::detect(*this);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412
        backend = AP_OpticalFlow_Pixart::detect("pixartflow", *this);
#endif
    }

    if (backend != nullptr)  //获取到驱动，就进行初始化
    {
        backend->init(); //光流驱动初始化
    }
}

---

---

重点分析函数：
AP_OpticalFlow_PX4Flow::detect(*this)。

---

---

AP_OpticalFlow_PX4Flow *AP_OpticalFlow_PX4Flow::detect(OpticalFlow &_frontend)
{
    AP_OpticalFlow_PX4Flow *sensor = new AP_OpticalFlow_PX4Flow(_frontend);
    if (!sensor)
    {
        return nullptr;
    }
    if (!sensor->setup_sensor()) //初始化传感器
    {
        delete sensor;
        return nullptr;
    }
    return sensor;
}

bool AP_OpticalFlow_PX4Flow::setup_sensor(void)
{
    if (!scan_buses())  //流量设备总线
    {
        return false;
    }
    // 读取数据周期100ms----read at 10Hz
    dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_PX4Flow::timer, void));
    return true;
}

---

---

1.分析函数：scan_buses（）

---

---

bool AP_OpticalFlow_PX4Flow::scan_buses(void)
{
    bool success = false;
    uint8_t retry_attempt = 0;

    while (!success && retry_attempt < PX4FLOW_INIT_RETRIES) 
    {
        for (uint8_t bus = 0; bus < 3; bus++) 
        {
    #ifdef HAL_OPTFLOW_PX4FLOW_I2C_BUS
            // only one bus from HAL
            if (bus != HAL_OPTFLOW_PX4FLOW_I2C_BUS) 
            {
                continue;
            }
    #endif
            AP_HAL::OwnPtr tdev = hal.i2c_mgr->get_device(bus, PX4FLOW_BASE_I2C_ADDR + get_address());
            if (!tdev) 
            {
                continue;
            }
            if (!tdev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) 
            {
                continue;
            }
            struct i2c_integral_frame frame;
            success = tdev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame));
            tdev->get_semaphore()->give();
            if (success) {
                printf("Found PX4Flow on bus %u\n", bus);
                dev = std::move(tdev);
                break;
            }
        }
        retry_attempt++;
        if (!success) {
            hal.scheduler->delay(10);
        }
    }
    return success;
}

---

---

2.分析函数：AP_OpticalFlow_PX4Flow::timer（）

---

---

void AP_OpticalFlow_PX4Flow::timer(void)
{
    struct i2c_integral_frame frame;
    if (!dev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame))) 
    {
        return;
    }
    struct OpticalFlow::OpticalFlow_state state {};
    state.device_id = get_address();

    if (frame.integration_timespan > 0) {
        const Vector2f flowScaler = _flowScaler();
        float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
        float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
        float integralToRate = 1.0e6 / frame.integration_timespan;
        
        state.surface_quality = frame.qual;
        state.flowRate = Vector2f(frame.pixel_flow_x_integral * flowScaleFactorX,
                                  frame.pixel_flow_y_integral * flowScaleFactorY) * 1.0e-4 * integralToRate;
        state.bodyRate = Vector2f(frame.gyro_x_rate_integral, frame.gyro_y_rate_integral) * 1.0e-4 * integralToRate;
        
        _applyYaw(state.flowRate);
        _applyYaw(state.bodyRate);
    }

    _update_frontend(state); //更新到前端
}

---

---

分析函数：_applyYaw（）

---

void OpticalFlow_backend::_applyYaw(Vector2f &v)
{
    float yawAngleRad = _yawAngleRad();
    if (is_zero(yawAngleRad)) 
    {
        return;
    }
    float cosYaw = cosf(yawAngleRad);
    float sinYaw = sinf(yawAngleRad);
    float x = v.x;
    float y = v.y;
    v.x = cosYaw * x - sinYaw * y;
    v.y = sinYaw * x + cosYaw * y;
}

---

---

void OpticalFlow_backend::_update_frontend(const struct OpticalFlow::OpticalFlow_state &state)
{
    frontend._state = state;
    frontend._last_update_ms = AP_HAL::millis();
}

---

---

---

---

4.光流传感器数据更新

---

---

SCHED_TASK(update_optical_flow,  200,    160), //更新光流数据

#if OPTFLOW == ENABLED 
void Copter::update_optical_flow(void)
{
    static uint32_t last_of_update = 0;

    //如果没有使能，立即退出------exit immediately if not enabled
    if (!optflow.enabled()) 
    {
        return;
    }

    //从传感器读取数据-----read from sensor
    optflow.update();

    // write to log and send to EKF if new data has arrived
    if (optflow.last_update() != last_of_update) 
    {
        last_of_update = optflow.last_update();
        uint8_t flowQuality = optflow.quality();
        Vector2f flowRate = optflow.flowRate();               //光流速度
        Vector2f bodyRate = optflow.bodyRate();               //机体速度
        const Vector3f &posOffset = optflow.get_pos_offset(); //偏移量
        ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, posOffset); //这里就是获取了需要的数据，进行处理
        if (g.log_bitmask & MASK_LOG_OPTFLOW) 
        {
            Log_Write_Optflow();
        }
    }
}
#endif  // OPTFLOW == ENABLED

---

---

分析核心代码

ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, posOffset); //获取测量值

void  AP_AHRS_NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset)
{
    EKF2.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset); 
    EKF3.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset);
}

---

---

这里主要以EKF2的研究进行分析

---

---

void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset)
{
    // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
    // The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
    // A positive X rate is produced by a positive sensor rotation about the X axis
    // A positive Y rate is produced by a positive sensor rotation about the Y axis
    // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
    // negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate
	//原始测量值必须是自上次更新以来的时间，单位是弧度/s，光流速度的平均光流通过时间
	//px4flow传感器输出光流速度，具有以下轴和符号协议：
	//正的x速率是由围绕x轴的正的传感器旋转产生的。
	//正Y速率是由围绕Y轴的正传感器旋转产生的
	//该滤波器使用不同的光学流量定义，传感器的光学流量是由绕该轴的负旋转产生的正光学流量。例如，飞行器绕其X（横滚）轴的正旋转将产生负的X流速。
    flowMeaTime_ms = imuSampleTime_ms;
    // calculate bias errors on flow sensor gyro rates, but protect against spikes in data
    // reset the accumulated body delta angle and time
    // don't do the calculation if not enough time lapsed for a reliable body rate measurement
    //计算流量传感器陀螺仪速率的偏差误差，但要防止数据中出现峰值。
    //复位累积的机体角度和时间
    //如果没有足够的时间来进行可靠的机体速度测量，则不要进行计算
    if (delTimeOF > 0.01f)
    {
        flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
        flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
        delAngBodyOF.zero();
        delTimeOF = 0.0f;
    }
    // by definition if this function is called, then flow measurements have been provided so we
    // need to run the optical flow takeoff detection
    //如果代码运行到这里，光流测量的数据我们已经可以获得了。因为需要运行光流起飞检查
    detectOptFlowTakeoff();

    // calculate rotation matrices at mid sample time for flow observations
    //在流量观测的中间采样过程中，计算旋转矩阵
    stateStruct.quat.rotation_matrix(Tbn_flow);
    // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
    //不要使用低质量指标或极端速率的数据（有助于捕获损坏的传感器数据）
    if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f)
    {
        // correct flow sensor body rates for bias and write
    	//更正光流传感器机体速度偏差和写入
        ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
        ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
        // the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead
        //传感器接口不提供Z轴速率，因此使用导航传感器的速率。
        if (delTimeOF > 0.001f)
        {
            // first preference is to use the rate averaged over the same sampling period as the flow sensor
        	//首选是使用与流量传感器相同采样周期内的平均速率
            ofDataNew.bodyRadXYZ.z = delAngBodyOF.z / delTimeOF;
        } else if (imuDataNew.delAngDT > 0.001f)
        {
            // second preference is to use most recent IMU data
        	//第二个首选项是使用最新的IMU数据
            ofDataNew.bodyRadXYZ.z = imuDataNew.delAng.z / imuDataNew.delAngDT;
        } else
        {
            // third preference is use zero
        	//第三个首选项是使用零
            ofDataNew.bodyRadXYZ.z =  0.0f;
        }
        // write uncorrected flow rate measurements
        // note correction for different axis and sign conventions used by the px4flow sensor
        //写入未修正的光流测量值
        ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)--绕X轴的原始（非运动补偿）光流角速率（rad/sec）
        // write the flow sensor position in body frame
        //在机体坐标系上写入光流传感器
        ofDataNew.body_offset = &posOffset;
        //写光流速度测量更正机体速度------ write flow rate measurements corrected for body rates
        ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
        ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;
        
        // record time last observation was received so we can detect loss of data elsewhere
        //记录最后一次观测的时间，以便我们可以在其他地方检测数据丢失
        flowValidMeaTime_ms = imuSampleTime_ms;
        // estimate sample time of the measurement
        //评估测量的采样时间
        ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
        
        // Correct for the average intersampling delay due to the filter updaterate
        //修正由于滤波器更新率导致的平均采样间延迟
        ofDataNew.time_ms -= localFilterTimeStep_ms/2;
        // Prevent time delay exceeding age of oldest IMU data in the buffer
        //防止时间延迟超过缓冲区中最旧IMU数据的期限
        ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms);
        // Save data to buffer
        //将数据保存到缓冲区
        storedOF.push(ofDataNew);
        // Check for data at the fusion time horizon
        //检查融合时间范围内的数据
        flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms);
    }
}

获取的有效数据，保存缓冲区，等待使用： storedOF.push(ofDataNew);

---

---

分析函数：detectOptFlowTakeoff（）
主要功能：通过光流来识别起飞

void NavEKF2_core::detectOptFlowTakeoff(void)
{
    if (!onGround && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
        // we are no longer confidently on the ground so check the range finder and gyro for signs of takeoff
        const AP_InertialSensor &ins = AP::ins();
        Vector3f angRateVec;
        Vector3f gyroBias;
        getGyroBias(gyroBias);
        bool dual_ins = ins.get_gyro_health(0) && ins.get_gyro_health(1);
        if (dual_ins) {
            angRateVec = (ins.get_gyro(0) + ins.get_gyro(1)) * 0.5f - gyroBias;
        } else {
            angRateVec = ins.get_gyro() - gyroBias;
        }

        takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rangeDataNew.rng > (rngAtStartOfFlight + 0.1f)));
    } else if (onGround) {
        // we are confidently on the ground so set the takeoff detected status to false
        takeOffDetected = false;
    }
}

---

---

5.px4flow代码下载编译

---

对于px4flow的编译下载请参考博客：光流定点编译下载

源码地址：源码下载地址

---

---

6.DIY自己的光流

---

DIY光流博客地址