无名飞控姿态解算和控制（三）

最新推荐文章于 2024-02-05 10:04:06 发布
最新推荐文章于 2024-02-05 10:04:06 发布
阅读量2.5k 收藏 34
点赞数 6
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/wang2012113132/article/details/78833282
本文详细介绍了无人机定高及定点控制的实现原理与过程，包括高度控制的三阶PID控制流程，以及GPS定点控制中从位置误差到姿态角期望值的转换。

摘要生成于 C知道 ，由 DeepSeek-R1 满血版支持， 前往体验 >

继续码代码

上一篇主要写了自稳模式下的代码流程，这次主要是飞控的定高和定点控制流程

首先是定高

控制模式在Main_Leading_Control里选择

定高模式代码：

else if(Controler_Mode==2)//定高模式
  {
/**************************定高模式，水平姿态期望角来源于遥控器******************************************/     
     Total_Controler.Pitch_Angle_Control.Expect=Target_Angle[0];
     Total_Controler.Roll_Angle_Control.Expect=Target_Angle[1];   
     
/*高度控制器第1步*/    
/********
    **
    **
    **
    **
    **
 ********/ 
/*******************************高度控制器开始****************************************/ 
/****************定高：高度位置环+速度环+加速度环，控制周期分别为8ms、4ms、4ms*******************/     
      if(RC_NewData[0]>=Deadzone_Min&&RC_NewData[0]<=Deadzone_Max)
     {
          static int16_t High_Hold_Cnt=0;//定高控制周期计数器
          //高度位置环输出给定速度期望
          if(Total_Controler.High_Position_Control.Expect==0)//油门回中后，只设置一次
          {
            Total_Controler.High_Position_Control.Expect=NamelessQuad.Position[_YAW];//油门回中后，更新高度期望
          }
          High_Hold_Cnt++;
          if(High_Hold_Cnt>=2)//竖直高度控制周期2*4=8ms
          {
              High_Hold_Cnt=0;
              Total_Controler.High_Position_Control.FeedBack=NamelessQuad.Position[_YAW];//反馈 
              PID_Control_High(&Total_Controler.High_Position_Control);//海拔高度位置控制器
              //内环速度期望
              Total_Controler.High_Speed_Control.Expect=Total_Controler.High_Position_Control.Control_OutPut;
          }
     }
     else if(RC_NewData[0]>Deadzone_Max)//给定上升速度期望
     {
            //油门杆上推、给定速度期望
           Total_Controler.High_Speed_Control.Expect=Climb_Up_Speed_Max*(RC_NewData[0]-Deadzone_Max)/(Thr_Top-Deadzone_Max);//最大上升速度50cm/s
           Total_Controler.High_Position_Control.Expect=0;//位置环期望置0
     }
     else if(RC_NewData[0]<Deadzone_Min)//给定下降速度期望
     {
            //油门杆下推、给定速度期望
           Total_Controler.High_Speed_Control.Expect=Climb_Down_Speed_Max*(RC_NewData[0]-Deadzone_Min)/(Deadzone_Min-Thr_Buttom);//最大下降速度40cm/s
           Total_Controler.High_Position_Control.Expect=0;//位置环期望置0
     }
     
/*高度控制器第2步*/ 
/********
        *
        *
   ****
 *
 *
 ********/  
     
/*******************************竖直速度控制器开始*******************************************************************/     
     Total_Controler.High_Speed_Control.FeedBack=NamelessQuad.Speed[_YAW];//惯导速度估计给速度反馈
     PID_Control_High(&Total_Controler.High_Speed_Control);//海拔高度速度控制
/*******************************竖直速度控制器结束******************************************************************/ 
     
/*高度控制器第3步*/ 
/********
       **
     **
   **
     **
       **
 ********/      
     
/*******************************竖直加速度控制器开始******************************************************************/
      Total_Controler.High_Acce_Control.Expect=Total_Controler.High_Speed_Control.Control_OutPut;//加速度期望
      Total_Controler.High_Acce_Control.FeedBack=FilterAfter_NamelessQuad.Acceleration[_YAW];//加速度反馈
      PID_Control_High(&Total_Controler.High_Acce_Control);//海拔高度加速度控制
/*******************************竖直加速度控制器结束******************************************************************/ 
     Throttle=Int_Sort(Throttle_Hover//悬停油门
              +Total_Controler.High_Acce_Control.Control_OutPut);//油门来源于高度控制器输出
/*****************************************高度控制器结束，给定油门控制量***********************************************************/   
  }
首先根据Controler_Mode_Select里的Controler_Mode=2来选择定高 

Total_Controler.Pitch_Angle_Control.Expect=Target_Angle[0];
Total_Controler.Roll_Angle_Control.Expect=Target_Angle[1];
遥控器输入作为俯仰角和横滚角的期望值

高度控制器第1步

判断遥控输入是否在死区内

if(RC_NewData[0]>=Deadzone_Min&&RC_NewData[0]<=Deadzone_Max)

RC_NewData[0]=Throttle_Control;//遥感油门原始行程量

static int16_t High_Hold_Cnt=0;//定高控制周期计数器
          //高度位置环输出给定速度期望
          if(Total_Controler.High_Position_Control.Expect==0)//油门回中后，只设置一次
          {
            Total_Controler.High_Position_Control.Expect=NamelessQuad.Position[_YAW];//油门回中后，更新高度期望
          }
油门回中，把现在的高度当做期望高度

 High_Hold_Cnt++;///++后此时为1，

以这个量来限制定高控制的周期High_Hold_Cnt++两次自加以后进行一次控制

