ardupilot开发 --- 位置控制与导航制导篇

几个疑问

如何根据GPS定位信息进行位置控制？
经纬度海拔高度如何转成导航坐标系？
飞控中的航迹点waypoint是基于那个坐标系的点？导航坐标系？
Home点？导航坐标系的原点？电机解锁时的点？xyz？

在代码中寻找答案

文档关于position control 的介绍：

几点个人理解

上电的时刻先建立一个NED全局坐标？

Home点的设定

设置Home点的线程：

    // update home from EKF if necessary
    FAST_TASK(update_home_from_EKF),

根据以下代码可知，电机解锁时设置当前位置为Home点；

// checks if we should update ahrs/RTL home position from the EKF
void Copter::update_home_from_EKF()
{
    // exit immediately if home already set
    if (ahrs.home_is_set()) {
        return;
    }

    // special logic if home is set in-flight
    if (motors->armed()) {
        set_home_to_current_location_inflight();
    } else {
        // move home to current ekf location (this will set home_state to HOME_SET)
        if (!set_home_to_current_location(false)) {
            // ignore failure
        }
    }
}

是不是彻底弄清这个图就能明白一切？

答案是否定的！
RTL mode 的调用逻辑：

加速度控制量转成飞机倾角，倾角是内环的控制目标

调用的地方：
// update angle targets that will be passed to stabilize controller
accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);
calculate_yaw_and_rate_yaw();
定义的地方：
// get_lean_angles_to_accel - convert roll, pitch lean angles to NE frame accelerations in cm/s/s
void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const
没错了，引用类型的量是被赋值的量，按这个原则去分析准没错；

• 把 NEU 坐标系下的目标加速度转成机体系下的目标加速度；
• 把机体系下的目标加速度转换成机体系飞机倾角，作为内环控制目标；

void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const
{
    // rotate accelerations into body forward-right frame
    const float accel_forward = accel_x_cmss * _ahrs.cos_yaw() + accel_y_cmss * _ahrs.sin_yaw();
    const float accel_right = -accel_x_cmss * _ahrs.sin_yaw() + accel_y_cmss * _ahrs.cos_yaw();

    // update angle targets that will be passed to stabilize controller
    pitch_target = accel_to_angle(-accel_forward * 0.01) * 100;
    float cos_pitch_target = cosf(pitch_target * M_PI / 18000.0f);
    roll_target = accel_to_angle((accel_right * cos_pitch_target)*0.01) * 100;
}

那 NEU 系下的目标加速度怎么来的？
即谁更新了_accel_target.x, _accel_target.y 的值？？或谁负责更新的_accel_target？？
向上追溯、查找发现：
要不先看定义吧：

• Vector3f _accel_target; // acceleration target in NEU cm/s/s

auto 模式下，xy位置控制器的实现逻辑

• 一开始肯定都是先判断飞行模式
  update_flight_mode
  然后 flightmode->run();
• 进到 auto 模式里面找到 run 函数；
• 执行航点任务式会执行这个case（可以通过打印消息到gcs来判断）：

    case SubMode::CIRCLE_MOVE_TO_EDGE:
        wp_run();
        break;

• wp_run() 中会调用具体的导航制导控制器：
  
• update_wpnav() 中：
  
  看没有避障功能的视实现：
  
  进advance_wp_target_along_track去一探究竟：
  看看函数的注释：沿轨道将目标位置从起点移动到终点

/// advance_wp_target_along_track - move target location along track from origin to destination
bool AC_WPNav::advance_wp_target_along_track(float dt)

当前时刻的期望位置如何获取这里不在赘叙…

• 获取到当前控制时刻的位置期望值后就可以调用具体的位置控制器：
  
• 那么位置控制又是如何实现的呢？
  类AP_Position中定义了以下控制器实例：

AC_P_2D         _p_pos_xy;          // XY axis position controller to convert distance error to desired velocity 
AC_PID_2D       _pid_vel_xy;        // XY axis velocity controller to convert velocity error to desired acceleration  

位置控制器的结构是：p位置 ~> pid 速度 ~> 加速度 ~>飞机倾角 的这样一种串联结构，控制器实例调用本身的 update_all 就可以计算出控制量：
显而易见，p位置控制器的输出是期望速度值：

    // Position Controller
    const Vector3f &curr_pos = _inav.get_position_neu_cm();
    Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, curr_pos);

pid速度控制器的输出是期望加速度值：

    // Velocity Controller
    const Vector2f &curr_vel = _inav.get_velocity_xy_cms();
    Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), curr_vel, _dt, _limit_vector.xy());

