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、检查修