if(High_Hold_Cnt>=2)//竖直高度控制周期2*4=8ms
          {
              High_Hold_Cnt=0;
              Total_Controler.High_Position_Control.FeedBack=NamelessQuad.Position[_YAW];//反馈 
              PID_Control_High(&Total_Controler.High_Position_Control);//海拔高度位置控制器
              //内环速度期望
              Total_Controler.High_Speed_Control.Expect=Total_Controler.High_Position_Control.Control_OutPut;
          }
进入定高控制，High_Hold_Cnt置0；

 Total_Controler.High_Position_Control.FeedBack=NamelessQuad.Position[_YAW];//反馈 得到现在的高度；

 PID_Control_High(&Total_Controler.High_Position_Control);//海拔高度位置控制器
              //内环速度期望
得到期望速度

看一下PID_Control_High定义

float PID_Control_High(PID_Controler *Controler)
{
/*******偏差计算*********************/
  Controler->Last_Err=Controler->Err;//保存上次偏差
  Controler->Err=Controler->Expect-Controler->FeedBack;//期望减去反馈得到偏差
  //Controler->Err=LPButter_Vel_Error(Controler->Err);
  if(Controler->Err_Limit_Flag==1)//偏差限幅度标志位
  {
  if(Controler->Err>=Controler->Err_Max)   Controler->Err= Controler->Err_Max;
  if(Controler->Err<=-Controler->Err_Max)  Controler->Err=-Controler->Err_Max;
  }
/*******积分计算*********************/
  if(Controler->Integrate_Separation_Flag==1)//积分分离标志位
  {
    if(ABS(Controler->Err)<=Controler->Integrate_Separation_Err)
    Controler->Integrate+=Controler->Ki*Controler->Err;
  }
  else
  {
    Controler->Integrate+=Controler->Ki*Controler->Err;
  }
/*******积分限幅*********************/
 if(Controler->Integrate_Limit_Flag==1)//积分限制幅度标志
 {
  if(Controler->Integrate>=Controler->Integrate_Max)
    Controler->Integrate=Controler->Integrate_Max;
  if(Controler->Integrate<=-Controler->Integrate_Max)
    Controler->Integrate=-Controler->Integrate_Max ;
 }
/*******总输出计算*********************/
  Controler->Last_Control_OutPut=Controler->Control_OutPut;//输出值递推
  Controler->Control_OutPut=Controler->Kp*Controler->Err//比例
                         +Controler->Integrate//积分
                         +Controler->Kd*(Controler->Err-Controler->Last_Err);//微分
/*******总输出限幅*********************/
  if(Controler->Control_OutPut>=Controler->Control_OutPut_Limit)
  Controler->Control_OutPut=Controler->Control_OutPut_Limit;
  if(Controler->Control_OutPut<=-Controler->Control_OutPut_Limit)
  Controler->Control_OutPut=-Controler->Control_OutPut_Limit;
/*******返回总输出*********************/
  return Controler->Control_OutPut;
}
对比一下 PID_Control函数（类似）： 

float PID_Control(PID_Controler *Controler)  
{  
/*******偏差计算*********************/  
  Controler->Last_Err=Controler->Err;//保存上次偏差  
  Controler->Err=Controler->Expect-Controler->FeedBack;//期望减去反馈得到偏差  
  if(Controler->Err_Limit_Flag==1)//偏差限幅度标志位  
  {  
  if(Controler->Err>=Controler->Err_Max)   Controler->Err= Controler->Err_Max;  
  if(Controler->Err<=-Controler->Err_Max)  Controler->Err=-Controler->Err_Max;  
  }  
/*******积分计算*********************/  
  if(Controler->Integrate_Separation_Flag==1)//积分分离标志位  
  {  
    if(ABS(Controler->Err)<=Controler->Integrate_Separation_Err)//ABS绝对值，#define ABS(X)  (((X)>0)?(X):-(X))  
    Controler->Integrate+=Controler->Ki*Controler->Err;  
  }  
  else  
  {  
    Controler->Integrate+=Controler->Ki*Controler->Err;  
  }  
/*******积分限幅*********************/  
 if(Controler->Integrate_Limit_Flag==1)//积分限制幅度标志  
 {  
  if(Controler->Integrate>=Controler->Integrate_Max)  
    Controler->Integrate=Controler->Integrate_Max;  
  if(Controler->Integrate<=-Controler->Integrate_Max)  
    Controler->Integrate=-Controler->Integrate_Max ;  
 }  
/*******总输出计算*********************/  
  Controler->Last_Control_OutPut=Controler->Control_OutPut;//输出值递推  
  Controler->Control_OutPut=Controler->Kp*Controler->Err//比例  
                         +Controler->Integrate//积分  
                         +Controler->Kd*(Controler->Err-Controler->Last_Err);//微分  
/*******总输出限幅*********************/  
  if(Controler->Control_OutPut>=Controler->Control_OutPut_Limit)  
  Controler->Control_OutPut=Controler->Control_OutPut_Limit;  
  if(Controler->Control_OutPut<=-Controler->Control_OutPut_Limit)  
  Controler->Control_OutPut=-Controler->Control_OutPut_Limit;  
/*******返回总输出*********************/  
  return Controler->Control_OutPut;  
}
返回Total_Controler.High_Position_Control.Control_OutPut，将其赋给Total_Controler.High_Speed_Control.Expect即期望速度；

