PX4开源飞控位置控制解析

位置控制的总体思路为串级PID控制算法，内环为对速度的PID控制，外环为对位置的比例控制，被控对象为四旋翼无人机，被控量为四旋翼无人机输出的推力矢量和，将测量到的位置与速度进行反馈，与期望位置和期望速度进行比较，实现控制。本文以AUTO模式中的FollowTarget模式为例，介绍其中的控制原理。

一、期望位置的产生

　　在PX4中，期望位置由经纬度与海拔构成。由navigator模块发布的由三个具有时间序列的期望位置构成的pos_sp_triplet提取产生。Pos_sp_triplet包含prev_sp、curr_sp、next_sp三个期望位置点，在followTarget模式下，只有prev_sp和curr_sp即前一个期望位置点与当前的期望位置点有效，next_sp并不予以使用。然而由navigator模块传入的curr_sp并不是实际输入以上控制系统中的期望位置。期望位置的产生仍需进行进一步的加工，以保证其合理性，确保在每个采样间隔时间段内四旋翼都能收敛到期望位置。合理性包括对四旋翼航迹进行平滑与规划，并使当前位置距离期望位置是最大速度在采样间隔时间内可到达的，前一个目标点prev_sp与当前期望位置pos_sp之间的距离也是采样间隔时间段内最大速度可以达到的。
　　1.1、其对航迹的平滑与规划。采用了cross sphere line方法。首先将prev_sp、curr_sp和pos（当前四旋翼在本地坐标系中的坐标）转换到scale空间内prev_sp_s、curr_sp_s和pos_s，转换为即将距离无量纲归一化，使模为1的矢量表示最大速度时四旋翼可以移动的距离。令转换到scale空间内的三个位置点prev_sp_s、curr_sp_s和pos_s分别为a、b、c。令a_b=b-a，得向量a_b由前一期望位置指向当前期望位置。将点c投影至a_b即得投影点d。Ab_n为a_b方向上的单位向量。cd_len为c点与d点之间距离。pos_sp_s为经1次修正的scale空间中的期望位置。
　　Pos_sp_s的计算方法：
　　
　　以d为圆心做单位圆在a、b、c构成的平面上。当c和圆心d之间距离小于半径1时，令直线过c点，垂直于该点与圆心d的连线，与圆相交于两点，其中一点为x。根据勾股定理可得d_x距离。在a_b方向上,如果c在b位置前面，那么pos_sp_s为b。否则pos_sp_s为d+ab_norm*dx。当c_d距离大于等于半径时，如果在a_b方向上，a在c前面,那么pos_sp_s为a，如果c在b前面，那么目标点为b。如果c的投影在a_b之间，那么目标点为c的投影点d。求得目标点pos_sp_s即经过cross sphere line计算的，在sacle空间里的期望位置。将pos_sp_s用p表示。
　　当c相对于a、b处于不同位置时，对应p处于不同的位置：
　　

bool
MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r,
        const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res)
{
    /* project center of sphere on line */
    /* normalized AB */
    math::Vector<3> ab_norm = line_b - line_a;
    ab_norm.normalize();
    math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
    float cd_len = (sphere_c - d).length();

    if (sphere_r > cd_len) {
        /* we have triangle CDX with known CD and CX = R, find DX */
        float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);

        if ((sphere_c - line_b) * ab_norm > 0.0f) {
            /* target waypoint is already behind us */
            res = line_b;

        } else {
            /* target is in front of us */
            res = d + ab_norm * dx_len; // vector A->B on line
        }

        return true;

    } else {
        /* have no roots, return D */
        res = d; /* go directly to line */

        /* previous waypoint is still in front of us */
        if ((sphere_c - line_a) * ab_norm < 0.0f) {
            res = line_a;
        }

        /* target waypoint is already behind us */
        if ((sphere_c - line_b) * ab_norm > 0.0f) {
            res = line_b;
        }

        return false;
    }
}

