算法核心思想

首先我们的目的是控制被控对象，靠近目标轨迹并且沿着目标轨迹运行。在实际运行中被控目标可能受外界干扰，与目标轨迹存在一定偏差，运用横向轨迹误差法(Cross-track Error），通过目标轨迹与被控对象当前位置的距离，不断的计算调整被控对象的状态，使其不断的靠近目标轨迹。Cross-track Error的值越大，被控对象偏离原行驶方向的角度就越大，随着Cross-track Erro的减小，被控对象偏离原行驶方向的角度要不断减小，直至轨迹重合。由于 由 Cross-track Error 转换成 调整速度有PID—P控制实现得。所有此算法估且叫为 基于Cross-track Error 的P 导航二维控制。

单纯的Pcontroller会导致被控对象在目标轨迹附近波动前行，这种波动可能非常小，以至于在某些场景下我们可以忽略。

算法实现方法

如上图所示表明了什么是 crosstrack角度 和 crosstrack距离
实现方法就是通过三角函数关系 将 crosstrack距离 在x，y方向上进行分解 转换成纠正速度

实现无人机水平面导航控制

更新 由 crosstrack 计算出来的 大地坐标系下得 x y 方向 的 纠正 速度

#define CROSSTRACK_GAIN    0.4f

// 更新 由 crosstrack  计算出来的   大地坐标系下得  x y  方向 的 纠正 速度
void QuadrotorControl::Update_Nav_CrossTrack_Speed_To_Earth(double Angle_PreToNext_rad,double Angle_PreToCur_rad,double Distance_CurToPre)
{
    //  Angle_PreToNext_rad  上一航点到 下一航点的方位角
    //  Angle_PreToCur_rad  上一航点到  当前位置的方位角

    //  Angle_Nav_CrossTrack_Error_rad 偏离原始航线的 角度
  double   Angle_Nav_CrossTrack_Error_rad  =  Angle_PreToNext_rad  -  Angle_PreToCur_rad ;


      // 计算偏航距的距离  用来记录  实际轨迹偏差
   Nav_CrossTrack_Error_Distance_True_ =  sin(Angle_Nav_CrossTrack_Error_rad) * Distance_CurToPre ;


      // 被限幅得偏航距
    // 最大偏航距为 5m，对应速度2m/s  
  double  Nav_CrossTrack_Error_Distance_Constrain = Math_fConstrain(Nav_CrossTrack_Error_Distance_True_, -5.0f, 5.0f);    


      // 把偏航距的值 转化为 速度  再分解到 大地坐标系下
     Nav_CrossTrack_Speed_To_Earth_x_ = -CROSSTRACK_GAIN * Nav_CrossTrack_Error_Distance_Constrain *sin(Angle_PreToNext_rad);
     Nav_CrossTrack_Speed_To_Earth_y_ =  CROSSTRACK_GAIN * Nav_CrossTrack_Error_Distance_Constrain *cos(Angle_PreToNext_rad);
}

计算 自动航线 时的 期望速度 （（水平方向） 大地坐标系下的 【加上了由crosstrack计算的纠正速度】

// 计算  自动航线 时的 期望速度 （（水平方向）  大地坐标系下的
void QuadrotorControl::calc_nav_rate(double  max_speed )
{
    
    
    Update_Nav_CrossTrack_Speed_To_Earth(Angle_PreToNext_rad_ ,Angle_PreToCur_rad_ ,Distance_CurToPre_);
        
    
        // 把最大速度进行分解 xy 方向  然后加上 CrossTrack_Speed
    double    Desire_Velocity_x_world = cos(Angle_PreToNext_rad_) * max_speed + Nav_CrossTrack_Speed_To_Earth_x_;
    double    Desire_Velocity_y_world = sin(Angle_PreToNext_rad_) * max_speed + Nav_CrossTrack_Speed_To_Earth_y_;

       //将世界坐标系下速度转化为 机体坐标系下的速度
      double  Desire_Velocity_x_body =  Desire_Velocity_x_world * cos(UAV_yaw_* DEG2RAD)  + Desire_Velocity_y_world * sin(UAV_yaw_* DEG2RAD);
      double  Desire_Velocity_y_body =  Desire_Velocity_y_world * cos(UAV_yaw_* DEG2RAD)  - Desire_Velocity_x_world * sin(UAV_yaw_* DEG2RAD);

      UAV_VelocityControl(Desire_Velocity_x_body,Desire_Velocity_y_body,0,0);

}

自动飞向一点 水平方向 用CrossTrack算法

// 自动飞向一点 水平方向 用CrossTrack算法
/*
    参数：
     double   Pose_Pre_x , Pose_Pre_y ;  // 上一航点的 x，y位置
     double   Pose_Next_x, Pose_Next_y ;  // 下一航点的 x，y位置
*/
bool QuadrotorControl::Fly_To_Point_CrossTrack_XY(double Pose_Pre_x,double Pose_Pre_y, double Pose_Next_x,double Pose_Next_y)
{

    Cruise_Speed_Set_ = 3 ; //设定巡航速度

    //最大飞行速度
     double Max_Nav_Speed = 0; // m/s
    
      //当前x，y 的位置
     double   Pose_Cur_x = UAV_Pose_.position.x ;
     double   Pose_Cur_y = UAV_Pose_.position.y ;  

    
     // 计算各角度
       Angle_PreToCur_rad_ = atan2(Pose_Cur_y-Pose_Pre_y,Pose_Cur_x-Pose_Pre_x); // 上一航点到当前位置的方位角  以上一航点为原点
       Angle_PreToNext_rad_ = atan2(Pose_Next_y-Pose_Pre_y,Pose_Next_x-Pose_Pre_x); // 上一航点到下一航点的方位角  以上一航点为原点
    
    
    
     // 计算各距离
     double D_x_temp = Pose_Cur_x - Pose_Pre_x ; 
     double D_y_temp = Pose_Cur_y - Pose_Pre_y ;
      Distance_CurToPre_ = sqrtf(D_x_temp*D_x_temp+D_y_temp*D_y_temp);//当前点到上一航点得距离
     
     D_x_temp = Pose_Next_x - Pose_Pre_x ;
     D_y_temp = Pose_Next_y - Pose_Pre_y ;     
      Distance_NextToPre_ = sqrtf(D_x_temp*D_x_temp+D_y_temp*D_y_temp);//下一航点到上一航点得距离
     
    //计算垂点和下 一个航点的距离
     double  Distance_VerticalToNext = Distance_NextToPre_ - Distance_CurToPre_*cos(Angle_PreToNext_rad_-Angle_PreToCur_rad_);
     
     

     
     bool IsFinalPoint=1;//是否是最后一个航点    先做无刹车得
       if (IsFinalPoint) //如果不是最后一个航点
       {
           //判断是否在转弯过程中
           bool IsTurnHeading=0;
           if(IsTurnHeading)//正在转弯  Stabilize_Yaw.target - Stabilize_Yaw.current  判断  //可能是协调转弯的
           {
               Max_Nav_Speed = Math_fmin(Cruise_Speed_Set_, 4);       
           }else{
                Max_Nav_Speed =  Cruise_Speed_Set_ ;
           }
           
           
         //由当前速度来限制最大速度 ，防止加速阔快  减速过快
        double compound_speed  = Math_fMagnitude_three( UAV_TwistBody_.linear.x , UAV_TwistBody_.linear.y , 0);
        Max_Nav_Speed =  compound_speed +  Math_fConstrain((Max_Nav_Speed - compound_speed), -1,+1);
         
       }else{  // 是最后一个航点
    
    // 这一部分也可以作为 即将到点的刹车使用
           if (Distance_VerticalToNext >= 30)
        {
            Max_Nav_Speed = Cruise_Speed_Set_;
        }
        if ((Distance_VerticalToNext > 20) &&
            (Distance_VerticalToNext < 30))
        { 
            Max_Nav_Speed = Math_fmin(4, Cruise_Speed_Set_);
        }
        else if (Distance_VerticalToNext <= 20)
        {
            Max_Nav_Speed =Math_fmin((Distance_VerticalToNext * 0.2f),Cruise_Speed_Set_);
        }           
           
           //该加上高度处理部分， 先不做这一部分
       }
    
            // 根据  最终求得  最大飞行速度   结合crosstrack  得到实际 期望速度 x，y  Nav_CrossTrack_Speed_To_Earth_x   Nav_CrossTrack_Speed_To_Earth_y
           calc_nav_rate(Max_Nav_Speed );


     //判断是否到点
      if(Distance_VerticalToNext<Math_fConstrain(Max_Nav_Speed * 3,0.1,0.3))
      {
          return true ;
      }else{
          
          return false ;
      }
}

算法优化方向

1. 单纯的Pcontroller会导致被控对象在目标轨迹附近波动前行，这种波动可能非常小，以至于在某些场景下我们可以忽略。 可以加入D控制 ，来减少波动。
2. 上面是二维的，无人机可以实现3维运动，下篇博客将z方向加进去。
3. 结束阶段，没有加入减速控制，和最后的结束精度还没有优化。