如果油门杆不在死区内部，那么有两种情况爬升和下降

 else if(RC_NewData[0]>Deadzone_Max)//给定上升速度期望
     {
            //油门杆上推、给定速度期望
           Total_Controler.High_Speed_Control.Expect=Climb_Up_Speed_Max*(RC_NewData[0]-Deadzone_Max)/(Thr_Top-Deadzone_Max);//最大上升速度50cm/s
           Total_Controler.High_Position_Control.Expect=0;//位置环期望置0
     }
     else if(RC_NewData[0]<Deadzone_Min)//给定下降速度期望
     {
            //油门杆下推、给定速度期望
           Total_Controler.High_Speed_Control.Expect=Climb_Down_Speed_Max*(RC_NewData[0]-Deadzone_Min)/(Deadzone_Min-Thr_Buttom);//最大下降速度40cm/s
           Total_Controler.High_Position_Control.Expect=0;//位置环期望置0
     }
对于这两种情况根据油门输入赋值给期望速度

期望高度值置0；

/*高度控制器第2步*/ 

  Total_Controler.High_Speed_Control.FeedBack=NamelessQuad.Speed[_YAW];//惯导速度估计给速度反馈
  PID_Control_High(&Total_Controler.High_Speed_Control);//海拔高度速度控制
其中：NamelessQuad.Speed[_YAW];//惯导速度估计给速度反馈

这里的NamelessQuad.Speed是来自于姿态解算的高度结算

这里大致记一下：

高度结算大致如下函数：

float pos_correction[3]={0,0,0};
float acc_correction[3]={0,0,0};
float vel_correction[3]={0,0,0};
/****气压计三阶互补滤波方案——参考开源飞控APM****/
//#define TIME_CONTANST_ZER       1.5f
float TIME_CONTANST_ZER=3.0f;
#define K_ACC_ZER 	        (1.0f / (TIME_CONTANST_ZER * TIME_CONTANST_ZER * TIME_CONTANST_ZER))
#define K_VEL_ZER	        (3.0f / (TIME_CONTANST_ZER * TIME_CONTANST_ZER))//20															// XY????·′à??μêy,3.0
#define K_POS_ZER               (3.0f / TIME_CONTANST_ZER)
//#define High_Delay_Cnt  5 //20ms
uint16 High_Delay_Cnt=1;
float Altitude_Dealt=0;
float Altitude_Estimate=0;
void Strapdown_INS_High()
{
      uint16 Cnt=0;
      static uint16_t Save_Cnt=0;
      Save_Cnt++;//数据存储周期
      #ifdef  IMU_BOARD_GY86
          Altitude_Estimate=AirPresure_Altitude;//高度观测量
      #elsedef IMU_BOARD_NC686
          Altitude_Estimate=SPL06_001_Filter_high;
      #elsedef IMU_BOARD_NC683
          Altitude_Estimate=FBM320_Filter_high;
      #endif
      //由观测量（气压计）得到状态误差
      Altitude_Dealt=Altitude_Estimate-NamelessQuad.Pos_History[_YAW][High_Delay_Cnt];//气压计(超声波)与SINS估计量的差，单位cm
      //三路积分反馈量修正惯导
      acc_correction[_YAW] +=Altitude_Dealt* K_ACC_ZER*CNTLCYCLE ;//加速度矫正量
      vel_correction[_YAW] +=Altitude_Dealt* K_VEL_ZER*CNTLCYCLE ;//速度矫正量
      pos_correction[_YAW] +=Altitude_Dealt* K_POS_ZER*CNTLCYCLE ;//位置矫正量
      //加速度计矫正后更新
      NamelessQuad.Acceleration[_YAW]=Origion_NamelessQuad.Acceleration[_YAW]+acc_correction[_YAW];
      //速度增量矫正后更新，用于更新位置
      SpeedDealt[_YAW]=NamelessQuad.Acceleration[_YAW]*CNTLCYCLE;
      //原始位置更新
      Origion_NamelessQuad.Position[_YAW]+=(NamelessQuad.Speed[_YAW]+0.5*SpeedDealt[_YAW])*CNTLCYCLE;
      //位置矫正后更新
      NamelessQuad.Position[_YAW]=Origion_NamelessQuad.Position[_YAW]+pos_correction[_YAW];
      //原始速度更新
      Origion_NamelessQuad.Speed[_YAW]+=SpeedDealt[_YAW];
      //速度矫正后更新
      NamelessQuad.Speed[_YAW]=Origion_NamelessQuad.Speed[_YAW]+vel_correction[_YAW];

      if(Save_Cnt>=5)//20ms
      {
        for(Cnt=Num-1;Cnt>0;Cnt--)//20ms滑动一次
        {
        NamelessQuad.Pos_History[_YAW][Cnt]=NamelessQuad.Pos_History[_YAW][Cnt-1];
        }
        NamelessQuad.Pos_History[_YAW][0]=NamelessQuad.Position[_YAW];
        Save_Cnt=0;
      }

}
根据TIME.c里的代码，高度结算并没有用卡尔曼滤波

根据定义#define IMU_BOARD_NC686，则有 Altitude_Estimate=SPL06_001_Filter_high;

有函数

void SPL06_001_StateMachine(void)
{
  user_spl0601_get();
  if(SPL06_001_Cnt<=50)  SPL06_001_Cnt++;
  if(SPL06_001_Cnt==49)
  {
    SPL06_001_Offset_Okay=1;
    High_Okay_flag=1;
    SPL06_001_Offset=pressure;
  }
    
  if(SPL06_001_Offset_Okay==1)//初始气压零点设置完毕
  {
    SPL06_001_Filter_P=pressure;      
    SPL06_001_Filter_high=Get_Altitude_SPL06_001(SPL06_001_Filter_P);
  }
   
}
有气压计得到高度
float SPL06_001_Offset=0;
float Get_Altitude_SPL06_001(float baroPress)
{
    float Tempbaro=(float)(baroPress/SPL06_001_Offset)*1.0;
    True_Altitude = 4433000.0f * (1 - powf((float)(Tempbaro),0.190295f));   
    return True_Altitude;
}
回到高度控制： 

 Total_Controler.High_Speed_Control.FeedBack=NamelessQuad.Speed[_YAW];//惯导速度估计给速度反馈
 PID_Control_High(&Total_Controler.High_Speed_Control);//海拔高度速度控制
