mc_att_control源码解析

版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/qq_28773183/article/details/80375104

目录

之前写了博客分析了一下旋翼姿态控制的基础知识:mc_att_control基础知识,这次就对照代码将整个旋翼姿态控制过程呈现一遍.先看一下整个程序的框图:
这里写图片描述
从图中可以看到,实际上整个控制分成内外两个环进行控制,外环的输入是位置控制产生的姿态角设定值 ,输出的是角速度设定值,内环根据输入的角速度设定值和陀螺仪测得的角速度值进行控制,产生最终给混控器的量.这个串级PID的控制流程如下图:
这里写图片描述\


源码分析

我们直接进入任务主进程函数task_mian(),首先是数据订阅:

/*
     * do subscriptions
     */
    _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));//飞机姿态
    _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));//姿态设定值
    _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
    _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
    _params_sub = orb_subscribe(ORB_ID(parameter_update));
    _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
    _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
    _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));//电机转子的限制(判断是否饱和)
    _battery_status_sub = orb_subscribe(ORB_ID(battery_status));

    _gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT);//需要订阅最多三组陀螺仪的数据
    //orb_group_count():获取主题组的已发布实例的数量

这里需要注意的是订阅了三组的陀螺仪的数据,然后选择偏差最小的陀螺仪.代码如下:

for (unsigned s = 0; s < _gyro_count; s++) {
        _sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
    }

    _sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
    _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));

    /* initialize parameters cache */
    parameters_update();

    /* wakeup source: gyro data from sensor selected by the sensor app *///所选择的陀螺仪数据变化,则唤醒进程
    //陀螺仪的选择在sensor_correction.msg中有定义,选择误差小的陀螺仪数据

我们看一下sensor_correction.msg:

uint8 selected_gyro_instance    # gyro uORB topic instance for the voted sensor 选择的陀螺仪
uint8 selected_accel_instance   # accelerometer uORB topic instance for the voted sensor
uint8 selected_baro_instance    # barometric pressure uORB topic instance for the voted sensor

后面就是轮询,开始我们的控制任务,这里我们主要分析外环和内环控制实现的两个函数:

  1. control_attitude(dt):先得到各轴的角度偏差e_R,将angle error经过PF控制器得到_rates_sp(角速度设定值.
  2. control_attitude_rates(dt):通过选择数据最好的陀螺仪得到当前角速度信息,再和_rates_sp比较,通过PID+前馈控制,得到_att_control:即给混控器的四个u1,u2,u3,u4

    内环的控制参考论文:High Performance Full Attitude Control of a Quadrotor on SO(3)
    其主要思想是将整个旋转分成两部分:

    Re=RtorsionRtilt

    我们知道旋转是左乘旋转矩阵,这个公式的意思也就是先进行倾斜的旋转(roll和pitch),再进行扭矩的旋转(yaw),过程如下:
    这里写图片描述
    该怎么理解呢,我简单的说一下自己理解:
    用论文中的原话来解释我们为什么不同时控制三轴姿态:All movements on SO(3) must take the manifold structure into account as velocity constraints。这句话什么意思呢?简单的来说,就是我们roll-pitch是由于电机间力的偏差实现的,而yaw是通过扭矩实现。因此,控制roll-pitch比控制yaw更容易实现。比如同样是实现10°的变化,roll-pitch需要60ms左右;但是yaw控制器却需要接近150ms(V2版本的处理器)。如果我们进行分离的控制,旋转将不是平滑的,我们先进行易于控制的roll-pitch旋转,再进行yaw旋转,并且我们根据roll-pitch的误差角给yaw加权重,这样就最大限度减小了 yaw对roll-pitch的影响(可以想象的是,yaw在旋转过程中,roll-pitch由于控制更快,所以可以及时调节),论文展示了在不符合速度约束和符合速度约束的SO(3)群上旋转的演示:
    这里写图片描述


内环控制

vehicle_attitude_setpoint_poll();

    _thrust_sp = _v_att_sp.thrust;

    /* construct attitude setpoint rotation matrix */
    math::Quaternion q_sp(_v_att_sp.q_d[0], _v_att_sp.q_d[1], _v_att_sp.q_d[2], _v_att_sp.q_d[3]);//期望姿态
    math::Matrix<3, 3> R_sp = q_sp.to_dcm();//四元数转换成DCM矩阵(机体系到大地系(b-n))

    /* get current rotation matrix from control state quaternions */
    math::Quaternion q_att(_v_att.q[0], _v_att.q[1], _v_att.q[2], _v_att.q[3]);//当前姿态
    math::Matrix<3, 3> R = q_att.to_dcm();//当前的DCM矩阵

    /* all input data is ready, run controller itself */

    /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
    /**
     * @brief 先进行roll/pitch控制,再进行yaw控制,因为yaw的反应比它们慢
     *控制的思路是:对齐当前的Z轴和期望的Z轴,然后绕Z轴旋转一个偏航角
     */
    math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));//当前的Z轴(左乘的时候只和坐标z相乘)
    math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));//期望的z轴

    /* axis and sin(angle) of desired rotation (indexes: 0=pitch, 1=roll, 2=yaw).
     * This is for roll/pitch only (tilt), e_R(2) is 0 */
    math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);//叉乘得到两个Z轴的误差,再转到机体系(最后的旋转轴是机体系Z轴)

    /* calculate angle error */
    /**
     * @brief  这里的sin(tilt angle error)不直接用叉乘,而cos(tilt angle error)直接用点乘是因为叉乘得到的
     * 是向量,这里注意DCM的单位正交性,所以||R_z||和||R_sp_z||都为1
     */
    float e_R_z_sin = e_R.length(); // == sin(tilt angle error) length()函数求向量的模长 即|R_z|*|R_sp_z|sin(theta)
    float e_R_z_cos = R_z * R_sp_z; // == cos(tilt angle error) == (R.transposed() * R_sp)(2, 2)

我在代码中也做了很多注释,这段代码主要是取出当前姿态和期望姿态的Z轴,再利用点乘和叉乘得到Z轴间的差角θ.我们利用轴角法得到误差,同时得到旋转轴和旋转角度,这样就可以得到罗德里格斯旋转了,得到对齐Z轴后的姿态角:

 if (e_R_z_sin > 0.0f) {//角度在0-180度
        /* get axis-angle representation *///轴角法表示
        float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);//旋转角度
        math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;//归一化,变为单位向量,为了求反对称矩阵(旋转轴)


        e_R = e_R_z_axis * e_R_z_angle;//轴角法表示误差

        /* cross product matrix for e_R_axis *///求反对称矩阵
         // [0,-wz,wy;wz,0,-wx;-wy,wx,0]
        math::Matrix<3, 3> e_R_cp;//反对乘矩阵
        e_R_cp.zero();
        e_R_cp(0, 1) = -e_R_z_axis(2);
        e_R_cp(0, 2) = e_R_z_axis(1);
        e_R_cp(1, 0) = e_R_z_axis(2);
        e_R_cp(1, 2) = -e_R_z_axis(0);
        e_R_cp(2, 0) = -e_R_z_axis(1);
        e_R_cp(2, 1) = e_R_z_axis(0);

        /* rotation matrix for roll/pitch only rotation */
        //罗德里格斯旋转 这里是将R 进行了旋转,对齐了Z轴
        R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));//绕旋转轴旋转角度theta后,z轴对齐,只剩yaw了

    } else {//如果误差不在0-180度,只进行yaw旋转
        /* zero roll/pitch rotation */
        R_rp = R;
    }

Z轴对齐后,我们就需要进行偏航的旋转,代码如下:

math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));//期望的x轴
    math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));//旋转之后的x轴

    /* calculate the weight for yaw control
     * Make the weight depend on the tilt(倾斜) angle error: the higher the error of roll and/or pitch, the lower
     * the weight that we use to control the yaw. This gives precedence to roll & pitch correction.
     * The weight is 1 if there is no tilt error.
     */
    /**
     * 计算偏航控制的权重,tilt angle error越大,yaw权重就越小,这就是优先滚动和俯仰校正
     * 这里用余弦表示是因为当tilt angle越大,余弦就越小,那么yaw_w就越小,也就是说pitch/roll的角度误差越大,对于偏航控制权重就越小
     * 当没有pitch/roll偏差时,则可以进行完全的yaw控制了
     */
    float yaw_w = e_R_z_cos * e_R_z_cos;//误差的平方(cos(tilt angel)) 点乘

    /* calculate the angle between R_rp_x and R_sp_x (yaw angle error), and apply the yaw weight */
    //计算yaw angle error
    /**
yaw_w 就是对偏航转动的权重分量。如果 z 轴重合,yaw_w 就是 1,权重最大,也就是以
只有偏航转动,或者以其为主。随着 z 轴夹角增大,yaw 会两次方减小,降低偏航权重,使
得转动以俯仰滚转为主。
      **/
    e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;//e_R(2):yaw e_R(0):pitch e_R(1):roll 轴角法 各轴的角度差
    //这里%(叉乘之后)再*R_sp_z是因为叉乘得到向量,在点乘单位向量得到sin(theta),因为二者同方向

    if (e_R_z_cos < 0.0f) {//角度在90度到270度
        /* for large thrust vector rotations use another rotation method:
         * calculate angle and axis for R -> R_sp rotation directly */
        /**
         * 大角度下直接算误差e_R_d
         * 其中q_error.imag()得到误差角的一半
         */
        math::Quaternion q_error;
        q_error.from_dcm(R.transposed() * R_sp);
        math::Vector<3> e_R_d = q_error(0) >= 0.0f ? q_error.imag()  * 2.0f : -q_error.imag() * 2.0f;

        /* use fusion of Z axis based rotation and direct rotation */
        float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
        e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;//互补滤波
    }

代码注释得很清楚,在yaw的控制中需要加入倾斜角误差决定的权重,最后内环的控制是一个P控制器加前馈控制,这里不用积分控制是因为积分控制有延迟,不利于这里的快速变化控制的实时性.控制器如下:

/**
  外环是一个PF控制(比例前馈控制) 误差控制,也就是角度差的控制
  **/
    /* calculate angular rates setpoint */
    _rates_sp = _params.att_p.emult(e_R);//att_p:P 增益   即att_p(0)*e_R(0) att_p(1)*e_R(1) att_p(2)*e_R(2)


    /* Feed forward the yaw setpoint rate. We need to transform the yaw from world into body frame.
     * The following is a simplification(简单化) of:
     * R.transposed() * math::Vector<3>(0.f, 0.f, _v_att_sp.yaw_sp_move_rate * _params.yaw_ff)
     */
    math::Vector<3> yaw_feedforward_rate(R(2, 0), R(2, 1), R(2, 2));//这里可以看上面的注释,相称之后只剩下R.transposed的最后一列(即R的最后一行)
    yaw_feedforward_rate *= _v_att_sp.yaw_sp_move_rate * _params.yaw_ff;
   //前馈等于自身乘以前馈系数
    yaw_feedforward_rate(2) *= yaw_w;
    _rates_sp += yaw_feedforward_rate;

外环控制

外环控制相对比较简单,我将带有注释的代码放在这,主要是一个PID控制器加前馈控制:

/* reset integral if disarmed */
    if (!_v_control_mode.flag_armed || !_vehicle_status.is_rotary_wing) {
        _rates_int.zero();
    }

    // get the raw gyro data and correct for thermal errors
    //获取原始陀螺仪数据并进行温度补偿
    math::Vector<3> rates;
 //从三个陀螺仪中选一个
    if (_selected_gyro == 0) {
        rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0];
        rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1];
        rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_0[2]) * _sensor_correction.gyro_scale_0[2];

    } else if (_selected_gyro == 1) {
        rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_1[0]) * _sensor_correction.gyro_scale_1[0];
        rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_1[1]) * _sensor_correction.gyro_scale_1[1];
        rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_1[2]) * _sensor_correction.gyro_scale_1[2];

    } else if (_selected_gyro == 2) {
        rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[0]) * _sensor_correction.gyro_scale_2[0];
        rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_2[1]) * _sensor_correction.gyro_scale_2[1];
        rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_2[2]) * _sensor_correction.gyro_scale_2[2];

    } else {
        rates(0) = _sensor_gyro.x;
        rates(1) = _sensor_gyro.y;
        rates(2) = _sensor_gyro.z;
    }

    // rotate corrected measurements from sensor to body frame
    //将传感器测得的值旋转到机体坐标系
    rates = _board_rotation * rates;//rates:陀螺仪测得的三轴角速率

    // correct for in-run bias errors 纠正运行中偏差错误
    rates(0) -= _sensor_bias.gyro_x_bias;
    rates(1) -= _sensor_bias.gyro_y_bias;
    rates(2) -= _sensor_bias.gyro_z_bias;
   //note:1.6.4版本代码中没有对I项进行TPA操作,但是1.7.4中则进行了TPA操作
    math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p));//P增益衰减
    //math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i));
    math::Vector<3> rates_d_scaled = _params.rate_d.emult(pid_attenuations(_params.tpa_breakpoint_d, _params.tpa_rate_d));//

    /* angular rates error */
    math::Vector<3> rates_err = _rates_sp - rates;
//PID加前馈控制
    _att_control = rates_p_scaled.emult(rates_err) +
               _rates_int +
               rates_d_scaled.emult(_rates_prev - rates) / dt +
               _params.rate_ff.emult(_rates_sp);
//记录下期望角速度和当前角速度
    _rates_sp_prev = _rates_sp;
    _rates_prev = rates;

    /* update integral only if motors are providing enough thrust to be effective */
    //只有电机提供足够的推力才能生效,更新积分(保证开始姿态控制,怠速是不改变姿态的)
    if (_thrust_sp > MIN_TAKEOFF_THRUST) {
        for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
            // Check for positive control saturation 检测正向是否饱和
            bool positive_saturation =
                ((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_pos) ||
                ((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_pos) ||
                ((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_pos);

            // Check for negative control saturation 检测反向是否饱和
            bool negative_saturation =
                ((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_neg) ||
                ((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_neg) ||
                ((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_neg);

            // prevent further positive control saturation
            if (positive_saturation) {
                rates_err(i) = math::min(rates_err(i), 0.0f);

            }

            // prevent further negative control saturation
            if (negative_saturation) {
                rates_err(i) = math::max(rates_err(i), 0.0f);

            }

            // Perform the integration using a first order method and do not propaate the result if out of range or invalid
            //使用一阶方法执行积分,如果超出范围或无效,则不要预测结果 实际上就是为了防止积分饱和
            //当某一方向饱和时,rates_err(i)为0,不再进行积分分误差累计 一阶是指dt
            float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;

            if (PX4_ISFINITE(rate_i) && rate_i > -_params.rate_int_lim(i) && rate_i < _params.rate_int_lim(i)) {
                _rates_int(i) = rate_i;

            }
        }
    }

    /* explicitly limit the integrator state *///限制积分器的状态
    for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
        _rates_int(i) = math::constrain(_rates_int(i), -_params.rate_int_lim(i), _params.rate_int_lim(i));

    }

没有更多推荐了,返回首页