第八，九，十天：陀螺仪姿态解算_陀螺仪放歪了怎么计算角度呢

目录

一：使用陀螺仪读取爬坡角度，试图在跷跷板上面平衡

二：使用陀螺仪进行旋转偏移的回正

0.目标：

1.先实现不动摇杆时的自动回正

2.之后实现摇杆控制小车行进过程中踢一脚能够进行自动回正并继续受摇杆控制

---

一：使用陀螺仪读取爬坡角度，试图在跷跷板上面平衡

1.往只有陀螺仪的工程里面移植进入遥控器，CAN通信的对应文件，并添加好对应路径，同时配置好CubeMX。

2.读取INS_angle[1]的值，使得小车可以自行爬坡。

如果是在mian.c里面开的线程，记得先在上面声明一下INS_task里面的INS_angle：

extern fp32 INS_angle[3];      //euler angle, unit rad.欧拉角，单位： rad

然后可以通过判断INS_angle[1]的值是否为0来决定小车是否向前或后退，同时倾斜角度越大，动力应越大：

 if(INS_angle[1]!=0)
 {
        set_angle[0]=INS_angle[1]*500;
        set_angle[1]=INS_angle[1]*500;
        set_angle[2]=INS_angle[1]*500;
        set_angle[3]=INS_angle[1]*500;                
        current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,-set_angle[0]); //1
                              current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,set_angle[1]); //2
                       current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,set_angle[2]); //3
        current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,-set_angle[3]);//4           
                            can_cmd_motor(&hcan1,0x200,current_angle[0],current_angle[1],current_angle[2],current_angle[3]);        
  } 

 值得注意的是，INS_angle[1]的值为负数时对应的转速和电流也会相应为负，所以轮子会自动实现不同方向的转动。

但最后未能实现小车在跷跷板上平衡，小车总是无法及时修正。

PS；当然也可以使用串行PID的方法去控制小车，外环PID是输入角度输出转速，内环PID是输入转速得到电流，最后发送给电机。

二：使用陀螺仪进行旋转偏移的回正

0.目标：

通过陀螺仪实现小车偏航自稳，通过读取陀螺仪数据使小车能当在偏航角（yaw）非0时通过自旋进行yaw轴归中，实现偏航角度闭环控制

1.先实现不动摇杆时的自动回正

最简单的做法是通过判断INS_angle[0]的值是否为0来进行自动回正，和判断INS_angle[1]来试图在跷跷板上面平衡的方法类似：

//When INS_angle[0] is bigger than 0, spin right 
//But in the computer, the variable of INS_angle[0] can't be 0.00 ,because the float can't be 0.00 
if(INS_angle[0]>2)
{
	set_angle[0]=INS_angle[0]*50;
	set_angle[1]=INS_angle[0]*50;
	set_angle[2]=INS_angle[0]*50;
	set_angle[3]=INS_angle[0]*50;				
				        
    current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,set_angle[0]); 
				 
    current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,set_angle[1]); 
				 
    current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,set_angle[2]); 
				 
    current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,set_angle[3]); 			
				 
can_cmd_motor(&hcan1,0x200,current_angle[0],current_angle[1],current_angle[2],current_angle[3]);				
}
else if(INS_angle[0]<-2)//When INS_angle[0] is bigger than 0, spin left 
{
	set_angle[0]=INS_angle[0]*50;
	set_angle[1]=INS_angle[0]*50;
	set_angle[2]=INS_angle[0]*50;
	set_angle[3]=INS_angle[0]*50;				
	current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,-set_angle[0]); 
    current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,-set_angle[1]); 
	current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,-set_angle[2]); 
	current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,-set_angle[3]); 			
				can_cmd_motor(&hcan1,0x200,current_angle[0],current_angle[1],current_angle[2],current_angle[3]);					
}
else
{			
	current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,0); 
	current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,0); 
	current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,0); 
	current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,0); 			
				can_cmd_motor(&hcan1,0x200,current_angle[0],current_angle[1],current_angle[2],current_angle[3]);					
}

由于浮点不可能为0，加上陀螺仪读取到的数值会有一定程度的波动，所以可以设一个死区。

在死区内认为小车已回正，停止转动。否则小车会”摇摆“。

 以上是陀螺仪没有校准时候的参量设置（INS_angle[0]可以破百），所以倍率偏小。

