前言：主包在网上查阅了很多资料，发现几乎没有系统关于姿态传感器（陀螺仪）的学习资料，那就自己做一期吧！本人是基于逐飞的开源库学习的，这种封装好的库很多东西可以直接拿来使用，芯片是AI8051u，姿态传感器型号是imu660ra。
姿态传感器浅析
IMU660RA是一款九轴惯性测量单元，里面集成了陀螺仪和加速度计，主要用于姿态估计、角速度检测等。
而我们主要使用Z 轴角速度，也就是绕垂直方向的旋转速度，来帮助判断小车当前转弯速度，从而调整左右轮的速度，实现更准确的转向控制。Z轴角速度表示小车绕垂直轴（如图放置，就是几何中的z轴）的转动快慢，正值表示逆时针，负值表示顺时针。
零偏（零漂）
本人编写运行了以下的基础读取代码
#include "zf_common_headfile.h"
#define PIT (TIM0_PIT )
void pit_hanlder (void);
void main(){
clock_init(SYSTEM_CLOCK_40M);
debug_init();
if(imu660ra_init()){ //姿态传感器初始化
while(1){ //失败进入死循环
}
}
tim0_irq_handler = pit_hanlder; //周期中断回调函数
pit_ms_init(PIT, 100); //初始化中断为100ms
while(1)
{
}
}
void pit_hanlder (void)
{
imu660ra_get_acc(); //获取x、y、z方向加速度数值
imu660ra_get_gyro(); //获取x、y、z方向角速度数值
printf("
IMU660RA acc data: x=%5d, y=%5d, z=%5d
", imu660ra_acc_x, imu660ra_acc_y, imu660ra_acc_z); //逐飞库函数已自动更新这些值
printf("
IMU660RA gyro data: x=%5d, y=%5d, z=%5d
", imu660ra_gyro_x, imu660ra_gyro_y, imu660ra_gyro_z);
}读取到的数值如下（主要看gyro data角速度里的z轴角速度）。奇怪了，我的车子就放在桌面上没有动，怎么读取到的数值还自己震动呢？难道它连地壳震动都能检测到？
[13:59:06.549]接收←
IMU660RA acc data: x= 102, y= 63, z= 4054
IMU660RA gyro data: x= 0, y= 0, z= -1
[13:59:06.644]接收←
IMU660RA acc data: x= 108, y= 58, z= 4058
IMU660RA gyro data: x= -1, y= -2, z= 1
[13:59:06.756]接收←
IMU660RA acc data: x= 107, y= 63, z= 4065
IMU660RA gyro data: x= -1, y= 2, z= 0
[13:59:06.851]接收←
IMU660RA acc data: x= 103, y= 61, z= 4059
IMU660RA gyro data: x= 1, y= 0, z= -2
[13:59:06.946]接收←
IMU660RA acc data: x= 103, y= 61, z= 4061
IMU660RA gyro data: x= 2, y= -1, z= 0
[13:59:07.057]接收←
IMU660RA acc data: x= 105, y= 57, z= 4053
IMU660RA gyro data: x= -1, y= 2, z= -2
[13:59:07.153]接收←
IMU660RA acc data: x= 102, y= 64, z= 4058
IMU660RA gyro data: x= 1, y= -1, z= -2
[13:59:07.248]接收←
IMU660RA acc data: x= 100, y= 58, z= 4061
IMU660RA gyro data: x= -1, y= 1, z= 0这是因为IMU 是一种精密传感器，它的芯片和电路会受以下因素影响：电源不稳定（比如电压波动），温度变化（芯片加热也会漂），制造误差（比如 ADC 偏移），板子轻微震动或噪声，PCB 上的电磁干扰
一般都是 ±1~3 的范围，属于非常小的零漂，非常正常！
单位转换
然后我们看逐飞的源代码
前面两个函数都只是从 IMU 的寄存器里直接取出值，并没有除以转换系数，所以单位是原始 ADC 值。 再看后面的注释，所以我们要去查看IMU660RA_GYRO_SAMPLE_DEFAULT宏的值，从而确定逐飞库寄存器初始化时到底设置了哪个量程，最后进行单位转化
去.h文件中查看，所以寄存器值为0x00，除以 16.4 得到单位 °/s
编写角度环
由上文得，姿态传感器存在零偏
我们可以做一次静态校准，初始化后采样200次，计算z轴的平均值，作为基准零点。
float gyro_z_offset = 0;
void imu660ra_calibrate()
{
int sum = 0;
for (int i = 0; i < 200; i++)
{
imu660ra_get_gyro();
sum += imu660ra_gyro_z;
system_delay_ms(5); //系统延时5ms
}
gyro_z_offset = sum / 200.0f;
}
然后每次角度值读取后减掉偏差
float imu660ra_get_gyro_z_dps()
{
imu660ra_get_gyro();
return (imu660ra_gyro_z - gyro_z_offset) / 16.4f;
}（有没有人想问这个16.4f是什么呀？16.4后面的f是用来明确表示这是一个单精度浮点数（float类型）的后缀，不加f在C语言中，编译器会默认把它当作双精度浮点数来处理。ai8051是32位单片机，在32位MCU上，double(64位)需要两条指令存储，而float在32位系统上是原子操作，所以我们使用float性能更高且保证安全）
角速度积分成角度，我们可以设置定时器中断每隔 10ms 调用 update_yaw()，yaw代表从开始到现在累计转了多少度。
float yaw = 0; // 当前角度
float dt = 0.01f; // 采样间隔10ms
void update_yaw()
{
float gz = imu660ra_get_gyro_z_dps(); // 角速度 °/s
yaw += gz * dt; //角度增量=角速度*时间间隔
}
简单角度环控制结构
float target_angle = 90; // 目标转角
float kp = 1.0f; // 比例系数，自己调
float turn_pwm = 0;
void update_turn_control()
{
float error = target_angle - yaw;
turn_pwm = kp * error;
// 限幅
if (turn_pwm > 100) turn_pwm = 100;
if (turn_pwm < -100) turn_pwm = -100;
set_motor_turn(turn_pwm);
}编写角速度环
角度环会产生 PWM 控制量，但容易震荡或不平滑，特别是目标角度快达到时容易来回抖动。所以还要加上角速度环（速度控制）用来限制最大旋转速度，更平稳。
控制流程：角度环输出期望角速度；
角速度环再将目标角速度与实际角速度比较，输出最终 PWM。
float yaw_angle = 0;
float target_angle = 90.0f;
float target_angular_speed = 0;
float actual_angular_speed = imu_gyro_z;
float pwm_output = 0;
// 参数（你需要调试）
float kp_angle = 2.0f;
float kp_speed = 1.0f;
void turn_control_loop() {
// 位置环：角度差 -> 期望角速度
float angle_error = target_angle - yaw_angle;
target_angular_speed = kp_angle * angle_error;
// 限制最大角速度
if (target_angular_speed > 150) target_angular_speed = 150;
if (target_angular_speed < -150) target_angular_speed = -150;
// 速度环：角速度差 -> 最终 PWM
float speed_error = target_angular_speed - actual_angular_speed;
pwm_output = kp_speed * speed_error;
// 限幅
if (pwm_output > 100) pwm_output = 100;
if (pwm_output < -100) pwm_output = -100;
motor_set(pwm_output, -pwm_output); // 左右轮反转
}
这样就实现了一个简单的角度控制模块啦，好像也还行，就是调参是根据角度的，更抽象一点。
主包实操
如你们所知，主包是电机坏了，所以最近才出这些文章。那本人的实操也没法跑小车，主包写了一个简单的代码，每个100ms输出当前角度（从开始到现在旋转的角度）
#include "zf_common_headfile.h"
#define PIT (TIM0_PIT )
void pit_hanlder (void);
float gyro_z_offset = 0;
float yaw = 0;
float dt = 0.1f;
void imu660ra_calibrate()
{
int sum = 0,i;
for (i = 0; i < 200; i++)
{
imu660ra_get_gyro();
sum += imu660ra_gyro_z;
system_delay_ms(5);
}
gyro_z_offset = sum / 200.0f;
}
float imu660ra_get_gyro_z_dps()
{
imu660ra_get_gyro();
return (imu660ra_gyro_z - gyro_z_offset) / 16.4f;
}
void update_yaw()
{
float gz = imu660ra_get_gyro_z_dps();
yaw += gz * dt;
}
void main(){
clock_init(SYSTEM_CLOCK_40M);
debug_init();
if(imu660ra_init()){
while(1)
{
}
}
imu660ra_calibrate();
tim0_irq_handler = pit_hanlder;
pit_ms_init(PIT, 100);
while(1)
{
}
}
void pit_hanlder (void)
{
update_yaw();
printf("%.2f
",yaw);
}然后将小车慢慢旋转了180°（人为感觉，可能有偏差哈），输出如下
[16:11:48.521]接收←imu660ra_read_register = 0xFF
imu660ra_read_register = 0x24
[16:11:49.689]接收←-0.18
[16:11:49.784]接收←-0.23
[16:11:49.879]接收←-0.25
[16:11:49.991]接收←-0.12
[16:11:50.087]接收←1.66
[16:11:50.183]接收←8.44
[16:11:50.278]接收←20.15
[16:11:50.390]接收←31.63
…… //太多了，省略了一些
[16:11:51.885]接收←176.66
[16:11:51.980]接收←176.24
[16:11:52.075]接收←176.57
[16:11:52.187]接收←175.74结果还算是不错的吧——旋转半周，角度输出大概为175。好了，大功告小成~
暂无评论内容