ardupilot@天穹飞控计算遥杆控制量到加速度/速度-CFANZ编程社区

摘要

本节主要记录ardupilot 计算遥控器横滚，俯仰控制量转换成目标期望加速度和预测加速度，进而转换成期望速度，结合天穹飞控代码获取目标速度一起讲解。写这篇文章目的，ardupilot的loiter刹车不是很好，希望可以找到更好的loiter刹车控制效果，欢迎批评指正，一起探讨！看代码之前重点注意两个坐标系，机体坐标系和地理坐标系，控制量是在哪个坐标系进行的运算。

1. 获取遥控器横滚/俯仰输入量转换成角度量

//定义目标横滚角、目标俯仰角度
        float target_roll, target_pitch;
        //对飞行员输入申请简单模式转换---- apply SIMPLE mode transform to pilot inputs
        update_simple_mode();

        //转换飞行员输入到倾斜角度--- convert pilot input to lean angles
        get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(),
            attitude_control->get_althold_lean_angle_max());

重点关注的函数是get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(),

attitude_control->get_althold_lean_angle_max());

这个函数的形参：target_roll, target_pitch, loiter_nav->get_angle_max_cd(),
attitude_control->get_althold_lean_angle_max()

void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
{
    //油门故障检查------ throttle failsafe check
    if (copter.failsafe.radio || !copter.ap.rc_receiver_present)
    {
        roll_out = 0;
        pitch_out = 0;
        return;
    }
    //获取横滚和俯仰输入----- fetch roll and pitch inputs
    roll_out = channel_roll->get_control_in();
    pitch_out = channel_pitch->get_control_in();

    //限制最大倾斜角度----- limit max lean angle
    angle_limit = constrain_float(angle_limit, 1000.0f, angle_max);

    // scale roll and pitch inputs to ANGLE_MAX parameter range
    //将横滚和俯仰输入缩放到角度_最大参数范围
    float scaler = angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX; //最大不超过45度
    roll_out *= scaler;
    pitch_out *= scaler;

    //做循环限制------ do circular limit
    float total_in = norm(pitch_out, roll_out);
    if (total_in > angle_limit)
    {
        float ratio = angle_limit / total_in; //2/2gen2
        roll_out *= ratio;
        pitch_out *= ratio;
    }

    // do lateral tilt to euler roll conversion
    //进行横向倾斜到欧拉俯仰角转换,范围不超过【-17 +17】度
    //这里进行的处理应该是：横滚角度=artan(rzy,rzz)=arctan(sin(r)cos(p)/cos(r)/cos(p))
    //roll=arctan(sin(r)cos(p)/cos(r)/cos(p))=arctan(cosp*tanr/1)也就是把cosp设置最大值，roll有最小值
    roll_out = (18000/M_PI) * atanf(cosf(pitch_out*(M_PI/18000))*tanf(roll_out*(M_PI/18000)));


    // roll_out and pitch_out are returned
}

到这里得到了遥杆横滚+俯仰输入量到目标横滚角+俯仰角。

2. 目标横滚/俯仰角度控制量转换成飞行器的地理坐标系下加速度及预测加速度

void AC_Loiter::set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd, float dt)
{
    // Convert from centidegrees on public interface to radians
    //从公共界面上的cm度转换为弧度
    const float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);    //转换成了弧度
    const float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);  //转换成了弧度

    //convert our desired attitude to an acceleration vector assuming we are hovering
    //假设我们在悬停,将我们想要的姿态转换成加速度矢量
    const float pilot_cos_pitch_target = constrain_float(cosf(euler_pitch_angle), 0.5f, 1.0f);  //限制在0-60度
    //右侧的加速度,这个直接根据https://www.sohu.com/a/243549866_466960实现
    const float pilot_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(euler_roll_angle)/pilot_cos_pitch_target;
    //前进的加速度
    const float pilot_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(euler_pitch_angle);

    //旋转加速度矢量到地理坐标系lat/lon坐标系------ rotate acceleration vectors input to lat/lon frame
    _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());

    // difference between where we think we should be and where we want to be
    //我们认为我们应该去的地方和我们想要去的地方之间的差异
    Vector2f angle_error(wrap_PI(euler_roll_angle - _predicted_euler_angle.x), wrap_PI(euler_pitch_angle - _predicted_euler_angle.y));
    // calculate the angular velocity that we would expect given our desired and predicted attitude
    //根据我们期望和预测的姿态，计算出我们期望的角速度
    _attitude_control.input_shaping_rate_predictor(angle_error, _predicted_euler_rate, dt);
    // update our predicted attitude based on our predicted angular velocity
    //根据我们的预测角速度更新我们的预测姿态
    _predicted_euler_angle += _predicted_euler_rate * dt;

    // convert our predicted attitude to an acceleration vector assuming we are hovering
    //假设我们在悬停，将预测的姿态转换为加速度矢量
    const float pilot_predicted_cos_pitch_target = cosf(_predicted_euler_angle.y);
    const float pilot_predicted_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(_predicted_euler_angle.x)/pilot_predicted_cos_pitch_target;
    const float pilot_predicted_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(_predicted_euler_angle.y);

    // rotate acceleration vectors input to lat/lon frame
    //旋转输入到lat/lon框架的加速度矢量
    _predicted_accel.x = (pilot_predicted_accel_fwd_cms*_ahrs.cos_yaw() - pilot_predicted_accel_rgt_cms*_ahrs.sin_yaw());
    _predicted_accel.y = (pilot_predicted_accel_fwd_cms*_ahrs.sin_yaw() + pilot_predicted_accel_rgt_cms*_ahrs.cos_yaw());

}

