光流定点程序梳理

          本文主要分析飞控获取到光流数据之后，如何实现定点，至于光流算法，不在本文讨论范围内。
          官网介绍的PX4 Flow采用STM32F4作为主控，定点效果不错，但价格稍贵，而且体积比较大，对于口袋无人机来说，肯定是不合适的。像安霸、联咏等芯片的某些系列可以接多个摄像头，主摄像头用来航拍，辅摄像头用来做光流，既节省了成本，又减小了体积。
          切换到定点模式下，例如loiter或poshold模式，会初始化Home Position。打开AP_Inertial_Nav.cpp，找到如下程序。

// setup_home_position - reset state for home position changevoid AP_InertialNav::setup_home_position(void){    // set longitude to meters scaling to offset the shrinking longitude as we go towards the poles    _lon_to_cm_scaling = longitude_scale(_ahrs.get_home()) * LATLON_TO_CM;    // reset corrections to base position to zero    _position_base.x = 0.0f;    _position_base.y = 0.0f;    _position_correction.x = 0.0f;    _position_correction.y = 0.0f;    _position.x = 0.0f;       _position.y = 0.0f;       // clear historic estimates    _hist_position_estimate_x.clear();    _hist_position_estimate_y.clear();    // set xy as enabled    _xy_enabled = true;}

将光流数据根据镜头参数和超声高度数据转化为实际的移动距离，减去由于roll/pitch变化造成的偏差，并对其进行累加，得到相对于机体Home位置的坐标。结合磁力计将机体坐标系转化为地理坐标系，X、Y方向与GPS经纬度保持一致。

// updates internal lon and lat with estimation based on optical flow// reference: http://ardupilot.org/copter/docs/common-mouse-based-optical-flow-sensor-adns3080.htmlvoid AP_InertialNav::update_optflow_position(float roll, float pitch, float sin_yaw, float cos_yaw, float altitude)  {    static float _last_roll;        static float _last_pitch;    static float _last_altitude;    float diff_roll, diff_pitch;    float compensation_x = 0, compensation_y = 0;   // compensation caused by roll pitch angle        float rate_threshold = 0.15f;       diff_roll = _last_roll - roll;    diff_pitch = _last_pitch - pitch;    // only update position if angle is not over 45 degrees    if( fabsf(roll) <= FORTYFIVE_DEGREES && fabsf(pitch) <= FORTYFIVE_DEGREES ) {           float avg_altitude = (altitude + _last_altitude) * 0.5;        // change in position is actual change measured by sensor minus expected change due to change in roll, pitch        if(fabsf(roll) > rate_threshold)            compensation_x = diff_roll * avg_altitude * OPTFLOW_COEFFICIENT_X;  // right        if(fabsf(pitch) > rate_threshold)            compensation_y = -diff_pitch * avg_altitude * OPTFLOW_COEFFICIENT_Y;  // forward // exchange x and y to adjust for GPS direction.        optflow_x_cm = _rcin_values_y * avg_altitude / 457.0 - compensation_y;   // translate to real x,y changing distance,unit cm.optflow_y_cm = _rcin_values_x * avg_altitude / 457.0 - compensation_x;  // translate optflow body frame to earth frameoptflow_ef_x = optflow_x_cm * cos_yaw - optflow_y_cm * sin_yaw;  // north    optflow_ef_y = optflow_x_cm * sin_yaw + optflow_y_cm * cos_yaw;  // eastoptflow_ef_sum_x += optflow_ef_x;      optflow_ef_sum_y += optflow_ef_y;  //printf("optflow_x is: %.3f,optflow_y is: %.3f，avg_altitude is: %.3f  \n",optflow_ef_sum_x,optflow_ef_sum_y,sonar_altitude);    }    _last_altitude = altitude;    _last_roll = roll;    _last_pitch = pitch;

}

利用光流数据计算位置误差，对基准位置进行更新。

