Ardupilot固定翼无人机L1制导律原理及代码解读

        L1制导律是固定翼无人机飞行路径跟踪的重要算法之一，这个算法由MIT的研究人员提出。在这之前，无人机进行路径跟踪一般采用基于侧偏距的PD控制器方法。至于为什么使用PD而不使用PID，那是因为位置环响应慢，所以一般在外环采用PD算法，而在内环采用PID。

        L1制导律就是在期望路径上选取一个参考点，根据这个参考点和当前水平速度计算出水平期望加速度的算法。而L1指的就是期望路径上的参考点与无人机当前水平位置的距离。

                                                      

 

 

 

                                                                    

        无人机在当前时刻的期望运动是一个半径为R，速度为V的圆周运动。所以在航向控制上也应该给一个相应的期望偏航角速率前馈，否则要等到出现航向角偏差时再进行反馈控制，会影响跟踪效果。

 

                                                        

          在实际飞行中，一般要跟踪的航线都是直线或者圆，因为计算机是离散系统，所以如果是不规则曲线也会进行线性化处理。所以，接下来针对直线路径和圆路径两种情况进行分析。

直线路径：

        在进行具体分析之前，我们先进行两个小角度假设，和角度很小，其实本质上是因为d相对于L1较小，相对于V较小，这符合实际飞行中的大部分情况:

                                                            

 

     根据上图以及加速度的公式，我们可以得出以下结论：

                                       

 

        这跟传统的基于侧偏距的PD方法是一样的。但这里的PD参数是跟V和L1同时相关的。速度越大，PD的参数也越大。

圆形路径：

                                                      

 

        对于圆形路径，同样我们先进行两个小角度假设，和角度很小，期望加速度为：

                                               

 

 

 

 

 

                                                                        

 

        这个跟传统的侧偏距PD算法相比多了一项，而一般情况下我们在使用传统算法时也会在前馈项中加入此项，从而达到更好的控制效果。

       其实，从上面的推导过程可以看出，L1制导律和传统算法具有很大的相似性，但是其中一个很大的却别是L1算法包含了V的变化情况，总的来讲，相比传统基于侧偏距的PD算法，L1制导律具有三个优势：

1、      对于各种不同的路径，不需要单独进行设计，一个公式就可以覆盖全部路径，在实际应用中尤其是代码实现时更加方便。

2、      对于初始条件偏差较大的情况，比如侧偏距和侧偏速度较大时可以比较平滑地向期望路径去过渡。

3、      跟传统基于侧偏距的PD算法相比，L1算法中会根据V的速度不同，计算出的期望加速度也不同，这对于实际飞行时不同的期望飞行速度以及环境风的干扰都有很好的适应能力。

       在PX4飞控中，L1对应的参数主要有两个：FW_L1_PERIOD和FW_L1_DAMPING。我们在飞行中主要调试FW_L1_PERIOD，当无人机转弯不够快时把此参数调小，直到飞机出现些许震荡后调大此参数2-3左右即可，至于FW_L1_DAMPING一般在0.65-0.85之间，每次调整0.05找到最好的飞行状态即可。

       首先，PX4中的L1制导律有两个参数需要我们进行调试，分别是FW_L1_PERIOD和FW_L1_DAMPING，为了下面计算方便，我们将它们缩写成L1_P和L1_D，它们的字面意思就是周期和阻尼比的意思。而PX4软件中关于L1的计算是这样的：

                                                                                       L1=L1_P* L1_D*V/π

     上一篇文章中我们得到关于跟踪直线路径时期望加速度为:

                                   

 

在η2很小，而且不考虑内环姿态响应时间的情况下，就会有，那上面的公式就变成了：

 

                                                    ，  

       这是一个典型的二阶系统。阻尼比是固定的0.707，自然频率是由飞行器的速度与L1的比值，也就是说如果L1是固定的，那这个二阶系统的自然频率是随着飞行速度的变化而变化的，这显然无法保证飞机在任何速度的情况下都能保持好的跟踪性能，这就是为什么px4软件中设计的L1与V是成线性关系的，这样可以保证系统的自然频率是固定的。
因为系统的阻尼比是固定的，就是0.707，但是考虑到实际飞行时，系统中加速度期望值存在响应时间，速度测量也存在时延，因此，阻尼比参数FW_L1_DAMPING可以在0.7左右调试，但不应该差得太多，一般在0.65-0.8之间就OK了。
而另外一个周期参数FW_L1_PERIOD是用来调试系统的自然频率的，我们把软件中的L1计算公式代入上式，就可以得出我们的自然频率是：

                                              

 

不同的周期参数下系统会有不同的自然频率，所得到的跟踪效果当然是不同的。

                         

 

 

void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP, float dist_min)

{

struct Location _current_loc;

    float Nu;

    float xtrackVel;

    float ltrackVel;

 

    uint32_t now = AP_HAL::micros();

    float dt = (now - _last_update_waypoint_us) * 1.0e-6f;

    if (dt > 0.1)

    {

        dt = 0.1;

        _L1_xtrack_i = 0.0f;

    }

    _last_update_waypoint_us = now;

 

    // 计算指定阻尼所需的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;

    }

    Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();

 

    // 更新 _target_bearing_cd

    _target_bearing_cd = _current_loc.get_bearing_to(next_WP);

 

    //计算地速

    float groundSpeed = _groundspeed_vector.length();

    if (groundSpeed < 0.1f)

   {

        // use a small ground speed vector in the right direction,

        // allowing us to use the compass heading at zero GPS velocity

        groundSpeed = 0.1f;

        _groundspeed_vector = Vector2f(cosf(get_yaw()), sinf(get_yaw())) * groundSpeed;

    }

 