将现在竖直速度和期望速度对比产生误差控制输出Total_Controler.High_Speed_Control.Control_OutPut

/*高度控制器第3步*/

竖直加速度控制器开始******************************************************************/
      Total_Controler.High_Acce_Control.Expect=Total_Controler.High_Speed_Control.Control_OutPut;//加速度期望
      Total_Controler.High_Acce_Control.FeedBack=FilterAfter_NamelessQuad.Acceleration[_YAW];//加速度反馈
      PID_Control_High(&Total_Controler.High_Acce_Control);//海拔高度加速度控制

类似第二步，总共3阶pid控制 

/*******************************竖直加速度控制器结束******************************************************************/ 
     Throttle=Int_Sort(Throttle_Hover//悬停油门
              +Total_Controler.High_Acce_Control.Control_OutPut);//油门来源于高度控制器输出
/*****************************************高度控制器结束，给定油门控制量***********************************************************/
输出油门

油门在死区里怎么处理，死区外怎么处理，根据高度差输出期望速度，速度差产生期望加速度，加速度差产生油门



定高+定点模式

第一步定高控制，

      if(RC_NewData[0]>=Deadzone_Min&&RC_NewData[0]<=Deadzone_Max)
     {
          static int16_t High_Hold_Cnt=0;//定高控制周期计数器
          //高度位置环输出给定速度期望
          if(Total_Controler.High_Position_Control.Expect==0)//油门回中后，只设置一次
          {
            Total_Controler.High_Position_Control.Expect=NamelessQuad.Position[_YAW];//油门回中后，更新高度期望
          }
          High_Hold_Cnt++;
          if(High_Hold_Cnt>=2)//竖直高度控制周期2*4=8ms
          {
              High_Hold_Cnt=0;
              Total_Controler.High_Position_Control.FeedBack=NamelessQuad.Position[_YAW];//反馈 
              PID_Control_High(&Total_Controler.High_Position_Control);//海拔高度位置控制器
              //内环速度期望
              Total_Controler.High_Speed_Control.Expect=Total_Controler.High_Position_Control.Control_OutPut;
          }
     }
     else if(RC_NewData[0]>Deadzone_Max)//给定上升速度期望
     {
            //油门杆上推、给定速度期望
           Total_Controler.High_Speed_Control.Expect=Climb_Up_Speed_Max*(RC_NewData[0]-Deadzone_Max)/(Thr_Top-Deadzone_Max);//最大上升速度50cm/s
           Total_Controler.High_Position_Control.Expect=0;//位置环期望置0
     }
     else if(RC_NewData[0]<Deadzone_Min)//给定下降速度期望
     {
            //油门杆下推、给定速度期望
           Total_Controler.High_Speed_Control.Expect=Climb_Down_Speed_Max*(RC_NewData[0]-Deadzone_Min)/(Deadzone_Min-Thr_Buttom);//最大下降速度40cm/s
           Total_Controler.High_Position_Control.Expect=0;//位置环期望置0
     }/*高度控制器第2步*/ 
/********
        *
        *
   ****
 *
 *
 ********/  
     
/*******************************竖直速度控制器开始*******************************************************************/     
     Total_Controler.High_Speed_Control.FeedBack=NamelessQuad.Speed[_YAW];//惯导速度估计给速度反馈
     PID_Control_High(&Total_Controler.High_Speed_Control);//海拔高度速度控制
/*******************************竖直速度控制器结束******************************************************************/ 
     
/*高度控制器第3步*/ 
/********
       **
     **
   **
     **
       **
 ********/      
     
/*******************************竖直加速度控制器开始******************************************************************/
      Total_Controler.High_Acce_Control.Expect=Total_Controler.High_Speed_Control.Control_OutPut;//加速度期望
      Total_Controler.High_Acce_Control.FeedBack=FilterAfter_NamelessQuad.Acceleration[_YAW];//加速度反馈
      PID_Control_High(&Total_Controler.High_Acce_Control);//海拔高度加速度控制
/*******************************竖直加速度控制器结束******************************************************************/ 
     Throttle=Int_Sort(Throttle_Hover//悬停油门
              +Total_Controler.High_Acce_Control.Control_OutPut);//油门来源于高度控制器输出
/*****************************************高度控制器结束，给定油门控制量***********************************************************/ 

第二步GPS定点控制：