void AP_InertialNav::correct_with_optflow(uint32_t now){    float dt,x,y;    float hist_position_base_x, hist_position_base_y;    // calculate time since last gps reading    dt = (float)(now - _optflow_last_update) * 0.001f;    // update last gps update time    _optflow_last_update = now;    // discard samples where dt is too large    if(!using_optflow) {    _position_error.x = 0;        _position_error.y = 0;    }       x = position_x_initial + optflow_ef_sum_x;    y = position_y_initial + optflow_ef_sum_y;    if( _hist_position_estimate_x.is_full()) {        hist_position_base_x = _hist_position_estimate_x.front();        hist_position_base_y = _hist_position_estimate_y.front();    }else{        hist_position_base_x = _position_base.x;        hist_position_base_y = _position_base.y;    }    // calculate error in position from gps with our historical estimate    _position_error.x = x - (hist_position_base_x + _position_correction.x);    _position_error.y = y - (hist_position_base_y + _position_correction.y);}

对当前速度和位置进行估计，将过去的几个位置数据放到队列里以备后用。

// update - updates velocities and positions using latest info from ahrs and barometer if new data is available;void AP_InertialNav::update(float dt){    // discard samples where dt is too large    if( dt > 0.1f ) {        return;    }    // decrement ignore error count if required    if (_flags.ignore_error > 0) {        _flags.ignore_error--;    }    // check if new baro readings have arrived and use them to correct vertical accelerometer offsets.    check_baro();    // check if new gps or optflow readings have arrived and use them to correct position estimatescheck_and_correct_with_optflow_and_gps();    Vector3f accel_ef = _ahrs.get_accel_ef();    // remove influence of gravity    accel_ef.z += GRAVITY_MSS;    accel_ef *= 100.0f;    if(Motor_arm==false)accel_ef.z=0.0f;    // remove xy if not enabled    if( !using_optflow &&  !_xy_enabled) {          accel_ef.x = 0.0f;        accel_ef.y = 0.0f;    }    //Convert North-East-Down to North-East-Up    accel_ef.z = -accel_ef.z;    // convert ef position error to horizontal body frame    Vector2f position_error_hbf;    position_error_hbf.x = _position_error.x * _ahrs.cos_yaw() + _position_error.y * _ahrs.sin_yaw();    position_error_hbf.y = -_position_error.x * _ahrs.sin_yaw() + _position_error.y * _ahrs.cos_yaw();    float tmp = _k3_xy * dt;    accel_correction_hbf.x += position_error_hbf.x * tmp;    accel_correction_hbf.y += position_error_hbf.y * tmp;    accel_correction_hbf.z += _position_error.z * _k3_z  * dt;    tmp = _k2_xy * dt;    _velocity.x += _position_error.x * tmp;    _velocity.y += _position_error.y * tmp;    _velocity.z += _position_error.z * _k2_z  * dt;    tmp = _k1_xy * dt;    _position_correction.x += _position_error.x * tmp;    _position_correction.y += _position_error.y * tmp;    _position_correction.z += _position_error.z * _k1_z  * dt;    // convert horizontal body frame accel correction to earth frame    Vector2f accel_correction_ef;    accel_correction_ef.x = accel_correction_hbf.x * _ahrs.cos_yaw() - accel_correction_hbf.y * _ahrs.sin_yaw();    accel_correction_ef.y = accel_correction_hbf.x * _ahrs.sin_yaw() + accel_correction_hbf.y * _ahrs.cos_yaw();    // calculate velocity increase adding new acceleration from accelerometers    Vector3f velocity_increase;    velocity_increase.x = (accel_ef.x + accel_correction_ef.x) * dt;    velocity_increase.y = (accel_ef.y + accel_correction_ef.y) * dt;    velocity_increase.z = (accel_ef.z + accel_correction_hbf.z) * dt;    // calculate new estimate of position    _position_base += (_velocity + velocity_increase*0.5) * dt;    // update the corrected position estimate    _position = _position_base + _position_correction;    // calculate new velocity    _velocity += velocity_increase;    // store 3rd order estimate (i.e. estimated vertical position) for future use    _hist_position_estimate_z.push_back(_position_base.z);    // store 3rd order estimate (i.e. horizontal position) for future use at 10hz    _historic_xy_counter++;    if( _historic_xy_counter >= AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS ) {        _historic_xy_counter = 0;        _hist_position_estimate_x.push_back(_position_base.x);        _hist_position_estimate_y.push_back(_position_base.y);    }}

接下来是定点，对位置控制进行更新。打开AC_WPNav.cpp，找到如下程序，

/// update_loiter - run the loiter controller - should be called at 100hzvoid AC_WPNav::update_loiter(){    // calculate dt    uint32_t now = hal.scheduler->millis();    float dt = (now - _loiter_last_update) / 1000.0f;    // reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle    if (dt >= WPNAV_LOITER_UPDATE_TIME) {        // double check dt is reasonable        if (dt >= 1.0f) {            dt = 0.0;        }        // capture time since last iteration        _loiter_last_update = now;        // translate any adjustments from pilot to loiter target        calc_loiter_desired_velocity(dt);        // trigger position controller on next update        _pos_control.trigger_xy();    }else{        // run horizontal position controller        _pos_control.update_xy_controller(true);    }}

跳转到函数calc_loiter_desired_velocity(dt)，该函数将飞行员的输入转化为期望的加速度，积分后转化为期望的速度。

/// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance///updated velocity sent directly to position controllervoid AC_WPNav::calc_loiter_desired_velocity(float nav_dt){    // range check nav_dt    if( nav_dt < 0 ) {        return;    }    // check loiter speed and avoid divide by zero    if( _loiter_speed_cms < WPNAV_LOITER_SPEED_MIN) {        _loiter_speed_cms = WPNAV_LOITER_SPEED_MIN;        _loiter_accel_cms = _loiter_speed_cms/2.0f;    }    // rotate pilot input to lat/lon frame    Vector2f desired_accel;    desired_accel.x = (_pilot_accel_fwd_cms*_ahrs.cos_yaw() - _pilot_accel_rgt_cms*_ahrs.sin_yaw());    desired_accel.y = (_pilot_accel_fwd_cms*_ahrs.sin_yaw() + _pilot_accel_rgt_cms*_ahrs.cos_yaw());    // calculate the difference    Vector2f des_accel_diff = (desired_accel - _loiter_desired_accel);    // constrain and scale the desired acceleration    float des_accel_change_total = pythagorous2(des_accel_diff.x, des_accel_diff.y);    float accel_change_max = _loiter_jerk_max_cmsss * nav_dt;    if (des_accel_change_total > accel_change_max && des_accel_change_total > 0.0f) {        des_accel_diff.x = accel_change_max * des_accel_diff.x/des_accel_change_total;        des_accel_diff.y = accel_change_max * des_accel_diff.y/des_accel_change_total;    }    // adjust the desired acceleration    _loiter_desired_accel += des_accel_diff;    // get pos_control's feed forward velocity    Vector3f desired_vel = _pos_control.get_desired_velocity();    // add pilot commanded acceleration    desired_vel.x += _loiter_desired_accel.x * nav_dt;    desired_vel.y += _loiter_desired_accel.y * nav_dt;    // reduce velocity with fake wind resistance    if (_pilot_accel_fwd_cms != 0.0f || _pilot_accel_rgt_cms != 0.0f) {        desired_vel.x -= (_loiter_accel_cms)*nav_dt*desired_vel.x/_loiter_speed_cms;        desired_vel.y -= (_loiter_accel_cms)*nav_dt*desired_vel.y/_loiter_speed_cms;    } else {        desired_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.x/_loiter_speed_cms;        if(desired_vel.x > 0 ) {            desired_vel.x = max(desired_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);        }else if(desired_vel.x < 0) {            desired_vel.x = min(desired_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);        }        desired_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.y/_loiter_speed_cms;        if(desired_vel.y > 0 ) {            desired_vel.y = max(desired_vel.y - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);        }else if(desired_vel.y < 0) {            desired_vel.y = min(desired_vel.y + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);        }    }    // send adjusted feed forward velocity back to position controller    _pos_control.set_desired_velocity_xy(desired_vel.x,desired_vel.y);}

打开AC_PosControl.cpp，找到位置更新函数update_xy_controller(true)，分四步处理。

/// update_xy_controller - run the horizontal position controller - should be called at 100hz or highervoid AC_PosControl::update_xy_controller(bool use_desired_velocity){    // catch if we've just been started    uint32_t now = hal.scheduler->millis();    if ((now - _last_update_xy_ms) >= POSCONTROL_ACTIVE_TIMEOUT_MS) {        init_xy_controller();        now = _last_update_xy_ms;    }    // check if xy leash needs to be recalculated    calc_leash_length_xy();    // reset step back to 0 if loiter or waypoint parents have triggered an update and we completed the last full cycle    if (_flags.force_recalc_xy && _xy_step > 3) {        _flags.force_recalc_xy = false;        _xy_step = 0;    }    // run loiter steps    switch (_xy_step) {        case 0:            // capture time since last iteration            _dt_xy = (now - _last_update_xy_ms) / 1000.0f;            _last_update_xy_ms = now;            // translate any adjustments from pilot to loiter target            desired_vel_to_pos(_dt_xy);            _xy_step++;            break;        case 1:            // run position controller's position error to desired velocity step            pos_to_rate_xy(use_desired_velocity,_dt_xy);            _xy_step++;            break;        case 2:            // run position controller's velocity to acceleration step            rate_to_accel_xy(_dt_xy);            _xy_step++;            break;        case 3:            // run position controller's acceleration to lean angle step            accel_to_lean_angles();            _xy_step++;            break;    }}

第一步，将期望的速度转化为期望的位置。

/// desired_vel_to_pos - move position target using desired velocitiesvoid AC_PosControl::desired_vel_to_pos(float nav_dt){    // range check nav_dt    if( nav_dt < 0 ) {        return;    }    // update target position    if (_flags.reset_desired_vel_to_pos) {        _flags.reset_desired_vel_to_pos = false;    } else {        _pos_target.x += _vel_desired.x * nav_dt;        _pos_target.y += _vel_desired.y * nav_dt;    }}

第二步，根据期望位置和当前位置计算出位置误差_pos_error，注意此处的位置误差不同于光流位置估计的_position_error。_position_error是用来估计当前位置的，而_pos_error是目标位置和当前位置的误差。再根据_pos_error计算出目标速度_vel_target。

/// pos_to_rate_xy - horizontal position error to velocity controller///     converts position (_pos_target) to target velocity (_vel_target)///     when use_desired_rate is set to true:///         desired velocity (_vel_desired) is combined into final target velocity and///         velocity due to position error is reduce to a maximum of 1m/svoid AC_PosControl::pos_to_rate_xy(bool use_desired_rate, float dt){    Vector3f curr_pos = _inav.get_position();    float linear_distance;      // the distance we swap between linear and sqrt velocity response    float kP = _p_pos_xy.kP();    // avoid divide by zero    if (kP <= 0.0f) {        _vel_target.x = 0.0f;        _vel_target.y = 0.0f;    }else{        // calculate distance error        _pos_error.x = _pos_target.x - curr_pos.x;        _pos_error.y = _pos_target.y - curr_pos.y;        // constrain target position to within reasonable distance of current location        _distance_to_target = pythagorous2(_pos_error.x, _pos_error.y);        if (_distance_to_target > _leash && _distance_to_target > 0.0f) {            _pos_target.x = curr_pos.x + _leash * _pos_error.x/_distance_to_target;            _pos_target.y = curr_pos.y + _leash * _pos_error.y/_distance_to_target;            // re-calculate distance error            _pos_error.x = _pos_target.x - curr_pos.x;            _pos_error.y = _pos_target.y - curr_pos.y;            _distance_to_target = _leash;        }        // calculate the distance at which we swap between linear and sqrt velocity response        linear_distance = _accel_cms/(2.0f*kP*kP);        if (_distance_to_target > 2.0f*linear_distance) {            // velocity response grows with the square root of the distance            float vel_sqrt = safe_sqrt(2.0f*_accel_cms*(_distance_to_target-linear_distance));            _vel_target.x = vel_sqrt * _pos_error.x/_distance_to_target;            _vel_target.y = vel_sqrt * _pos_error.y/_distance_to_target;        }else{            // velocity response grows linearly with the distance            _vel_target.x = _p_pos_xy.kP() * _pos_error.x;            _vel_target.y = _p_pos_xy.kP() * _pos_error.y;        }        // decide velocity limit due to position error        float vel_max_from_pos_error;        if (use_desired_rate) {            // if desired velocity (i.e. velocity feed forward) is being used we limit the maximum velocity correction due to position error to 2m/s            vel_max_from_pos_error = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR;        }else{            // if desired velocity is not used, we allow position error to increase speed up to maximum speed            vel_max_from_pos_error = _speed_cms;        }        // scale velocity to stays within limits        float vel_total = pythagorous2(_vel_target.x, _vel_target.y);        if (vel_total > vel_max_from_pos_error) {            _vel_target.x = vel_max_from_pos_error * _vel_target.x/vel_total;            _vel_target.y = vel_max_from_pos_error * _vel_target.y/vel_total;        }        // add desired velocity (i.e. feed forward).        if (use_desired_rate) {            _vel_target.x += _vel_desired.x;            _vel_target.y += _vel_desired.y;        }    }}

第三步，跟据当前速度_vel_curr计算出速度误差_vel_error，位置误差的P值 + 速度误差的I值 + 速度误差的D值，得到目标加速度_accel_target。

/// rate_to_accel_xy - horizontal desired rate to desired acceleration///    converts desired velocities in lat/lon directions to accelerations in lat/lon framevoid AC_PosControl::rate_to_accel_xy(float dt){    const Vector3f &vel_curr = _inav.get_velocity();  // current velocity in cm/s    float accel_total;                          // total acceleration in cm/s/s    float lat_i, lon_i;    // reset last velocity target to current target    if (_flags.reset_rate_to_accel_xy) {        _vel_last.x = _vel_target.x;        _vel_last.y = _vel_target.y;        _flags.reset_rate_to_accel_xy = false;    }    // feed forward desired acceleration calculation    if (dt > 0.0f) {    if (!_flags.freeze_ff_xy) {    _accel_feedforward.x = (_vel_target.x - _vel_last.x)/dt;    _accel_feedforward.y = (_vel_target.y - _vel_last.y)/dt;        } else {    // stop the feed forward being calculated during a known discontinuity    _flags.freeze_ff_xy = false;    }    } else {    _accel_feedforward.x = 0.0f;    _accel_feedforward.y = 0.0f;    }    // store this iteration's velocities for the next iteration    _vel_last.x = _vel_target.x;    _vel_last.y = _vel_target.y;    // calculate velocity error    _vel_error.x = _vel_target.x - vel_curr.x;    _vel_error.y = _vel_target.y - vel_curr.y;    // get current i term    lat_i = _pid_rate_lat.get_integrator();    lon_i = _pid_rate_lon.get_integrator();    // update i term if we have not hit the accel or throttle limits OR the i term will reduce    if ((!_limit.accel_xy && !_motors.limit.throttle_upper) || ((lat_i>0&&_vel_error.x<0)||(lat_i<0&&_vel_error.x>0))) {        lat_i = _pid_rate_lat.get_i(_vel_error.x, dt);    }    if ((!_limit.accel_xy && !_motors.limit.throttle_upper) || ((lon_i>0&&_vel_error.y<0)||(lon_i<0&&_vel_error.y>0))) {        lon_i = _pid_rate_lon.get_i(_vel_error.y, dt);    }    // combine feed forward accel with PID output from velocity error    _accel_target.x = _accel_feedforward.x + _pid_rate_lat.get_p(_vel_error.x) + lat_i + _pid_rate_lat.get_d(_vel_error.x, dt);    _accel_target.y = _accel_feedforward.y + _pid_rate_lon.get_p(_vel_error.y) + lon_i + _pid_rate_lon.get_d(_vel_error.y, dt);    // scale desired acceleration if it's beyond acceptable limit    // To-Do: move this check down to the accel_to_lean_angle method?    accel_total = pythagorous2(_accel_target.x, _accel_target.y);    if (accel_total > POSCONTROL_ACCEL_XY_MAX && accel_total > 0.0f) {        _accel_target.x = POSCONTROL_ACCEL_XY_MAX * _accel_target.x/accel_total;        _accel_target.y = POSCONTROL_ACCEL_XY_MAX * _accel_target.y/accel_total;        _limit.accel_xy = true;     // unused    } else {        // reset accel limit flag        _limit.accel_xy = false;    }}

第四步，将目标加速度转化为目标roll/pitch角度，通过改变飞行器姿态角实现位置控制。

/// accel_to_lean_angles - horizontal desired acceleration to lean angles///    converts desired accelerations provided in lat/lon frame to roll/pitch anglesvoid AC_PosControl::accel_to_lean_angles(){    float accel_right, accel_forward;    float lean_angle_max = _attitude_control.lean_angle_max();    // To-Do: add 1hz filter to accel_lat, accel_lon    // rotate accelerations into body forward-right frame    accel_forward = _accel_target.x*_ahrs.cos_yaw() + _accel_target.y*_ahrs.sin_yaw();    accel_right = -_accel_target.x*_ahrs.sin_yaw() + _accel_target.y*_ahrs.cos_yaw();    // update angle targets that will be passed to stabilize controller    _roll_target = constrain_float(fast_atan(accel_right*_ahrs.cos_pitch()/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max);    _pitch_target = constrain_float(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max);}

整个流程大致如此，如有不对之处，欢迎批评指正！