//L1_dist = 1/π * damping * period * speed ≈ 0.3183099* damping * period * speed 

    _L1_dist = MAX(0.3183099f * _L1_damping * _L1_period * groundSpeed, dist_min);

 

 

//计算经纬坐标系下的AB向量

    Vector2f AB = prev_WP.get_distance_NE(next_WP);

    float AB_length = AB.length();

 

 

 

    // Check for AB zero length and track directly to the destination

    // if too small

    if (AB.length() < 1.0e-6f)

    {

        AB = _current_loc.get_distance_NE(next_WP);

        if (AB.length() < 1.0e-6f)

{

            AB = Vector2f(cosf(get_yaw()), sinf(get_yaw()));

        }

    }

    AB.normalize();//变成单位向量

 

//计算经纬坐标系下A到当前点的向量（假设为AC向量）

    const Vector2f A_air = prev_WP.get_distance_NE(_current_loc);

 

//|AC|*sin(AB与AC的夹角)   即偏离航线的距离

    _crosstrack_error = A_air % AB;

 

    float WP_A_dist = A_air.length();    //向量AC的长度 |AC|

float alongTrackDist = A_air * AB;    //|AC|*cos(AB与AC的夹角)   即AC在AB上的投影

if (WP_A_dist > _L1_dist && alongTrackDist/MAX(WP_A_dist, 1.0f) < -0.7071f)   //|AC|*cos(AB与AC的夹角)/|AC|=alongTrackDist/MAX(WP_A_dist, 1.0f)

{

//|AC|长度大于_L1_dist 且 AC和AB之间的夹角在+-135度之外  使用A点作为L1的参考点

        //Calc Nu to fly To WP A

        Vector2f A_air_unit = (A_air).normalized(); //向量AC的单位向量

        xtrackVel = _groundspeed_vector % (-A_air_unit); //垂直AC向量方向的速度 

        ltrackVel = _groundspeed_vector * (-A_air_unit);  // 平行于AC向量方向的速度

        Nu = atan2f(xtrackVel,ltrackVel); //地速与AC向量之间的夹角

        _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point

    }

    else if (alongTrackDist > AB_length + groundSpeed*3)

    {

//飞行超过B点3秒，要飞回B点，计算飞往B点的Nu（与飞往A点的计算方法一致）

        const Vector2f B_air = next_WP.get_distance_NE(_current_loc); //计算BC向量

        Vector2f B_air_unit = (B_air).normalized();  //单位化BC向量

        xtrackVel = _groundspeed_vector % (-B_air_unit); //垂直BC向量方向的速度 

        ltrackVel = _groundspeed_vector * (-B_air_unit);   //平行于BC向量方向的速度

        Nu = atan2f(xtrackVel,ltrackVel);

        _nav_bearing = atan2f(-B_air_unit.y , -B_air_unit.x); // bearing (radians) from AC to L1 point

    }

else //否则使用正常的L1引导

{

//计算飞往AB点轨迹线的Nu

//Nu2为速度与轨迹线之间的夹角

        xtrackVel = _groundspeed_vector % AB; //垂直AB向量方向的速度 

        ltrackVel = _groundspeed_vector * AB; //平行于AB向量方向的速度 

        float Nu2 = atan2f(xtrackVel,ltrackVel); //地速与AB向量之间的夹角

    

//Nu1为飞机与L1点连线和AB轨迹线之间的夹角

        float sine_Nu1 = _crosstrack_error/MAX(_L1_dist, 0.1f); //计算sin(Nu1)

        //限定Nu1在45度内

        sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f);

        float Nu1 = asinf(sine_Nu1); //反三角函数计算Nu1

 

//如果积分参数小于零或改变，则复位积分误差值

        if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain.get(), _L1_xtrack_i_gain_prev))

  {

            _L1_xtrack_i = 0;

            _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain;

        }

else if (fabsf(Nu1) < radians(5)) //Nu1小于5度

{

            _L1_xtrack_i += Nu1 * _L1_xtrack_i_gain * dt; //积分

 

          

//AHRS_TRIM_X=0.1会漂移到0.08，所以0.1是最坏的情况

//限幅

            _L1_xtrack_i = constrain_float(_L1_xtrack_i, -0.1f, 0.1f);

        }

 

//为了收敛到零，我们必须更努力地推动Nu1

        Nu1 += _L1_xtrack_i;

 

        Nu = Nu1 + Nu2;

        _nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point

    }

 

//如果我们在一个狭窄的角度范围内，并且转弯角度已经改变，可以使用之前的转弯决定来防止转弯时的犹豫不决

    _prevent_indecision(Nu);

    _last_Nu = Nu;

 

//限定Nu在+-90°

Nu = constrain_float(Nu, -1.5708f, +1.5708f);

 

//latAccDem = 4 * damping² * speed² * sin(Nu) / L1_dist .     (Nu = Nu1 + Nu2)

    _latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);

 

    //在跟踪Waypoint期间，Waypoint捕获状态始终为false

    _WPcircle = false;

 

    _bearing_error = Nu; // bearing error angle (radians), +ve to left of track

 

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

}

 

_L1_dist = MAX(0.3183099f * _L1_damping * _L1_period * groundSpeed, dist_min);

    K_L1 = 4.0f * _L1_damping * _L1_damping;

   _latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu)