L1 control——ArduPilot——更新圆圈update_Loiter

版权声明：本文为博主原创博文，未经允许不得转载，若要转载，请说明出处并给出博文链接 

本文详细讲解了ArduPilot中AP_L1_control库中的update_loiter函数的具体实现！！！

下面是源代码：

// update L1 control for loitering
void AP_L1_Control::update_loiter(const struct Location &center_WP, float radius, int8_t loiter_direction)
{
    struct Location _current_loc;

    // 计算loiter半径
    radius = loiter_radius(radius);

    // 计算PD控制增益(用于跟踪圆圈)
    float omega = (6.2832f / _L1_period);
    float Kx = omega * omega;
    float Kv = 2.0f * _L1_damping * omega;

    // 计算L1增益(用于捕获航点)
    float K_L1 = 4.0f * _L1_damping * _L1_damping;

    // 获取当前飞机的位置
    if (_ahrs.get_position(_current_loc) == false) {
        // if no GPS loc available, maintain last nav/target_bearing
        _data_is_stale = true;
        return;
    }
    // 通过融合空速计和GPS二者的速度，计算出地速
    Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();

    // 计算地速
    float groundSpeed = MAX(_groundspeed_vector.length() , 1.0f);


    // 更新目标方位角
    _target_bearing_cd = get_bearing_cd(_current_loc, center_WP);


    // 计算L1 length
    // 0.3183099 = 1/pi
    _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;

    // 计算飞机相对航点A的水平位置向量
    Vector2f A_air = location_diff(center_WP, _current_loc);

    // 计算A_air的单位长度
    Vector2f A_air_unit;
    if (A_air.length() > 0.1f) {
        A_air_unit = A_air.normalized();
    } else {
        if (_groundspeed_vector.length() < 0.1f) {
            A_air_unit = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw));
        } else {
            A_air_unit = _groundspeed_vector.normalized();
        }
    }

    // 计算捕获中心航点的Nu
    float xtrackVelCap = A_air_unit % _groundspeed_vector; // 指向航点的垂直方向速度
    float ltrackVelCap = - (_groundspeed_vector * A_air_unit); // 指向航点的速度
    float Nu = atan2f(xtrackVelCap,ltrackVelCap);

    _prevent_indecision(Nu);
    _last_Nu = Nu;

    Nu = constrain_float(Nu, -M_PI_2, M_PI_2); //Limit Nu to +- Pi/2

    // 用L1制导律计算水平倾斜加速度
    float latAccDemCap = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);

    // 计算圆圈半径和速度的误差
    float xtrackVelCirc = -ltrackVelCap; // Radial outbound velocity - reuse previous radial inbound velocity
    float xtrackErrCirc = A_air.length() - radius; // Radial distance from the loiter circle

    // 记录航迹误差
    _crosstrack_error = xtrackErrCirc;

    // 用PD控制器计算水平倾斜加速度
    float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv);

    // 计算切向速度
    float velTangent = xtrackVelCap * float(loiter_direction);

    //Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way
    if (ltrackVelCap < 0.0f && velTangent < 0.0f) {
        latAccDemCircPD =  MAX(latAccDemCircPD, 0.0f);
    }

    // 计算向心加速度
    float latAccDemCircCtr = velTangent * velTangent / MAX((0.5f * radius), (radius + xtrackErrCirc));

    // P+D+ff
    float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr);

    // Perform switchover between 'capture' and 'circle' modes at the
    // point where the commands cross over to achieve a seamless transfer
    // Only fly 'capture' mode if outside the circle
    if (xtrackErrCirc > 0.0f && loiter_direction * latAccDemCap < loiter_direction * latAccDemCirc) {
        _latAccDem = latAccDemCap;
        _WPcircle = false;
        _bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track
        _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
    } else {
        _latAccDem = latAccDemCirc;
        _WPcircle = true;
        _bearing_error = 0.0f; // bearing error (radians), +ve to left of track
        _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point
    }

    _data_is_stale = false; // status are correctly updated with current waypoint data
}
           

通过对上面的代码进行分析，可以发现该函数包含两个控制器，①capture模式—L1控制器 ②circle模式—PD控制器，两个控制器都会计算出飞机的水平加速度的目标值，即_latAccDem。这两个控制器的工作范围不同，当飞机还没有跟踪上圆圈，也就是还在圆圈外环的时候用L1控制器，当飞机已经跟踪上了圆圈的时候用PD控制器。

在RTL、GUIDED和LOITER模式下都会执行上面这个函数，其中涉及到的相关理论原理以及示意图如下所示。