第八，九，十天：陀螺仪姿态解算

基于陀螺仪的小车平衡与回正控制

最新推荐文章于 2024-12-01 03:14:30 发布

原创

最新推荐文章于 2024-12-01 03:14:30 发布 · 1.2k 阅读

8 ·

CC 4.0 BY-SA版权

文章标签：

#单片机 #c语言 #stm32

本文围绕使用陀螺仪控制小车展开，一是尝试让小车在跷跷板上平衡，通过读取爬坡角度控制小车行进，但未成功；二是实现小车旋转偏移回正，先实现不动摇杆时自动回正，后探讨摇杆控制行进中受外力自动回正，给出不同思路及代码实现，还提及官方例程思路。

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

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

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].spee