*******************************水平位置控制器开始***********************************************************/   
  if(GPS_Sate_Num>=6//定位卫星超过6个
       &&GPS_Quality<=5//水平精度因子大于6不可用
         )
   {
     if(Roll_Control==0&&Pitch_Control==0)//无水平遥控量给定 
    {
    //位置期望,经纬、航行速度、高度          
    if(Total_Controler.Latitude_Position_Control.Expect==0&&Total_Controler.Longitude_Position_Control.Expect==0)//方向杆回中后，只设置一次
    {
      Total_Controler.Latitude_Position_Control.Expect=NamelessQuad.Position[_ROLL];
      Total_Controler.Longitude_Position_Control.Expect=NamelessQuad.Position[_PITCH];
    }
   
    //位置反馈，来源于当前惯导的位置估计
      Total_Controler.Latitude_Position_Control.FeedBack=NamelessQuad.Position[_ROLL];
      Total_Controler.Longitude_Position_Control.FeedBack=NamelessQuad.Position[_PITCH];


      PID_Control(&Total_Controler.Latitude_Position_Control);//水平位置控制 
      PID_Control(&Total_Controler.Longitude_Position_Control);

      Total_Controler.Latitude_Speed_Control.Expect =Total_Controler.Latitude_Position_Control.Control_OutPut;// 南北方向、俯仰、纬度 
      Total_Controler.Longitude_Speed_Control.Expect=Total_Controler.Longitude_Position_Control.Control_OutPut;//东西方向、恒滚、经度
      
      
      //Total_Controler.Latitude_Speed_Control.Expect =0;// 南北方向、俯仰、纬度 
      //Total_Controler.Longitude_Speed_Control.Expect=0;//东西方向、恒滚、经度
    
        
         //水平速度控制
      Total_Controler.Latitude_Speed_Control.FeedBack=NamelessQuad.Speed[_ROLL];
      Total_Controler.Longitude_Speed_Control.FeedBack=NamelessQuad.Speed[_PITCH];
   
      PID_Control(&Total_Controler.Latitude_Speed_Control); 
      PID_Control(&Total_Controler.Longitude_Speed_Control);
      /***************水平经纬度方向上的角度期望，机头指向正北的时候*******
      ***********旋转到对应航向的Pitch,Roll方向上的期望角度****************

**********************************************************************************************/ 
   Position_Hold_Pitch=Total_Controler.Latitude_Speed_Control.Control_OutPut*Cos_Yaw
                        -Total_Controler.Longitude_Speed_Control.Control_OutPut*Sin_Yaw;
   
   Position_Hold_Roll=Total_Controler.Latitude_Speed_Control.Control_OutPut*Sin_Yaw
                        +Total_Controler.Longitude_Speed_Control.Control_OutPut*Cos_Yaw;
/**********************************************************************************************/     
   //定点模式，水平角度期望来源于水平速度环控制器输出
      Total_Controler.Pitch_Angle_Control.Expect=-Position_Hold_Pitch;
      Total_Controler.Roll_Angle_Control.Expect=Position_Hold_Roll;
   
 /*******************************水平位置控制器结束***********************************************************/
    }
    else //只进行水平速度控制，无水平位置控制
   {
      //分两种情况，1、导航坐标系的航向速度控制；
     //            2、载体坐标系方向上的速度控制
     if(Yaw_Control_Mode==Guide_Direction_Mode)//推动方向杆，对应导航系正东、正北的期望速度
     {
      Total_Controler.Latitude_Speed_Control.Expect =-Pitch_Control*4.0;// 最大期望速度50*3/100=1.5m/s
      Total_Controler.Longitude_Speed_Control.Expect=Roll_Control*4.0;
      
         //沿正东、正北水平速度控制
      Total_Controler.Latitude_Speed_Control.FeedBack=NamelessQuad.Speed[_ROLL];
      Total_Controler.Longitude_Speed_Control.FeedBack=NamelessQuad.Speed[_PITCH];
   
      PID_Control(&Total_Controler.Latitude_Speed_Control); 
      PID_Control(&Total_Controler.Longitude_Speed_Control);
 
      Position_Hold_Pitch=Total_Controler.Latitude_Speed_Control.Control_OutPut*Cos_Yaw
                        -Total_Controler.Longitude_Speed_Control.Control_OutPut*Sin_Yaw;
   
      Position_Hold_Roll =Total_Controler.Latitude_Speed_Control.Control_OutPut*Sin_Yaw
                        +Total_Controler.Longitude_Speed_Control.Control_OutPut*Cos_Yaw;         
    //水平角度期望来源于水平速度环控制器输出
      Total_Controler.Pitch_Angle_Control.Expect=-Position_Hold_Pitch;
      Total_Controler.Roll_Angle_Control.Expect=Position_Hold_Roll;    
     }
     else//推动方向杆，对应给定载体坐标系的沿Pitch,Roll方向运动速度
     {
      Total_Controler.Latitude_Speed_Control.Expect =-Pitch_Control*4.0;// 最大期望速度50*3/100=1.5m/s
      Total_Controler.Longitude_Speed_Control.Expect=Roll_Control*4.0;  
       //导航系的水平速度，转化到机体坐标系X-Y方向上
      Speed_Hold_Pitch=NamelessQuad.Speed[_ROLL]*Cos_Yaw
                        -NamelessQuad.Speed[_PITCH]*Sin_Yaw;
      Speed_Hold_Roll =NamelessQuad.Speed[_ROLL]*Sin_Yaw
                        +NamelessQuad.Speed[_PITCH]*Cos_Yaw;
       //沿载体Pitch、Roll方向水平速度控制
      Total_Controler.Latitude_Speed_Control.FeedBack=Speed_Hold_Pitch;
      Total_Controler.Longitude_Speed_Control.FeedBack=Speed_Hold_Roll;
      
      PID_Control(&Total_Controler.Latitude_Speed_Control);
      PID_Control(&Total_Controler.Longitude_Speed_Control);
     
      Total_Controler.Pitch_Angle_Control.Expect=-Total_Controler.Latitude_Speed_Control.Control_OutPut;
      Total_Controler.Roll_Angle_Control.Expect=Total_Controler.Longitude_Speed_Control.Control_OutPut;       
     }
     
      Total_Controler.Latitude_Position_Control.Expect=0;
      Total_Controler.Longitude_Position_Control.Expect=0;
   }
   }
  else//不满足定点条件，控制量给水平姿态
  {
/********对GPS定点模式位置0，直接进入姿态模式，等待GPS信号再次满足条件时，***********
*********自动切换至GPS定点模式，结合Controler_Mode_Select函数理解运行过程**********/
     Pos_Hold_SetFlag=0;//置0，当开关档位仍为定点模式时，
                        //在控制模式里面自检是否允许再次进入GPS定点模式
     Total_Controler.Pitch_Angle_Control.Expect=Target_Angle[0];
     Total_Controler.Roll_Angle_Control.Expect=Target_Angle[1];
  }
  }