以下则是使用官方例程的含有校准功能的工程进行的参数设置，并且进行了弧度转角度的计算：

//弧度转角度
for(int i=0;i<4;i++)
{
	angle_360[i]=INS_angle[i]/pi*180;
}
//When INS_angle[0] is bigger than 0, spin right 
//But in the computer, the variable of INS_angle[0] can't be 0.00 ,because the float can't be 0.00 
if(angle_360[0]>2)
{
	set_angle[0]=1000;
	set_angle[1]=1000;
	set_angle[2]=1000;
	set_angle[3]=1000;				
				    
    current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,set_angle[0]); 
				 
    current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,set_angle[1]); 
				 
    current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,set_angle[2]); 
				 
    current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,set_angle[3]); 			
				CAN_cmd_chassis(current_angle[0],current_angle[1],current_angle[2],current_angle[3]);				}
else if(angle_360[0]<-2)//When INS_angle[0] is bigger than 0, spin left 
{
	set_angle[0]=-1000;
	set_angle[1]=-1000;
	set_angle[2]=-1000;
	set_angle[3]=-1000;				
				 
     current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,set_angle[0]); 
				 
     current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,set_angle[1]); 
				 
     current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,set_angle[2]); 
				 
     current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,set_angle[3]); 			
				CAN_cmd_chassis(current_angle[0],current_angle[1],current_angle[2],current_angle[3]);					
}
else 
{			
	current_angle[0]=PID_calc(&PID_angle[0],motor_data[0].speed_rpm,0); 
    current_angle[1]=PID_calc(&PID_angle[1],motor_data[1].speed_rpm,0); 
	current_angle[2]=PID_calc(&PID_angle[2],motor_data[2].speed_rpm,0); 
	current_angle[3]=PID_calc(&PID_angle[3],motor_data[3].speed_rpm,0); 			
				CAN_cmd_chassis(current_angle[0],current_angle[1],current_angle[2],current_angle[3]);					
}

---

进阶一点的做法是使用串级PID，也是和上面说的一样，角度控制转速再让转速控制电流。

先定义好需要用到的PID结构和变量：

pid_type_def Pid[4];
pid_type_def PID_rpm;//outside PID
pid_type_def PID_current[4];//inside PID

const fp32 kp_i_d[3]={14,0,0};
const fp32 k_angle[3]={40,10,0};
const fp32 k_angle_rpm[3]={20,0,0};

extern RC_ctrl_t rc_ctrl;  
extern fp32 INS_quat[4] ;
extern fp32 INS_angle[3];    
extern motor_measure_t  motor_data[4]; 

float set_rpm[4];      
float give_current[4];    
float set_angle; 
float give_rpm;  
float current_angle_rpm[4];

并进行相应初始化：

  delay_init();
  
  can_filter_init();                   //can过滤器初始化
  for(int i=0;i<4;i++)
  {
        PID_init(&Pid[i],PID_POSITION,kp_i_d,1200,200);        //pid初始化  
        PID_init(&PID_current[i],PID_POSITION,k_angle_rpm,1500,200);    
  }
  PID_init(&PID_rpm,PID_POSITION,k_angle_rpm,1200,200);    
  remote_control_init();            //遥控器初始化

这里PID设置的不是很好，导致回正的时候转动速度偏慢。（并且设置的ki不为0，以达到在逼近目标值的时候能够通过积分输出更好的去达到预期值）

然后是对应计算过程：

		
set_angle=INS_angle[0]/pi*180;
if(set_angle<-1||set_angle>1)//skip right
{
	give_rpm=PID_calc(&PID_rpm,set_angle,0);
	for(int i=0;i<4;i++)
	{
		current_angle_rpm[i]=PID_calc(&PID_current[i],motor_data[i].speed_rpm,-give_rpm);
	}	
	 
CAN_cmd_chassis(current_angle_rpm[0],current_angle_rpm[1],current_angle_rpm[2],current_angle_rpm[3]);	
}
else//stop
{
	for(int i=0;i<4;i++)
	{
					current_angle_rpm[i]=PID_calc(&PID_current[i],motor_data[i].speed_rpm,0);
	}				
				CAN_cmd_chassis(current_angle_rpm[0],current_angle_rpm[1],current_angle_rpm[2],current_angle_rpm[3]);									
}

这里同样设置了死区，但是死区不要设置太小，因为数值会有一定程度的抖动，即便是0.01范围的抖动，弧度制放大变成角度制抖动角度也不小了。所以死区设置太小可能还是会在逼近平衡位置的时候出现”摇摆“的现象。

2.之后实现摇杆控制小车行进过程中踢一脚能够进行自动回正并继续受摇杆控制

思路是使用串行PID，

外环：输入角度输出角速度，并将角速度给到麦轮分解的公式里面；

内环：输入转速输出电流，并将电流给到电机。

---

最开始想到的是外环PID输入的当前值是INS_angle[0]，预期是控制自旋的摇杆通道rc_ctrl.rc.ch[0]的值，于是有：

vw=PID_calc(&PID_yaw,INS_angle[0],rc_ctrl.rc.ch[0]);

但是由于当前值是一个弧度制，范围在-pi~pi，而rc_ctrl.rc.ch[0]的值范围大概在-660~660,所以k要设在几百甚至上千。

但如果直接把vw当作当作麦轮分解的第三个参数vw(角速度分量)，那就会发现：

不仅不动摇杆的时候能够偏航回正，就连摇杆控制自旋一定的角度后小车也会自动回到最开始的角度，因此上述过程将通道0的值当作了期望，于是会出现小车无法偏离原来角度的结果。

于是想到给摇杆加一个偏移量，时刻记录摇杆通道0所偏移的里程：

定义一个 angle_set且让： angle_set+=rc_ctrl.rc.ch[0];

再vw=PID_calc(&PID_yaw,INS_angle[0],angle_set);

但是这样会导致angle_set的值在推几次摇杆后变的特别大，于是为了从一个不超过pi的值涨到几千甚至几万的值，vw会一直输出一个非常大的值（如果k调得够大的话）并且不能够短时间接近目标值，于是就会发现小车开始疯转许久停不下来。

并且由于vw的值远远超出了另外两个通道（控制平移）的值，所以会导致小车的平移不受控制，无论怎么推摇杆，都会以一个极大的值原地疯转。

---

以下提供另外的思路——

                   一个有缺陷的版本：

先进行必要的变量或结构定义并初始化：

//声明其他文件的变量
extern RC_ctrl_t rc_ctrl;   
extern fp32 INS_angle[3]; 
extern motor_measure_t  motor_data[4]; 

//PID结构
pid_type_def Pid[4];

pid_type_def PID_yaw;

//包含kp,ki,kd的数组
const fp32 kp_i_d[3]={14,0,0};

const fp32 k_yaw[3]={500,0,0};

float set_rpm[4];            //设定的转速 /每分钟
float give_current[4];     //发送给电机的电流
float angle_set;  //控制回正的期望
float vw;//角速度

//初始化

  delay_init();
  //can过滤器初始化
  can_filter_init(); 
   //遥控器初始化
  remote_control_init();    
  //pid初始化    
  for(int i=0;i<4;i++)
  {
        PID_init(&Pid[i],PID_POSITION,kp_i_d,1200,200);        
        PID_init(&PID_current[i],PID_POSITION,k_angle_rpm,1500,200);    
  }
  PID_init(&PID_rpm,PID_POSITION,k_angle_rpm,1200,200);
  PID_init(&PID_yaw,PID_POSITION,k_yaw,1200,100);

 然后是实现部分：

            //通过PID，输入角度，输出角速度
            if(rc_ctrl.rc.ch[0]<-2 || rc_ctrl.rc.ch[0]>2)
            {                
                angle_set=INS_angle[0];
            }
            vw=PID_calc(&PID_yaw,INS_angle[0],angle_set);        
            //将包含角速度的值进行麦轮分解
            set_rpm[0]=(-rc_ctrl.rc.ch[3]+rc_ctrl.rc.ch[2]+rc_ctrl.rc.ch[0])*10;  //1
            set_rpm[1]=(rc_ctrl.rc.ch[3]+rc_ctrl.rc.ch[2]+rc_ctrl.rc.ch[0])*10;  //2
            set_rpm[2]=(rc_ctrl.rc.ch[3]-rc_ctrl.rc.ch[2]+rc_ctrl.rc.ch[0])*10;  //3
            set_rpm[3]=(-rc_ctrl.rc.ch[3]-rc_ctrl.rc.ch[2]+rc_ctrl.rc.ch[0])*10;  //4
            for(int i=0;i<4;i++)
            {
            give_current[i]=PID_calc(&Pid[i],motor_data[i].speed_rpm,set_rpm[i]-50*vw);   //发送数据计算电流    
            }
            CAN_cmd_chassis(give_current[0],give_current[1],give_current[2],give_current[3]);