　　1.2、检查期望位置从当前位置出发的可达性。在scale空间中，再次检查当前期望位置与当前位置之间的距离如果超过1则说明四旋翼即使采用最大速度飞行也无法达到期望位置，需要将从pos点到pos_sp_s的向量单位化。
　　

bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);

                    if (!near) {
                        /* we're far away from trajectory, pos_sp_s is set to the nearest point on the trajectory */
                        pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
                    }

　　1.3、检查修正2次后的期望位置pos_sp_s相对curr_sp_s是否存在移动速度过快的情况。比较的方法为计算从curr_sp_s到pos_sp_s的距离curr_pos1_m是否超过采样间隔时间dt,如果超过说明期望位置修正过大，应将其重新修正。
　　
　　

/* move setpoint not faster than max allowed speed */
        math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);

        /* difference between current and desired position setpoints, 1 = max speed */
        math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
        float d_pos_m_len = d_pos_m.length();

        if (d_pos_m_len > dt) {
            pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
        }

　　至此经过3次修正，得出scale空间中的期望位置pos_sp_s，将其转化为本地坐标系中的pos_sp即为输入位置控制系统中的期望位置。

_pos_sp = pos_sp_s.edivide(scale);

二、期望速度的产生

　　根据系统结构图_vel_sp=(_pos_sp-pos)*_params.pos_p，期望速度等于位置误差乘以比例系数。这样产生的速度还需经过三次处理，分别为四旋翼跟随目标的速度不低于目标，水平和竖直两个方向上的速度不能超过各自的最大速度，和两通道上的速度的变化速度不能超过最大加速度。
　　2.1、跟随目标的速度不低于目标。用Ft_vel记录下当前四旋翼水平方向的速度矢量。Cos_radio记录下当前水平方向的速度矢量与水平方向期望速度之间的夹角的cos值。当cos值大于0时，即两速度方向夹角小于90度。计算ft_vel。
　　
　　
　　若cos值小于等于0，即两速度夹角大于90度时，设置ft_vel为0。在水平方向上比较x，y两个轴方向ft_vel和_vel_sp哪个数值比较大，令_vel_sp在两个轴上的分量分别等于各自轴上较大的一方。以此达到僚机的速度不小于长机的速度，产生适当追赶的效果。

/* do not go slower than the follow target velocity when position tracking is active (set to valid)*/

if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
velocity_valid &&_pos_sp_triplet.current.position_valid) {

  math::Vector<3> ft_vel(_pos_sp_triplet.current.vx,         _pos_sp_triplet.current.vy, 0);

  float cos_ratio = (ft_vel * _vel_sp) / (ft_vel.length() * _vel_sp.length());

  // only override velocity set points when uav is traveling in same direction as target and vector component
  // is greater than calculated position set point velocity component

  if (cos_ratio > 0) {
    ft_vel *= (cos_ratio);
    // min speed a little faster than target vel
    ft_vel += ft_vel.normalized() * 1.5f;

  } else {
          ft_vel.zero();
  }

  _vel_sp(0) = fabs(ft_vel(0)) > fabs(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0);
  _vel_sp(1) = fabs(ft_vel(1)) > fabs(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1);

  // track target using velocity only

}
// limit vertical acceleration
float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;

if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
    acc_v /= fabsf(acc_v);
    _vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
}

　　2.2、水平方向与竖直方向双通道的速度限幅。检查vel_sp在水平方向的分量是否超过了该方向上的最大飞行速度，如果超过了则进行等比例缩放。检查_vel_sp在竖直方向上是否超过了最大上升或者下降的速度，如果超过了对其进行限幅。
　　

/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +_vel_sp(1) * _vel_sp(1));

if (vel_norm_xy > _params.vel_max(0)) {
    /* note assumes vel_max(0) == vel_max(1) */
    _vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
    _vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
}