首先判断有无遥控控制

if(Total_Controler.Latitude_Position_Control.Expect==0&&Total_Controler.Longitude_Position_Control.Expect==0)//方向杆回中后，只设置一次

判断期望位置控制是否置0

Total_Controler.Latitude_Position_Control.Expect=NamelessQuad.Position[_ROLL];
Total_Controler.Longitude_Position_Control.Expect=NamelessQuad.Position[_PITCH];

若是将当前位置设置为期望位置

      Total_Controler.Latitude_Position_Control.FeedBack=NamelessQuad.Position[_ROLL];
      Total_Controler.Longitude_Position_Control.FeedBack=NamelessQuad.Position[_PITCH];


      PID_Control(&Total_Controler.Latitude_Position_Control);//水平位置控制 
      PID_Control(&Total_Controler.Longitude_Position_Control);

      Total_Controler.Latitude_Speed_Control.Expect =Total_Controler.Latitude_Position_Control.Control_OutPut;// 南北方向、俯仰、纬度 
      Total_Controler.Longitude_Speed_Control.Expect=Total_Controler.Longitude_Position_Control.Control_OutPut;//东西方向、恒滚、经度
获取当前位置，做差经PID控制输出期望速度 

      Total_Controler.Latitude_Speed_Control.FeedBack=NamelessQuad.Speed[_ROLL];
      Total_Controler.Longitude_Speed_Control.FeedBack=NamelessQuad.Speed[_PITCH];
   
      PID_Control(&Total_Controler.Latitude_Speed_Control); 
      PID_Control(&Total_Controler.Longitude_Speed_Control);
产生期望加速度
   Position_Hold_Pitch=Total_Controler.Latitude_Speed_Control.Control_OutPut*Cos_Yaw
                        -Total_Controler.Longitude_Speed_Control.Control_OutPut*Sin_Yaw;
   
   Position_Hold_Roll=Total_Controler.Latitude_Speed_Control.Control_OutPut*Sin_Yaw
                        +Total_Controler.Longitude_Speed_Control.Control_OutPut*Cos_Yaw;
/**********************************************************************************************/     
   //定点模式，水平角度期望来源于水平速度环控制器输出
      Total_Controler.Pitch_Angle_Control.Expect=-Position_Hold_Pitch;
      Total_Controler.Roll_Angle_Control.Expect=Position_Hold_Roll;

水平角度期望来源于水平速度环控制器输出
定点模式，水平角度期望来源于水平速度环控制器输出？？？？？？？

if(Roll_Control==0&&Pitch_Control==0)//无水平遥控量给定 结束

    else //只进行水平速度控制，无水平位置控制
   {
      //分两种情况，1、导航坐标系的航向速度控制；
     //            2、载体坐标系方向上的速度控制
     if(Yaw_Control_Mode==Guide_Direction_Mode)//推动方向杆，对应导航系正东、正北的期望速度
     {
      Total_Controler.Latitude_Speed_Control.Expect =-Pitch_Control*4.0;// 最大期望速度50*3/100=1.5m/s
      Total_Controler.Longitude_Speed_Control.Expect=Roll_Control*4.0;
      
         //沿正东、正北水平速度控制
      Total_Controler.Latitude_Speed_Control.FeedBack=NamelessQuad.Speed[_ROLL];
      Total_Controler.Longitude_Speed_Control.FeedBack=NamelessQuad.Speed[_PITCH];
   
      PID_Control(&Total_Controler.Latitude_Speed_Control); 
      PID_Control(&Total_Controler.Longitude_Speed_Control);
 
      Position_Hold_Pitch=Total_Controler.Latitude_Speed_Control.Control_OutPut*Cos_Yaw
                        -Total_Controler.Longitude_Speed_Control.Control_OutPut*Sin_Yaw;
   
      Position_Hold_Roll =Total_Controler.Latitude_Speed_Control.Control_OutPut*Sin_Yaw
                        +Total_Controler.Longitude_Speed_Control.Control_OutPut*Cos_Yaw;         
    //水平角度期望来源于水平速度环控制器输出
      Total_Controler.Pitch_Angle_Control.Expect=-Position_Hold_Pitch;
      Total_Controler.Roll_Angle_Control.Expect=Position_Hold_Roll;    
     }
速度转换到机体下，仍然是速度怎么是角度？？？？？？？？

然后

     else //只进行水平速度控制，无水平位置控制
   {
      //分两种情况，1、导航坐标系的航向速度控制；
     //            2、载体坐标系方向上的速度控制
     if(Yaw_Control_Mode==Guide_Direction_Mode)//推动方向杆，对应导航系正东、正北的期望速度
     {
      Total_Controler.Latitude_Speed_Control.Expect =-Pitch_Control*4.0;// 最大期望速度50*3/100=1.5m/s
      Total_Controler.Longitude_Speed_Control.Expect=Roll_Control*4.0;
      
         //沿正东、正北水平速度控制
      Total_Controler.Latitude_Speed_Control.FeedBack=NamelessQuad.Speed[_ROLL];
      Total_Controler.Longitude_Speed_Control.FeedBack=NamelessQuad.Speed[_PITCH];
   
      PID_Control(&Total_Controler.Latitude_Speed_Control); 
      PID_Control(&Total_Controler.Longitude_Speed_Control);
 
      Position_Hold_Pitch=Total_Controler.Latitude_Speed_Control.Control_OutPut*Cos_Yaw
                        -Total_Controler.Longitude_Speed_Control.Control_OutPut*Sin_Yaw;
   
      Position_Hold_Roll =Total_Controler.Latitude_Speed_Control.Control_OutPut*Sin_Yaw
                        +Total_Controler.Longitude_Speed_Control.Control_OutPut*Cos_Yaw;         
    //水平角度期望来源于水平速度环控制器输出
      Total_Controler.Pitch_Angle_Control.Expect=-Position_Hold_Pitch;
      Total_Controler.Roll_Angle_Control.Expect=Position_Hold_Roll;    
     }
     else//推动方向杆，对应给定载体坐标系的沿Pitch,Roll方向运动速度
     {
      Total_Controler.Latitude_Speed_Control.Expect =-Pitch_Control*4.0;// 最大期望速度50*3/100=1.5m/s
      Total_Controler.Longitude_Speed_Control.Expect=Roll_Control*4.0;  
       //导航系的水平速度，转化到机体坐标系X-Y方向上
      Speed_Hold_Pitch=NamelessQuad.Speed[_ROLL]*Cos_Yaw
                        -NamelessQuad.Speed[_PITCH]*Sin_Yaw;
      Speed_Hold_Roll =NamelessQuad.Speed[_ROLL]*Sin_Yaw
                        +NamelessQuad.Speed[_PITCH]*Cos_Yaw;
       //沿载体Pitch、Roll方向水平速度控制
      Total_Controler.Latitude_Speed_Control.FeedBack=Speed_Hold_Pitch;
      Total_Controler.Longitude_Speed_Control.FeedBack=Speed_Hold_Roll;
      
      PID_Control(&Total_Controler.Latitude_Speed_Control);
      PID_Control(&Total_Controler.Longitude_Speed_Control);
     
      Total_Controler.Pitch_Angle_Control.Expect=-Total_Controler.Latitude_Speed_Control.Control_OutPut;
      Total_Controler.Roll_Angle_Control.Expect=Total_Controler.Longitude_Speed_Control.Control_OutPut;       
     }
     
      Total_Controler.Latitude_Position_Control.Expect=0;
      Total_Controler.Longitude_Position_Control.Expect=0;
   }
   }
