2026/9/1 19:03:27
FEATURED · 精选文章 
在STM32小车控制项目中利用STM32F103C8T6配合MPU6050和五路灰度传感器实现角度环与位置环控制是复刻电赛24年H题比较典型的一套方案。很多初学者一开始会把灰度传感器的输出直接换算成左右电机PWM这种做法在低速直道勉强能跑但一到弯道或者场地光线变化就很容出现忽左忽右、冲线甚至原地打转。问题不在于传感器不够用而在于控制结构太简单没有把“车相对线的位置偏差”和“车实际姿态角”分开处理。这篇文章会围绕一套可复现的最小工程展开目标是让你搞清楚五路灰度传感器、MPU6050、角度环和位置环是如何配合的。我会从硬件资源分析开始再讲MPU6050的姿态采集、五路灰度位置计算、串级PID实现最后给出联调验证和常见问题排查。文章里的代码用于说明思路实际项目要根据自己的引脚、电机驱动和库版本调整。1. 先理解这套方案硬件分工和双闭环控制结构1.1 STM32F103C8T6在项目里的资源边界STM32F103C8T6是一颗Cortex-M3内核的芯片主频72MHz内置64KB Flash和20KB RAM。在循迹小车这类项目中它的资源并不算紧张但也不是无限可用。一个典型的控制工程通常包含以下负载MPU6050通过I2C读取原始加速度和角速度数据每秒大约需要读取几百字节。五路灰度传感器如果是模拟量每一路都需要ADC采样五路共用一个ADC外设的不同通道。两个电机的PWM输出、方向控制以及串口调试输出。控制中断里需要执行姿态融合、位置计算和串级PID计算量主要取决于是否使用DMP或四元数库。实际项目中20KB RAM足以运行标准外设库或HAL库生成的基础工程。如果使用DMP库要注意库本身占用一定RAM并且要把MPU6050的I2C速率和堆栈空间预留出来。下表是常见资源需求参考资源类型需求说明Flash约8KB到16KBHAL库比标准库占用更多但通常仍小于64KBRAM约2KB到6KB缓冲区、滤波数组、PID结构体、堆栈定时器至少1个用于控制周期中断常用TIM2或TIM3I2C1个连接MPU6050ADC1个五路灰度共用采用扫描或多通道轮流采样串口1个调试和参数监控这里的判断是用STM32F103C8T6做这套控制是够用的但需要管理好中断频率和库的开销不要在Main函数里做大量阻塞式延时也不要频繁调用HAL_I2C_Mem_Read这类函数而不控制调用周期。1.2 MPU6050负责角度环五路灰度负责位置环先说两个环的分工。位置环的反馈来源是五路灰度传感器。五路灰度横向排列在车底当黑线在车体正中间时中间一路灰度值最接近黑色当车偏左或偏右时靠近中间的传感器分布会不一致。通过给五路传感器设定不同权重可以计算出一个数值范围比如-100到100表示车体相对黑线横向偏移了多少。这个值就是位置误差。角度环的反馈来源是MPU6050。如果做的是自平衡车角度环通常用加速度计和陀螺仪融合出来的Pitch角或Roll角让车体保持直立。如果做的是差速循迹小车角度环更常指转向角度的闭环用MPU6050的Z轴角速度积分出偏航角Yaw或者直接用角速度做阻尼。两种理解在工程上都是合理的差别在于把哪个物理量当作被控量。这里采用更通用的差速循迹小车结构位置环根据灰度偏移输出一个“目标转向角”角度环根据MPU6050测量出的当前实际转向角把小车拉向目标角。这样车在入弯时不会因为灰度突然变化就打满方向出弯时也不会因为已经回到线上还继续转向。1.3 控制流位置环输出目标角度角度环输出差速PWM整个控制流程可以压缩成如下几条读取五路灰度计算位置误差。位置环PID把位置误差换算成“目标转向角”。读取MPU6050融合出当前实际角度。角度环PID把目标角度和实际角度的偏差换算成左右电机差速控制量。设置两路PWM输出让小车转动到目标角度。用伪代码表示就是while (1) { position_error GetPositionFromGray(); target_yaw PositionLoop(position_error); current_yaw GetYawFromMPU6050(); speed_diff AngleLoop(target_yaw - current_yaw); SetLeftRightPWM(base_speed speed_diff, base_speed - speed_diff); }这套结构的核心价值在于位置环只负责“该朝哪个方向转”角度环只负责“怎么转得又快又稳”。两个环不会相互抢输出调试时也可以先调内环再调外环。2. 环境准备和工程骨架2.1 工具链选择Keil MDK、库和烧录工具开发STM32F103C8T6常用Keil MDK也可以用STM32CubeIDE或VSCode加编译链。无论选哪种关键是你需要有一个可以编译、下载、调试的基础工程。工程可以基于标准外设库也可以基于HAL库。标准外设库代码直观寄存器操作容易理解适合学习HAL库配合STM32CubeMX配置外设更快切换引脚和时钟方便。这篇文章的代码示例偏HAL风格但控制逻辑部分完全可以移植到标准库。烧录工具方面常见的是ST-Link V2。除了在Keil里直接下载也可以使用STM32 ST-Link Utility查看Flash内容、确认固件是否下载成功或者做整片擦除。建议在联调前先准备一个能稳定烧录的环境避免把软件问题和烧录问题混在一起。2.2 最小工程需要的启动文件、时钟和调试接口一个最小的STM32F103C8T6工程至少包含启动文件例如startup_stm32f103xb.s。系统时钟配置文件例如system_stm32f10x.c或者CubeMX生成的SystemClock_Config。GPIO、I2C、ADC、TIM、USART等外设初始化。主循环和控制定时器中断。时钟配置要特别注意。F103最高主频72MHz外部晶振通常是8MHz通过PLL倍频到72MHz。如果时钟配置错误串口波特率、定时器周期、I2C时序都会跟着错而且排查起来很难发现。调试接口建议先保留串口输出。串口在前期的作用比任何示波器都好用可以打印MPU6050原始数据、灰度ADC值、位置误差、PID控制量。后续调参数时串口接一个蓝牙模块或者无线透传模块还能在跑车过程中实时查看数据。2.3 外设资源分配参考下面是一套不冲突的引脚分配示例可以直接在CubeMX里照着配置功能引脚外设说明MPU6050 SCLPB6I2C1_SCL需要外接4.7k上拉电阻MPU6050 SDAPB7I2C1_SDA需要外接4.7k上拉电阻灰度传感器五路PA0到PA4ADC1_IN0到IN4模拟量输出或数字量输入左电机PWMPA8TIM1_CH1占空比可调右电机PWMPA11TIM1_CH4占空比可调电机方向引脚PB0到PB3GPIO输出控制正反转和刹车串口TXPA9USART1_TX调试输出串口RXPA10USART1_RX可选用于接收指令这里把PWM放在TIM1的CH1和CH4避免和USART1的PA9、PA10冲突。如果你手头的板子引脚被固定了至少先列出表格确认每个外设不会占用同一个引脚。2.4 控制周期设计为什么不用延时循环小车控制最忌讳在主循环里用HAL_Delay做延时因为主循环的执行时间不稳定灰度采样、姿态解算、PID计算都会受到影响。推荐用定时器中断产生固定控制周期。常见做法是设置一个1ms或2ms的定时器中断然后在中断里递增标志位。比如每2ms执行一次MPU6050读取和角度环控制每10ms执行一次灰度采样和位置环控制。这样角度环比较快能保证转向稳定位置环不需要太快因为灰度传感器在高速运动时变化频率并没有高到需要1ms采样一次。定时器预分频和重装载值需要按你的定时器时钟计算。72MHz主频下如果想让TIM2产生1ms中断常见配置是预分频72减1重装载1000减1。具体值取决于你的APB1时钟树建议用CubeMX直接生成避免手工算错。3. MPU6050姿态采集从读取寄存器到角度估计3.1 电路连接和上拉电阻MPU6050通过I2C连接STM32SCL和SDA都需要上拉电阻。很多MPU6050模块上已经自带了10k或4.7k上拉电阻但如果你用的是裸芯片或者模块上省略了上拉通信会出现偶发失败。连接时还要注意电平匹配。STM32F103C8T6的I2C引脚耐压是5V但更稳妥的做法是让模块和你开发板使用相同的3.3V供电避免电平不一致导致读写异常。如果模块支持5V供电但板上没有电平转换I2C引脚可能被拉到5V这种电路设计虽然常见但在调试时容易损坏引脚不建议长期使用。3.2 寄存器初始化顺序MPU6050上电后默认处于睡眠模式必须先唤醒再配置采样率、量程和数字低通滤波器。下面是典型的寄存器配置顺序#define MPU6050_ADDR 0xD0 // 7位地址0x68左移一位 void MPU6050_WriteReg(uint8_t reg, uint8_t data) { HAL_I2C_Mem_Write(hi2c1, MPU6050_ADDR, reg, I2C_MEMADD_SIZE_8BIT, data, 1, 100); } void MPU6050_Init(void) { MPU6050_WriteReg(0x6B, 0x00); // 电源管理唤醒选择时钟源 MPU6050_WriteReg(0x19, 0x07); // 采样率分频控制采样频率 MPU6050_WriteReg(0x1A, 0x06); // 数字低通滤波 MPU6050_WriteReg(0x1B, 0x08); // 陀螺仪量程 ±500°/s MPU6050_WriteReg(0x1C, 0x00); // 加速度量程 ±2g MPU6050_WriteReg(0x38, 0x00); // 关闭中断 }这段代码的关键点如下0x19寄存器会影响最终采样率。如果开启数字低通滤波陀螺仪输出频率会降到1kHz实际采样率等于1kHz除以1 分频值。这里写0x07得到的采样率大约是125Hz对应8ms左右一次数据更新对角度环来说偏慢。如果要更快的采样率可以把分频值调小或者关闭DLPF直接用内部8kHz输出。陀螺仪量程和加速度量程会影响后续单位换算的灵敏度系数。配置完成后最好先验证能读到WHO_AM_I寄存器。WHO_AM_I地址是0x75读出值通常是0x68。如果读不到后面的所有姿态计算都没有意义。3.3 原始数据读取和单位换算MPU6050的加速度计数据从0x3B开始陀螺仪数据从0x43开始连续14个字节可以一次读取。读取代码如下void MPU6050_ReadRaw(int16_t *ax, int16_t *ay, int16_t *az, int16_t *gx, int16_t *gy, int16_t *gz) { uint8_t buf[14]; HAL_I2C_Mem_Read(hi2c1, MPU6050_ADDR, 0x3B, I2C_MEMADD_SIZE_8BIT, buf, 14, 100); *ax (int16_t)((buf[0] 8) | buf[1]); *ay (int16_t)((buf[2] 8) | buf[3]); *az (int16_t)((buf[4] 8) | buf[5]); *gx (int16_t)((buf[8] 8) | buf[9]); *gy (int16_t)((buf[10] 8) | buf[11]); *gz (int16_t)((buf[12] 8) | buf[13]); }读取到的是16位原始值需要根据量程换算成物理单位。加速度量程±2g时灵敏度为16384 LSB/g陀螺仪量程±500°/s时灵敏度为65.5 LSB/(°/s)。换算代码float ax_g (float)*ax / 16384.0f; float ay_g (float)*ay / 16384.0f; float az_g (float)*az / 16384.0f; float gx_dps (float)*gx / 65.5f; // 单位度/秒 float gy_dps (float)*gy / 65.5f; float gz_dps (float)*gz / 65.5f;这里的LSB值必须和你在0x1B、0x1C寄存器里配置的量程匹配否则后面的角度会整体偏大或偏小。3.4 互补滤波输出Pitch和Roll加速度计在静止时能直接算出重力方向但动态响应慢而且车体振动时会引入噪声。陀螺仪短时间积分角度很准但会随时间漂移。互补滤波把两者结合起来长时间信用加速度计短时间信用陀螺仪。一阶互补滤波的常见实现如下float pitch 0.0f; float roll 0.0f; void UpdateAngle(float dt) { float acc_pitch atan2f(ay_g, sqrtf(ax_g * ax_g az_g * az_g)) * 57.29578f; float acc_roll atan2f(ax_g, sqrtf(ay_g * ay_g az_g * az_g)) * 57.29578f; pitch 0.98f * (pitch gx_dps * dt) 0.02f * acc_pitch; roll 0.98f * (roll gy_dps * dt) 0.02f * acc_roll; }这段代码是思路演示实际使用时要根据MPU6050安装方向确认X、Y轴与角度正负号的关系。尤其是车模安装角度不同加速度计和陀螺仪的符号可能需要对调或取反。系数0.98和0.02是经验值。系数越偏向陀螺仪角度越平滑但动态跟随越慢越偏向加速度计角度响应越快但振动噪声越明显。一般可以从0.95和0.05开始调。3.5 转向角度估计Yaw积分及其漂移处理在差速循迹小车里角度环的反馈需要是车体转过的角度。最简单的方法是用Z轴角速度积分static float yaw 0.0f; void UpdateYaw(float dt) { yaw gz_dps * dt; }这个写法的问题在于MPU6050的陀螺仪零点不可能绝对为0。静止时gz_dps可能持续输出0.1°/s的零偏积分几十秒后就会产生明显角度漂移。如果位置环输出的是目标转向角角度环又把这个漂移角度当作真实角度车就会在直道上慢慢跑偏。缓解漂移的常用办法有三个上电静止时采集几十组gz数据的平均值作为零偏后续读取时减去这个零偏。不长时间依赖Yaw位置环的灰度反馈作为主要修正Yaw只用来提供阻尼。如果赛道结构固定每次经过起跑线或特定标记时对Yaw清零。如果要做更稳定的偏航角需要增加磁力计或者使用DMP库输出四元数再配合磁力计做融合。但这对硬件要求更高很多竞赛场景下用零偏校准加定期清零就够了。3.6 MPU6050常见坑MPU6050的坑比灰度传感器多常见问题集中在接线、量程和姿态更新周期上。问题现象常见原因检查方式处理建议I2C扫描不到设备SCL/SDA上拉缺失或模块地址不对用I2C扫描代码确认地址接4.7k上拉确认地址是0x68还是0x69角度静止时缓慢漂移陀螺仪零偏未校准串口打印静止时gz_dps启动时采集零偏并补偿角度计算数值明显偏大灵敏度系数与量程不匹配检查0x1B和0x1C配置按实际量程改LSB系数姿态更新不平滑控制周期不固定或滤波系数不合适打印时间戳或使用定时器中断固定dt用定时器中断跑一段时间后Yaw严重偏移陀螺仪积分漂移直道观察灰度误差使用零偏校准定期清零4. 五路灰度传感器位置误差计算4.1 数字输出与模拟输出的差异五路灰度传感器模块有两种常见输出形式数字量输出模块上自带比较器检测到黑线时输出高电平或低电平。优点是直接接GPIO简单可靠缺点是阈值固定在电位器上现场调节麻烦。模拟量输出输出随反射光强度变化的电压值。优点是可以后期软件标定阈值抗光线干扰能力更强缺点是占用ADC通道代码复杂一些。如果使用数字量五路传感器可以直接接PA0到PA4但要注意模块输出的是高还是低有效。如果使用模拟量PA0到PA4配置为ADC输入在代码里设定阈值做二值化。输出类型接线优点缺点数字量GPIO读取代码简单调试方便阈值由电位器决定换场地需重新调模拟量ADC采样阈值可软件标定抗环境光干扰好需要多通道ADC配置建议优先使用模拟量输出。五路灰度在比赛场地的光照条件下经常变化软件阈值可以在启动时自动标定比每次拿螺丝刀调电位器可靠。4.2 ADC采样与阈值标定如果使用HAL库ADC采样需要先配置ADC1的通道和扫描模式。简化后的读取逻辑如下uint16_t gray_adc[5]; void ReadGrayADC(void) { for (int i 0; i 5; i) { HAL_ADC_Start(hadc1); HAL_ADC_PollForConversion(hadc1, 10); gray_adc[i] HAL_ADC_GetValue(hadc1); HAL_ADC_Stop(hadc1); } }更高效的做法是配置ADC扫描模式和多通道DMA一次转换结束后读取全部数据。上面这种逐通道启动停止的方式演示思路没问题但每个通道都启动停止会浪费时间控制周期短时可以用DMA版本替代。阈值标定的思路很简单车放在白色地面上记录一组白底ADC值放在黑线上记录一组黑线ADC值取两者的中间值作为阈值。uint16_t threshold[5]; void CalibrateThreshold(void) { // 车先放在白底上采样保存到white[i] // 车再放在黑线上采样保存到black[i] for (int i 0; i 5; i) { threshold[i] (white[i] black[i]) / 2; } }标定完成后每次采样都做一次二值化uint8_t gray_level[5]; void BinarizeGray(void) { for (int i 0; i 5; i) { gray_level[i] (gray_adc[i] threshold[i]) ? 1 : 0; } }这里假设黑线反射值比白底低。如果传感器输出逻辑相反只需要改比较方向。4.3 加权位置算法五路灰度之所以比单路或双路更有优势是因为可以根据哪一路压到黑线来判断车偏移了多少。常用方法是给五路传感器分配权重然后计算加权平均值。假设五路传感器从左到右编号为0到4权重设为-100、-50、0、50、100int GetPosition(void) { const int weight[5] {-100, -50, 0, 50, 100}; int32_t sum 0; uint8_t count 0; for (int i 0; i 5; i) { if (gray_level[i] 1) { sum weight[i]; count; } } if (count 0) { return sum / count; } return last_position 0 ? 127 : -127; }这里的返回值就是位置环的误差输入。当黑线在车正中间时只有编号2的传感器压线返回0。当车偏左时靠近左边的传感器压线返回负数。4.4 丢线处理策略小车高速过弯时五路传感器可能一时全部离开黑线这时count等于0位置计算没有意义。不能直接把误差置0否则车会以为自己在正中间直冲出去。丢线处理的常见策略是保持上一次的位置误差并给出一个较大的方向修正值让车继续往之前偏的方向转。上面的代码中用127或-127表示强制转向同时保留last_position用于下次判断方向。更好的方式是结合上一个正常位置和本次丢线方向做推断。比如最后压线的是左侧传感器那小车大概率还在往左偏丢线时继续输出一个较大的负误差。5. 角度环和位置环的实现与参数整定5.1 串级PID结构串级PID的写法是外环输出不直接给电机而是作为内环的目标值。这里的外环是位置环输入是灰度位置误差输出是目标转向角内环是角度环输入是目标角度和实际角度的偏差输出是差速控制量。float target_yaw 0.0f; float position_out 0.0f; float angle_out 0.0f; #define BASE_SPEED 1500 #define KP_POS 0.6f #define KP_ANG 3.0f #define KD_ANG 0.2f void ControlLoop(float dt) { int pos_error GetPosition(); // 外环位置环 target_yaw KP_POS * pos_error; if (target_yaw 30.0f) target_yaw 30.0f; if (target_yaw -30.0f) target_yaw -30.0f; // 内环角度环 float ang_error target_yaw - yaw; float gyro_damp -gz_dps; angle_out KP_ANG * ang_error KD_ANG * gyro_damp; int left_pwm BASE_SPEED (int)angle_out; int right_pwm BASE_SPEED - (int)angle_out; SetMotorPWM(left_pwm, right_pwm); }这里把PID简化成PD控制。位置环只用比例因为位置误差本来就来自灰度积分容易在弯道处造成超调。角度环加了一个微分项直接用陀螺仪角速度做阻尼相当于对转向速度进行限制能显著减少小车左右摆动。5.2 内环角度环参数作用角度环是整个控制系统的稳定核心。如果角度环参数不调好位置环再准也会让车抖动甚至失控。参数含义初值参考调大后的表现调小后的表现KP_ANG角度误差比例2.0到5.0响应快但容易抖动转向迟钝入弯慢KD_ANG角速度阻尼0.1到0.5转向平稳但过大会发沉容易振荡过弯不干脆角度环的调试原则是先让车在小范围转动时能快速回到目标角同时不出现高频抖动。可以临时固定target_yaw为15度观察MPU6050实际角度能不能快速稳定到15度。稳定了再交给位置环去改target_yaw。5.3 外环位置环参数作用位置环让小车在跑偏时知道该转多少角。位置环比例KP_POS太大灰度稍微偏移一点就输出很大的目标角度车会在线上来回穿插太小车过弯时跟不上线的变化直接冲出赛道。参数含义初值参考调大后的表现调小后的表现KP_POS位置误差比例0.3到1.0入弯快但易摆动过弯迟缓出弯压线位置环调试要在角度环稳定的前提下进行。先把角度环参数固定然后从小到大加KP_POS观察车在直道上的摆动幅度和弯道跟随能力。5.4 参数整定顺序和检查点串级PID整定顺序不能反必须先内环后外环。第一步标定灰度阈值和MPU6050零偏确保传感器数据可信。第二步单独调试角度环。让小车悬空或者用支架把后轮架起来给一个固定目标角观察实际角度跟随情况。目标角从5度到30度逐步增加
STM32循迹小车双闭环控制：MPU6050角度环与灰度位置环实战解析
在STM32小车控制项目中利用STM32F103C8T6配合MPU6050和五路灰度传感器实现角度环与位置环控制是复刻电赛24年H题比较典型的一套方案。很多初学者一开始会把灰度传感器的输出直接换算成左右电机PWM这种做法在低速直道勉强能跑但一到弯道或者场地光线变化就很容出现忽左忽右、冲线甚至原地打转。问题不在于传感器不够用而在于控制结构太简单没有把“车相对线的位置偏差”和“车实际姿态角”分开处理。这篇文章会围绕一套可复现的最小工程展开目标是让你搞清楚五路灰度传感器、MPU6050、角度环和位置环是如何配合的。我会从硬件资源分析开始再讲MPU6050的姿态采集、五路灰度位置计算、串级PID实现最后给出联调验证和常见问题排查。文章里的代码用于说明思路实际项目要根据自己的引脚、电机驱动和库版本调整。1. 先理解这套方案硬件分工和双闭环控制结构1.1 STM32F103C8T6在项目里的资源边界STM32F103C8T6是一颗Cortex-M3内核的芯片主频72MHz内置64KB Flash和20KB RAM。在循迹小车这类项目中它的资源并不算紧张但也不是无限可用。一个典型的控制工程通常包含以下负载MPU6050通过I2C读取原始加速度和角速度数据每秒大约需要读取几百字节。五路灰度传感器如果是模拟量每一路都需要ADC采样五路共用一个ADC外设的不同通道。两个电机的PWM输出、方向控制以及串口调试输出。控制中断里需要执行姿态融合、位置计算和串级PID计算量主要取决于是否使用DMP或四元数库。实际项目中20KB RAM足以运行标准外设库或HAL库生成的基础工程。如果使用DMP库要注意库本身占用一定RAM并且要把MPU6050的I2C速率和堆栈空间预留出来。下表是常见资源需求参考资源类型需求说明Flash约8KB到16KBHAL库比标准库占用更多但通常仍小于64KBRAM约2KB到6KB缓冲区、滤波数组、PID结构体、堆栈定时器至少1个用于控制周期中断常用TIM2或TIM3I2C1个连接MPU6050ADC1个五路灰度共用采用扫描或多通道轮流采样串口1个调试和参数监控这里的判断是用STM32F103C8T6做这套控制是够用的但需要管理好中断频率和库的开销不要在Main函数里做大量阻塞式延时也不要频繁调用HAL_I2C_Mem_Read这类函数而不控制调用周期。1.2 MPU6050负责角度环五路灰度负责位置环先说两个环的分工。位置环的反馈来源是五路灰度传感器。五路灰度横向排列在车底当黑线在车体正中间时中间一路灰度值最接近黑色当车偏左或偏右时靠近中间的传感器分布会不一致。通过给五路传感器设定不同权重可以计算出一个数值范围比如-100到100表示车体相对黑线横向偏移了多少。这个值就是位置误差。角度环的反馈来源是MPU6050。如果做的是自平衡车角度环通常用加速度计和陀螺仪融合出来的Pitch角或Roll角让车体保持直立。如果做的是差速循迹小车角度环更常指转向角度的闭环用MPU6050的Z轴角速度积分出偏航角Yaw或者直接用角速度做阻尼。两种理解在工程上都是合理的差别在于把哪个物理量当作被控量。这里采用更通用的差速循迹小车结构位置环根据灰度偏移输出一个“目标转向角”角度环根据MPU6050测量出的当前实际转向角把小车拉向目标角。这样车在入弯时不会因为灰度突然变化就打满方向出弯时也不会因为已经回到线上还继续转向。1.3 控制流位置环输出目标角度角度环输出差速PWM整个控制流程可以压缩成如下几条读取五路灰度计算位置误差。位置环PID把位置误差换算成“目标转向角”。读取MPU6050融合出当前实际角度。角度环PID把目标角度和实际角度的偏差换算成左右电机差速控制量。设置两路PWM输出让小车转动到目标角度。用伪代码表示就是while (1) { position_error GetPositionFromGray(); target_yaw PositionLoop(position_error); current_yaw GetYawFromMPU6050(); speed_diff AngleLoop(target_yaw - current_yaw); SetLeftRightPWM(base_speed speed_diff, base_speed - speed_diff); }这套结构的核心价值在于位置环只负责“该朝哪个方向转”角度环只负责“怎么转得又快又稳”。两个环不会相互抢输出调试时也可以先调内环再调外环。2. 环境准备和工程骨架2.1 工具链选择Keil MDK、库和烧录工具开发STM32F103C8T6常用Keil MDK也可以用STM32CubeIDE或VSCode加编译链。无论选哪种关键是你需要有一个可以编译、下载、调试的基础工程。工程可以基于标准外设库也可以基于HAL库。标准外设库代码直观寄存器操作容易理解适合学习HAL库配合STM32CubeMX配置外设更快切换引脚和时钟方便。这篇文章的代码示例偏HAL风格但控制逻辑部分完全可以移植到标准库。烧录工具方面常见的是ST-Link V2。除了在Keil里直接下载也可以使用STM32 ST-Link Utility查看Flash内容、确认固件是否下载成功或者做整片擦除。建议在联调前先准备一个能稳定烧录的环境避免把软件问题和烧录问题混在一起。2.2 最小工程需要的启动文件、时钟和调试接口一个最小的STM32F103C8T6工程至少包含启动文件例如startup_stm32f103xb.s。系统时钟配置文件例如system_stm32f10x.c或者CubeMX生成的SystemClock_Config。GPIO、I2C、ADC、TIM、USART等外设初始化。主循环和控制定时器中断。时钟配置要特别注意。F103最高主频72MHz外部晶振通常是8MHz通过PLL倍频到72MHz。如果时钟配置错误串口波特率、定时器周期、I2C时序都会跟着错而且排查起来很难发现。调试接口建议先保留串口输出。串口在前期的作用比任何示波器都好用可以打印MPU6050原始数据、灰度ADC值、位置误差、PID控制量。后续调参数时串口接一个蓝牙模块或者无线透传模块还能在跑车过程中实时查看数据。2.3 外设资源分配参考下面是一套不冲突的引脚分配示例可以直接在CubeMX里照着配置功能引脚外设说明MPU6050 SCLPB6I2C1_SCL需要外接4.7k上拉电阻MPU6050 SDAPB7I2C1_SDA需要外接4.7k上拉电阻灰度传感器五路PA0到PA4ADC1_IN0到IN4模拟量输出或数字量输入左电机PWMPA8TIM1_CH1占空比可调右电机PWMPA11TIM1_CH4占空比可调电机方向引脚PB0到PB3GPIO输出控制正反转和刹车串口TXPA9USART1_TX调试输出串口RXPA10USART1_RX可选用于接收指令这里把PWM放在TIM1的CH1和CH4避免和USART1的PA9、PA10冲突。如果你手头的板子引脚被固定了至少先列出表格确认每个外设不会占用同一个引脚。2.4 控制周期设计为什么不用延时循环小车控制最忌讳在主循环里用HAL_Delay做延时因为主循环的执行时间不稳定灰度采样、姿态解算、PID计算都会受到影响。推荐用定时器中断产生固定控制周期。常见做法是设置一个1ms或2ms的定时器中断然后在中断里递增标志位。比如每2ms执行一次MPU6050读取和角度环控制每10ms执行一次灰度采样和位置环控制。这样角度环比较快能保证转向稳定位置环不需要太快因为灰度传感器在高速运动时变化频率并没有高到需要1ms采样一次。定时器预分频和重装载值需要按你的定时器时钟计算。72MHz主频下如果想让TIM2产生1ms中断常见配置是预分频72减1重装载1000减1。具体值取决于你的APB1时钟树建议用CubeMX直接生成避免手工算错。3. MPU6050姿态采集从读取寄存器到角度估计3.1 电路连接和上拉电阻MPU6050通过I2C连接STM32SCL和SDA都需要上拉电阻。很多MPU6050模块上已经自带了10k或4.7k上拉电阻但如果你用的是裸芯片或者模块上省略了上拉通信会出现偶发失败。连接时还要注意电平匹配。STM32F103C8T6的I2C引脚耐压是5V但更稳妥的做法是让模块和你开发板使用相同的3.3V供电避免电平不一致导致读写异常。如果模块支持5V供电但板上没有电平转换I2C引脚可能被拉到5V这种电路设计虽然常见但在调试时容易损坏引脚不建议长期使用。3.2 寄存器初始化顺序MPU6050上电后默认处于睡眠模式必须先唤醒再配置采样率、量程和数字低通滤波器。下面是典型的寄存器配置顺序#define MPU6050_ADDR 0xD0 // 7位地址0x68左移一位 void MPU6050_WriteReg(uint8_t reg, uint8_t data) { HAL_I2C_Mem_Write(hi2c1, MPU6050_ADDR, reg, I2C_MEMADD_SIZE_8BIT, data, 1, 100); } void MPU6050_Init(void) { MPU6050_WriteReg(0x6B, 0x00); // 电源管理唤醒选择时钟源 MPU6050_WriteReg(0x19, 0x07); // 采样率分频控制采样频率 MPU6050_WriteReg(0x1A, 0x06); // 数字低通滤波 MPU6050_WriteReg(0x1B, 0x08); // 陀螺仪量程 ±500°/s MPU6050_WriteReg(0x1C, 0x00); // 加速度量程 ±2g MPU6050_WriteReg(0x38, 0x00); // 关闭中断 }这段代码的关键点如下0x19寄存器会影响最终采样率。如果开启数字低通滤波陀螺仪输出频率会降到1kHz实际采样率等于1kHz除以1 分频值。这里写0x07得到的采样率大约是125Hz对应8ms左右一次数据更新对角度环来说偏慢。如果要更快的采样率可以把分频值调小或者关闭DLPF直接用内部8kHz输出。陀螺仪量程和加速度量程会影响后续单位换算的灵敏度系数。配置完成后最好先验证能读到WHO_AM_I寄存器。WHO_AM_I地址是0x75读出值通常是0x68。如果读不到后面的所有姿态计算都没有意义。3.3 原始数据读取和单位换算MPU6050的加速度计数据从0x3B开始陀螺仪数据从0x43开始连续14个字节可以一次读取。读取代码如下void MPU6050_ReadRaw(int16_t *ax, int16_t *ay, int16_t *az, int16_t *gx, int16_t *gy, int16_t *gz) { uint8_t buf[14]; HAL_I2C_Mem_Read(hi2c1, MPU6050_ADDR, 0x3B, I2C_MEMADD_SIZE_8BIT, buf, 14, 100); *ax (int16_t)((buf[0] 8) | buf[1]); *ay (int16_t)((buf[2] 8) | buf[3]); *az (int16_t)((buf[4] 8) | buf[5]); *gx (int16_t)((buf[8] 8) | buf[9]); *gy (int16_t)((buf[10] 8) | buf[11]); *gz (int16_t)((buf[12] 8) | buf[13]); }读取到的是16位原始值需要根据量程换算成物理单位。加速度量程±2g时灵敏度为16384 LSB/g陀螺仪量程±500°/s时灵敏度为65.5 LSB/(°/s)。换算代码float ax_g (float)*ax / 16384.0f; float ay_g (float)*ay / 16384.0f; float az_g (float)*az / 16384.0f; float gx_dps (float)*gx / 65.5f; // 单位度/秒 float gy_dps (float)*gy / 65.5f; float gz_dps (float)*gz / 65.5f;这里的LSB值必须和你在0x1B、0x1C寄存器里配置的量程匹配否则后面的角度会整体偏大或偏小。3.4 互补滤波输出Pitch和Roll加速度计在静止时能直接算出重力方向但动态响应慢而且车体振动时会引入噪声。陀螺仪短时间积分角度很准但会随时间漂移。互补滤波把两者结合起来长时间信用加速度计短时间信用陀螺仪。一阶互补滤波的常见实现如下float pitch 0.0f; float roll 0.0f; void UpdateAngle(float dt) { float acc_pitch atan2f(ay_g, sqrtf(ax_g * ax_g az_g * az_g)) * 57.29578f; float acc_roll atan2f(ax_g, sqrtf(ay_g * ay_g az_g * az_g)) * 57.29578f; pitch 0.98f * (pitch gx_dps * dt) 0.02f * acc_pitch; roll 0.98f * (roll gy_dps * dt) 0.02f * acc_roll; }这段代码是思路演示实际使用时要根据MPU6050安装方向确认X、Y轴与角度正负号的关系。尤其是车模安装角度不同加速度计和陀螺仪的符号可能需要对调或取反。系数0.98和0.02是经验值。系数越偏向陀螺仪角度越平滑但动态跟随越慢越偏向加速度计角度响应越快但振动噪声越明显。一般可以从0.95和0.05开始调。3.5 转向角度估计Yaw积分及其漂移处理在差速循迹小车里角度环的反馈需要是车体转过的角度。最简单的方法是用Z轴角速度积分static float yaw 0.0f; void UpdateYaw(float dt) { yaw gz_dps * dt; }这个写法的问题在于MPU6050的陀螺仪零点不可能绝对为0。静止时gz_dps可能持续输出0.1°/s的零偏积分几十秒后就会产生明显角度漂移。如果位置环输出的是目标转向角角度环又把这个漂移角度当作真实角度车就会在直道上慢慢跑偏。缓解漂移的常用办法有三个上电静止时采集几十组gz数据的平均值作为零偏后续读取时减去这个零偏。不长时间依赖Yaw位置环的灰度反馈作为主要修正Yaw只用来提供阻尼。如果赛道结构固定每次经过起跑线或特定标记时对Yaw清零。如果要做更稳定的偏航角需要增加磁力计或者使用DMP库输出四元数再配合磁力计做融合。但这对硬件要求更高很多竞赛场景下用零偏校准加定期清零就够了。3.6 MPU6050常见坑MPU6050的坑比灰度传感器多常见问题集中在接线、量程和姿态更新周期上。问题现象常见原因检查方式处理建议I2C扫描不到设备SCL/SDA上拉缺失或模块地址不对用I2C扫描代码确认地址接4.7k上拉确认地址是0x68还是0x69角度静止时缓慢漂移陀螺仪零偏未校准串口打印静止时gz_dps启动时采集零偏并补偿角度计算数值明显偏大灵敏度系数与量程不匹配检查0x1B和0x1C配置按实际量程改LSB系数姿态更新不平滑控制周期不固定或滤波系数不合适打印时间戳或使用定时器中断固定dt用定时器中断跑一段时间后Yaw严重偏移陀螺仪积分漂移直道观察灰度误差使用零偏校准定期清零4. 五路灰度传感器位置误差计算4.1 数字输出与模拟输出的差异五路灰度传感器模块有两种常见输出形式数字量输出模块上自带比较器检测到黑线时输出高电平或低电平。优点是直接接GPIO简单可靠缺点是阈值固定在电位器上现场调节麻烦。模拟量输出输出随反射光强度变化的电压值。优点是可以后期软件标定阈值抗光线干扰能力更强缺点是占用ADC通道代码复杂一些。如果使用数字量五路传感器可以直接接PA0到PA4但要注意模块输出的是高还是低有效。如果使用模拟量PA0到PA4配置为ADC输入在代码里设定阈值做二值化。输出类型接线优点缺点数字量GPIO读取代码简单调试方便阈值由电位器决定换场地需重新调模拟量ADC采样阈值可软件标定抗环境光干扰好需要多通道ADC配置建议优先使用模拟量输出。五路灰度在比赛场地的光照条件下经常变化软件阈值可以在启动时自动标定比每次拿螺丝刀调电位器可靠。4.2 ADC采样与阈值标定如果使用HAL库ADC采样需要先配置ADC1的通道和扫描模式。简化后的读取逻辑如下uint16_t gray_adc[5]; void ReadGrayADC(void) { for (int i 0; i 5; i) { HAL_ADC_Start(hadc1); HAL_ADC_PollForConversion(hadc1, 10); gray_adc[i] HAL_ADC_GetValue(hadc1); HAL_ADC_Stop(hadc1); } }更高效的做法是配置ADC扫描模式和多通道DMA一次转换结束后读取全部数据。上面这种逐通道启动停止的方式演示思路没问题但每个通道都启动停止会浪费时间控制周期短时可以用DMA版本替代。阈值标定的思路很简单车放在白色地面上记录一组白底ADC值放在黑线上记录一组黑线ADC值取两者的中间值作为阈值。uint16_t threshold[5]; void CalibrateThreshold(void) { // 车先放在白底上采样保存到white[i] // 车再放在黑线上采样保存到black[i] for (int i 0; i 5; i) { threshold[i] (white[i] black[i]) / 2; } }标定完成后每次采样都做一次二值化uint8_t gray_level[5]; void BinarizeGray(void) { for (int i 0; i 5; i) { gray_level[i] (gray_adc[i] threshold[i]) ? 1 : 0; } }这里假设黑线反射值比白底低。如果传感器输出逻辑相反只需要改比较方向。4.3 加权位置算法五路灰度之所以比单路或双路更有优势是因为可以根据哪一路压到黑线来判断车偏移了多少。常用方法是给五路传感器分配权重然后计算加权平均值。假设五路传感器从左到右编号为0到4权重设为-100、-50、0、50、100int GetPosition(void) { const int weight[5] {-100, -50, 0, 50, 100}; int32_t sum 0; uint8_t count 0; for (int i 0; i 5; i) { if (gray_level[i] 1) { sum weight[i]; count; } } if (count 0) { return sum / count; } return last_position 0 ? 127 : -127; }这里的返回值就是位置环的误差输入。当黑线在车正中间时只有编号2的传感器压线返回0。当车偏左时靠近左边的传感器压线返回负数。4.4 丢线处理策略小车高速过弯时五路传感器可能一时全部离开黑线这时count等于0位置计算没有意义。不能直接把误差置0否则车会以为自己在正中间直冲出去。丢线处理的常见策略是保持上一次的位置误差并给出一个较大的方向修正值让车继续往之前偏的方向转。上面的代码中用127或-127表示强制转向同时保留last_position用于下次判断方向。更好的方式是结合上一个正常位置和本次丢线方向做推断。比如最后压线的是左侧传感器那小车大概率还在往左偏丢线时继续输出一个较大的负误差。5. 角度环和位置环的实现与参数整定5.1 串级PID结构串级PID的写法是外环输出不直接给电机而是作为内环的目标值。这里的外环是位置环输入是灰度位置误差输出是目标转向角内环是角度环输入是目标角度和实际角度的偏差输出是差速控制量。float target_yaw 0.0f; float position_out 0.0f; float angle_out 0.0f; #define BASE_SPEED 1500 #define KP_POS 0.6f #define KP_ANG 3.0f #define KD_ANG 0.2f void ControlLoop(float dt) { int pos_error GetPosition(); // 外环位置环 target_yaw KP_POS * pos_error; if (target_yaw 30.0f) target_yaw 30.0f; if (target_yaw -30.0f) target_yaw -30.0f; // 内环角度环 float ang_error target_yaw - yaw; float gyro_damp -gz_dps; angle_out KP_ANG * ang_error KD_ANG * gyro_damp; int left_pwm BASE_SPEED (int)angle_out; int right_pwm BASE_SPEED - (int)angle_out; SetMotorPWM(left_pwm, right_pwm); }这里把PID简化成PD控制。位置环只用比例因为位置误差本来就来自灰度积分容易在弯道处造成超调。角度环加了一个微分项直接用陀螺仪角速度做阻尼相当于对转向速度进行限制能显著减少小车左右摆动。5.2 内环角度环参数作用角度环是整个控制系统的稳定核心。如果角度环参数不调好位置环再准也会让车抖动甚至失控。参数含义初值参考调大后的表现调小后的表现KP_ANG角度误差比例2.0到5.0响应快但容易抖动转向迟钝入弯慢KD_ANG角速度阻尼0.1到0.5转向平稳但过大会发沉容易振荡过弯不干脆角度环的调试原则是先让车在小范围转动时能快速回到目标角同时不出现高频抖动。可以临时固定target_yaw为15度观察MPU6050实际角度能不能快速稳定到15度。稳定了再交给位置环去改target_yaw。5.3 外环位置环参数作用位置环让小车在跑偏时知道该转多少角。位置环比例KP_POS太大灰度稍微偏移一点就输出很大的目标角度车会在线上来回穿插太小车过弯时跟不上线的变化直接冲出赛道。参数含义初值参考调大后的表现调小后的表现KP_POS位置误差比例0.3到1.0入弯快但易摆动过弯迟缓出弯压线位置环调试要在角度环稳定的前提下进行。先把角度环参数固定然后从小到大加KP_POS观察车在直道上的摆动幅度和弯道跟随能力。5.4 参数整定顺序和检查点串级PID整定顺序不能反必须先内环后外环。第一步标定灰度阈值和MPU6050零偏确保传感器数据可信。第二步单独调试角度环。让小车悬空或者用支架把后轮架起来给一个固定目标角观察实际角度跟随情况。目标角从5度到30度逐步增加 