/* make sure velocity setpoint is saturated in z*/
if (_vel_sp(2) < -1.0f * _params.vel_max_up) {
    _vel_sp(2) = -1.0f * _params.vel_max_up;
}

if (_vel_sp(2) >  _params.vel_max_down) {
    _vel_sp(2) = _params.vel_max_down;
}

　　2.3、水平方向与竖直方向双通道加速度限幅。检查当前_vel_sp与prev_vel_sp在dt时间内产生的水平与竖直方向的加速度，是否超过各自通道的最大加速度，如果超过了对其限幅至最值。
　　

// limit total horizontal acceleration
math::Vector<2> acc_hor;
acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;

if (acc_hor.length() > _params.acc_hor_max) {
    acc_hor.normalize();
    acc_hor *= _params.acc_hor_max;
    math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0),         _vel_sp_prev(1));
    math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev;
    _vel_sp(0) = vel_sp_hor(0);
    _vel_sp(1) = vel_sp_hor(1);
}

// limit vertical acceleration
float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;

if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
    acc_v /= fabsf(acc_v);
    _vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt +   _vel_sp_prev(2);
}

三、期望姿态的产生

　　内环速度PID控制器产生了推力的设定值，再由推力计算得姿态的期望值。
　　PID计算公式：
　　
　　

vel_err = _vel_sp - _vel;
thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int

　　vel_err为速度误差,_vel_err_d为速度误差的导数。P为比例系数，D为微分系数，I为积分系数。
　　thrust_int 为速度误差的积分，更新公式为:
　　

/* update integrals */
if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
    thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
    thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
}

if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
    thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;

    /* protection against flipping on ground when landing */
    if (thrust_int(2) > 0.0f) {
        thrust_int(2) = 0.0f;
    }
}

　　3.1、PID控制器得出期望推力。PID得出的推力期望值为thrust_sp，在此基础上需要依次进行三次修正分别为：确保升力大于最小升力，保证四旋翼的倾斜程度不超过最大倾斜角，保证推力的期望值不超过最大推力。
　　3.1.1、确保升力大于最小值。当-thrust_sp(2) < thr_min时，令thrust_sp(2) = -thr_min。

/* limit min lift */
if (-thrust_sp(2) < thr_min) {
    thrust_sp(2) = -thr_min;
    saturation_z = true;
}

　　3.1.2、为保证四旋翼不超过其最大倾斜角。保持竖直方向的升力不变，计算当前水平方向推力是否超过水平方向可提供的最大推力，如果超过将其等比例缩小，使推力的合力为最大推力值。

if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {

    /* limit max tilt */
    if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
        /* absolute horizontal thrust */
        float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0),       thrust_sp(1)).length();

        if (thrust_sp_xy_len > 0.01f) {
        /* max horizontal thrust for given vertical thrust*/
        float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);

            if (thrust_sp_xy_len > thrust_xy_max) {
                float k = thrust_xy_max / thrust_sp_xy_len;
                thrust_sp(0) *= k;
                thrust_sp(1) *= k;
                saturation_xy = true;
            }
        }
    }
}

　　3.1.3、限制最大推力。当z轴预设推力超过最大值时，令z轴向上的推力为最大值，x、y轴上的推力置为0。当z轴推力为正，但未超过最大推力时，保持z轴的推力不变，等比例缩小x、y轴的推力。当推力向下时，并且整体推力大于最大推力时，整体等比缩小至推力矢量模值为最大值。
　　

thrust_abs = thrust_sp.length(); /* recalculate because it might have changed */