加速度控制器的输出是姿态内环的期望姿态角（倾角），此处的加速度控制器其实是一种加速度到倾角的映射，代码中用函数accel_to_lean_angles实现，如下代码所示：

    // Acceleration Controller
    // limit acceleration using maximum lean angles
    float angle_max = MIN(_attitude_control.get_althold_lean_angle_max_cd(), get_lean_angle_max_cd());
    float accel_max = angle_to_accel(angle_max * 0.01) * 100;
    // Define the limit vector before we constrain _accel_target 
    _limit_vector.xy() = _accel_target.xy();
    if (!limit_accel_xy(_vel_desired.xy(), _accel_target.xy(), accel_max)) {
        // _accel_target was not limited so we can zero the xy limit vector
        _limit_vector.xy().zero();
    }
    // update angle targets that will be passed to stabilize controller
    accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);

• 进入update_all就可以看到具体的pid算法实现：

//  update_all - set target and measured inputs to PID controller and calculate outputs
//  target and error are filtered
//  the derivative is then calculated and filtered
//  the integral is then updated based on the setting of the limit flag
float AC_PID::update_all(float target, float measurement, float dt, bool limit, float boost)
{
    // don't process inf or NaN
    if (!isfinite(target) || !isfinite(measurement)) {
        return 0.0f;
    }

    // reset input filter to value received
    if (_flags._reset_filter) {
        _flags._reset_filter = false;
        _target = target;
        _error = _target - measurement;
        _derivative = 0.0f;
    } else {
        float error_last = _error;
        _target += get_filt_T_alpha(dt) * (target - _target);
        _error += get_filt_E_alpha(dt) * ((_target - measurement) - _error);

        // calculate and filter derivative
        if (is_positive(dt)) {
            float derivative = (_error - error_last) / dt;
            _derivative += get_filt_D_alpha(dt) * (derivative - _derivative);
        }
    }

    // update I term
    update_i(dt, limit);

    float P_out = (_error * _kp);
    float D_out = (_derivative * _kd);

    // calculate slew limit modifier for P+D
    _pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, dt);
    _pid_info.slew_rate = _slew_limiter.get_slew_rate();

    P_out *= _pid_info.Dmod;
    D_out *= _pid_info.Dmod;

    // boost output if required
    P_out *= boost;
    D_out *= boost;

    _pid_info.target = _target;
    _pid_info.actual = measurement;
    _pid_info.error = _error;
    _pid_info.P = P_out;
    _pid_info.D = D_out;

    return P_out + _integrator + D_out;
}

• 那么问题来了，kpkikd是如何被动态修改的呢？
  略，后续补充…
• 经过以上一通天花乱坠的分析可知，最后位置外环的最终控制量被保存在变量 _roll_target, _pitch_target 中；

WGS84坐标系的航点–>位置控制器的期望位置值？EKF位置估计？NED,NEU，机体坐标系？

位置控制器中的的当前值量

curr_pos = _inav.get_position_neu_cm()
// references to inertial nav and ahrs libraries
const AP_InertialNav& _inav;
_relpos_cm ：position relative

// get the NE position relative to the local earth frame origin
Vector2f posNE;
if (_ahrs_ekf.get_relative_position_NE_origin(posNE)) {
    _relpos_cm.x = posNE.x * 100; // convert from m to cm
    _relpos_cm.y = posNE.y * 100; // convert from m to cm
}

// get the D position relative to the local earth frame origin
float posD;
if (_ahrs_ekf.get_relative_position_D_origin(posD)) {
    _relpos_cm.z = - posD * 100; // convert from m in NED to cm in NEU
}


AP_AHRS &_ahrs_ekf;

AHRS：状态估计器，包括姿态速度，位置信息，
AHRS调用了EKF和的信息：

#include 
#include 
#include   

bool position_is_valid = EKF3.getPosNE(posNE);

    // This is the normal mode of operation where we can use the EKF position states
    // correct for the IMU offset (EKF calculations are at the IMU)
    posNE.x = outputDataNew.position.x + posOffsetNED.x;
    posNE.y = outputDataNew.position.y + posOffsetNED.y;

机体系原点相对于参考点（全局坐标系NED）的位置估计？？
outputDataNew.position是IMU速度积分得到的位置信息：
// apply a trapezoidal integration to velocities to calculate position
outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f);
posOffsetNED是为了防止积分误差而引入的偏置，来源于GPS？
Vector3F accelPosOffset; ：//IMU加速计单元在车身框架中的位置（m）

// calculate the earth frame position of the body frame origin relative to the IMU
posOffsetNED = Tbn_temp * (- accelPosOffset);
Tbn_temp ：机体系到地面系的旋转矩阵；