这个函数有几个关键点需要注意：

1.目标横滚角，俯仰角说的是机体坐标系下的，需要把这个目标角度转换成机体坐标系下的加速度量。重点是下面公式

//convert our desired attitude to an acceleration vector assuming we are hovering
    //假设我们在悬停,将我们想要的姿态转换成加速度矢量
    const float pilot_cos_pitch_target = constrain_float(cosf(euler_pitch_angle), 0.5f, 1.0f);  //限制在0-60度
    //右侧的加速度,这个直接根据https://www.sohu.com/a/243549866_466960实现
    const float pilot_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(euler_roll_angle)/pilot_cos_pitch_target;
    //前进的加速度
    const float pilot_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(euler_pitch_angle);

这个是如何实现的？

上面代码核心实现：把飞行员操作输入的目标横滚角及目标俯仰角度转换成飞行器NED坐标系下的的目标加速度值，这里假设轴受力平衡，即在轴上保持静止，目标高度不变。则其受力如下图所示。这里把无人机看成质点，若想让无人机在NED坐标系轴保持静止，需要的垂直拉力刚好抵消重力，外力的分力产生维持轴上产生向前的加速度及轴上产生向右的加速度。

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_矩阵

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_计算机视觉_02

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_悬停_03

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_内环_04

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_矩阵_05

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_内环_06

对于上面的正负号，不一致，原因在于ardupilot规定横滚左负，右正；俯仰，上正，下负。

2.得到机体坐标系下的加速度后，我们还需要得到无人机地理坐标系下的速度信息。

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_矩阵_07

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_悬停_08

3.加速度预测加速度

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_算法_09

这里需要注意的是
_predicted_euler_angle.x表示横滚角，_predicted_euler_angle.y表示俯仰角，这里的转换公式和上面一样，这里不在进行讲述。

总结：

这里我们需要问这里是做什么？我们得到了两个加速度信息，一个是期望加速度，一个是预测加速度，这两个对后续的期望速度都有影响，同时根据期望加速度进行刹车相关处理，预测加速度对期望速度进行更新。

3.期望加速度及预测加速度更新期望速度

calc_desired_velocity(dt)

