本文主要分析飞控获取到光流数据之后,如何实现定点,至于光流算法,不在本文讨论范围内。
官网介绍的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);
}
整个流程大致如此,如有不对之处,欢迎批评指正!