光流定点程序梳理

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

// setup_home_position - reset state for home position change
void 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.html
void 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 frame
	optflow_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;  // east
	
	optflow_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 estimates
	check_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 100hz
void 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 controller
void 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 higher
void 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 velocities
void 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/s
void 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 frame
void 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 angles
void 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);
}

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