void AC_Loiter::calc_desired_velocity(float nav_dt)
{
    static Vector2f  lastVelCtlTarget;
    bool desired_accel_is_zero=0; //定义加速度
    bool desired_vel_is_zero=0;   //定义速度
    //新增刹车增量
    float brake_increment=0;
    float ekfGndSpdLimit, ekfNavVelGainScaler;

    //导航的更新实际
    ym_log_nav_dt=nav_dt; //这个值大概在0.0015-0.0025

    AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
    //ekf地速限制时间
    ym_log_ekfGndSpdLimit=ekfGndSpdLimit; //400
    //loiter模式下速度
    ym_log_speed_cms=_speed_cms;//1200
    //计算悬停速度限值，该限值为悬停速度设定值的最小值
    //参数和EKF设置的值，以观察光流限制
    float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit*100.0f); //最小地速限制
    gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, LOITER_SPEED_MIN);
    //地速限制
    ym_log_gnd_speed_limit_cms=gnd_speed_limit_cms; //1200
    //飞行最大加速度
    float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_angle_max_cd()*0.01f));

    //检查nav_dt 范围------ range check nav_dt
    if (nav_dt < 0)
    {
        return;
    }
    //设置最大速度
    _pos_control.set_max_speed_xy(gnd_speed_limit_cms);
    //设置最大加速度
    _pos_control.set_max_accel_xy(_accel_cmss);
    _pos_control.set_leash_length_xy(LOITER_POS_CORRECTION_MAX);

    //get loiters desired velocity from the position controller where it is being stored.
    //从被存储的地方,获取位置控制器获取悬停所需速度。
    const Vector3f &desired_vel_3d = _pos_control.get_desired_velocity();
    Vector2f desired_vel(desired_vel_3d.x,desired_vel_3d.y);

    // update the desired velocity using our predicted acceleration
    //使用预测加速度更新我们的目标速度，实时更新
    desired_vel.x += _predicted_accel.x * nav_dt;
    desired_vel.y += _predicted_accel.y * nav_dt; //【-292 294】

    ymv_log_predicted_speed_x=_predicted_accel.x * nav_dt;          //速度x
    ymv_log_predicted_speed_y=_predicted_accel.y * nav_dt;          //速度y

    log_desired_vel_x=desired_vel.x;          //目标速度x 【-401 351】
    log_desired_vel_y=desired_vel.y;          //目标速度y  【-832 766】
    //下面是刹车相关处理
    Vector2f loiter_accel_brake; //查查加速度
    float desired_speed = desired_vel.length(); //计算速度长度


    //判断速度是否等于0
    desired_vel_is_zero=is_zero(desired_speed);
    //判断速度长度是否为零
    if (!desired_vel_is_zero)//当有目标速度时执行
    {
        //归一化处理
        Vector2f desired_vel_norm = desired_vel/desired_speed;

        // TODO: consider using a velocity squared relationship like
        // pilot_acceleration_max*(desired_speed/gnd_speed_limit_cms)^2;
        // the drag characteristic of a multirotor should be examined to generate a curve
        // we could add a expo function here to fine tune it
        //todo:考虑使用速度平方关系
        //飞行员加速度最大值*（期望速度/地面速度极限值）^2；
        //应检查多转子的阻力特性，以生成曲线
        //我们可以在这里添加一个expo功能来进行微调
        //calculate a drag acceleration based on the desired speed.
        //根据所需速度计算阻力加速度
        float drag_decel = pilot_acceleration_max*desired_speed/gnd_speed_limit_cms;  //一半的阻力限制
        // calculate a braking acceleration if sticks are at zero
        //如果摇杆为零,计算刹车加速度
        float loiter_brake_accel = 0.0f; //如果摇杆为零，计算一个刹车加速度
        //当目标加速度是零

        desired_accel_is_zero=_desired_accel.is_zero();
        //判断加速度是否等于0
        if (desired_accel_is_zero) //开始进行刹车,否则不进行刹车操作
        {
            if ((AP_HAL::millis()-_brake_timer) > _brake_delay * 1000.0f)
            {
                //计算刹车增益
                float brake_gain = _pos_control.get_vel_xy_pid().kP() * 0.5f; //刹车增益是0.5
                //计算刹车加速度 //float error, float p, float second_ord_lim, float dt
                //得到悬停刹车加速度范围
                loiter_brake_accel = constrain_float(AC_AttitudeControl::sqrt_controller
                        (desired_speed, brake_gain, _brake_jerk_max_cmsss, nav_dt), 0.0f
                        , _brake_accel_cmss);
            }
        } else
        {
            loiter_brake_accel = 0.0f;
            _brake_timer = AP_HAL::millis();
        }


        //增加刹车增量，也就是刹车加速度
        brake_increment=loiter_brake_accel-_brake_accel;
        //计算刹车加速度
        _brake_accel += constrain_float(brake_increment, -_brake_jerk_max_cmsss*nav_dt,
                _brake_jerk_max_cmsss*nav_dt);
        // update the desired velocity using the drag and braking accelerations
        //使用阻力和制动加速度更新所需速度
        //整体合理阻力
        desired_speed = MAX(desired_speed-(drag_decel+_brake_accel)*nav_dt,0.0f);

        //归一化处理
        desired_vel = desired_vel_norm*desired_speed; //得到实时运动的速度
    }


    //增加刹车到期望加速度里面
    _desired_accel -= loiter_accel_brake;

    // Apply EKF limit to desired velocity -  this limit is calculated by the EKF and adjusted as required to ensure certain sensor limits are respected (eg optical flow sensing)
    //申请EKF限制到目标速度-
    //该限值由EKF计算,并根据需要进行调整，以确保遵守某些传感器限值（例如光流传感）
    float horizSpdDem = desired_vel.length();
    if (horizSpdDem > gnd_speed_limit_cms)
    {
        desired_vel.x = desired_vel.x * gnd_speed_limit_cms / horizSpdDem;
        desired_vel.y = desired_vel.y * gnd_speed_limit_cms / horizSpdDem;
    }

    // Limit the velocity to prevent fence violations
    // TODO: We need to also limit the _desired_accel
    //限制速度以防止违反围栏
    //TODO:我们还需要限制所需的加速度
    AC_Avoid *_avoid = AP::ac_avoid(); //避障使用
    if (_avoid != nullptr)
    {
        //调整速度
        _avoid->adjust_velocity(_pos_control.get_pos_xy_p().kP(), _accel_cmss, desired_vel, nav_dt);
    }

    // send adjusted feed forward acceleration and velocity back to the Position Controller
    //将调整后的前馈加速度和速度发送回位置控制器
    //计算最终的速度信息和加速度信息
    _pos_control.set_desired_accel_xy(_desired_accel.x, _desired_accel.y);
    _pos_control.set_desired_velocity_xy(desired_vel.x, desired_vel.y);
}

这里有两处比较重要代码

第一处：

//使用预测加速度更新我们的目标速度，实时更新，这里是地理坐标系
    desired_vel.x += _predicted_accel.x * nav_dt;
    desired_vel.y += _predicted_accel.y * nav_dt; //【-292 294】

第二处

根据期望加速度进行判断是否刹车

desired_accel_is_zero=_desired_accel.is_zero();
        //设定速度信息
        ymdv_log_desired_accel_is_zero=(uint8_t)desired_accel_is_zero;
        //判断加速度是否等于0
        if (desired_accel_is_zero) //开始进行刹车,否则不进行刹车操作

