MSPM0G3507循迹小车PID源码解析：从位置式到串级控制实战

2024年电赛H题公布后我们队没有纠结太久直接把主控定成了MSPM0G3507。原因不复杂这个芯片在性能和上手难度之间平衡得非常好做循迹小车刚刚合适。这篇文章算是一次完整复盘重点放在基于MSPM0G3507的循迹小车PID源码解析上包括位置式和增量式PID怎么选、串级速度环怎么接、传感器偏差怎么算以及一堆只跑过现场才会踩到的坑。无论你是第一次打电赛还是正在被调参折磨的老选手照着这篇文章的思路走都能少走不少弯路。我先把结论放在前面循迹小车能不能跑得稳七成取决于PID参数三成取决于传感器偏差算得准不准。很多队伍把精力花在换传感器、换电机上忽略了控制算法本身最后车还是左右乱甩。所以这篇文章不绕弯子直接拆代码、讲原理、给参数把你从“能跑”带到“跑得稳”的状态。1. 内容整体设计与思路拆解1.1 为什么选MSPM0G3507做电赛主控先说芯片本身。MSPM0G3507是TI推出的Cortex-M0内核MCU主频最高80MHz内置128KB Flash、32KB SRAM外设包含12位1Msps ADC、DMA、多路16位高级定时器、模拟比较器、运放等。对一辆循迹小车来说这些资源完全是溢出状态哪怕你要同时跑灰度采集、PID运算、编码器测速、OLED显示和串口调试都不需要精打细算。为什么在电赛场景里我更推荐它而不是老牌的STM32F103核心就是开发效率。MSPM0系列支持TI的SysConfig图形化配置工具引脚功能、时钟树、外设初始化都可以图形化点出来然后自动生成代码。比赛周期只有四天三夜省下配置寄存器的时间全部可以用来调PID。我们的队伍第一天上午就把PWM、ADC、UART全部初始化完了下午开始写控制逻辑这个节奏在以往用STM32时很难达到。还有一个容易忽略的点MSPM0G3507的库函数风格本身就比较现代API分层清晰不像老一代库那样需要频繁翻寄存器手册。它还有专门的零漂移运放和比较器后续如果题目要求电磁循迹或者加一些模拟信号处理这块芯片也能直接顶上去不需要额外接运放芯片。选型的时候综合考虑性能、上手难度、外设整合度MSPM0G3507在电赛这个场景里性价比确实突出。1.2 循迹小车完整系统架构循迹小车本质上是一个“感知—决策—执行”的闭环系统。感知端是灰度传感器阵列负责识别赛道上的黑线决策端是MCU负责根据偏差计算控制量执行端是电机驱动模块和直流减速电机负责让车轮按照控制量转动。我们队的硬件配置是这样的主控板MSPM0G3507最小系统板。循迹模块5路红外灰度传感器支持数字/模拟输出切换。电机驱动TB6612FNG模块。动力部分两个带霍尔编码器的直流减速电机减速比1:30。供电7.4V 2S锂电池经过稳压模块给MCU和传感器供电。显示与调试0.96寸OLED用于显示当前误差、PID输出和状态码。软件层面的架构更关键。整个程序分成几个独立模块传感器采集模块负责读5路灰度偏差计算模块把灰度值转换为一个连续误差量PID控制模块根据误差计算修正量电机控制模块把修正量映射到左右两路PWM占空比编码器测速模块负责读取霍尔编码器脉冲用于速度闭环最后是调试输出模块通过串口或者OLED观察内部状态。这个架构的好处是模块之间耦合度低。调传感器的时候不用碰PID调PID的时候也不用管电机底层驱动。很多新手喜欢把逻辑全写在一个main函数里当时确实方便但一旦开始调参想改一个传感器阈值都要全局搜代码非常浪费时间。我建议从一开始就把代码按功能拆开后面就知道有多香了。1.3 控制链路从传感器到车轮的数据流整个控制链路可以理解成一条单向流水线。灰度传感器先采集当前赛道信息得到一个包含5个通道的原始数据数组然后对这个数组做阈值判断把每个通道归为“看见黑线”或者“看见白底”再把二值化后的数组转换成一个带符号的误差值负数表示车偏左正数表示车偏右误差值进入PID控制器得到输出最后把输出叠加到左右轮的基础PWM上实现差速转向。这里面最容易被忽略的是“采样周期”。PID公式里的微分项和积分项都依赖于采样时间间隔如果这个间隔忽长忽短就算Kp、Ki、Kd参数再合适控制效果也会乱七八糟。所以程序里的PID计算必须放到固定时间的中断或者定时任务里执行绝对不要用HAL_Delay()配合主循环做控制那是给自己挖坑。另外我们队在第一版程序里就把速度环和转向环组成了串级PID外环根据赛道偏差计算出目标轮速差内环根据编码器反馈把左右轮实际速度稳定在目标值。好处是当电池电压下降或者路面阻力变化时车速不会明显漂移直道加速和上坡都稳很多。后面会专门讲串级PID的具体做法。2. 核心细节解析与实操要点2.1 循迹传感器的布局与偏差计算5路灰度传感器的布局直接决定偏差计算的精度。我们用的传感器间隔是1.5cm总宽度大概6cm刚好覆盖常见赛道黑线线宽2cm左右。安装高度控制在离地1cm到1.5cm之间太近了容易刮到赛道表面太远了阈值不稳定。传感器输出的是模拟量需要先做归一化。最简单的方式是上电时做一次自动校准原地分别扫过白底和黑线记录每个通道的最大值和最小值然后取中间值作为该通道的判断阈值。这一步非常关键因为不同环境光照下传感器读数差异很大手动硬编码阈值很容易翻车。偏差计算公式我们用的是加权平均法。假设5个通道从左到右编号为S0到S4权重分别是-2、-1、0、1、2那么误差计算公式为float get_track_error(uint8_t *sensor, uint8_t count) { int8_t weight[5] {-2, -1, 0, 1, 2}; float sum 0.0f; uint8_t on_count 0; for (uint8_t i 0; i count; i) { if (sensor[i]) { // 该路检测到黑线 sum weight[i]; on_count; } } if (on_count 0) { // 全部为白色说明丢线 return last_error; // 返回上一次误差 } last_error sum / on_count; // 归一化到 [-2, 2] return last_error; }这里的关键细节是做了归一化。如果不除以on_count当两个传感器同时压线时误差会变成3甚至更大PID会突然输出一个很大的转向量车就会猛地一抽归一化之后不管压线的是1个还是3个传感器误差范围都保持在正常区间控制量平滑很多。还有一点需要备注如果所有传感器都检测到黑线一般是遇到了十字路口或者很粗的横线这时候要单独处理不要进入正常偏差计算逻辑。我们的做法是识别到全黑持续超过20ms就认为是十字路口小车保持直行通过等离开十字后传感器恢复再继续循迹。2.2 PID控制器的选型增量式还是位置式PID控制算法在循迹小车上有两种常见形态位置式PID和增量式PID。很多初学者搞不清什么时候用哪个我直接说结论如果你把PID输出直接当成一个“目标PWM修正量”叠加到基础占空比上用增量式更顺手如果你的执行机构是舵机输出直接对应舵机角度那必须用位置式。位置式PID公式是u(k) Kp * e(k) Ki * sum(e) Kd * (e(k) - e(k-1))它输出的就是当前时刻的完整控制量包含了过去所有误差的积分。问题在于积分项容易累计过头一旦出现一段时间的大误差积分饱和会让输出长时间卡在限幅值小车就会一直猛打方向所以位置式一般都要配合限幅和积分分离。增量式PID公式是du(k) Kp * (e(k) - e(k-1)) Ki * e(k) Kd * (e(k) - 2*e(k-1) e(k-2))它输出的只是本次相对于上一次的变化量。实际执行时你需要在主程序里维护一个累计变量每次把du累加上去再限幅输出。增量式天然不容易出现积分饱和调起来更省心所以做循迹小车我更推荐它。还有一点想提一下PID和“前馈”结合。前馈不是PID公式的一部分它根据已知信息提前给出一个基础控制量PID只负责修正误差和扰动。比如你提前知道前方是一个急弯可以根据曲率或者弯道方向直接给一个基础转向占空比这时候PID的输出只是做细微修正响应速度会比纯PID快很多。时间充裕的话这个技巧值得加上。2.3 速度环与转向环要不要编码器闭环如果你的目标只是“让车慢慢爬完一圈”那转向环用PD控制就够了不需要编码器。但电赛H题通常会有时间限制、速度加分项有的赛道还有坡道和颠簸路段这时候开环PWM就很不靠谱同样的占空比新电池和旧电池跑出来的速度能差20%以上上坡时更是直接掉速。所以我们队毫不犹豫上了编码器速度环。两个直流减速电机各带一个霍尔编码器输出A、B两路正交信号MSPM0G3507的定时器可以工作在正交编码模式硬件解码出转速和方向。这样MCU每5ms读一次当前轮速与目标速度比较经过速度环PID计算后更新PWM占空比。整体控制结构是串级PID外环转向环输入是循迹偏差输出是左右轮目标速度差。内环速度环输入是目标轮速与实际轮速的偏差输出是PWM占空比。举个例子小车在直道上误差为0转向环输出0左右轮目标速度都是60cm/s进入右弯后误差变成正值转向环输出一个正的差值于是左轮目标速度变成75cm/s右轮目标速度变成45cm/s车身自然右转。这个结构的核心优势是速度不受电池电压和路面摩擦系数的影响比赛后期电池电量下降时小车依然能保持稳定圈速。如果你们用的是麦轮底盘道理一模一样只是把“左右轮差速”换成“四轮差速分配”或者“麦轮纵向横向速度分配”。麦轮车的PID转角本质上也是同一个思路这里不再单独展开。2.4 PID采样周期与参数标定PID是离散化控制采样周期必须固定。我们用的周期是5ms也就是200Hz。这个频率对循迹小车来说足够快同时CPU占用率也不高。如果你的赛道速度很快可以缩到2ms但不建议低于1ms因为传感器响应和电机响应都是有物理极限的过高频率只会让微分项放大噪声。参数标定我的建议是严格按顺序来先把Ki和Kd设成0只保留Kp从小到大慢慢加直到小车开始出现小幅振荡。然后把Kd加上去抑制振荡。Kd的作用是“阻尼”会降低系统的超调量让小车过弯更顺畅。最后加Ki消除稳态误差。对循迹小车来说积分项的作用没有温度控制那么大只要一点点就够了加多了反而会让车在直道上左右扭。调参时一定要借助上位机把误差曲线和控制量曲线拉出来看盲调效率极低。我们用的是Vofa配合串口把error、output、speed这些变量以浮点格式发出来上位机直接显示实时波形。没有上位机的话用串口助手把数据打印成文本也能大致看出趋势。Matlab PID Tuner可以作为参考但它未必能直接用因为那套工具是基于线性模型辨识的而小车是典型的非线性系统传感器非线性和电机摩擦都会让仿真参数与真实参数有偏差。把它当作一个调参思路的辅助工具可以最终还是要现场实车测试。2.5 电机驱动与PWM的边界条件TB6612驱动模块的接线其实不难但有几个细节容易坑人。首先STBY引脚必须拉高否则电机不转其次AIN1和AIN2决定电机方向PWM引脚控制速度三者配合才能正反转调速。很多队伍调了半天电机不动最后发现是STBY没接。PWM频率我设置的是20kHz这个频率在人耳可听范围之外电机不会发出刺耳的啸叫声。频率太低的话比如几百Hz电机会像蚊子一样嗡嗡响听着就烦还容易因为电流纹波大导致发热。MSPM0G3507的高级定时器输出PWM非常方便用SysConfig把两个通道配置成PWM模式占空比直接写比较寄存器就行。还需要注意PWM死区。直流电机在占空比低于某个值的时候驱动电压不够克服静摩擦力车轮是不会动的。这个死区范围大概在5%到10%之间取决于电机和驱动板。我们在速度环里加了一个死区补偿逻辑如果计算出的占空比落在死区范围内直接强制输出一个最小启动占空比同时配合极短的低速持续时间让车轮平滑起步。3. 实操过程与核心环节实现3.1 工程初始化与引脚规划我用的是TI官方SDK加SysConfig图形化配置。SysConfig里需要做的事情包括配置系统时钟为80MHz。配置TimerA0两个通道为PWM输出用于左右电机调速。配置GPIO作为AIN1/AIN2方向控制引脚。配置ADC0的5个通道连接5路灰度传感器。配置UART用于调试波形输出。配置TimerA1的正交编码模式用于读取两个霍尔编码器。初始化代码不需要手写寄存器SysConfig会生成SYSCFG_DL_init()函数在main里直接调用就行。这样最稳妥也不容易出现时钟配置错误。下面是一段main函数的框架#include ti_msp_dl_config.h volatile uint8_t pid_tick 0; int main(void) { SYSCFG_DL_init(); // 外设初始化由SysConfig生成 NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN); NVIC_EnableIRQ(ADC0_INST_INT_IRQN); while (1) { if (pid_tick) { pid_tick 0; track_loop(); // 5ms执行一次循迹控制 } } }定时器中断里只需要把pid_tick置1控制逻辑全部放在主循环里执行。这样做的好处是中断服务函数保持极短不容易发生中断嵌套和优先级冲突。如果你把PID计算直接塞进中断一旦代码里出现浮点运算耗时过长后续中断就会被阻塞传感器采集会失真。3.2 PID相关源码解析下面是我们实际使用的PID实现。首先定义一个结构体把PID参数和中间变量放在一起方便维护多个PID实例。转向环一个实例左右轮速度环各一个实例总共三个。// pid.h #ifndef __PID_H #define __PID_H typedef struct { float kp; float ki; float kd; float integral; // 位置式积分累计值 float last_error; // 上一次误差 float prev_error; // 上上次误差 float out; // 位置式PID输出 float du; // 增量式PID输出 float out_max; // 输出限幅 } PID_t; void PID_Init(PID_t *pid, float kp, float ki, float kd, float out_max); float PID_Pos_Calc(PID_t *pid, float error, float dt); float PID_Inc_Calc(PID_t *pid, float error); #endif// pid.c #include pid.h void PID_Init(PID_t *pid, float kp, float ki, float kd, float out_max) { pid-kp kp; pid-ki ki; pid-kd kd; pid-integral 0.0f; pid-last_error 0.0f; pid-prev_error 0.0f; pid-out 0.0f; pid-du 0.0f; pid-out_max out_max; } float PID_Pos_Calc(PID_t *pid, float error, float dt) { pid-integral error * dt; // 积分限幅防止积分饱和 if (pid-integral pid-out_max) pid-integral pid-out_max; if (pid-integral -pid-out_max) pid-integral -pid-out_max; pid-out pid-kp * error pid-ki * pid-integral pid-kd * (error - pid-last_error) / dt; pid-last_error error; if (pid-out pid-out_max) pid-out pid-out_max; if (pid-out -pid-out_max) pid-out -pid-out_max; return pid-out; } float PID_Inc_Calc(PID_t *pid, float error) { pid-du pid-kp * (error - pid-last_error) pid-ki * error pid-kd * (error - 2.0f * pid-last_error pid-prev_error); pid-prev_error pid-last_error; pid-last_error error; if (pid-du pid-out_max) pid-du pid-out_max; if (pid-du -pid-out_max) pid-du -pid-out_max; return pid-du; }位置式PID和增量式PID的核心区别从代码里一眼就能看出来。位置式每次输出一个绝对值增量式每次输出一个变化量。我们转向环用的是位置式PD因为转向本质上是一个“角度的绝对控制”误差多大就该打多少方向速度环用的是增量式PID因为PWM占空比适合做增量累加不容易出现积分饱和。在调用增量式PID时主程序要维护一个current_duty变量float current_duty 0.0f; float speed_duty current_duty PID_Inc_Calc(speed_pid, speed_error); speed_duty clamp(speed_duty, -100.0f, 100.0f); current_duty speed_duty;每次都把增量叠加到上一次占空比上再限幅这个逻辑就构成了一个带限幅的增量式执行器。3.3 循迹主循环与差速转向实现循迹主循环的逻辑是读传感器、算误差、计算转向输出、计算左右轮目标速度、调用速度环、输出PWM。代码看起来是这样void track_loop(void) { uint8_t sensor[5]; float error, turn_out; float target_speed_l, target_speed_r; float base_speed 55.0f; // 基础目标速度单位 cm/s // 1. 读取5路灰度0表示白底1表示黑线 read_sensor(sensor); // 2. 计算循迹误差 error get_track_error(sensor, 5); // 3. 转向环PID输出转向修正量 turn_out PID_Pos_Calc(turn_pid, error, 0.005f); // 4. 根据转向输出分配左右轮目标速度 target_speed_l base_speed turn_out; target_speed_r base_speed - turn_out; // 5. 左右速度环分别调速 set_motor_left_speed(target_speed_l); set_motor_right_speed(target_speed_r); }set_motor_left_speed内部会读取当前编码器速度计算偏差调用增量式速度环PID更新PWM占空比。因为速度环也是5ms执行一次所以两个速度环的计算时间必须非常短尽量避开除法运算减少CPU占用。编码器速度计算这一块要特别注意方向符号。霍尔编码器输出A、B相定时器正交解码模式可以自动判断方向读出来的计数值是有符号的。正转时计数值增加反转时减少这样速度就是一个带符号的浮点数。如果发现轮子明明正转但速度显示为负不用改硬件直接把速度取反或者在初始化时交换A、B相输入引脚。3.4 一组可以落地的初始PID参数给一组我们队在标准赛道、基础速度55cm/s条件下调出来的参数供参考。注意不同车模、不同传感器、不同电机减速比都会影响最优参数这里只当起始点控制环KpKiKd输出限幅采样周期转向环位置式PD3508±305ms左轮速度环增量式1.20.050.3±605ms右轮速度环增量式1.20.050.3±605ms调参的时候有个原则一次只动一个参数。先固定转向环把速度环调稳再把转向环的Kp往上加。最忌讳的是同时改了好几个参数车跑飞了都不知道是哪个改的。我们每次改完参数都会在调试日志里记一版备注比如“Kp从30改成35直道摆动减小弯道内侧偏晚”这样第二天调参还能接上昨天的进度。4. 常见问题与排查技巧实录4.1 典型问题速查表下面这张表是我们在调试和比赛现场遇到最多的问题整理成速查表现象可能原因排查思路解决方案小车跑飞完全不按线走传感器阈值错误或丢线处理不当串口打印5路原始值观察阈值是否合理自动校准阈值增大丢线保留上次误差逻辑小车原地打转或反向跑电机正负极接反PID输出符号反了单轮测试PWM方向和编码器方向调换电机接线或软件取反PID输出高速时左右剧烈摆动Kp过大或采样周期不稳定看误差曲线是否有高频振荡减小Kp增大Kd让采样时间固定过十字路口乱转全黑识别时间过短提前误判检查进入十字后传感器状态持续时间全黑持续20ms以上才执行直行逻辑电机不转STBY没拉高PWM频率或占空比太低测驱动板输入电压和逻辑电平拉高STBY添加死区补偿上坡明显掉速开环PWM受负载影响大观察编码器速度曲线上速度闭环或提高目标速度同一套参数上午稳下午乱环境光变化导致传感器数值漂移用上位机比较阈值和原始值上电自动校准或换用红外传感器电池满电时甩尾低电时无力没有速度闭环占空比直接决定速度测量不同电压下相同PWM的实际转速务必加入编码器速度环闭环控制这张表里最值得我们反思的是最后一类问题。第一次参赛时我们用开环PWM控制车速上午满怀信心测试结果跑到第二轮电池电压降了0.5V速度明显变慢原本调好的PID参数全部失效。后来狠下心来花半天时间把编码器速度环做了后面所有问题都迎刃而解。所以我的建议是哪怕你只做开环也能跑完赛道也请尽量上速度闭环这半天时间省不得。4.2 传感器读数不稳的排查思路灰度传感器读数不稳是循迹小车的经典问题。表现是传感器有时候亮有时候不亮或者模拟量跳动很大小车在直道上都会来回扭。排查顺序一般是检查供电。如果传感器模块直接从MCU的3.3V引脚取电电机启动瞬间拉低电压传感器波形会严重畸变。建议传感器和电机驱动分开供电或者至少用独立稳压模块。检查共地。传感器、驱动板、主控板之间如果不共地信号参考点不一致读数就会乱飘。一定要把所有模块的地接到一起。检查安装高度和遮光。环境光直射传感器会让白底和黑线读数差距缩小加一个遮光罩能明显改善。软件滤波。对同一路ADC连续采样4次去掉最大最小再取平均或者做限幅滤波就能滤掉大部分毛刺。另外一个技巧是善用MSPM0G3507的DMA功能。ADC采样结果通过DMA搬运到内存数组里CPU不需要在采样期间频繁进入中断数据一致性更好。我们配置了定时器触发ADC采样DMA自动把5个通道的结果存到数组整个过程CPU零干预这对控制周期的稳定性帮助很大。4.3 调试工具与现场避坑经验调试PID强烈建议把实时曲线工具用起来。我们用的是Vofa设置成justfloat协议代码里用printf把误差、转向输出、左右轮速度发出去上位机就能显示5条实时曲线。看着曲线调参你一眼就能分辨出“超调大”、“响应慢”、“有高频抖动”盲调完全做不到这个效率。不过串口打印有坑。默认的printf是阻塞的如果每条数据用9600波特率发一帧数据要好几毫秒直接拖垮5ms控制周期。解决办法有两个一是把波特率提高到115200甚至460800缩短打印时间二是降低打印频率比如每20个控制周期打印一次。比赛现场我们用的是后者控制周期保持5ms稳定调试数据慢一点但够用。OLED屏在比赛现场比电脑方便得多。我们会在OLED上显示当前控制状态、误差、PID参数编号、传感器状态一旦车子行为异常不用接串口线低头扫一眼屏幕就能判断是哪一路出了问题。硬件上再加一个蜂鸣器丢线的时候响一下现场排查会省力不少。最后是比赛现场的“保命”建议多备一块充满电的电池多备一个烧录器多备一套传感器模块赛前检查一遍轮子螺丝和传感器支架调好的参数写进一个固定文件不要现场随手改还不备份。这些小细节看起来不起眼但在四天三夜的比赛高压环境下每一条都可能帮你避开一次重大事故。4.4 关于PID参数整定的一些感性认识调PID这件事前期有公式和步骤可以遵循后期就是手感。我带了几年队伍发现一个很有意思的规律越是着急调参数越调不好越冷静地看波形、分析现象、一次只动一个参数反而越快收敛到可用状态。Kp就像方向盘的“灵敏度”Kp太小时车过弯反应迟钝太大时车一看到弯道就猛打方向然后冲出去又猛地回打形成高频摆动。Kd像是一个“阻尼器”能预测误差变化趋势误差快速增大时就提前减小输出抑制超调但Kd太大也会引入噪声毕竟微分对传感器跳变非常敏感。Ki对循迹小车来说是辅助角色主要用来消除直道上因为车模左右不完全对称造成的恒定偏差加多了反而会让车在直道上走“蛇形”。我个人的调整顺序是先用Kp把车调到“能走完一圈但会甩”再用Kd把甩尾压下去最后按需加一点点Ki。每组参数至少跑5圈观察稳定性和重复性不能因为某一圈表现好就认定参数没问题。比赛场地光线、地面摩擦、电池电量都会影响表现合理的目标是让参数在一整组比赛过程中都足够稳而不是只盯着单圈成绩。5. 写在最后一点个人经验这套基于MSPM0G3507的循迹方案我们前后调了一周左右从最初的“能跑”到最后的“稳定刷圈”最大的感悟不是PID公式多复杂而是控制里面的工程细节决定成败。传感器噪声、采样周期、定时器中断优先级、电机死区、编码器方向任何一个点处理不好都会让你的PID看起来像没调一样。如果你正在准备电赛我的建议是不要一开始就追求最优参数和最快圈速先把传感器读到的东西、误差算得准不准、电机响应正不正常这些最基础的东西用串口和屏幕确认清楚然后把PID从一个能用的初始值开始慢慢打磨。这个过程没有捷径但只要你坚持下去很快就能找到那种“车像吸在线上一样”的丝滑手感。最后再分享一个小技巧每次调参前把当前的车模状态、赛道类型、PID参数、现象描述记录下来哪怕只写在手机备忘录里都行。有了这些记录你才能在赛后复盘时清晰地知道哪些修改是有效的哪些是白折腾。祝你们的车也能稳稳地跑完全程。