两种情况产生期望角度

为什么两种情况？？？？？？？


然后就是姿态控制

/*************姿态环控制器*****************/
  Altitude_Control();

剩下的就和自稳模式一样了






















JRlipton
关注
无人机--飞控科普
weixin_30425949的博客
03-15 1870
无人机是无人驾驶飞机的简称（Unmanned Aerial Vehicle，UAV），是利用无线电遥控设备和自备的程序控制装置的不载人飞机，包括无人直升机、固定翼机、多旋翼飞行器、无人飞艇、无人伞翼机。广义地看也包括临近空间飞行器（20-100 公里空域），如平流层飞艇、高空气球、太阳能无人机等。从某种角度来看，无人机可以在无人驾驶的条件下完成复杂空中飞行任务和各种负载任务，可以被看做是“空中机器...
无名科创开源飞控-电路方案
04-21
1、四旋翼的传感器滤波(针对传感器不同使用情况:姿态解算、惯导、控制、传感器矫正等)分别采用窗口滑动滤波、不同截止频率的巴特沃斯数字低通滤波器)；2、姿态解算(互补滤波、梯度下降法等)；3、惯性导航（经典回路...
TECS——ArduPilot——高度与爬升速度的估计（3阶互补滤波）
慕离巷的博客
05-25 4499
版权声明：本文为博主原创博文，未经允许不得转载，若要转载，请说明出处并给出博文链接  本文详细讲解了ArduPilot中AP_TECS库中的用于估计高度和垂直方向上的爬升速率的三阶互补滤波的实现！       首先，需要强调的是，本文介绍的3阶互补滤波（Complementary Filter）是在飞控无法获取EKF（Extend Kalman Filter）提供的北东地三个方向上的速度的情况...
无名飞控姿态解算和控制（一）
lipton的博客
12-14 8844
无名飞控的姿态解算和控制
无名飞控的姿态解算和控制（二）
lipton的博客
12-17 3560
接上篇继续码函数 Total_Control();//总控制器 查找定义： void Total_Control(void) { static uint16_t Mode_Check_Cnt=0; /*************控制器模式选着******************/ Mode_Check_Cnt++; if(Mode_Check_Cnt>=5)//每
飞控
kernel
08-30 2480
无人机飞控是一个集单片机技术、传感器技术、GPS导航技术、通讯技术、飞行控制技术、任务控制技术、编程技术等多技术并依托于硬件的高科技产物，因此要能设计好一个飞控，缺少上面所述的任何一项技术都是不可能的，越多的飞行经历和经验能为设计初期提供很多避免出现问题的方法，使得试飞进展能够更顺利，要知道飞控的调试主要就是试飞，不比别的自控产品，试飞是高风险的，一旦坠机，硬件损坏，连事故原因都很难分析，就更难解
某无人机飞控系统的原理、组成及各传感器的作用
嵌入式与人工智能
10-18 1万+
以前，搞无人机的十个人有八个是航空、气动、机械出身，更多考虑的是如何让飞机稳定飞起来、飞得更快、飞得更高。如今，随着芯片、人工智能、大数据技术的发展，无人机开始了智能化、终端化、集群化的趋势，大批自动化、机械电子、信息工程、微电子的专业人材投入到了无人机研发大潮中，几年的时间让无人机从远离人们视野的军事应用飞入了寻常百姓家、让门外汉可以短暂的学习也能稳定可靠的飞行娱乐。不可否认，飞控技术的发展是这十年无人机变化的最大推手。 飞控是什么？ 飞行控制系统（Flight control syste..
如何利用无名科创开源飞控实现姿态解算，并通过硬件和算法优化提升其精度？
10-27
总之，结合《无名科创开源飞控V2.0.6详细教程：从零开始到高级应用》中的指导，你可以系统地理解和掌握无名科创开源飞控的姿态解算过程，并通过精心的硬件配置和算法调整，实现飞行器姿态解算的高精度。如果你希望...
如何使用无名科创开源飞控进行姿态解算，并优化其精度？请结合相关硬件和滤波算法给出具体操作。
10-27
通过这些步骤，无名科创开源飞控的用户可以有效地进行姿态解算，并在飞行控制中实现高精度的姿态维持。对于希望深入了解和实践飞控系统的技术人员或爱好者来说，这本使用说明书是掌握开源飞控操作和高级应用的极佳...
如何利用无名科创开源飞控实现四轴飞行器的高精度姿态解算，结合硬件选择和滤波算法？
最新发布
10-27
最终，通过综合考虑硬件的选择和滤波算法的实现，可以显著提升无名科创开源飞控的姿态解算精度，使飞行器具有更好的飞行稳定性和控制性能。如果你希望进一步深入学习无名科创开源飞控的高级应用和更多优化技巧，不妨...
SPL06-001气压计和LIS3DH加速度计的IIC驱动
05-16
SPL06-001气压计和LIS3DH加速度计的IIC驱动。IIC是hal库的IO模拟，亲测可用。不是完整工程，复制粘贴就可用。有详细注释。 SPL06好多人都说精度是0.05m，但数据手册上说的相对精度是0.5米，绝对精度接近10米。 通过卡尔曼滤波后，变化5cm也能看出来，但放着不动时，没有风的情况下，漂移也比较大
无名飞控 stm32代码
01-21
6. **传感器接口**：STM32通常连接陀螺仪、加速度计、磁力计等传感器，实现姿态解算和飞行控制。 7. **电机控制**：PID控制算法用于调节电机转速，以实现精准的飞行姿态控制。 8. **无线通信**：蓝牙、Wi-Fi模块...
无人机飞控基础知识介绍，无人机飞控基本原理及构成
专注多旋翼、复合翼无人机技术研究，从事低空经济行业。
02-05 6262
主要目的是为陆海空三大领域提供实时、全天候和全球性的导航服务，并用于情报搜集、核爆监测和应急通讯等一些军事目的，经过20余年的研究实验，耗资300亿美元，到1994年，全球覆盖率高达98%的24颗GPS卫星星座己布设完成。无人机飞控是指无人机的飞行控制系统，主要用于保持无人机的正常飞行姿态。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。
Flight Control —— 个人博客汇总
慕离巷的博客
02-26 2847
版权声明：本文为博主原创博文，未经允许不得转载，若要转载，请说明出处并给出博文链接 1. 姿态估计/姿态解算 ①互补滤波 ②EKF（扩展卡尔曼滤波） 2. ArduPilot ① ArduCopter a.飞行模式（flight mode）b.气压 & 地面效应 ②ArduCopter——WayPoint ...
可以自己设计飞控（从硬件到软件）
诗筱涵的博客
07-14 2万+
我也是看到一个群里的一个人发的图有感
无名创新飞控开源地面站软件NGroundStation源码发布说明
NamelessCotrunQaud的博客
04-06 1万+
无名创新飞控开源地面站——NGroundStation 一、地面站开源初衷与下载链接： 无名创新飞控开源飞控地面站是无名小哥为广大自研飞控开发者、电子发烧友们专门开发的一款多功能上位机软件，地面站功能涵盖：飞控状态显示、数据波形、参数调试、虚拟遥控器、飞控传感器校准串口助手常用等，开发此款地面站的初衷是方便大家高效学习与调试。 初学者在学习无人机飞控相关算法选...
飞行控制PID算法——无人机飞控
热门推荐
u012254599的博客
02-03 2万+
PID控制应该算是应用非常广泛的控制算法了。小到控制一个元件的温度，大到控制无人机的飞行姿态和飞行速度等等，都可以使用PID控制。这里我们从原理上来理解PID控制。 PID(proportion integration differentiation)其实就是指比例，积分，微分控制。先把图片和公式摆出来，看不懂没关系。 总的来说，当得到系统的输出后，将输出经过比例，积分，微分3种运算方式，叠加到输入中，从而控制系统的行为。 PID控制器是一种线性控制器，它主要根据给定值和实际输出值构成控制偏差，然
飞控简析-从入门到跑路序章
weixin_38693938的博客
11-01 2084
一、序言 茫茫天数此中求，世道兴衰不自由 万万千千说不尽，不如推背去归休 本人搞飞控差不多两年了，从一开始什么都不懂的真·小白，到现在的高级小白，我已经经历了太多太多。因为感觉飞控是一个比较小众的产品，所以国内的资料并不是很完善，有些文章重复太多了，而且每个人看问题的视角是不一样的。我虽然只是个半瓶水，但是也想为国内的飞控圈子做一点贡献。 二、学飞控有没有前途 这个话题有点小，大一点的问...
无名飞控
lipton的博客
12-13 5435
无名飞控姿态数据的获得
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值