//增加刹车到期望加速度里面
    _desired_accel -= loiter_accel_brake;

具体整体代码逻辑，如何实现对期望速度进行的调整实现的刹车，看下框图：

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_悬停_10

到这里我们就得到了遥杆控制量转换到的期望速度信息，这个随着遥杆的操作进行不同的变化。

4. 期望速度后续如何使用

1.更新目标位置

desired_vel_to_pos(dt);

void AC_PosControl::desired_vel_to_pos(float nav_dt)
{
    //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;
    }
}

2.前馈速度控制

//增加速度前馈------ add velocity feed-forward
    _vel_target.x += _vel_desired.x;
    _vel_target.y += _vel_desired.y;

这个代码在void AC_PosControl::run_xy_controller(float dt)中

void AC_PosControl::run_xy_controller(float dt)
{
    float ekfGndSpdLimit, ekfNavVelGainScaler;
    AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
    //获取当前位置信息
    Vector3f curr_pos = _inav.get_position();
    //获取系数
    //EKF中用于补偿噪声光流测量的标度增益
    float kP = ekfNavVelGainScaler * _p_pos_xy.kP(); // scale gains to compensate for noisy optical flow measurement in the EKF

    //避免被零除--- 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; //位置误差x=目标位置-当前位置
        _pos_error.y = _pos_target.y - curr_pos.y; //位置误差y=目标位置-当前位置

        // Constrain _pos_error and target position
        // Constrain the maximum length of _vel_target to the maximum position correction velocity
        // TODO: replace the leash length with a user definable maximum position correction
        //限制_pos_error和目标位置
        //限制最大长度的_vel_target到最大位置更正速度
        //TODO:使用用户可定义的最大修正位置替换leash长度
        if (limit_vector_length(_pos_error.x, _pos_error.y, _leash))
        {
            _pos_target.x = curr_pos.x + _pos_error.x;
            _pos_target.y = curr_pos.y + _pos_error.y;
        }
        //获取目标速度
        _vel_target = sqrt_controller(_pos_error, kP, _accel_cms);
    }

    //增加速度前馈------ add velocity feed-forward
    _vel_target.x += _vel_desired.x;
    _vel_target.y += _vel_desired.y;

    // the following section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
    //下一节将lat/lon方向上的期望速度转换为lat/lon框架中的加速度
    Vector2f accel_target, vel_xy_p, vel_xy_i, vel_xy_d;

    // check if vehicle velocity is being overridden
    //检查车速是否被超越
    if (_flags.vehicle_horiz_vel_override)
    {
        _flags.vehicle_horiz_vel_override = false;
    } else
    {
        _vehicle_horiz_vel.x = _inav.get_velocity().x;  //获取当前速度
        _vehicle_horiz_vel.y = _inav.get_velocity().y;  //获取当前速度
    }

    //计算速度误差------calculate velocity error
    _vel_error.x = _vel_target.x - _vehicle_horiz_vel.x;
    _vel_error.y = _vel_target.y - _vehicle_horiz_vel.y;

    log_vel_desired_x=_vel_desired.x;          //前馈速度x
    log_vel_target_x=_vel_target.x ;           //目标速度x
    log_curr_vel_x=_vehicle_horiz_vel.x;       //当前速度x
    log_vel_error_x= _vel_error.x ;            //当前速度x误差
    log_vel_desired_y=_vel_desired.y;          //前馈速度x
    log_vel_target_y=_vel_target.y ;           //目标速度x
    log_curr_vel_y=_vehicle_horiz_vel.y;       //当前速度x
    log_vel_error_y =_vel_error.y;             //当前速度x

    // TODO: constrain velocity error and velocity target

    // call pi controller
    //TODO:约束速度误差和速度目标
    //调用pi控制器
    _pid_vel_xy.set_input(_vel_error);

    //获取p--- get p
    vel_xy_p = _pid_vel_xy.get_p();

    // update i term if we have not hit the accel or throttle limits OR the i term will reduce
    // TODO: move limit handling into the PI and PID controller
    //如果未达到加速或油门限制,则更新i项,否则i项将减少
    //TODO:将极限处理移到PI和PID控制器中
    if (!_limit.accel_xy && !_motors.limit.throttle_upper)
    {
        vel_xy_i = _pid_vel_xy.get_i();
    } else
    {
        vel_xy_i = _pid_vel_xy.get_i_shrink();
    }

    //获取d---- get d
    vel_xy_d = _pid_vel_xy.get_d();

    // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
    用于校正速度误差的加速度和用于补偿光流测量引起的EKF噪声的比例PID输出
    accel_target.x = (vel_xy_p.x + vel_xy_i.x + vel_xy_d.x) * ekfNavVelGainScaler;
    accel_target.y = (vel_xy_p.y + vel_xy_i.y + vel_xy_d.y) * ekfNavVelGainScaler;



    // reset accel to current desired acceleration
    //将加速度重置为当前所需加速度
    if (_flags.reset_accel_to_lean_xy)
    {
        _accel_target_filter.reset(Vector2f(accel_target.x, accel_target.y));
        _flags.reset_accel_to_lean_xy = false;
    }
    log_accel_target_filter_x=accel_target.x;  //滤波前加速度x
    log_accel_target_filter_y=accel_target.y;  //滤波前加速度x
    //滤波更正的加速度------ filter correction acceleration
    _accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f * ekfNavVelGainScaler));
    _accel_target_filter.apply(accel_target, dt);

    // pass the correction acceleration to the target acceleration output
    //将校正加速度传递给目标加速度输出
    _accel_target.x = _accel_target_filter.get().x;
    _accel_target.y = _accel_target_filter.get().y;

    // Add feed forward into the target acceleration output
    //将前馈添加到目标加速度输出中
    _accel_target.x += _accel_desired.x;
    _accel_target.y += _accel_desired.y;

    log_accel_target_x=_accel_target.x ;         //目标加速度x
    log_accel_desired_x=_accel_desired.x;        //前馈加速度x
    log_accel_target_y=_accel_target.y ;         //目标加速度y
    log_accel_desired_y=_accel_desired.y;        //前馈加速度y

    // the following section converts desired accelerations provided in lat/lon frame to roll/pitch angles

    // limit acceleration using maximum lean angles
    //下一节将lat/lon框架中提供的所需加速度转换为滚转/俯仰角
    //使用最大倾斜角度限制加速度
    float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd());
    float accel_max = MIN(GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f)), POSCONTROL_ACCEL_XY_MAX);
    _limit.accel_xy = limit_vector_length(_accel_target.x, _accel_target.y, accel_max);

    // update angle targets that will be passed to stabilize controller

    //更新将传递给姿态控制器的角度目标
    accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);
    //增加log打印信息
    log_roll_target=_roll_target;
    log_pitch_target=_pitch_target;
}