if (thrust_abs > thr_max) {
    if (thrust_sp(2) < 0.0f) {
        if (-thrust_sp(2) > thr_max) {
            /* thrust Z component is too large, limit it */
            thrust_sp(0) = 0.0f;
            thrust_sp(1) = 0.0f;
            thrust_sp(2) = -thr_max;
            saturation_xy = true;
            saturation_z = true;

         } else {
         /* preserve thrust Z component and lower XY, keeping altitude is more important than position */
         float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2));
         float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
         float k = thrust_xy_max / thrust_xy_abs;
         thrust_sp(0) *= k;
         thrust_sp(1) *= k;
         saturation_xy = true;
         }

    } else {
    /* Z component is negative, going down, simply limit thrust vector */
    float k = thr_max / thrust_abs;
    thrust_sp *= k;
    saturation_xy = true;
    saturation_z = true;
    }

    thrust_abs = thr_max;
}

　　3.2、由期望推力得出期望姿态。为了实现设计的推力矢量方向，四旋翼的机体z轴应与推力矢量方向相反。机体x轴方向即机头方向应处于的方向的计算方法为，将期望航向在水平面向右旋转旋转90度，再用其叉乘期望姿态时的机体z轴。机体y轴的方向为，机体z轴叉乘机体x轴方向。令矩阵R每一列的数值分别对应机体x、y、z轴方向上，单位矢量在北东地坐标系下的坐标值，即得表示期望姿态的方向余弦矩阵。利用方向余弦矩阵即可转换成表示期望姿态的四元数，送至姿态控制器。
　　方向余弦矩阵转四元数：
　　

/* calculate attitude setpoint from thrust vector */
if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {
    /* desired body_z axis = -normalize(thrust_vector) */
    math::Vector<3> body_x;
    math::Vector<3> body_y;
    math::Vector<3> body_z;

    if (thrust_abs > SIGMA) {
        body_z = -thrust_sp / thrust_abs;

    } else {
        /* no thrust, set Z axis to safe value */
        body_z.zero();
        body_z(2) = 1.0f;
    }

    /* vector of desired yaw direction in XY plane, rotated by PI/2 */
    math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);

    if (fabsf(body_z(2)) > SIGMA) {
        /* desired body_x axis, orthogonal to body_z */
        body_x = y_C % body_z;

        /* keep nose to front while inverted upside down */
        if (body_z(2) < 0.0f) {
            body_x = -body_x;
        }

        body_x.normalize();

        } else {
            /* desired thrust is in XY plane, set X downside to construct correct matrix,
            * but yaw component will not be used actually */
            body_x.zero();
            body_x(2) = 1.0f;
        }

        /* desired body_y axis */
        body_y = body_z % body_x;

        /* fill rotation matrix */
        for (int i = 0; i < 3; i++) {
            R(i, 0) = body_x(i);
            R(i, 1) = body_y(i);
            R(i, 2) = body_z(i);
        }

        /* copy quaternion setpoint to attitude setpoint topic */
        matrix::Quatf q_sp = R;
        memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d));
        _att_sp.q_d_valid = true;

        /* calculate euler angles, for logging only, must not be used for control */
        matrix::Eulerf euler = R;
        _att_sp.roll_body = euler(0);
        _att_sp.pitch_body = euler(1);
        /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */

}

四、总结

　　由导航模块产生上一个期望位置和当前期望位置，位置控制部分对当前期望位置基于控制需要进行3次修正，产生有效的期望位置。期望位置与位置估计模块产生的真实位置比较，得出位置误差，位置误差乘以位置比例系数得出粗略的期望速度。对粗略的期望速度进行3次产生有效的期望速度。期望速度与姿态估计模块产生的真实速度比较，得出速度误差，计算速度误差的导数，采用PID算法，将速度误差乘以比例系数，速度误差的导数乘以微分系数，将速度误差的积分乘以积分系数，三者求和即为粗略的推力矢量和。对粗略的推力矢量和进行2次修正，得到有效的推力矢量和。由推力矢量和即可得出机体坐标系三轴的方向，将表示三轴方向的方向余弦矩阵转换为四元数表示。四元数表示的期望姿态由此产生，送至姿态控制器，经mixer与电机驱动输出推力，至此完成PX4开源飞控中位置控制的全部过程。

---

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