【ESP32-S3】mpu6050使用场景小结

【ESP32-S3】mpu6050使用场景小结基本原理通用初始化代码所有例子共用场景1小车直行自动纠偏最常用功能完整代码场景2倾倒/翻车检测保护停车功能场景3上坡/下坡检测 自动调速场景4碰撞/撞击检测场景5精确角度转向90°/180°场景6两轮自平衡小车核心场景7手势控制前后左右挥动手势场景8防盗震动报警场景9简易航位推算估算距离场景10摄像头防抖舵机补偿场景11小车大致匀速场景12PID微调技巧下面全部基于ESP32-S3 Arduino MPU6050使用Adafruit_MPU6050库接线统一VCC → 3.3VGND → GNDSDA →GPIO 4SCL →GPIO 5先安装库库管理器搜索Adafruit MPU6050、Adafruit Unified Sensor基本原理修正值 P偏差 I累计偏差 D*偏差变化速度示例代码// 目标角度 0floattarget0;// 当前角度来自MPU6050floaterrortarget-yaw;// P项floatPKp*error;// I项累计偏差integralerror*dt;floatIKi*integral;// D项变化速度floatDKd*(error-lastError)/dt;lastErrorerror;// 最终修正值floatcorrectionPID;通用初始化代码所有例子共用// 引入Adafruit官方MPU6050驱动库用于操作MPU6050传感器#includeAdafruit_MPU6050.h// 引入Adafruit统一传感器接口库提供标准的传感器数据格式#includeAdafruit_Sensor.h// 引入I2C通信库MPU6050通过I2C协议与ESP32通信#includeWire.h// 创建MPU6050传感器对象用于调用传感器的各种功能Adafruit_MPU6050 mpu;// 定义3个传感器事件结构体变量// a: 存储加速度计数据 g: 存储陀螺仪数据 temp: 存储温度数据sensors_event_t a,g,temp;// 定义姿态角变量float是浮点型能存储小数// pitch俯仰角小车抬头/低头的角度floatpitch,roll;// roll横滚角小车向左/向右倾斜的角度// yaw偏航角小车向左/向右转向的角度初始值为0floatyaw0;// 定义变量存储上一次计算的时间用于计算时间差unsignedlongprevTime0;// 初始化MPU6050传感器的函数voidinitMPU(){// 启动I2C通信指定ESP32-S3的I2C引脚SDAGPIO4SCLGPIO5Wire.begin(4,5);// 尝试初始化MPU6050如果返回false代表初始化失败if(!mpu.begin()){// 串口打印失败信息Serial.println(MPU6050 连接失败);// 无限循环卡住程序等待检修while(1)delay(10);}// 设置加速度计的测量范围±2G日常姿态测量足够用mpu.setAccelerometerRange(MPU6050_RANGE_2_G);// 设置陀螺仪的测量范围±250度/秒适合低速转向测量mpu.setGyroRange(MPU6050_RANGE_250_DEG);// 设置传感器滤波带宽为21Hz过滤高频噪声让数据更平稳mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);// 记录当前系统时间毫秒作为初始时间prevTimemillis();// 串口打印初始化成功提示Serial.println(MPU6050 初始化成功);// 延时100毫秒等待传感器稳定delay(100);}// 更新姿态角函数读取传感器数据并计算pitch/roll/yawvoidupdateAngle(){// 从MPU6050读取最新的加速度、陀螺仪、温度数据存入a、g、temp变量mpu.getEvent(a,g,temp);// 获取当前系统时间毫秒unsignedlongnowmillis();// 计算时间差dt单位 秒毫秒转秒 除以1000floatdt(now-prevTime)/1000.0;// 更新上一次时间为当前时间为下一次计算做准备prevTimenow;// 计算俯仰角pitch通过加速度计Y轴和Z轴数据计算// atan2是反正切函数RAD_TO_DEG将弧度转换为角度pitchatan2(a.acceleration.y,a.acceleration.z)*RAD_TO_DEG;// 计算横滚角roll通过加速度计X轴和Z轴数据计算rollatan2(-a.acceleration.x,a.acceleration.z)*RAD_TO_DEG;// 计算偏航角yaw陀螺仪Z轴角速度 × 时间差积分运算// 累加结果得到小车的转向角度yawg.gyro.z*dt;}场景1小车直行自动纠偏最常用功能前进时根据 yaw 角自动微调左右轮 PWM保持直线完整代码#includeAdafruit_MPU6050.h#includeAdafruit_Sensor.h#includeWire.hAdafruit_MPU6050 mpu;sensors_event_t a,g,temp;floatyaw0,lastYaw0;unsignedlongprevTime0;// 电机引脚#defineIN141#defineIN242#defineENA40#defineIN321#defineIN413#defineENB3// 速度与PIDconstintbaseSpeed160;floatKp2.5;// 纠偏系数intleftPWM,rightPWM;voidinitMPU(){Wire.begin(4,5);mpu.begin();mpu.setAccelerometerRange(MPU6050_RANGE_2_G);mpu.setGyroRange(MPU6050_RANGE_250_DEG);prevTimemillis();}voidupdateYaw(){mpu.getEvent(a,g,temp);floatdt(millis()-prevTime)/1000.0;prevTimemillis();yawg.gyro.z*dt;}voidmotorForwardCorrect(){updateYaw();floaterroryaw-lastYaw;intcorrectionerror*Kp;leftPWMbaseSpeed-correction;rightPWMbaseSpeedcorrection;leftPWMconstrain(leftPWM,80,255);rightPWMconstrain(rightPWM,80,255);digitalWrite(IN1,LOW);digitalWrite(IN2,HIGH);analogWrite(ENA,rightPWM);digitalWrite(IN3,HIGH);digitalWrite(IN4,LOW);analogWrite(ENB,leftPWM);}voidmotorStop(){analogWrite(ENA,0);analogWrite(ENB,0);yaw0;lastYaw0;}voidsetup(){Serial.begin(115200);initMPU();pinMode(IN1,OUTPUT);pinMode(IN2,OUTPUT);pinMode(ENA,OUTPUT);pinMode(IN3,OUTPUT);pinMode(IN4,OUTPUT);pinMode(ENB,OUTPUT);motorStop();}voidloop(){motorForwardCorrect();Serial.print(Yaw: );Serial.println(yaw);delay(20);}场景2倾倒/翻车检测保护停车功能pitch 或 roll 超过 45° 判定翻车立即停车voidloop(){updateAngle();if(abs(pitch)45||abs(roll)45){motorStop();Serial.println(翻车已停车);}else{motorForwardCorrect();}delay(20);}场景3上坡/下坡检测 自动调速voidloop(){updateAngle();intspeedbaseSpeed;// 上坡 pitch 增大 → 加力if(pitch10)speed40;// 下坡 pitch 减小 → 减速if(pitch-10)speed-40;speedconstrain(speed,100,255);Serial.print(坡度: );Serial.print(pitch);Serial.print( 速度: );Serial.println(speed);delay(50);}场景4碰撞/撞击检测floatlastAx0;voidcheckCollision(){mpu.getEvent(a,g,temp);floatdeltaabs(a.acceleration.x-lastAx);lastAxa.acceleration.x;if(delta12){// 撞击阈值Serial.println(检测到撞击);motorStop();delay(500);}}voidloop(){checkCollision();delay(20);}场景5精确角度转向90°/180°voidturnAngle(floattarget){yaw0;while(abs(yaw-target)2){updateYaw();// 左转digitalWrite(IN1,HIGH);digitalWrite(IN2,LOW);analogWrite(ENA,130);digitalWrite(IN3,HIGH);digitalWrite(IN4,LOW);analogWrite(ENB,130);}motorStop();}// 使用// turnAngle(90); // 右转90// turnAngle(-90); // 左转90场景6两轮自平衡小车核心floatbalanceAngle0;floatKp_bal20,Kd_bal10;voidbalance(){updateAngle();floaterrorpitch-balanceAngle;floatspeederror*Kp_balg.gyro.y*Kd_bal;intpwmconstrain(speed,-255,255);if(pwm0){digitalWrite(IN1,LOW);digitalWrite(IN2,HIGH);digitalWrite(IN3,HIGH);digitalWrite(IN4,LOW);}else{digitalWrite(IN1,HIGH);digitalWrite(IN2,LOW);digitalWrite(IN3,LOW);digitalWrite(IN4,HIGH);}analogWrite(ENA,abs(pwm));analogWrite(ENB,abs(pwm));}voidloop(){balance();delay(10);}场景7手势控制前后左右挥动手势voidgestureControl(){updateAngle();if(roll20){Serial.println(手势 → 右转);// motorRight();}elseif(roll-20){Serial.println(手势 → 左转);// motorLeft();}elseif(pitch20){Serial.println(手势 → 后退);}elseif(pitch-20){Serial.println(手势 → 前进);}}voidloop(){gestureControl();delay(100);}场景8防盗震动报警voidalarm(){mpu.getEvent(a,g,temp);if(abs(a.acceleration.x)1.5||abs(a.acceleration.y)1.5){Serial.println(震动报警);// 可接蜂鸣器或WiFi发消息}}voidloop(){alarm();delay(50);}场景9简易航位推算估算距离floatspeed0,distance0;voidcalcDistance(){mpu.getEvent(a,g,temp);floataxa.acceleration.x;speedax*0.02;distancespeed*0.02;Serial.print(距离(m): );Serial.println(distance);}voidloop(){calcDistance();delay(20);}场景10摄像头防抖舵机补偿#includeServo.hServo camServo;intservoPin38;voidsetup(){camServo.attach(servoPin);}voidstabilizer(){updateAngle();intangle90-roll;angleconstrain(angle,45,135);camServo.write(angle);}voidloop(){stabilizer();delay(20);}场景11小车大致匀速微调targetSpeed 8 目标速度大小Kp 15 调节强度basePWM 120 基础 PWM如果车晃得厉害把 currentSpeed * 0.98 改成 0.95 更强抑制漂移#includeAdafruit_MPU6050.h#includeAdafruit_Sensor.h#includeWire.hAdafruit_MPU6050 mpu;sensors_event_t a,g,temp;// 速度估算floatcurrentSpeed0;// 当前估算速度floattargetSpeed8;// 目标速度可改unsignedlonglastTime0;// 电机引脚和你之前一致#defineIN141#defineIN242#defineENA40#defineIN321#defineIN413#defineENB3// PWM 基础值intbasePWM120;floatKp15;// 调节强度voidinitMPU(){Wire.begin(4,5);if(!mpu.begin()){Serial.println(MPU6050 错误);while(1)delay(10);}mpu.setAccelerometerRange(MPU6050_RANGE_2_G);mpu.setGyroRange(MPU6050_RANGE_250_DEG);mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);lastTimemillis();Serial.println(MPU6050 就绪);delay(100);}// 估算速度加速度积分voidcalcSpeed(){mpu.getEvent(a,g,temp);unsignedlongnowmillis();floatdt(now-lastTime)/1000.0f;lastTimenow;// 前进方向加速度根据安装方向可能要改符号floataxa.acceleration.x;// 积分得到速度currentSpeedax*dt;// 简单漂移抑制静止时慢慢归0currentSpeed*0.98;}// 匀速前进voidforwardWithConstSpeed(){calcSpeed();// 速度偏差floaterrortargetSpeed-currentSpeed;// 调节 PWMintpwmbasePWMerror*Kp;pwmconstrain(pwm,80,255);// 电机前进digitalWrite(IN1,LOW);digitalWrite(IN2,HIGH);analogWrite(ENA,pwm);digitalWrite(IN3,HIGH);digitalWrite(IN4,LOW);analogWrite(ENB,pwm);}voidmotorStop(){analogWrite(ENA,0);analogWrite(ENB,0);currentSpeed0;}voidsetup(){Serial.begin(115200);initMPU();pinMode(IN1,OUTPUT);pinMode(IN2,OUTPUT);pinMode(ENA,OUTPUT);pinMode(IN3,OUTPUT);pinMode(IN4,OUTPUT);pinMode(ENB,OUTPUT);motorStop();}voidloop(){forwardWithConstSpeed();Serial.print(目标速度: );Serial.print(targetSpeed);Serial.print( | 估算速度: );Serial.println(currentSpeed);delay(20);}场景12PID微调技巧第一步只调 Kp最重要目标让小车能回正但不要抖操作Ki 0Kd 0只保留 Kp按住前进看小车怎么走情况 A几乎不纠偏还是跑偏→ Kp 太小每次 0.3 ~ 0.5例2.8 → 3.2 → 3.6 → 4.0情况 B小车左右摆、S 型走、晃来晃去→ Kp 太大每次 -0.2 ~ 0.4例2.8 → 2.4 → 2.0 → 1.6合格标准小车偏了能慢慢拉回来不剧烈抖动到这一步 Kp 就调好了一般在 1.8 ~ 4.0 之间第二步加 Ki消除静差Ki 作用解决“永远差一点回不正”的问题操作Kp 已经调好Ki 从 0 开始每次 0.050 → 0.05 → 0.10 → 0.15情况 A小车能走直但总微微偏一点→ 加大 Ki情况 B小车开始晃、来回摆→ Ki 太大立即减小合格标准小车能完全走直不慢慢偏走Ki 一般不要超过 0.2否则极易震荡第三步加 Kd减震、防晃Kd 作用抑制晃动让车更稳操作Kp、Ki 调好Kd 从 0 开始每次 0.1 ~ 0.2情况 A小车还有点轻微晃→ 加大 Kd情况 B小车变得很迟钝、反应慢→ Kd 太大减小4 种典型现象1小车走直线但慢慢偏→ Kp 偏小 或 Ki 偏小2小车左右晃、S 形→ Kp 太大 或 Ki 太大3小车偏了不回正→ Kp 太小4小车回正太猛直接冲过头→ Kd 太小90%使用的参数Kp 2.2Ki 0.08Kd 0.4小车偏重Kp 3.0Ki 0.12Kd 0.5