代码中，可以看出期望加速度也参与了前馈

//将前馈添加到目标加速度输出中
    _accel_target.x += _accel_desired.x;
    _accel_target.y += _accel_desired.y;

5.得到姿态控量

具体的位置PID控制，这里不进行阐述，主要实现位置，速度，加速度的PID控制，最终输出加速度控制量到姿态控制器。

accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);

在讲述这个之前，我们需要实时注意坐标系之间的转换，位置控制器输出的控制量是在地理坐标系下的，而姿态控制器是在机体坐标系，因此我们需要把位置控制输出量转换到地理坐标系。还有一个需要注意的是，我们到这里得到的控制量是位置控制器经过位置+速度，输出加速度，2级PID控制输出的量，这个输出的加速度量，不是一个角度值，需要进行转换，带着这些问题往下看代码就明白了

void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const
{
    float accel_right, accel_forward;

    //rotate accelerations into body forward-right frame
    //todo: this should probably be based on the desired heading not the current heading
    //将加速度旋转到机体前右坐标系
    //todo:这可能应该基于所需的机头，而不是当前机头
    accel_forward = accel_x_cmss * _ahrs.cos_yaw() + accel_y_cmss * _ahrs.sin_yaw();
    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 = atanf(-accel_forward / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
    float cos_pitch_target = cosf(pitch_target * M_PI / 18000.0f);
    roll_target = atanf(accel_right * cos_pitch_target / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
}

解释上面代码实现

accel_forward = accel_x_cmss * _ahrs.cos_yaw() + accel_y_cmss * _ahrs.sin_yaw();
    accel_right = -accel_x_cmss * _ahrs.sin_yaw() + accel_y_cmss * _ahrs.cos_yaw();

这个是怎么来的，我们还记得下面这个公式吗

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_算法_11

我们只需要把上面进行左有两边乘以一个转置矩阵即可，正常需要乘逆矩阵，这个是单位矩阵，逆矩阵和转置是等价的。

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_内环_12

所以就得到了

accel_forward = accel_x_cmss * _ahrs.cos_yaw() + accel_y_cmss * _ahrs.sin_yaw();
    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 = atanf(-accel_forward / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
    float cos_pitch_target = cosf(pitch_target * M_PI / 18000.0f);
    roll_target = atanf(accel_right * cos_pitch_target / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_悬停_13

这个公式可以求得目标横滚和俯仰角度值。后续姿态控制，不在进行讲解

6.天穹飞控位置控制实现逻辑

1.遥杆变化量转换成无人机速度控制。

整体代码如下，我们分步来分析。

static void AutoControl(RCCOMMAND_t rcCommand, RCTARGET_t* rcTarget)
{
    static Vector3f_t velCtlTarget, lastVelCtlTarget;

    static int32_t lastTimePosChanged = 0;
    static int32_t lastTimePosBrake   = 0;
    static uint8_t posHoldChanged     = 0;

    const  int16_t rcDeadband         = 50;
    static float   velRate            = (float)HORIZON_SPEED_MAX / MAXRCDATA;
    static float   brakeFilter        = 0;
    const  float   velTargetIncMax    = 0.7;      //摇杆增速限幅值

    //航向控制
    YawControl(rcCommand, rcTarget);

    //高度控制
    AltControl(rcCommand);

    /**********************************************************************************************************
    位置控制：该模式下摇杆量控制飞行速度，回中时飞机自动悬停
    **********************************************************************************************************/
    if(abs(rcCommand.roll) > rcDeadband || abs(rcCommand.pitch) > rcDeadband)
    {
        rcCommand.roll  = ApplyDeadbandInt(rcCommand.roll, rcDeadband * 0.8f);
        rcCommand.pitch = ApplyDeadbandInt(rcCommand.pitch, rcDeadband * 0.8f);

        //摇杆量转为目标速度，低通滤波改变操作手感
        velCtlTarget.x = velCtlTarget.x * 0.996f + ((float)rcCommand.pitch * velRate) * 0.004f;
        velCtlTarget.y = velCtlTarget.y * 0.996f + ((float)rcCommand.roll * velRate) * 0.004f;

        //目标速度增量限幅，以实现近似匀加速的效果
        if(rcCommand.pitch > 0)
        {
            if(velCtlTarget.x - lastVelCtlTarget.x > velTargetIncMax)
                velCtlTarget.x = lastVelCtlTarget.x + velTargetIncMax;
        }
        else
        {
            if(velCtlTarget.x - lastVelCtlTarget.x < -velTargetIncMax)
                velCtlTarget.x = lastVelCtlTarget.x - velTargetIncMax;
        }
        if(rcCommand.roll > 0)
        {
            if(velCtlTarget.y - lastVelCtlTarget.y > velTargetIncMax)
                velCtlTarget.y = lastVelCtlTarget.y + velTargetIncMax;
        }
        else
        {
            if(velCtlTarget.y - lastVelCtlTarget.y < -velTargetIncMax)
                velCtlTarget.y = lastVelCtlTarget.y - velTargetIncMax;
        }

        //保存本次速度目标值
        lastVelCtlTarget.x = velCtlTarget.x;
        lastVelCtlTarget.y = velCtlTarget.y;

        //直接控制速度，禁用位置控制
        SetPosCtlStatus(DISABLE);

        //更新位置内环控制目标
        SetPosInnerCtlTarget(velCtlTarget);

        //更新位置控制目标
        posCtlTarget.x = GetCopterPosition().x;
        posCtlTarget.y = GetCopterPosition().y;

        //更新位置控制状态
        SetPosControlStatus(POS_CHANGED);

        posHoldChanged = 1;
        lastTimePosChanged = GetSysTimeMs();
    }
    else if(posHoldChanged)
    {
        //重置历史速度目标值
        lastVelCtlTarget.x = 0;
        lastVelCtlTarget.y = 0;

        //进入刹车状态时先初始化目标速度
        if(GetPosControlStatus() == POS_CHANGED)
        {
            //计算减速速度（斜率）
            brakeFilter = Pythagorous2(velCtlTarget.x, velCtlTarget.y) / (500 * 1.0f);  //500Hz运行频率，1.0s刹车时间
            brakeFilter = ConstrainFloat(brakeFilter, 0.3, 2);

            //更新位置控制状态为刹车
            SetPosControlStatus(POS_BRAKE);
            //根据当前飞行速度更新最大刹车角度
            UpdateMaxBrakeAngle(GetCopterVelocity());
        }
        else if(GetPosControlStatus() == POS_BRAKE)
        {
            //匀减速刹车
            if(abs(velCtlTarget.x) > 0)
                velCtlTarget.x -= brakeFilter * (velCtlTarget.x / abs(velCtlTarget.x));
            if(abs(velCtlTarget.y) > 0)
                velCtlTarget.y -= brakeFilter * (velCtlTarget.y / abs(velCtlTarget.y));

            //飞机速度小于一定值或超出一定时间则认为刹车完成
            if((abs(GetCopterVelocity().x) < 20 && abs(GetCopterVelocity().y) < 20) || GetSysTimeMs() - lastTimePosChanged > 3000)
            {
                //更新位置控制状态为刹车完成
                SetPosControlStatus(POS_BRAKE_FINISH);
            }

            lastTimePosBrake = GetSysTimeMs();
        }
        else if(GetPosControlStatus() == POS_BRAKE_FINISH)
        {
            //刹车完成后再缓冲一小段时间便切换为自动悬停
            if(GetSysTimeMs() - lastTimePosBrake < 1500)
            {
                velCtlTarget.x -= velCtlTarget.x * 0.03f;
                velCtlTarget.y -= velCtlTarget.y * 0.03f;
            }
            else
            {
                posHoldChanged = 0;
            }
        }

        //更新位置内环控制目标
        SetPosInnerCtlTarget(velCtlTarget);

        //更新位置控制目标
        posCtlTarget.x = GetCopterPosition().x;
        posCtlTarget.y = GetCopterPosition().y;
    }
    else
    {
        //待机状态下不断刷新位置目标
        if(GetFlightStatus() == STANDBY)
        {
            posCtlTarget.x = GetCopterPosition().x;
            posCtlTarget.y = GetCopterPosition().y;
        }

        //使能位置控制
        SetPosCtlStatus(ENABLE);

        //更新位置控制状态
        SetPosControlStatus(POS_HOLD);

        //更新位置外环控制目标
        SetPosOuterCtlTarget(posCtlTarget);

        //设置位置环控制最大输出：cm/s
        SetMaxPosOuterCtl(150);
    }
}

1. 跟ardupilot一样，需要获取遥杆变化量

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_计算机视觉_14

在进行下一步之前，我们一定要实时记住坐标系之前的转换，控制是控制机体中还是地理坐标系中。不然很容易搞错！

2. 遥杆变化量转换成速度控制量

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_矩阵_15

这里标注出来的地方，不知道大家有没有疑惑，为什么俯仰就进行了x方向上的控制，横滚就进行了Y方向上的控制？

这里有个细节需要注意。因为速度是地理坐标系下的，那这里实际的机体坐标系是北东地，设想一下，假如我们飞机的机头正对北方向，这个时候操作遥杆打俯仰，是不是对应的无人机x轴往前飞。打俯仰对应的是Y轴左右移动。所以这里是有一个坐标系的。

对于刹车的处理，我们先不看，重点是遥杆不在中间代码实现。遥杆变化，对应着速度的增加和减小，遥杆回中，飞机开始刹车，速度变为零，这里有一个细节需要注意，就是ardupilot代码中pitch控制量是反向的，也就是往前打得到的遥杆控制量是负值【-4500,0】

3. 目标速度传递到内环速度换控制器

//更新位置内环控制目标
        SetPosInnerCtlTarget(velCtlTarget);

void SetPosInnerCtlTarget(Vector3f_t target)
{
    fc.posInnerTarget.x = target.x;
    fc.posInnerTarget.y = target.y;
}

同时更新当前目标位置

//更新位置控制目标
        posCtlTarget.x = GetCopterPosition().x;
        posCtlTarget.y = GetCopterPosition().y;

我们继续看天穹飞控内环控制器是如何实现。

void PositionInnerControl(void)
{
    static Vector3f_t velLpf;
    Vector3f_t posInnerCtlOutput;

    //计算函数运行时间间隔
    static uint64_t previousT;
    float deltaT = (GetSysTimeUs() - previousT) * 1e-6;
    previousT = GetSysTimeUs();

    //对速度测量值进行低通滤波，减少数据噪声对控制器的影响
    velLpf.x = velLpf.x * 0.9f + GetCopterVelocity().x * 0.1f;
    velLpf.y = velLpf.y * 0.9f + GetCopterVelocity().y * 0.1f;

    //计算控制误差
    fc.posInnerError.x = fc.posInnerTarget.x - velLpf.x;
    fc.posInnerError.y = fc.posInnerTarget.y - velLpf.y;

    //PID算法，计算出位置内环（X、Y轴速度）的控制量
    posInnerCtlOutput.y = PID_GetPID(&fc.pid[VEL_X], fc.posInnerError.x, deltaT) * 0.1f;
    posInnerCtlOutput.x = PID_GetPID(&fc.pid[VEL_Y], fc.posInnerError.y, deltaT) * 0.1f;

    //PID控制输出限幅，单位：°（目标角度）
    if(GetPosControlStatus() == POS_BRAKE)
    {
        posInnerCtlOutput.x = ConstrainFloat(posInnerCtlOutput.x, -fc.maxBrakeAngle, fc.maxBrakeAngle);
        posInnerCtlOutput.y = ConstrainFloat(posInnerCtlOutput.y, -fc.maxBrakeAngle, fc.maxBrakeAngle);
    }
    else
    {
        posInnerCtlOutput.x = ConstrainFloat(posInnerCtlOutput.x, -35, 35);
        posInnerCtlOutput.y = ConstrainFloat(posInnerCtlOutput.y, -35, 35);
    }

    //将位置内环控制量作为姿态外环的控制目标
    SetAttOuterCtlTarget(posInnerCtlOutput);
}

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_计算机视觉_16

void SetAttOuterCtlTarget(Vector3f_t target)
{
    fc.attOuterTarget.x = target.x;
    fc.attOuterTarget.y = -target.y;
}

我们需要看姿态外环控制器

void AttitudeOuterControl(void)
{
    uint8_t    flightMode;
    Vector3f_t angle;
    Vector3f_t attOuterCtlValue;
    float      yawRate = 0.35f;

    //获取当前飞机的姿态角
    angle = GetCopterAngle();
    //获取当前飞行模式
    flightMode = GetFlightMode();

    //对姿态测量值进行低通滤波，减少数据噪声对控制器的影响
    fc.angleLpf.x = fc.angleLpf.x * 0.92f + angle.x * 0.08f;
    fc.angleLpf.y = fc.angleLpf.y * 0.92f + angle.y * 0.08f;
    fc.angleLpf.z = fc.angleLpf.z * 0.92f + angle.z * 0.08f;

    //保留小数点后两位，减小数据误差对控制器的干扰（貌似没什么用）
    fc.angleLpf.x = (float)((int32_t)(fc.angleLpf.x * 100)) * 0.01f;
    fc.angleLpf.y = (float)((int32_t)(fc.angleLpf.y * 100)) * 0.01f;
    fc.angleLpf.z = (float)((int32_t)(fc.angleLpf.z * 100)) * 0.01f;

    //计算姿态外环控制误差：目标角度 - 实际角度
    //手动和半自动模式以及GPS失效下，摇杆量直接作为横滚和俯仰的目标量
    if(flightMode == MANUAL || flightMode == SEMIAUTO || GpsGetFixStatus() == false)
    {
        fc.attOuterError.x = fc.rcTarget.roll * 0.1f  - fc.angleLpf.x;
        fc.attOuterError.y = fc.rcTarget.pitch * 0.1f  - fc.angleLpf.y;
    }
    else
    {
        fc.attOuterError.x = fc.attOuterTarget.x - fc.angleLpf.x;
        fc.attOuterError.y = fc.attOuterTarget.y - fc.angleLpf.y;
    }

    //PID算法，计算出姿态外环的控制量，并以一定比例缩放来控制PID参数的数值范围
    attOuterCtlValue.x = PID_GetP(&fc.pid[ROLL_OUTER],  fc.attOuterError.x) * 1.0f;
    attOuterCtlValue.y = PID_GetP(&fc.pid[PITCH_OUTER], fc.attOuterError.y) * 1.0f;

    //PID控制输出限幅，目的是限制飞行中最大的运动角速度，单位为°/s
    //同时限制各种位置控制状态下的角速度，提升飞行过程中的控制感观
    if(flightMode == MANUAL || flightMode == SEMIAUTO || GpsGetFixStatus() == false)
    {
        attOuterCtlValue.x = ConstrainFloat(attOuterCtlValue.x, -200, 200);
        attOuterCtlValue.y = ConstrainFloat(attOuterCtlValue.y, -200, 200);
    }
    else if(GetPosControlStatus() == POS_CHANGED)
    {
        attOuterCtlValue.x = ConstrainFloat(attOuterCtlValue.x, -100, 100);
        attOuterCtlValue.y = ConstrainFloat(attOuterCtlValue.y, -100, 100);
    }
    else if(GetPosControlStatus() == POS_BRAKE)
    {
        attOuterCtlValue.x = ConstrainFloat(attOuterCtlValue.x, -100, 100);
        attOuterCtlValue.y = ConstrainFloat(attOuterCtlValue.y, -100, 100);
    }
    else
    {
        attOuterCtlValue.x = ConstrainFloat(attOuterCtlValue.x, -100, 100);
        attOuterCtlValue.y = ConstrainFloat(attOuterCtlValue.y, -100, 100);
    }

    //若航向锁定被失能则直接将摇杆数值作为目标角速度
    if(fc.yawHoldFlag == ENABLE)
    {
        fc.attOuterError.z = fc.attOuterTarget.z - angle.z;
        if(fc.attOuterError.z <= -180)
            fc.attOuterError.z += 360;
        if (fc.attOuterError.z>= +180)
            fc.attOuterError.z -= 360;

        //计算偏航轴PID控制量
        attOuterCtlValue.z = PID_GetP(&fc.pid[YAW_OUTER], fc.attOuterError.z) * 1.0f;
        //限幅，单位为°/s
        attOuterCtlValue.z = ConstrainFloat(attOuterCtlValue.z, -50, 50);
    }
    else
    {
        attOuterCtlValue.z = fc.rcTarget.yaw * yawRate;
    }

    //将姿态外环控制量作为姿态内环的控制目标
    SetAttInnerCtlTarget(attOuterCtlValue);
}

按照这种逻辑你是不是感觉少了坐标系转换，这里没有进行，说明前面一定进行了，我们返回去看代码。

4. 位置外环控制器

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_悬停_17

位置换外环的PID的输出量实内环速度换的目标输入量。

重点看坐标转换

/**********************************************************************************************************
*函 数 名: TransVelToBodyFrame
*功能说明: 将地理坐标系下的速度转化到机体坐标系
*形    参: 地理坐标系下的速度，机体坐标系速度指针，航向角
*返 回 值: 无
**********************************************************************************************************/
void TransVelToBodyFrame(Vector3f_t velEf, Vector3f_t* velBf, float yaw)
{
    float sinYaw = sinf(Radians(yaw));
    float cosYaw = cosf(Radians(yaw));

    velBf->x = velEf.x * cosYaw + velEf.y * sinYaw;
    velBf->y = -velEf.x * sinYaw + velEf.y * cosYaw;
    velBf->z = velEf.z;
}

还记得ardupilot代码吗？

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_矩阵_18

一切一幕了然！！！！

**也就是天穹飞控在外环实现了坐标系转换。**是否还有一个疑惑点，内环控制器的输入，是否有两个来源了。原因是：正常我们不进行遥杆操作时，无人机在定点模式下也是需要位置控制的，只不过这个时候目标位置是一个恒定值，需要进行位置控制，就进行了位置控制，得到外环PID控制输出量，就要传递给位置速度环控制器，也就是作为速度环控制的目标输入。当遥杆有变化时，这个时候无人机有了运动速度，目标位置也被更新了。具体这种实现是否效果很好，需要等待验证！

ardupilot@天穹飞控计算遥杆控制量到加速度/速度_悬停_19

目录

文章目录

摘要