实现思路是将自动回正的PID和麦轮分解的计算分开，保证要摇杆控制的稳定性。

而这部分代码的缺陷在控制摇杆自旋的时候踢一脚无法实现自动回正后再继续自旋。

当摇杆不控制的时候，angle_set是当前车头的朝向，这个时候能够通过外环PID自动回正，

因为此时set_rpm的值为0，而如果受到外力，vw的值不为0，就会给电流到电机实现对应的PID控制回正，

当摇杆控制的时候，会进入if的判断语句，使得预期角度等于当前角度，此时vw始终为0，这是内环PID的最后一个参数也就是期望完全取决于摇杆的set_rpm，实现摇杆有效控制；

而又由于平移的vx和vy不受yaw的影响，所以可以一边控制左摇杆平移一边实现偏航自稳。

唯独无法实现摇杆控制小车自旋的时候能够偏航自稳。

---

接下来是官方例程的部分实现思路：

void PID_task(void const * argument)
{    
    //底盘初始化
    //底盘数据更新
    //先更新电机速度
    for(int i=0;i<4;i++)
    {
        speed[i]=CHASSIS_MOTOR_RPM_TO_VECTOR_SEN * motor_data[i].speed_rpm;
    }
    //再更新vw
    vw=(-speed[0]-speed[1]-speed[2]-speed[3]) * MOTOR_SPEED_TO_CHASSIS_SPEED_WZ / MOTOR_DISTANCE_TO_CENTER;
    //然后计算底盘姿态角度,这里只处理yaw
    yaw=INS_angle[0];        
    while(1)
    {
        if(rc_ctrl.rc.s[0]==0x0001)
        {
            for(int i=0;i<4;i++)
            {
                give_current[i]=PID_calc(&Pid[i],motor_data[i].speed_rpm,0); //断流，写保护                
            }    
            CAN_cmd_chassis(give_current[0],give_current[1],give_current[2],give_current[3]);    
            osDelay(5);                
        }
        else if(rc_ctrl.rc.s[0]==0x0002)//右开关向下拨动开车
        {
            //底盘数据更新
            //先更新电机速度
            for(int i=0;i<4;i++)
            {
                speed[i]=CHASSIS_MOTOR_RPM_TO_VECTOR_SEN * motor_data[i].speed_rpm;
            }
            //再更新vw
            vw=(-speed[0]-speed[1]-speed[2]-speed[3]) * MOTOR_SPEED_TO_CHASSIS_SPEED_WZ / MOTOR_DISTANCE_TO_CENTER;
            //然后计算底盘姿态角度,这里只处理yaw
            yaw=INS_angle[0];    
            //底盘控制量设置，
            fp32 delat_angle = 0.0f;            
            yaw_set=loop_fp32_constrain(yaw-rc_ctrl.rc.ch[0],-pi,pi);
            delat_angle=loop_fp32_constrain(yaw_set-INS_angle[0],-pi,pi);
            vw=PID_calc(&yaw_angle,0.0f,delat_angle);
            //底盘控制PID计算
            set_rpm[0]=(-rc_ctrl.rc.ch[3]+rc_ctrl.rc.ch[2]+vw)*10;  //1
            set_rpm[1]=(rc_ctrl.rc.ch[3]+rc_ctrl.rc.ch[2]+vw)*10;  //2
            set_rpm[2]=(rc_ctrl.rc.ch[3]-rc_ctrl.rc.ch[2]+vw)*10;  //3
            set_rpm[3]=(-rc_ctrl.rc.ch[3]-rc_ctrl.rc.ch[2]+vw)*10;  //4
            for(int i=0;i<4;i++)
            {
                give_current[i]=PID_calc(&Pid[i],motor_data[0].speed_rpm,set_rpm[i]);
            }
            CAN_cmd_chassis(give_current[0],give_current[1],give_current[2],give_current[3]);
            osDelay(5); //注意一定要给延时，不然会疯
       }
        else if(rc_ctrl.rc.s[0]==0x0003)//the right middle is balance 
        {
            osDelay(5); //注意一定要给延时，不然会疯
        }    
    }
}

详见官方的chassis_task.c文件。