拓冰网站定制 
麦轮底盘第一次上手会让人有点上头四个轮子摆在那里既能横着走、又能斜着走、还能原地打转比两轮差速的小车灵活太多。可真到自己动手搓的时候坑一个接一个——编码器读数在跳、PID 稍微加大一点电机就开始啸叫、走直线走个两米就歪了十几公分、标定完车轮半径发现原地转一圈角度差了快二十度。这篇就把我从建 STM32 工程开始一路到轮速闭环、麦轮运动学解算、位置控制的整套流程完整摊开参数怎么定、为什么这么定、踩过哪些坑全部写清楚。适合已经会点 C 语言、玩过一阵单片机、想把移动机器人底盘真正做扎实的人看如果你只是想让小车能动起来前半部分也够用了。1. 先想清楚这台麦轮底盘到底要解决什么问题1.1 为什么选麦轮而不是差速或者全向轮很多人一上来就问麦轮是不是比差速好这个问题本身就不成立它们解决的是不同的需求。两轮差速底盘只有两个自由度——前进和转向想横着挪一米你得先转 90 度、走一米、再转回来在窄通道里根本施展不开。麦轮底盘给的是完整的三个平面自由度纵向、横向、自转三者可以任意叠加理论上车体可以沿着平面内任意方向平移同时保持车头朝向不变。这个能力在什么场景下值钱一是空间受限的场景比如货架之间只有 90 公分宽的 AGV二是需要精细对位的场景比如车头必须始终朝向某个方向的抓取平台三是比赛类场景尤其是那些需要快速绕桩、走 8 字、走复杂轨迹的机器人。代价也要讲明白。麦轮的辊子是和地面成 45 度斜着接触的力的传递效率天然比普通橡胶轮低常见的经验值是有效效率大概在 70% 上下同样的电机推同样重量的车麦轮需要的扭矩大概要多出三成。辊子和地面的接触面积小对地面平整度和摩擦系数都很敏感地毯、缝隙、砂石地面基本就别想了。还有一点容易被忽略麦轮的运动学模型是建立在轮子不打滑的理想假设上的一旦某个轮子悬空或者打滑正运动学解算出来的里程计位姿会直接跑飞而且没有任何自我纠正的能力。所以我的判断逻辑是如果你的车只需要在地面上跑直线加转向老老实实用差速结构简单、成本低、扭矩利用率高如果你确实需要横向平移这个能力而且地面是环氧地坪或者瓷砖这类硬质平整地面那麦轮就是对的。至于全向轮万向轮它的辊子是垂直于轮面、绕轮缘一圈布置的横向能力更强、运动学更简单但每个辊子受力时的冲击大、噪音大、承载能力差重载场合还是麦轮更合适。1.2 整机硬件配置与选型理由这套配置我是按能跑、能调、成本可控三个条件选的不是最便宜也不是最强但每个零件的选型都有理由。部件型号/规格选型理由主控STM32F103C8T6 最小系统板72MHz 主频4 个通用定时器 1 个高级定时器够用资料多出问题好查电机JGB37-520 直流减速电机12V减速比 1:30带 13 线霍尔编码器自带编码器省事扭矩够推 3~5kg 车体轴径 6mm 配 60mm 麦轮正合适驱动TB6612FNG 双路驱动模块 ×2单路持续 1.2A、峰值 3.2APWM 频率能上 100kHz逻辑电平 3.3V 兼容电源3S 锂聚合物电池 11.1V 2200mAh满电 12.6V放电倍率够重量轻降压5V/3A DC-DC 模块给主控和其他逻辑电路供电注意别用 LDO 硬扛压差大发热严重轮子60mm 麦轮 ×4左旋、右旋各两个直径小转弯半径小四个轮子必须区分左右旋车体铝合金或者亚克力板轴距 150mm × 轮距 160mm底盘刚性很重要软的车架会让里程计发散姿态MPU6050 六轴位置控制里做航向角融合纯靠里程计算航向会漂电机这块我多说两句。JGB37-520 这套组合的编码器是 13 线霍尔编码器A/B 两相正交输出四倍频之后是 52 个计数再乘 30 的减速比电机输出轴转一圈对应 1560 个计数。这个分辨率在 M 法测速下够不够用后面第三章会详细算。如果你预算宽裕直接上带 500 线光电编码器的电机一圈 60000 个计数低速时的速度分辨率会好一个数量级PID 也更好调。轮子的左右旋千万别搞错。麦轮分左旋轮和右旋轮从轮子正上方往下看辊子从轮轴处向外延伸的方向不同。标准做法是装成X 型左前和右后是一组右前和左后是另一组四个轮子的辊子接触线在地面上呈现出一个 X。装错了会怎样车能走但横移方向和你算的完全相反而且四个轮子会互相较劲跑起来声音很大。识别方法很简单手按住车体往前推看辊子是不是顺滑地滚如果你能明显感觉到卡顿或者需要很大力多半是装反了。1.3 软件分层为什么一定要分三层我见过太多人把整个工程写在一个 main.c 里初始化一堆外设然后在 while(1) 里塞满逻辑调着调着就调不动了——改一个 PID 参数结果串口发送也乱了加一个功能编码器读数就开始丢。根本原因是所有代码耦合在一起时序完全不可控。我的做法是分三层边界划清楚最底层是驱动层只干一件事把硬件抽象成函数。Motor_SetPWM(uint8_t id, int16_t duty)、Encoder_ReadDelta(uint8_t id)、UART_SendBuf()这些函数不包含任何控制逻辑换一块主控只改这一层。帕中间是控制层运动学解算、PID 计算、里程计积分都在这里。这一层的所有状态都存在全局或静态结构体里输入是目标值和反馈值输出是 PWM 占空比跟硬件没关系可以拿到 PC 上做单元测试。最上面是应用层负责指令解析、状态机、调度。遥控器发过来的速度指令怎么变成 vx/vy/ω什么时候切到位置控制模式这些决策逻辑放这里。分层的实际好处是调试效率。举个真实例子有次车跑起来往一边偏我先怀疑 PID改了半天没用后来把里程计数据通过串口以 20ms 周期打出来发现四个轮子的编码器计数有一个明显偏小——问题在硬件是那个电机轴上的联轴器螺丝松了。如果代码揉在一起光是把数据打出来这一步就要折腾半小时。2. STM32 工程从零建起来的正确姿势2.1 标准库还是 HAL 库先把账算清楚这个话题争论很多我只说我的实际取舍。STM32F103C8T6 这个级别的芯片标准库和 HAL 库都能跑但差别在哪HAL 库 CubeMX 的优势是上手快图形化配置引脚和时钟半天就能点灯。劣势也很明显一是代码体积大一个空工程编译出来 flash 占用能到标准库的两三倍二是抽象层厚HAL_GPIO_WritePin()这种函数执行一次要几十个时钟周期在 5ms 一次的控制中断里频繁调用会明显吃掉裕量三是封装的回调机制在需要精确时序的场合不好用比如你要在一个中断里同时读三个定时器的计数器HAL 的句柄结构访问起来绕。标准库的优势正好相反寄存器级操作函数基本就是几条汇编执行快、代码小、时序可控。劣势是资料相对老新芯片型号支持差而且 CubeMX 不能生成标准库工程。我的结论是做电机控制、编码器采集这类对时序敏感的项目用标准库做联网、文件系统、USB 这类偏应用的用 HAL。这套麦轮底盘我会用标准库。如果你已经习惯了 HAL 也不是不能做但速度环的采样周期建议从 5ms 放宽到 10ms给中断留出余量。新建标准库工程的步骤简单列一下很多教程漏掉的细节我标出来从芯片厂商官网或者开源社区下载 STM32F10x 标准外设库版本选 3.5.0这是最成熟的。新建工程目录建Start启动文件、CMSIS 内核文件、Library标准外设库源码、User自己写的代码、Hardware外设驱动四个文件夹。Keil 里新建 Target添加startup_stm32f10x_md.s注意中容量是md不是hd选错了链接会报错找不到中断向量。添加库文件时有个坑stm32f10x_tim.c、stm32f10x_gpio.c、stm32f10x_rcc.c、stm32f10x_usart.c、misc.c这几个是必加的但stm32f10x_it.c里的中断服务函数会和你自己写的重名我一般直接用自己写的不引入库里的这个文件。在stm32f10x.h或者工程的预编译宏里定义USE_STDPERIPH_DRIVER和STM32F10X_MD这两个宏不定义会出现一堆未定义符号很多人卡在这里。勾选 Create HEX File调试器选 ST-Link Debugger在 Debug 设置里把 Reset and Run 勾上省得每次烧完还要手动复位。2.2 时钟树配置那个被忽略的定时器倍频时钟树配置看起来是 CubeMX 一键搞定的事用标准库就得手写但这里有个细节我认为必须讲清楚因为它直接决定你的 PWM 频率算得对不对。F103 的外部晶振一般是 8MHz经过 PLL 九倍频得到 72MHz 的 SYSCLK。然后 AHB 不分频还是 72MHz。APB2 预分频系数是 1所以 APB2 也是 72MHz高级定时器 TIM1 挂在 APB2 上时钟是 72MHz。APB1 预分频系数是 2所以 APB1 是 36MHzTIM2、TIM3、TIM4 挂在 APB1 上。这里就是关键当 APB 预分频系数不为 1 时挂在该总线上的定时器时钟会自动乘以 2。所以 TIM2/3/4 的实际计数时钟不是 36MHz而是 72MHz。这个规则在参考手册的时钟树图里有标注但是画得很小很多人第一次看根本注意不到结果算 PWM 频率的时候按 36MHz 算实际输出频率差了一倍电机啸叫得厉害还找不到原因。配置代码大致是这样void RCC_Config(void) { RCC_DeInit(); RCC_HSEConfig(RCC_HSE_ON); while (RCC_GetFlagStatus(RCC_FLAG_HSERDY) RESET); RCC_PLLConfig(RCC_PLLSource_HSE_Div1, RCC_PLLMul_9); // 8MHz * 9 72MHz RCC_PLLCmd(ENABLE); while (RCC_GetFlagStatus(RCC_FLAG_PLLRDY) RESET); RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK); while (RCC_GetSYSCLKSource() ! 0x08); RCC_HCLKConfig(RCC_SYSCLK_Div1); // AHB 72MHz RCC_PCLK2Config(RCC_HCLK_Div1); // APB2 72MHz RCC_PCLK1Config(RCC_HCLK_Div2); // APB1 36MHz但定时器仍是 72MHz }还有一个必须打开的东西读保护不要开、写保护不要开但是 SWD 调试口一定要留。F103 默认 PA13/PA14/PA15/PB3/PB4 五个脚是 JTAG 功能其中 PA15 和 PB3 在很多板上被复用成普通 IO。如果你要用这两个脚必须关闭 JTAG 只保留 SWD否则这两个脚会一直输出 JTAG 信号你配成 GPIO 也没反应。代码就两行RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE); GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); // 关 JTAG留 SWD记住关了 JTAG 之后只能靠 SWD 下载所以 SWCLK 和 SWDIO 两个脚永远不能占作他用这一点在分配引脚的时候要一开始就规划好。2.3 定时器资源分配4 路 PWM 加 4 路编码器怎么塞STM32F103C8T6 总共就 TIM1 到 TIM4 四个定时器要做四路电机 PWM 和四路编码器接口资源是紧的。我的分配方案是TIM1高级定时器通道 1 到 4 输出 PWM对应引脚 PA8、PA9、PA10、PA11TIM2编码器接口模式通道 1 和 2对应引脚 PA0、PA1TIM3编码器接口模式对应引脚 PA6、PA7TIM4编码器接口模式对应引脚 PB6、PB7为什么这么分因为编码器接口模式只需要定时器的通道 1 和通道 2而 PWM 输出需要四个独立通道只有 TIM1、TIM2、TIM3、TIM4 有四个通道TIM1 有四个通用定时器也各有四个。TIM1 是高级定时器PWM 输出的功能最全用来做四路 PWM 最合适。剩下三个通用定时器各用一个编码器第四个编码器……等等这里只有三个通用定时器加上 TIM1 也就四个但 TIM1 已经被 PWM 占了。对F103C8T6 只有四个定时器四路 PWM 加四路编码器正好用满没有第二个编码器接口给 TIM1 用。所以方案是TIM1 出 PWMTIM2/3/4 做三路编码器第四个电机的编码器怎么办我用的做法是软件解码第四个编码器的 A/B 相接到两个普通 GPIO开外部中断或者用定时器中断轮询在中断里读两相电平查表得到方向累加计数。F103 的 GPIO 翻转速度是 50MHz电机 30:1 减速比下输出轴 200RPM 时编码器单相频率大概是 200/60 × 13 × 30 ≈ 1300Hz两相双边沿中断最多 5200 次每秒对 72MHz 的主控来说负担不大。实测下来 CPU 占用大概 3% 左右可以接受。如果不想用软件解码还有两个选择一是换 STM32F103RCT6 这类高容量型号多出 TIM5、TIM6、TIM7其中 TIM5 也支持编码器模式二是在编码器信号和主控之间加一片四路正交解码芯片比如 LS7366 或者国产替代通过 SPI 读计数。第二种方案成本高一点但最省 CPU做四轮以上底盘的时候很值。PWM 频率的选定也要说明一下。TB6612 的 PWM 输入支持到 100kHz但频率太高会有开关损耗和驱动发热。人耳听力上限大概 20kHz低于这个值电机线圈会发出明显的啸叫。所以我选择20kHz刚好在听觉上限附近实际听感上只有很轻微的电流声。计算方式TIM1 时钟 72MHz预分频系数设为 0那么自动重装载寄存器 ARR 72MHz / 20kHz - 1 3599。这个 3599 就是 PWM 满占空比对应的比较值后面 PID 输出的限幅上界就设它。void PWM_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_BaseInitStructure; TIM_OCInitTypeDef TIM_OCInitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1 | RCC_APB2Periph_GPIOA, ENABLE); // PA8~PA11 复用推挽输出 GPIO_InitStructure.GPIO_Pin GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11; GPIO_InitStructure.GPIO_Mode GPIO_Mode_AF_PP; GPIO_InitStructure.GPIO_Speed GPIO_Speed_50MHz; GPIO_Init(GPIOA, GPIO_InitStructure); TIM_BaseInitStructure.TIM_Prescaler 0; TIM_BaseInitStructure.TIM_Period 3599; // 20kHz TIM_BaseInitStructure.TIM_ClockDivision 0; TIM_BaseInitStructure.TIM_CounterMode TIM_CounterMode_Up; TIM_TimeBaseInit(TIM1, TIM_BaseInitStructure); TIM_OCInitStructure.TIM_OCMode TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OutputState TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse 0; TIM_OCInitStructure.TIM_OCPolarity TIM_OCPolarity_High; TIM_OC1Init(TIM1, TIM_OCInitStructure); TIM_OC2Init(TIM1, TIM_OCInitStructure); TIM_OC3Init(TIM1, TIM_OCInitStructure); TIM_OC4Init(TIM1, TIM_OCInitStructure); TIM_CtrlPWMOutputs(TIM1, ENABLE); // 高级定时器必须开这个否则没输出 TIM_Cmd(TIM1, ENABLE); }注意最后那个TIM_CtrlPWMOutputs只有 TIM1 和 TIM8 这类高级定时器需要调用通用定时器不用。这个函数不开PWM 口就是死的输出零电平很多人第一次用高级定时器做 PWM 会在这里卡半天。2.4 编码器接口模式配置与计数溢出处理编码器接口模式的原理其实不复杂定时器的通道 1 和通道 2 都配置成输入捕获把编码器的 A 相和 B 相接上去硬件根据两相的相位关系自动决定计数方向A 相超前 B 相 90 度就向上计数B 相超前就向下计数每来一个边沿计数一次。配置成双边沿计数就是四倍频一圈的计数是编码器线数乘四再乘减速比。void Encoder_TIM2_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_BaseInitStructure; TIM_ICInitTypeDef TIM_ICInitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); // PA0、PA1 浮空输入 GPIO_InitStructure.GPIO_Pin GPIO_Pin_0 | GPIO_Pin_1; GPIO_InitStructure.GPIO_Mode GPIO_Mode_IN_FLOATING; GPIO_Init(GPIOA, GPIO_InitStructure); TIM_BaseInitStructure.TIM_Prescaler 0; TIM_BaseInitStructure.TIM_Period 65535; // 16 位满量程 TIM_BaseInitStructure.TIM_ClockDivision TIM_CKD_DIV1; TIM_BaseInitStructure.TIM_CounterMode TIM_CounterMode_Up; TIM_TimeBaseInit(TIM2, TIM_BaseInitStructure); TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); TIM_ICInitStructure.TIM_Channel TIM_Channel_1; TIM_ICInitStructure.TIM_ICPolarity TIM_ICPolarity_Rising; TIM_ICInitStructure.TIM_ICSelection TIM_ICSelection_DirectTI; TIM_ICInitStructure.TIM_ICPrescaler TIM_ICPSC_DIV1; TIM_ICInitStructure.TIM_ICFilter 6; // 数字滤波抗毛刺 TIM_ICInit(TIM2, TIM_ICInitStructure); TIM_ICInitStructure.TIM_Channel TIM_Channel_2; TIM_ICInit(TIM2, TIM_ICInitStructure); TIM_SetCounter(TIM2, 0); TIM_Cmd(TIM2, ENABLE); }TIM_ICFilter这个参数我强烈建议设成 6 或者更大范围 0 到 15。它的作用是在硬件层面对输入信号做数字滤波采样几次都一致才认。霍尔编码器在有刷电机上工作电机换向时的火花会产生毛刺滤波设小了会出现莫名其妙的计数跳变一圈多出来几百个计数PID 直接就疯了。设 6 的代价是引入大约 6 个采样周期的延迟对 5ms 的采样周期来说完全可以忽略。读取计数有个必须处理的细节定时器是 16 位的范围 0 到 65535超过就回绕。两个采样周期之间的差值直接用short类型强制转换就能自动处理回绕因为补码运算的性质天然满足模运算。#define ENC_CPR 1560.0f // 13 线 * 4 倍频 * 30 减速比 static int16_t last_cnt[4] {0}; static float rpm[4] {0}; void Encoder_Sample(void) { uint16_t now; int16_t delta; now TIM_GetCounter(TIM2); delta (int16_t)(now - last_cnt[0]); // 用 short 自动处理溢出 last_cnt[0] now; rpm[0] (delta / ENC_CPR) / SPEED_DT * 60.0f; // TIM3、TIM4、软件解码的第四路同理 }这里的SPEED_DT是采样周期单位是秒。5ms 就是 0.005。注意读计数器的时候不要中途清零很多人习惯每次读完TIM_SetCounter(TIMx, 0)这样做虽然也能算出速度但一旦某个采样周期漏掉一次比如中断被高优先级任务打断速度就完全错了而且没法察觉。用差值法最稳。3. 轮速闭环从编码器计数到稳定的 RPM3.1 转速测量方法的选择与分辨率评估测速方法有三种M 法、T 法、M/T 法课本上讲得很多但对麦轮底盘这个具体场景我再结合实际参数算一遍。M 法是在固定的时间窗口内数编码器脉冲个数。优点是实现简单、低速时也能出结果缺点是采样周期固定速度分辨率随采样周期变化。我这套参数下的分辨率怎么算编码器一圈 1560 个计数采样周期 5ms那么最快的计数速度发生在什么转速如果电机输出轴跑到 300RPM也就是 5 转每秒每秒产生 7800 个计数5ms 内是 39 个计数。反过来说一个计数对应的速度分辨率就是 1 / 1560 / 0.005 × 60 7.7 RPM。这个精度够不够轮子直径 60mm周长 188.5mm7.7RPM 对应线速度 7.7/60 × 0.1885 0.024 m/s也就是 2.4 厘米每秒。在低速比如 0.1m/s时这个量化误差已经到 24% 了确实不够理想。T 法是反过来测两个脉冲之间的时间。高速时时间很短定时器分辨率不够所以 T 法适合低速。M/T 法是高频时钟同时计脉冲数和计时速度低的时候自动切换到测时间速度高的时候自动切换到数脉冲全速域都能保证精度但要占用额外的定时器资源。我的实际取舍是低速精度要求高的场景比如厘米级位置控制把采样周期从 5ms 延长到 20ms 用 M 法这样分辨率降到 1.9RPM线速度分辨率 0.6 厘米每秒可以接受。同时给速度加一阶低通滤波。位置控制的时候底盘速度本来就不高20ms 的控制周期对整机动态影响不大。如果你做的是高速竞速类机器人那就上 M/T 法或者直接换高线数编码器500 线编码器配 30 减速比是 60000 计数一圈5ms 采样时的分辨率直接降到 0.2RPM随便怎么调都够。滤波这一步不能不提。编码器是通过机械联轴器接在电机轴上的联轴器有间隙、齿轮有回差、霍尔元件本身也有量化误差所以原始速度信号必然是抖的。我的做法是一阶低通// alpha 越小越平滑但延迟越大。0.3~0.5 是比较好用的区间 rpm_filt[i] rpm_filt[i] * (1 - SPEED_LPF_A) rpm_raw[i] * SPEED_LPF_A;取SPEED_LPF_A 0.35实测在 5ms 周期下速度信号的抖动从正负 15RPM 降到正负 3RPM 左右而阶跃响应的延迟大约增加了 2 个周期10ms对轮速环完全可以接受。如果你发现 PID 怎么调都振荡先把滤波系数调小试试很多时候问题就出在这。3.2 位置式 PID 的工程实现与抗积分饱和轮速环我用位置式 PID不用增量式。原因很实际位置式的输出就是 PWM 占空比本身物理意义明确调试的时候可以直接看输出值判断有没有饱和增量式的输出是占空比的增量一旦某个时刻出现异常增量前面的积累全乱了排查起来麻烦。结构体定义和计算函数typedef struct { float Kp, Ki, Kd; float target; // 目标速度 RPM float integral; // 积分累加项已经乘过 Ki float last_meas; // 上次测量的速度用于微分先行 float out; // 本次输出 float out_max, out_min; float integral_max; } SpeedPID_t; float SpeedPID_Calc(SpeedPID_t *pid, float meas, float dt) { float err pid-target - meas; float d; // 积分项先累加再限幅 pid-integral pid-Ki * err * dt; if (pid-integral pid-integral_max) pid-integral pid-integral_max; if (pid-integral -pid-integral_max) pid-integral -pid-integral_max; // 微分先行对测量值求微分避免目标值突变时产生微分冲击 d -pid-Kd * (meas - pid-last_meas) / dt; pid-last_meas meas; pid-out pid-Kp * err pid-integral d; if (pid-out pid-out_max) pid-out pid-out_max; if (pid-out pid-out_min) pid-out pid-out_min; return pid-out; }这里有两个设计选择要解释。第一是积分限幅。底盘在遇到障碍物堵转的时候目标 100RPM实际 0RPM误差一直是 100积分项会一路涨到天上。等你把障碍物挪开积分项要花好几秒才能泄放掉车会猛地窜出去。限幅值取多少合适我一般取out_max的 60% 左右这样不会因为限幅太死导致静态误差消不掉也不会积太久。第二是微分先行。标准 PID 的微分项是对误差求微分误差 目标 - 测量当目标值发生阶跃比如从 0 突然跳到 100RPM微分项瞬间变成一个极大的值输出直接打到饱和电机哐一下窜出去。改成对测量值求微分目标值阶跃时微分项不受影响冲击就没了。轮速环对这点其实不太敏感但位置环的目标速度是连续变化的用了微分先行的代码在两层复用的时候更省事。还有一个绕不开的问题是死区补偿。直流电机在 PWM 占空比低于某个阈值时根本转不起来因为静摩擦力还没被克服。这个阈值实测下来大概在 10% 到 15% 之间不同电机差别挺大而且会随电池电压下降而变化。如果不处理PID 在目标速度很低的时候会拼命往上加输出加到 12% 电机突然启动速度一下冲过头然后再降下来反复循环表现为车在原地点头。处理方法有两种。简单的是输出死区补偿#define PWM_DEADZONE 0.12f // 12% 静摩擦阈值实测标定 #define PWM_FULL 3599.0f float deadzone_comp(float out) { float mag fabsf(out) / PWM_FULL; if (mag 1e-3f) return 0.0f; mag PWM_DEADZONE mag * (1.0f - PWM_DEADZONE); return (out 0 ? mag : -mag) * PWM_FULL; }补偿之后相当于把死区从系统里挖掉PID 的输出线性度好很多。更讲究的做法是前馈补偿根据目标速度直接算出一个基础占空比PWM_ff k1 * target_rpm k2 * sign(target_rpm)PID 只负责修正误差这样即使 PID 参数一般跟踪效果也很好。标定 k1 的方法很简单给固定占空比等速度稳定后记录占空比和 RPM多测几个点做线性拟合斜率就是 k1截距就是 k2。3.3 参数整定的实操顺序和典型值PID 整定这事没有万能公式但有可靠的流程。我的顺序是这样的第一步先把 Kp 单独调出来。Ki 和 Kd 都设 0目标速度给一个阶跃比如从 0 跳到 150RPM用串口把目标值和实际值以 5ms 周期打到上位机上看曲线。Kp 从 1.0 开始每次翻倍。当曲线出现明显的过冲超过目标值 20% 以上并且开始有几下振荡说明 Kp 差不多了往回退到过冲 5%~10% 的那个值。我这套电机的典型 Kp 在 8 到 15 之间取决于供电电压和车速。Kp 太小会怎样响应很慢从 0 到 150RPM 要 200ms 以上而且稳态下永远差那么几个 RPM。第二步加 Ki 消静态误差。单独用 P 控制稳态下一定会有误差因为要维持一定转速必须有非零的占空比输出。Ki 从 Kp 的 1/10 开始加观察静态误差是不是在一个采样周期内被消掉。注意 Ki 是加在积分累加里的系数量纲和 Kp 不一样代码里我写的是integral Ki * err * dt所以它的单位是输出/误差·秒。典型值取 Kp 的 0.5 到 2 倍再用 dt 归一化。Ki 加太大的表现是低速时电机发出嗡嗡的声音转速在目标值上下低频摆动这就是积分振荡。第三步Kd 基本不用。轮速环的反馈信号本身已经有滤波延迟再加微分只会放大噪声。我那套参数最终是 Kp 12Ki 0.9Kd 0四个轮子因为负载不同略有差异差在正负 15% 以内。四个轮子的参数最好分开标定特别是如果电机批次不一样共用一组参数会导致某个轮子响应总是慢半拍车就走不直。整定时的几个注意事项都是踩出来的调试的时候一定要把车架起来让轮子悬空。落地整定会引入地面摩擦力这个变负载参数在悬空和落地状态下差别很大落地标定虽然更准但车会到处跑很危险也麻烦。我的做法是先悬空调出大致参数再落地微调。每次改完参数一定要把数据打到上位机上看别光靠耳朵听。耳朵能分辨的只是有没有啸叫分辨不出来 5% 的过冲和稳态误差光凭感觉调很容易越调越乱。电池电压会随着放电下降从满电 12.6V 到低压 10.5V同样的占空比对应转速差 20%。所以如果你的 PID 参数是在满电调的跑到后半程电量车就会明显变软。讲究一点的做法是加电池电压采样把占空比做电压归一化或者干脆用带前馈的方案对电压变化不敏感。4. 麦轮运动学与整车控制4.1 逆运动学把 vx、vy、ω 拆成四个轮速这部分是整个项目里最容易出错的地方因为它涉及坐标系的约定只要你对约定和实际硬件的对应关系没理清就会出现横移方向反了自转方向反了这类问题而且排查起来毫无头绪。先定义坐标系。车体坐标系x 轴指向车头正前方y 轴指向车体左侧ω 定义为逆时针为正从上往下看。速度单位统一用米每秒ω 用弧度每秒。设车体几何中心为原点每个轮子中心到原点的纵向距离是 L前后方向横向距离是 W左右方向。注意麦轮的旋转力矩臂不是单独的 L 或 W而是k L W这个结论来自辊子 45 度安装的几何投影。轮子半径记作 R。那么四个轮子的线速度可以写成// L: 半轴距前后轮中心到几何中心的纵向距离单位 m // W: 半轮距左右轮中心到几何中心的横向距离单位 m // R: 轮子半径单位 m float k L W; v_wheel[0] vx - vy - k * wz; // 左前 v_wheel[1] vx vy k * wz; // 右前 v_wheel[2] vx vy - k * wz; // 左后 v_wheel[3] vx - vy k * wz; // 右后 // 转成角速度交给轮速环rad/s for (int i 0; i 4; i) { target_rpm[i] v_wheel[i] / R * 60.0f / (2 * 3.1415926f); }我用一个表格把三个纯运动的分解结果列出来对着这个表去验证硬件最直观运动模式输入左前右前左后右后车体表现前进vx1, vy0, ω0沿车头方向平移左移vx0, vy1, ω0−−车头不变整体向左平移右移vx0, vy−1, ω0−−车头不变整体向右平移左自转vx0, vy0, ω1−−原地逆时针旋转右自转vx0, vy0, ω−1−−原地顺时针旋转斜向左前vx1, vy1, ω000沿 45 度方向平移看左移那一行左前和右后反转右前和左后正转。这个组合看起来有点反直觉很多人第一次做的时候会想当然地认为左移应该是左边两个轮子正转、右边两个反转结果做出来车在原地打转。原因是辊子的 45 度安装把力分解到了两个方向上横向平移靠的是对角线上两个轮子的推力分量叠加。上电第一件事是标定符号。因为麦轮有左旋右旋之分电机接线也可能反所以公式里的符号和实际硬件不一定对得上。我的做法是一步步验证给四个轮子分别单独发正转指令用手摸或者贴张纸条看确认每个轮子正转时轮子确实是向前滚的方向对应车体前进方向。如果某个轮子装反了先把它的电机线正负极对调。然后按前进模式统一给正转看整车是不是向前走。如果向后走说明四个轮子的正负号整体反了把v_wheel[]的整体符号取反。按左自转模式测试。如果车向右转说明 ω 那两项的符号整体反了把公式里所有k * wz的符号取反。最后测左移。如果车向右平移说明 vy 那两项的符号反了。这个情况和上一条是独立的要单独调。也可以在硬件上把左右旋轮对调位置解决。这四步做完符号体系就完全对了。不要跳过星号那一步我见过太多人公式抄对了但因为一个轮子装反而导致整个底盘行为诡异然后开始在 PID 里找问题越找越偏。4.2 正运动学与里程计积分从轮速反推位姿逆运动学是给定车体速度算轮速正运动学是反过来用四个轮子的实际速度反推车体的实际运动。里程计就是把这个过程随时间的积分。对上面那组方程做线性组合反解得到float vx_meas ( v[0] v[1] v[2] v[3]) * 0.25f; float vy_meas (-v[0] v[1] v[2] - v[3]) * 0.25f; float wz_meas (-v[0] v[1] - v[2] v[3]) / (4.0f * k);注意这里用的是实际测得的轮速不是目标轮速。用目标速度做里程计等于在自欺欺人车打滑了你也看不出来。得到车体速度之后怎么积分成位姿最朴素的做法是直接乘以 dt 累加但误差大。更好的是中点法在一个积分周期内用起始角和终止角的平均值作为整个周期的朝向再用这个平均朝向把车体速度旋转到世界坐标系。void Odometry_Update(float vx, float vy, float wz, float dt) { float dtheta wz * dt; float theta_mid pose.theta dtheta * 0.5f; // 中点法 float c cosf(theta_mid), s sinf(theta_mid); pose.x (vx * c - vy * s) * dt; pose.y (vx * s vy * c) * dt; pose.theta dtheta; // 角度归一化到 [-PI, PI]防止积分到很大之后三角函数精度下降 while (pose.theta PI) pose.theta - 2 * PI; while (pose.theta -PI) pose.theta 2 * PI; }里程计的误差来源有三个按影响大小排序第一是轮子半径 R 的标定误差。R 如果标大了 2%所有位移都会小 2%走到 3 米外就差 6 厘米。标定方法很土但管用在地上画一条 3 米的直线让车走完读里程计输出的位移用R_真实 R_当前 × 3.0 / 里程计读数修正。第二是 k 值也就是 L W的标定误差。这个主要影响自转时的角度解算。标定方法是让车原地转十圈读里程计累计的角度调整 k 让读数接近 20π。注意每次修改 k 之后逆运动学里的 k 也要同步改因为两者必须一致否则会出现走直线没问题但原地转角度差很多的现象。第三是打滑。这个没法通过标定解决只能靠外部传感器。最便宜的方案是加一个 MPU6050用陀螺仪的 z 轴角速度对时间积分来算航向角和里程计航向用互补滤波或者卡尔曼滤波融合。陀螺仪的优势是不受打滑影响缺点是会漂移零偏积分时间长了角度就飘了里程计的优势是长时间不发散缺点是一打滑就错。两者融合正好互补。// 互补滤波系数 0.02 意味着每 10ms 修正 2% 的陀螺仪零偏 pose.theta (1 - 0.02f) * (pose.theta gyro_z * dt) 0.02f * odom_theta;4.3 位置控制外环怎么串进去为什么这么串有了可靠的轮速内环和里程计位置控制就是在外层再套一层环。这里要先明确位置控制有两种完全不同的目标形态不能混为一谈。形态一纯航向保持。就是以某个速度和方向走直线不许跑偏。这个最简单也最常用外层只有一个航向 PID// 输入目标航向 theta_ref当前航向 pose.theta // 输出wz float yaw_pid(float theta_ref, float theta_now) { float err theta_ref - theta_now; while (err PI) err - 2 * PI; // 角度误差必须归一化 while (err -PI) err 2 * PI; return yaw_pid_calc(g_yaw_pid, err, CTRL_DT); }这个角度归一化是必须的如果忘了写一旦车转过 ±180° 附近误差会突然从 179° 跳到 -181°PID 会瞬间输出一个反向的极大值车会剧烈地甩一下。我踩过这个坑当时以为是陀螺仪坏了排查了两个小时。形态二点到点位置控制。给定目标坐标 (x_ref, y_ref)让车自己走过去。完整的做法是把世界坐标系的坐标误差旋转到车体坐标系然后在车体坐标系里分别做两个 PIDvoid Position_Loop(void) { float dx x_ref - pose.x; float dy y_ref - pose.y; float c cosf(pose.theta); float s sinf(pose.theta); // 世界系误差 - 车体系误差 float ex dx * c dy * s; // 车头方向上的误差 float ey -dx * s dy * c; // 车体左侧方向上的误差 // 距离足够近就停 float dist sqrtf(dx * dx dy * dy); if (dist 0.02f) { // 2cm 容差 vx_ref vy_ref wz_ref 0; g_pos_reached 1; return; } vx_ref pos_pid_x(ex); vy_ref pos_pid_y(ey); wz_ref yaw_pid(theta_ref, pose.theta); // 保持车头朝向 // 限速避免离得远的时候冲刺 float vmax 0.4f; if (fabsf(vx_ref) vmax) vx_ref (vx_ref 0 ? vmax : -vmax); if (fabsf(vy_ref) vmax) vy_ref (vy_ref 0 ? vmax : -vmax); }三层环的周期怎么分配这是关键。轮速环 5ms位置环 20ms中间那个航向/速度分配放在哪里我的做法是位置环20ms算出 vx_ref、vy_ref、wz_ref 之后直接通过逆运动学换算成四个轮子的目标 RPM交给 5ms 的轮速环去跟踪。中间不加环理由是这样最简单而且够用——轮速环的响应时间在 50ms 以内比位置环的 20ms 周期长但这不算问题因为位置环本身就是在做慢速调节两个环的时间常数差 4 倍已经足够分开了。误区提醒位置环的 PID 参数要比轮速环小得多。我见过有人把位置环的 Kp 也设到 10 以上结果就是车在目标点附近来回画圈、永远停不下来。位置环的 Kp 典型值在 0.5 到 2 之间输出单位是米每秒输入单位是米Ki 在 0.1 到 0.5 之间。还有一个常被忽略的点位置环的积分项在接近目标点的时候必须清掉否则车到了目标点积分项还积累着一个不小的值车会冲过去然后再回来来回振荡。调试点到点控制的时候我建议按这个顺序验证阶段测试内容通过标准1只做航向保持手动推车旋转车主动往回收回正过程平稳不过冲2只做 vx 方向点到点目标在正前方走 1 米停止误差小于 2cm无过冲3只做 vy 方向点到点目标在正左方横向平移 1 米误差小于 3cm4任意角度目标点走 1.5 米终点误差小于 5cm5连续多点巡线每个点误差小于 8cm无累积偏差横向平移那一步误差会比纵向大因为麦轮横移时的有效摩擦特性和纵向不同会有轻微的打滑这个属于正常现象。如果横向误差超过 5cm先检查是不是某个轮子的死区补偿没做好导致低速起步时四个轮子响应不一致。5. 调车现场那些文档里不会写的坑5.1 编码器读数异常的四类根因编码器问题是我遇到最多的一类表现都是计数不对但根因差别很大我用一个表把它们分开现象根因排查方法解决计数偶尔跳变一个很大的数电机换向火花干扰示波器看编码器 A/B 波形是否有毛刺提高TIM_ICFilter编码器线加磁环远离电机线走线车不动但计数一直在涨输入引脚悬空采集到噪声直接量引脚电平把 GPIO 模式从浮空输入改成上拉输入或者接外部上拉电阻已知转速下计数偏小A 或 B 相没接好缺相单独转电机看两个通道有没有信号检查线序和焊点霍尔编码器供电 3.3V/5V 要确认一次采样读到极大值中断里读数被更高优先级中断打断加标志位或者临时关中断把编码器采样放在优先级最高的定时器中断里还有一个隐藏很深的问题编码器采样函数被放在了会阻塞的地方。比如你有个delay_ms()在某个流程里如果这个流程和编码器采样在同一个中断里采样就会被推迟采样间隔不稳定算出来的速度就会抖。所以速度采样一定要放在固定的定时器中断里而且这个中断的优先级要设高其他任务的耗时操作绝对不能放进去。另外提一个容易忽视的点上电初始相位。编码器上电时计数器是 0但实际上电机轴可能停在任意角度。这不影响速度测量我们用的是差值但如果你要做单轮绝对位置控制就需要在初始化时加一个机械零点校准流程——让轮子慢速转到触发一个光电或者霍尔限位然后清零。麦轮底盘通常不需要这么做因为我们关心的是相对位移但如果你做的是机械臂这类需要绝对零点的这一点必须处理。5.2 电机啸叫、抖动、跑偏的排查路径啸叫。声音尖锐、频率固定一般是 PWM 频率落在可听范围内。20kHz 是人耳听力的临界如果你的 PWM 频率设成了 10kHz 以下一定会听到明显的啸叫。先在代码里确认 ARR 和预分频算出来的频率对不对特别注意前面提到的定时器时钟倍频问题——如果你按 36MHz 算 TIM2 的频率实际输出会是 20kHz 而不是你以为的 10kHz。反过来如果你希望是 20kHz 但实际是 40kHz驱动芯片可能发热严重。如果频率算对了还是叫那可能是机械共振。表现是只在某个特定转速区间叫其他转速正常。这种可以试着改变 PWM 频率比如从 20kHz 调到 22kHz或者调到 16kHz避开共振点。还有一种情况是电源纹波引起的电机电流突变时电池电压跌落会通过电源耦合到控制电路表现是车速越快叫得越厉害。解决办法是在电机电源端并一个大容量电解电容470uF 以上加一个小容量陶瓷电容。抖动。抖动和啸叫不一样是低频的、肉眼可见的车轮来回摆。绝大多数情况下是 PID 参数问题Kp 太大、积分振荡或者微分项放大了噪声。但也别忽略机械因素——联轴器松动、轮子和轴的配合有间隙、车架刚性不够这些都会表现为抖动。判断方法很简单把 PID 输出固定成一个常数占空比不开闭环如果还抖那就是机械或者电源问题跟控制无关。跑偏。走直线的时候往一边偏三个层次的排查顺序第一层是参数一致性。四个轮子的 PID 参数是不是一样编码器分辨率标定是不是一样电机本身的一致性如何我遇到过一台车四个电机是同型号但不同批次同一组 PID 参数下有一个轮子的稳态转速比其他三个低 8RPM导致车慢速时往那边偏。解决办法是每个轮子单独标定一组 PID或者更彻底一点用前馈加闭环的方案让 PID 只负责小范围修正。第二层是机械对称性。四个轮子的安装高度是否一致有没有哪个轮子略微悬空用手转一下四个轮子感受阻力是否均匀。麦轮底盘对这点特别敏感因为一旦有个轮子悬空它在运动学上提供的分量就完全消失了正运动学解算出来的 vy 会直接错掉。第三层是运动学标定。L 和 W 是不是准确轮子半径是不是准确地面是不是足够平用尺子量一下四个轮子中心到车体几何中心的距离如果左右不等那 vy 解算一定有问题。这个几何误差比参数误差更基础也更难发现因为它不会导致车明显跑偏而是导致横移方向有偏差——你让它往左平移它实际走的是左前方 15 度走一米就偏出去 25 厘米。5.3 常见问题速查表调试过程中的问题太多我把最耗时间的几个整理成速查表方便对照症状最可能的原因快速验证处理上电后电机不转TIM1 没开 PWM 输出量 PWM 引脚有没有波形调TIM_CtrlPWMOutputs(TIM1, ENABLE)电机只能往一个方向转方向控制引脚没配分别给正反转指令量方向脚电平检查 TB6612 的 AIN1/AIN2 接线和配置速度环一直饱和编码器方向反了负反馈变成正反馈给正转指令看速度读数是正是负交换编码器 A/B 相接线或改计数符号车横移时原地打转vy 项符号错按 4.1 节第四步验证对调两个轮子的左右旋位置或改符号原地转角度差很多k 值标定不准转十圈看里程计角度偏差重新标定 k同时修改逆解和正解走 3 米偏 10cm轮径标定不准或机械不对称分别测量四个轮子的实际滚动距离重新标定 R检查轮子气压/磨损位置控制停不下来位置环积分没清或 Kp 太大观察接近目标点时的速度曲线加到达判定清积分减小 Kp低速时车点头死区补偿没做或标定不准逐步加大占空比看电机从几伏开始转重新测量死区阈值或加前馈串口数据偶尔乱码波特率偏差或者丢字节空闲时看是否正常跑起来才乱检查时钟配置加帧头帧尾和校验烧录后程序不跑启动文件选错型号看是否卡在 HardFault确认用的是startup_stm32f10x_md.s串口这一条我多讲两句。速度环调试的时候数据量不小四个轮子的目标值和实际值加时间戳5ms 一次就是 9 个 float如果每个都用文本格式打印波特率 115200 根本不够而且文本转换本身很耗时。我的做法是二进制协议帧头两字节 0xAA、0x55一个字节长度然后是数据体最后两个字节的简单校验和。上位机用 Python 解析成波形比文本快十倍不止。实测下来 5ms 周期发 40 字节115200 波特率占用不到 40%还有余量。还有个小技巧调试期间把关键状态打到 OLED 上。有时候现场没带电脑OLED 能显示当前速度、PID 输出、编码器计数这些值效率比连串口高。我一般留一页显示四个轮子的目标/实际速度一页显示里程计位姿出现问题扫一眼就知道是哪一层的问题。这台底盘我从建工程到跑通位置控制前后折腾了大概三周其中一半时间花在标定和排查硬件上真正写控制算法的代码量其实不大——轮速 PID 三十行运动学解算二十行里程计三十行加起来不到两百行核心代码。难度不在代码在于把每一层的误差控制在可接受的范围内让它们叠加起来还能满足要求。后续如果想把精度再往上推我的建议是加一个下视光流模块直接测地面位移和里程计做融合能把打滑造成的误差干掉一大半或者上 ROS 做多传感器融合代价是开发复杂度直线上升看你的实际需求值不值。 STM32麦轮底盘轮速闭环与运动学位置控制实战
麦轮底盘第一次上手会让人有点上头四个轮子摆在那里既能横着走、又能斜着走、还能原地打转比两轮差速的小车灵活太多。可真到自己动手搓的时候坑一个接一个——编码器读数在跳、PID 稍微加大一点电机就开始啸叫、走直线走个两米就歪了十几公分、标定完车轮半径发现原地转一圈角度差了快二十度。这篇就把我从建 STM32 工程开始一路到轮速闭环、麦轮运动学解算、位置控制的整套流程完整摊开参数怎么定、为什么这么定、踩过哪些坑全部写清楚。适合已经会点 C 语言、玩过一阵单片机、想把移动机器人底盘真正做扎实的人看如果你只是想让小车能动起来前半部分也够用了。1. 先想清楚这台麦轮底盘到底要解决什么问题1.1 为什么选麦轮而不是差速或者全向轮很多人一上来就问麦轮是不是比差速好这个问题本身就不成立它们解决的是不同的需求。两轮差速底盘只有两个自由度——前进和转向想横着挪一米你得先转 90 度、走一米、再转回来在窄通道里根本施展不开。麦轮底盘给的是完整的三个平面自由度纵向、横向、自转三者可以任意叠加理论上车体可以沿着平面内任意方向平移同时保持车头朝向不变。这个能力在什么场景下值钱一是空间受限的场景比如货架之间只有 90 公分宽的 AGV二是需要精细对位的场景比如车头必须始终朝向某个方向的抓取平台三是比赛类场景尤其是那些需要快速绕桩、走 8 字、走复杂轨迹的机器人。代价也要讲明白。麦轮的辊子是和地面成 45 度斜着接触的力的传递效率天然比普通橡胶轮低常见的经验值是有效效率大概在 70% 上下同样的电机推同样重量的车麦轮需要的扭矩大概要多出三成。辊子和地面的接触面积小对地面平整度和摩擦系数都很敏感地毯、缝隙、砂石地面基本就别想了。还有一点容易被忽略麦轮的运动学模型是建立在轮子不打滑的理想假设上的一旦某个轮子悬空或者打滑正运动学解算出来的里程计位姿会直接跑飞而且没有任何自我纠正的能力。所以我的判断逻辑是如果你的车只需要在地面上跑直线加转向老老实实用差速结构简单、成本低、扭矩利用率高如果你确实需要横向平移这个能力而且地面是环氧地坪或者瓷砖这类硬质平整地面那麦轮就是对的。至于全向轮万向轮它的辊子是垂直于轮面、绕轮缘一圈布置的横向能力更强、运动学更简单但每个辊子受力时的冲击大、噪音大、承载能力差重载场合还是麦轮更合适。1.2 整机硬件配置与选型理由这套配置我是按能跑、能调、成本可控三个条件选的不是最便宜也不是最强但每个零件的选型都有理由。部件型号/规格选型理由主控STM32F103C8T6 最小系统板72MHz 主频4 个通用定时器 1 个高级定时器够用资料多出问题好查电机JGB37-520 直流减速电机12V减速比 1:30带 13 线霍尔编码器自带编码器省事扭矩够推 3~5kg 车体轴径 6mm 配 60mm 麦轮正合适驱动TB6612FNG 双路驱动模块 ×2单路持续 1.2A、峰值 3.2APWM 频率能上 100kHz逻辑电平 3.3V 兼容电源3S 锂聚合物电池 11.1V 2200mAh满电 12.6V放电倍率够重量轻降压5V/3A DC-DC 模块给主控和其他逻辑电路供电注意别用 LDO 硬扛压差大发热严重轮子60mm 麦轮 ×4左旋、右旋各两个直径小转弯半径小四个轮子必须区分左右旋车体铝合金或者亚克力板轴距 150mm × 轮距 160mm底盘刚性很重要软的车架会让里程计发散姿态MPU6050 六轴位置控制里做航向角融合纯靠里程计算航向会漂电机这块我多说两句。JGB37-520 这套组合的编码器是 13 线霍尔编码器A/B 两相正交输出四倍频之后是 52 个计数再乘 30 的减速比电机输出轴转一圈对应 1560 个计数。这个分辨率在 M 法测速下够不够用后面第三章会详细算。如果你预算宽裕直接上带 500 线光电编码器的电机一圈 60000 个计数低速时的速度分辨率会好一个数量级PID 也更好调。轮子的左右旋千万别搞错。麦轮分左旋轮和右旋轮从轮子正上方往下看辊子从轮轴处向外延伸的方向不同。标准做法是装成X 型左前和右后是一组右前和左后是另一组四个轮子的辊子接触线在地面上呈现出一个 X。装错了会怎样车能走但横移方向和你算的完全相反而且四个轮子会互相较劲跑起来声音很大。识别方法很简单手按住车体往前推看辊子是不是顺滑地滚如果你能明显感觉到卡顿或者需要很大力多半是装反了。1.3 软件分层为什么一定要分三层我见过太多人把整个工程写在一个 main.c 里初始化一堆外设然后在 while(1) 里塞满逻辑调着调着就调不动了——改一个 PID 参数结果串口发送也乱了加一个功能编码器读数就开始丢。根本原因是所有代码耦合在一起时序完全不可控。我的做法是分三层边界划清楚最底层是驱动层只干一件事把硬件抽象成函数。Motor_SetPWM(uint8_t id, int16_t duty)、Encoder_ReadDelta(uint8_t id)、UART_SendBuf()这些函数不包含任何控制逻辑换一块主控只改这一层。帕中间是控制层运动学解算、PID 计算、里程计积分都在这里。这一层的所有状态都存在全局或静态结构体里输入是目标值和反馈值输出是 PWM 占空比跟硬件没关系可以拿到 PC 上做单元测试。最上面是应用层负责指令解析、状态机、调度。遥控器发过来的速度指令怎么变成 vx/vy/ω什么时候切到位置控制模式这些决策逻辑放这里。分层的实际好处是调试效率。举个真实例子有次车跑起来往一边偏我先怀疑 PID改了半天没用后来把里程计数据通过串口以 20ms 周期打出来发现四个轮子的编码器计数有一个明显偏小——问题在硬件是那个电机轴上的联轴器螺丝松了。如果代码揉在一起光是把数据打出来这一步就要折腾半小时。2. STM32 工程从零建起来的正确姿势2.1 标准库还是 HAL 库先把账算清楚这个话题争论很多我只说我的实际取舍。STM32F103C8T6 这个级别的芯片标准库和 HAL 库都能跑但差别在哪HAL 库 CubeMX 的优势是上手快图形化配置引脚和时钟半天就能点灯。劣势也很明显一是代码体积大一个空工程编译出来 flash 占用能到标准库的两三倍二是抽象层厚HAL_GPIO_WritePin()这种函数执行一次要几十个时钟周期在 5ms 一次的控制中断里频繁调用会明显吃掉裕量三是封装的回调机制在需要精确时序的场合不好用比如你要在一个中断里同时读三个定时器的计数器HAL 的句柄结构访问起来绕。标准库的优势正好相反寄存器级操作函数基本就是几条汇编执行快、代码小、时序可控。劣势是资料相对老新芯片型号支持差而且 CubeMX 不能生成标准库工程。我的结论是做电机控制、编码器采集这类对时序敏感的项目用标准库做联网、文件系统、USB 这类偏应用的用 HAL。这套麦轮底盘我会用标准库。如果你已经习惯了 HAL 也不是不能做但速度环的采样周期建议从 5ms 放宽到 10ms给中断留出余量。新建标准库工程的步骤简单列一下很多教程漏掉的细节我标出来从芯片厂商官网或者开源社区下载 STM32F10x 标准外设库版本选 3.5.0这是最成熟的。新建工程目录建Start启动文件、CMSIS 内核文件、Library标准外设库源码、User自己写的代码、Hardware外设驱动四个文件夹。Keil 里新建 Target添加startup_stm32f10x_md.s注意中容量是md不是hd选错了链接会报错找不到中断向量。添加库文件时有个坑stm32f10x_tim.c、stm32f10x_gpio.c、stm32f10x_rcc.c、stm32f10x_usart.c、misc.c这几个是必加的但stm32f10x_it.c里的中断服务函数会和你自己写的重名我一般直接用自己写的不引入库里的这个文件。在stm32f10x.h或者工程的预编译宏里定义USE_STDPERIPH_DRIVER和STM32F10X_MD这两个宏不定义会出现一堆未定义符号很多人卡在这里。勾选 Create HEX File调试器选 ST-Link Debugger在 Debug 设置里把 Reset and Run 勾上省得每次烧完还要手动复位。2.2 时钟树配置那个被忽略的定时器倍频时钟树配置看起来是 CubeMX 一键搞定的事用标准库就得手写但这里有个细节我认为必须讲清楚因为它直接决定你的 PWM 频率算得对不对。F103 的外部晶振一般是 8MHz经过 PLL 九倍频得到 72MHz 的 SYSCLK。然后 AHB 不分频还是 72MHz。APB2 预分频系数是 1所以 APB2 也是 72MHz高级定时器 TIM1 挂在 APB2 上时钟是 72MHz。APB1 预分频系数是 2所以 APB1 是 36MHzTIM2、TIM3、TIM4 挂在 APB1 上。这里就是关键当 APB 预分频系数不为 1 时挂在该总线上的定时器时钟会自动乘以 2。所以 TIM2/3/4 的实际计数时钟不是 36MHz而是 72MHz。这个规则在参考手册的时钟树图里有标注但是画得很小很多人第一次看根本注意不到结果算 PWM 频率的时候按 36MHz 算实际输出频率差了一倍电机啸叫得厉害还找不到原因。配置代码大致是这样void RCC_Config(void) { RCC_DeInit(); RCC_HSEConfig(RCC_HSE_ON); while (RCC_GetFlagStatus(RCC_FLAG_HSERDY) RESET); RCC_PLLConfig(RCC_PLLSource_HSE_Div1, RCC_PLLMul_9); // 8MHz * 9 72MHz RCC_PLLCmd(ENABLE); while (RCC_GetFlagStatus(RCC_FLAG_PLLRDY) RESET); RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK); while (RCC_GetSYSCLKSource() ! 0x08); RCC_HCLKConfig(RCC_SYSCLK_Div1); // AHB 72MHz RCC_PCLK2Config(RCC_HCLK_Div1); // APB2 72MHz RCC_PCLK1Config(RCC_HCLK_Div2); // APB1 36MHz但定时器仍是 72MHz }还有一个必须打开的东西读保护不要开、写保护不要开但是 SWD 调试口一定要留。F103 默认 PA13/PA14/PA15/PB3/PB4 五个脚是 JTAG 功能其中 PA15 和 PB3 在很多板上被复用成普通 IO。如果你要用这两个脚必须关闭 JTAG 只保留 SWD否则这两个脚会一直输出 JTAG 信号你配成 GPIO 也没反应。代码就两行RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE); GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); // 关 JTAG留 SWD记住关了 JTAG 之后只能靠 SWD 下载所以 SWCLK 和 SWDIO 两个脚永远不能占作他用这一点在分配引脚的时候要一开始就规划好。2.3 定时器资源分配4 路 PWM 加 4 路编码器怎么塞STM32F103C8T6 总共就 TIM1 到 TIM4 四个定时器要做四路电机 PWM 和四路编码器接口资源是紧的。我的分配方案是TIM1高级定时器通道 1 到 4 输出 PWM对应引脚 PA8、PA9、PA10、PA11TIM2编码器接口模式通道 1 和 2对应引脚 PA0、PA1TIM3编码器接口模式对应引脚 PA6、PA7TIM4编码器接口模式对应引脚 PB6、PB7为什么这么分因为编码器接口模式只需要定时器的通道 1 和通道 2而 PWM 输出需要四个独立通道只有 TIM1、TIM2、TIM3、TIM4 有四个通道TIM1 有四个通用定时器也各有四个。TIM1 是高级定时器PWM 输出的功能最全用来做四路 PWM 最合适。剩下三个通用定时器各用一个编码器第四个编码器……等等这里只有三个通用定时器加上 TIM1 也就四个但 TIM1 已经被 PWM 占了。对F103C8T6 只有四个定时器四路 PWM 加四路编码器正好用满没有第二个编码器接口给 TIM1 用。所以方案是TIM1 出 PWMTIM2/3/4 做三路编码器第四个电机的编码器怎么办我用的做法是软件解码第四个编码器的 A/B 相接到两个普通 GPIO开外部中断或者用定时器中断轮询在中断里读两相电平查表得到方向累加计数。F103 的 GPIO 翻转速度是 50MHz电机 30:1 减速比下输出轴 200RPM 时编码器单相频率大概是 200/60 × 13 × 30 ≈ 1300Hz两相双边沿中断最多 5200 次每秒对 72MHz 的主控来说负担不大。实测下来 CPU 占用大概 3% 左右可以接受。如果不想用软件解码还有两个选择一是换 STM32F103RCT6 这类高容量型号多出 TIM5、TIM6、TIM7其中 TIM5 也支持编码器模式二是在编码器信号和主控之间加一片四路正交解码芯片比如 LS7366 或者国产替代通过 SPI 读计数。第二种方案成本高一点但最省 CPU做四轮以上底盘的时候很值。PWM 频率的选定也要说明一下。TB6612 的 PWM 输入支持到 100kHz但频率太高会有开关损耗和驱动发热。人耳听力上限大概 20kHz低于这个值电机线圈会发出明显的啸叫。所以我选择20kHz刚好在听觉上限附近实际听感上只有很轻微的电流声。计算方式TIM1 时钟 72MHz预分频系数设为 0那么自动重装载寄存器 ARR 72MHz / 20kHz - 1 3599。这个 3599 就是 PWM 满占空比对应的比较值后面 PID 输出的限幅上界就设它。void PWM_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_BaseInitStructure; TIM_OCInitTypeDef TIM_OCInitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1 | RCC_APB2Periph_GPIOA, ENABLE); // PA8~PA11 复用推挽输出 GPIO_InitStructure.GPIO_Pin GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11; GPIO_InitStructure.GPIO_Mode GPIO_Mode_AF_PP; GPIO_InitStructure.GPIO_Speed GPIO_Speed_50MHz; GPIO_Init(GPIOA, GPIO_InitStructure); TIM_BaseInitStructure.TIM_Prescaler 0; TIM_BaseInitStructure.TIM_Period 3599; // 20kHz TIM_BaseInitStructure.TIM_ClockDivision 0; TIM_BaseInitStructure.TIM_CounterMode TIM_CounterMode_Up; TIM_TimeBaseInit(TIM1, TIM_BaseInitStructure); TIM_OCInitStructure.TIM_OCMode TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OutputState TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse 0; TIM_OCInitStructure.TIM_OCPolarity TIM_OCPolarity_High; TIM_OC1Init(TIM1, TIM_OCInitStructure); TIM_OC2Init(TIM1, TIM_OCInitStructure); TIM_OC3Init(TIM1, TIM_OCInitStructure); TIM_OC4Init(TIM1, TIM_OCInitStructure); TIM_CtrlPWMOutputs(TIM1, ENABLE); // 高级定时器必须开这个否则没输出 TIM_Cmd(TIM1, ENABLE); }注意最后那个TIM_CtrlPWMOutputs只有 TIM1 和 TIM8 这类高级定时器需要调用通用定时器不用。这个函数不开PWM 口就是死的输出零电平很多人第一次用高级定时器做 PWM 会在这里卡半天。2.4 编码器接口模式配置与计数溢出处理编码器接口模式的原理其实不复杂定时器的通道 1 和通道 2 都配置成输入捕获把编码器的 A 相和 B 相接上去硬件根据两相的相位关系自动决定计数方向A 相超前 B 相 90 度就向上计数B 相超前就向下计数每来一个边沿计数一次。配置成双边沿计数就是四倍频一圈的计数是编码器线数乘四再乘减速比。void Encoder_TIM2_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_BaseInitStructure; TIM_ICInitTypeDef TIM_ICInitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); // PA0、PA1 浮空输入 GPIO_InitStructure.GPIO_Pin GPIO_Pin_0 | GPIO_Pin_1; GPIO_InitStructure.GPIO_Mode GPIO_Mode_IN_FLOATING; GPIO_Init(GPIOA, GPIO_InitStructure); TIM_BaseInitStructure.TIM_Prescaler 0; TIM_BaseInitStructure.TIM_Period 65535; // 16 位满量程 TIM_BaseInitStructure.TIM_ClockDivision TIM_CKD_DIV1; TIM_BaseInitStructure.TIM_CounterMode TIM_CounterMode_Up; TIM_TimeBaseInit(TIM2, TIM_BaseInitStructure); TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); TIM_ICInitStructure.TIM_Channel TIM_Channel_1; TIM_ICInitStructure.TIM_ICPolarity TIM_ICPolarity_Rising; TIM_ICInitStructure.TIM_ICSelection TIM_ICSelection_DirectTI; TIM_ICInitStructure.TIM_ICPrescaler TIM_ICPSC_DIV1; TIM_ICInitStructure.TIM_ICFilter 6; // 数字滤波抗毛刺 TIM_ICInit(TIM2, TIM_ICInitStructure); TIM_ICInitStructure.TIM_Channel TIM_Channel_2; TIM_ICInit(TIM2, TIM_ICInitStructure); TIM_SetCounter(TIM2, 0); TIM_Cmd(TIM2, ENABLE); }TIM_ICFilter这个参数我强烈建议设成 6 或者更大范围 0 到 15。它的作用是在硬件层面对输入信号做数字滤波采样几次都一致才认。霍尔编码器在有刷电机上工作电机换向时的火花会产生毛刺滤波设小了会出现莫名其妙的计数跳变一圈多出来几百个计数PID 直接就疯了。设 6 的代价是引入大约 6 个采样周期的延迟对 5ms 的采样周期来说完全可以忽略。读取计数有个必须处理的细节定时器是 16 位的范围 0 到 65535超过就回绕。两个采样周期之间的差值直接用short类型强制转换就能自动处理回绕因为补码运算的性质天然满足模运算。#define ENC_CPR 1560.0f // 13 线 * 4 倍频 * 30 减速比 static int16_t last_cnt[4] {0}; static float rpm[4] {0}; void Encoder_Sample(void) { uint16_t now; int16_t delta; now TIM_GetCounter(TIM2); delta (int16_t)(now - last_cnt[0]); // 用 short 自动处理溢出 last_cnt[0] now; rpm[0] (delta / ENC_CPR) / SPEED_DT * 60.0f; // TIM3、TIM4、软件解码的第四路同理 }这里的SPEED_DT是采样周期单位是秒。5ms 就是 0.005。注意读计数器的时候不要中途清零很多人习惯每次读完TIM_SetCounter(TIMx, 0)这样做虽然也能算出速度但一旦某个采样周期漏掉一次比如中断被高优先级任务打断速度就完全错了而且没法察觉。用差值法最稳。3. 轮速闭环从编码器计数到稳定的 RPM3.1 转速测量方法的选择与分辨率评估测速方法有三种M 法、T 法、M/T 法课本上讲得很多但对麦轮底盘这个具体场景我再结合实际参数算一遍。M 法是在固定的时间窗口内数编码器脉冲个数。优点是实现简单、低速时也能出结果缺点是采样周期固定速度分辨率随采样周期变化。我这套参数下的分辨率怎么算编码器一圈 1560 个计数采样周期 5ms那么最快的计数速度发生在什么转速如果电机输出轴跑到 300RPM也就是 5 转每秒每秒产生 7800 个计数5ms 内是 39 个计数。反过来说一个计数对应的速度分辨率就是 1 / 1560 / 0.005 × 60 7.7 RPM。这个精度够不够轮子直径 60mm周长 188.5mm7.7RPM 对应线速度 7.7/60 × 0.1885 0.024 m/s也就是 2.4 厘米每秒。在低速比如 0.1m/s时这个量化误差已经到 24% 了确实不够理想。T 法是反过来测两个脉冲之间的时间。高速时时间很短定时器分辨率不够所以 T 法适合低速。M/T 法是高频时钟同时计脉冲数和计时速度低的时候自动切换到测时间速度高的时候自动切换到数脉冲全速域都能保证精度但要占用额外的定时器资源。我的实际取舍是低速精度要求高的场景比如厘米级位置控制把采样周期从 5ms 延长到 20ms 用 M 法这样分辨率降到 1.9RPM线速度分辨率 0.6 厘米每秒可以接受。同时给速度加一阶低通滤波。位置控制的时候底盘速度本来就不高20ms 的控制周期对整机动态影响不大。如果你做的是高速竞速类机器人那就上 M/T 法或者直接换高线数编码器500 线编码器配 30 减速比是 60000 计数一圈5ms 采样时的分辨率直接降到 0.2RPM随便怎么调都够。滤波这一步不能不提。编码器是通过机械联轴器接在电机轴上的联轴器有间隙、齿轮有回差、霍尔元件本身也有量化误差所以原始速度信号必然是抖的。我的做法是一阶低通// alpha 越小越平滑但延迟越大。0.3~0.5 是比较好用的区间 rpm_filt[i] rpm_filt[i] * (1 - SPEED_LPF_A) rpm_raw[i] * SPEED_LPF_A;取SPEED_LPF_A 0.35实测在 5ms 周期下速度信号的抖动从正负 15RPM 降到正负 3RPM 左右而阶跃响应的延迟大约增加了 2 个周期10ms对轮速环完全可以接受。如果你发现 PID 怎么调都振荡先把滤波系数调小试试很多时候问题就出在这。3.2 位置式 PID 的工程实现与抗积分饱和轮速环我用位置式 PID不用增量式。原因很实际位置式的输出就是 PWM 占空比本身物理意义明确调试的时候可以直接看输出值判断有没有饱和增量式的输出是占空比的增量一旦某个时刻出现异常增量前面的积累全乱了排查起来麻烦。结构体定义和计算函数typedef struct { float Kp, Ki, Kd; float target; // 目标速度 RPM float integral; // 积分累加项已经乘过 Ki float last_meas; // 上次测量的速度用于微分先行 float out; // 本次输出 float out_max, out_min; float integral_max; } SpeedPID_t; float SpeedPID_Calc(SpeedPID_t *pid, float meas, float dt) { float err pid-target - meas; float d; // 积分项先累加再限幅 pid-integral pid-Ki * err * dt; if (pid-integral pid-integral_max) pid-integral pid-integral_max; if (pid-integral -pid-integral_max) pid-integral -pid-integral_max; // 微分先行对测量值求微分避免目标值突变时产生微分冲击 d -pid-Kd * (meas - pid-last_meas) / dt; pid-last_meas meas; pid-out pid-Kp * err pid-integral d; if (pid-out pid-out_max) pid-out pid-out_max; if (pid-out pid-out_min) pid-out pid-out_min; return pid-out; }这里有两个设计选择要解释。第一是积分限幅。底盘在遇到障碍物堵转的时候目标 100RPM实际 0RPM误差一直是 100积分项会一路涨到天上。等你把障碍物挪开积分项要花好几秒才能泄放掉车会猛地窜出去。限幅值取多少合适我一般取out_max的 60% 左右这样不会因为限幅太死导致静态误差消不掉也不会积太久。第二是微分先行。标准 PID 的微分项是对误差求微分误差 目标 - 测量当目标值发生阶跃比如从 0 突然跳到 100RPM微分项瞬间变成一个极大的值输出直接打到饱和电机哐一下窜出去。改成对测量值求微分目标值阶跃时微分项不受影响冲击就没了。轮速环对这点其实不太敏感但位置环的目标速度是连续变化的用了微分先行的代码在两层复用的时候更省事。还有一个绕不开的问题是死区补偿。直流电机在 PWM 占空比低于某个阈值时根本转不起来因为静摩擦力还没被克服。这个阈值实测下来大概在 10% 到 15% 之间不同电机差别挺大而且会随电池电压下降而变化。如果不处理PID 在目标速度很低的时候会拼命往上加输出加到 12% 电机突然启动速度一下冲过头然后再降下来反复循环表现为车在原地点头。处理方法有两种。简单的是输出死区补偿#define PWM_DEADZONE 0.12f // 12% 静摩擦阈值实测标定 #define PWM_FULL 3599.0f float deadzone_comp(float out) { float mag fabsf(out) / PWM_FULL; if (mag 1e-3f) return 0.0f; mag PWM_DEADZONE mag * (1.0f - PWM_DEADZONE); return (out 0 ? mag : -mag) * PWM_FULL; }补偿之后相当于把死区从系统里挖掉PID 的输出线性度好很多。更讲究的做法是前馈补偿根据目标速度直接算出一个基础占空比PWM_ff k1 * target_rpm k2 * sign(target_rpm)PID 只负责修正误差这样即使 PID 参数一般跟踪效果也很好。标定 k1 的方法很简单给固定占空比等速度稳定后记录占空比和 RPM多测几个点做线性拟合斜率就是 k1截距就是 k2。3.3 参数整定的实操顺序和典型值PID 整定这事没有万能公式但有可靠的流程。我的顺序是这样的第一步先把 Kp 单独调出来。Ki 和 Kd 都设 0目标速度给一个阶跃比如从 0 跳到 150RPM用串口把目标值和实际值以 5ms 周期打到上位机上看曲线。Kp 从 1.0 开始每次翻倍。当曲线出现明显的过冲超过目标值 20% 以上并且开始有几下振荡说明 Kp 差不多了往回退到过冲 5%~10% 的那个值。我这套电机的典型 Kp 在 8 到 15 之间取决于供电电压和车速。Kp 太小会怎样响应很慢从 0 到 150RPM 要 200ms 以上而且稳态下永远差那么几个 RPM。第二步加 Ki 消静态误差。单独用 P 控制稳态下一定会有误差因为要维持一定转速必须有非零的占空比输出。Ki 从 Kp 的 1/10 开始加观察静态误差是不是在一个采样周期内被消掉。注意 Ki 是加在积分累加里的系数量纲和 Kp 不一样代码里我写的是integral Ki * err * dt所以它的单位是输出/误差·秒。典型值取 Kp 的 0.5 到 2 倍再用 dt 归一化。Ki 加太大的表现是低速时电机发出嗡嗡的声音转速在目标值上下低频摆动这就是积分振荡。第三步Kd 基本不用。轮速环的反馈信号本身已经有滤波延迟再加微分只会放大噪声。我那套参数最终是 Kp 12Ki 0.9Kd 0四个轮子因为负载不同略有差异差在正负 15% 以内。四个轮子的参数最好分开标定特别是如果电机批次不一样共用一组参数会导致某个轮子响应总是慢半拍车就走不直。整定时的几个注意事项都是踩出来的调试的时候一定要把车架起来让轮子悬空。落地整定会引入地面摩擦力这个变负载参数在悬空和落地状态下差别很大落地标定虽然更准但车会到处跑很危险也麻烦。我的做法是先悬空调出大致参数再落地微调。每次改完参数一定要把数据打到上位机上看别光靠耳朵听。耳朵能分辨的只是有没有啸叫分辨不出来 5% 的过冲和稳态误差光凭感觉调很容易越调越乱。电池电压会随着放电下降从满电 12.6V 到低压 10.5V同样的占空比对应转速差 20%。所以如果你的 PID 参数是在满电调的跑到后半程电量车就会明显变软。讲究一点的做法是加电池电压采样把占空比做电压归一化或者干脆用带前馈的方案对电压变化不敏感。4. 麦轮运动学与整车控制4.1 逆运动学把 vx、vy、ω 拆成四个轮速这部分是整个项目里最容易出错的地方因为它涉及坐标系的约定只要你对约定和实际硬件的对应关系没理清就会出现横移方向反了自转方向反了这类问题而且排查起来毫无头绪。先定义坐标系。车体坐标系x 轴指向车头正前方y 轴指向车体左侧ω 定义为逆时针为正从上往下看。速度单位统一用米每秒ω 用弧度每秒。设车体几何中心为原点每个轮子中心到原点的纵向距离是 L前后方向横向距离是 W左右方向。注意麦轮的旋转力矩臂不是单独的 L 或 W而是k L W这个结论来自辊子 45 度安装的几何投影。轮子半径记作 R。那么四个轮子的线速度可以写成// L: 半轴距前后轮中心到几何中心的纵向距离单位 m // W: 半轮距左右轮中心到几何中心的横向距离单位 m // R: 轮子半径单位 m float k L W; v_wheel[0] vx - vy - k * wz; // 左前 v_wheel[1] vx vy k * wz; // 右前 v_wheel[2] vx vy - k * wz; // 左后 v_wheel[3] vx - vy k * wz; // 右后 // 转成角速度交给轮速环rad/s for (int i 0; i 4; i) { target_rpm[i] v_wheel[i] / R * 60.0f / (2 * 3.1415926f); }我用一个表格把三个纯运动的分解结果列出来对着这个表去验证硬件最直观运动模式输入左前右前左后右后车体表现前进vx1, vy0, ω0沿车头方向平移左移vx0, vy1, ω0−−车头不变整体向左平移右移vx0, vy−1, ω0−−车头不变整体向右平移左自转vx0, vy0, ω1−−原地逆时针旋转右自转vx0, vy0, ω−1−−原地顺时针旋转斜向左前vx1, vy1, ω000沿 45 度方向平移看左移那一行左前和右后反转右前和左后正转。这个组合看起来有点反直觉很多人第一次做的时候会想当然地认为左移应该是左边两个轮子正转、右边两个反转结果做出来车在原地打转。原因是辊子的 45 度安装把力分解到了两个方向上横向平移靠的是对角线上两个轮子的推力分量叠加。上电第一件事是标定符号。因为麦轮有左旋右旋之分电机接线也可能反所以公式里的符号和实际硬件不一定对得上。我的做法是一步步验证给四个轮子分别单独发正转指令用手摸或者贴张纸条看确认每个轮子正转时轮子确实是向前滚的方向对应车体前进方向。如果某个轮子装反了先把它的电机线正负极对调。然后按前进模式统一给正转看整车是不是向前走。如果向后走说明四个轮子的正负号整体反了把v_wheel[]的整体符号取反。按左自转模式测试。如果车向右转说明 ω 那两项的符号整体反了把公式里所有k * wz的符号取反。最后测左移。如果车向右平移说明 vy 那两项的符号反了。这个情况和上一条是独立的要单独调。也可以在硬件上把左右旋轮对调位置解决。这四步做完符号体系就完全对了。不要跳过星号那一步我见过太多人公式抄对了但因为一个轮子装反而导致整个底盘行为诡异然后开始在 PID 里找问题越找越偏。4.2 正运动学与里程计积分从轮速反推位姿逆运动学是给定车体速度算轮速正运动学是反过来用四个轮子的实际速度反推车体的实际运动。里程计就是把这个过程随时间的积分。对上面那组方程做线性组合反解得到float vx_meas ( v[0] v[1] v[2] v[3]) * 0.25f; float vy_meas (-v[0] v[1] v[2] - v[3]) * 0.25f; float wz_meas (-v[0] v[1] - v[2] v[3]) / (4.0f * k);注意这里用的是实际测得的轮速不是目标轮速。用目标速度做里程计等于在自欺欺人车打滑了你也看不出来。得到车体速度之后怎么积分成位姿最朴素的做法是直接乘以 dt 累加但误差大。更好的是中点法在一个积分周期内用起始角和终止角的平均值作为整个周期的朝向再用这个平均朝向把车体速度旋转到世界坐标系。void Odometry_Update(float vx, float vy, float wz, float dt) { float dtheta wz * dt; float theta_mid pose.theta dtheta * 0.5f; // 中点法 float c cosf(theta_mid), s sinf(theta_mid); pose.x (vx * c - vy * s) * dt; pose.y (vx * s vy * c) * dt; pose.theta dtheta; // 角度归一化到 [-PI, PI]防止积分到很大之后三角函数精度下降 while (pose.theta PI) pose.theta - 2 * PI; while (pose.theta -PI) pose.theta 2 * PI; }里程计的误差来源有三个按影响大小排序第一是轮子半径 R 的标定误差。R 如果标大了 2%所有位移都会小 2%走到 3 米外就差 6 厘米。标定方法很土但管用在地上画一条 3 米的直线让车走完读里程计输出的位移用R_真实 R_当前 × 3.0 / 里程计读数修正。第二是 k 值也就是 L W的标定误差。这个主要影响自转时的角度解算。标定方法是让车原地转十圈读里程计累计的角度调整 k 让读数接近 20π。注意每次修改 k 之后逆运动学里的 k 也要同步改因为两者必须一致否则会出现走直线没问题但原地转角度差很多的现象。第三是打滑。这个没法通过标定解决只能靠外部传感器。最便宜的方案是加一个 MPU6050用陀螺仪的 z 轴角速度对时间积分来算航向角和里程计航向用互补滤波或者卡尔曼滤波融合。陀螺仪的优势是不受打滑影响缺点是会漂移零偏积分时间长了角度就飘了里程计的优势是长时间不发散缺点是一打滑就错。两者融合正好互补。// 互补滤波系数 0.02 意味着每 10ms 修正 2% 的陀螺仪零偏 pose.theta (1 - 0.02f) * (pose.theta gyro_z * dt) 0.02f * odom_theta;4.3 位置控制外环怎么串进去为什么这么串有了可靠的轮速内环和里程计位置控制就是在外层再套一层环。这里要先明确位置控制有两种完全不同的目标形态不能混为一谈。形态一纯航向保持。就是以某个速度和方向走直线不许跑偏。这个最简单也最常用外层只有一个航向 PID// 输入目标航向 theta_ref当前航向 pose.theta // 输出wz float yaw_pid(float theta_ref, float theta_now) { float err theta_ref - theta_now; while (err PI) err - 2 * PI; // 角度误差必须归一化 while (err -PI) err 2 * PI; return yaw_pid_calc(g_yaw_pid, err, CTRL_DT); }这个角度归一化是必须的如果忘了写一旦车转过 ±180° 附近误差会突然从 179° 跳到 -181°PID 会瞬间输出一个反向的极大值车会剧烈地甩一下。我踩过这个坑当时以为是陀螺仪坏了排查了两个小时。形态二点到点位置控制。给定目标坐标 (x_ref, y_ref)让车自己走过去。完整的做法是把世界坐标系的坐标误差旋转到车体坐标系然后在车体坐标系里分别做两个 PIDvoid Position_Loop(void) { float dx x_ref - pose.x; float dy y_ref - pose.y; float c cosf(pose.theta); float s sinf(pose.theta); // 世界系误差 - 车体系误差 float ex dx * c dy * s; // 车头方向上的误差 float ey -dx * s dy * c; // 车体左侧方向上的误差 // 距离足够近就停 float dist sqrtf(dx * dx dy * dy); if (dist 0.02f) { // 2cm 容差 vx_ref vy_ref wz_ref 0; g_pos_reached 1; return; } vx_ref pos_pid_x(ex); vy_ref pos_pid_y(ey); wz_ref yaw_pid(theta_ref, pose.theta); // 保持车头朝向 // 限速避免离得远的时候冲刺 float vmax 0.4f; if (fabsf(vx_ref) vmax) vx_ref (vx_ref 0 ? vmax : -vmax); if (fabsf(vy_ref) vmax) vy_ref (vy_ref 0 ? vmax : -vmax); }三层环的周期怎么分配这是关键。轮速环 5ms位置环 20ms中间那个航向/速度分配放在哪里我的做法是位置环20ms算出 vx_ref、vy_ref、wz_ref 之后直接通过逆运动学换算成四个轮子的目标 RPM交给 5ms 的轮速环去跟踪。中间不加环理由是这样最简单而且够用——轮速环的响应时间在 50ms 以内比位置环的 20ms 周期长但这不算问题因为位置环本身就是在做慢速调节两个环的时间常数差 4 倍已经足够分开了。误区提醒位置环的 PID 参数要比轮速环小得多。我见过有人把位置环的 Kp 也设到 10 以上结果就是车在目标点附近来回画圈、永远停不下来。位置环的 Kp 典型值在 0.5 到 2 之间输出单位是米每秒输入单位是米Ki 在 0.1 到 0.5 之间。还有一个常被忽略的点位置环的积分项在接近目标点的时候必须清掉否则车到了目标点积分项还积累着一个不小的值车会冲过去然后再回来来回振荡。调试点到点控制的时候我建议按这个顺序验证阶段测试内容通过标准1只做航向保持手动推车旋转车主动往回收回正过程平稳不过冲2只做 vx 方向点到点目标在正前方走 1 米停止误差小于 2cm无过冲3只做 vy 方向点到点目标在正左方横向平移 1 米误差小于 3cm4任意角度目标点走 1.5 米终点误差小于 5cm5连续多点巡线每个点误差小于 8cm无累积偏差横向平移那一步误差会比纵向大因为麦轮横移时的有效摩擦特性和纵向不同会有轻微的打滑这个属于正常现象。如果横向误差超过 5cm先检查是不是某个轮子的死区补偿没做好导致低速起步时四个轮子响应不一致。5. 调车现场那些文档里不会写的坑5.1 编码器读数异常的四类根因编码器问题是我遇到最多的一类表现都是计数不对但根因差别很大我用一个表把它们分开现象根因排查方法解决计数偶尔跳变一个很大的数电机换向火花干扰示波器看编码器 A/B 波形是否有毛刺提高TIM_ICFilter编码器线加磁环远离电机线走线车不动但计数一直在涨输入引脚悬空采集到噪声直接量引脚电平把 GPIO 模式从浮空输入改成上拉输入或者接外部上拉电阻已知转速下计数偏小A 或 B 相没接好缺相单独转电机看两个通道有没有信号检查线序和焊点霍尔编码器供电 3.3V/5V 要确认一次采样读到极大值中断里读数被更高优先级中断打断加标志位或者临时关中断把编码器采样放在优先级最高的定时器中断里还有一个隐藏很深的问题编码器采样函数被放在了会阻塞的地方。比如你有个delay_ms()在某个流程里如果这个流程和编码器采样在同一个中断里采样就会被推迟采样间隔不稳定算出来的速度就会抖。所以速度采样一定要放在固定的定时器中断里而且这个中断的优先级要设高其他任务的耗时操作绝对不能放进去。另外提一个容易忽视的点上电初始相位。编码器上电时计数器是 0但实际上电机轴可能停在任意角度。这不影响速度测量我们用的是差值但如果你要做单轮绝对位置控制就需要在初始化时加一个机械零点校准流程——让轮子慢速转到触发一个光电或者霍尔限位然后清零。麦轮底盘通常不需要这么做因为我们关心的是相对位移但如果你做的是机械臂这类需要绝对零点的这一点必须处理。5.2 电机啸叫、抖动、跑偏的排查路径啸叫。声音尖锐、频率固定一般是 PWM 频率落在可听范围内。20kHz 是人耳听力的临界如果你的 PWM 频率设成了 10kHz 以下一定会听到明显的啸叫。先在代码里确认 ARR 和预分频算出来的频率对不对特别注意前面提到的定时器时钟倍频问题——如果你按 36MHz 算 TIM2 的频率实际输出会是 20kHz 而不是你以为的 10kHz。反过来如果你希望是 20kHz 但实际是 40kHz驱动芯片可能发热严重。如果频率算对了还是叫那可能是机械共振。表现是只在某个特定转速区间叫其他转速正常。这种可以试着改变 PWM 频率比如从 20kHz 调到 22kHz或者调到 16kHz避开共振点。还有一种情况是电源纹波引起的电机电流突变时电池电压跌落会通过电源耦合到控制电路表现是车速越快叫得越厉害。解决办法是在电机电源端并一个大容量电解电容470uF 以上加一个小容量陶瓷电容。抖动。抖动和啸叫不一样是低频的、肉眼可见的车轮来回摆。绝大多数情况下是 PID 参数问题Kp 太大、积分振荡或者微分项放大了噪声。但也别忽略机械因素——联轴器松动、轮子和轴的配合有间隙、车架刚性不够这些都会表现为抖动。判断方法很简单把 PID 输出固定成一个常数占空比不开闭环如果还抖那就是机械或者电源问题跟控制无关。跑偏。走直线的时候往一边偏三个层次的排查顺序第一层是参数一致性。四个轮子的 PID 参数是不是一样编码器分辨率标定是不是一样电机本身的一致性如何我遇到过一台车四个电机是同型号但不同批次同一组 PID 参数下有一个轮子的稳态转速比其他三个低 8RPM导致车慢速时往那边偏。解决办法是每个轮子单独标定一组 PID或者更彻底一点用前馈加闭环的方案让 PID 只负责小范围修正。第二层是机械对称性。四个轮子的安装高度是否一致有没有哪个轮子略微悬空用手转一下四个轮子感受阻力是否均匀。麦轮底盘对这点特别敏感因为一旦有个轮子悬空它在运动学上提供的分量就完全消失了正运动学解算出来的 vy 会直接错掉。第三层是运动学标定。L 和 W 是不是准确轮子半径是不是准确地面是不是足够平用尺子量一下四个轮子中心到车体几何中心的距离如果左右不等那 vy 解算一定有问题。这个几何误差比参数误差更基础也更难发现因为它不会导致车明显跑偏而是导致横移方向有偏差——你让它往左平移它实际走的是左前方 15 度走一米就偏出去 25 厘米。5.3 常见问题速查表调试过程中的问题太多我把最耗时间的几个整理成速查表方便对照症状最可能的原因快速验证处理上电后电机不转TIM1 没开 PWM 输出量 PWM 引脚有没有波形调TIM_CtrlPWMOutputs(TIM1, ENABLE)电机只能往一个方向转方向控制引脚没配分别给正反转指令量方向脚电平检查 TB6612 的 AIN1/AIN2 接线和配置速度环一直饱和编码器方向反了负反馈变成正反馈给正转指令看速度读数是正是负交换编码器 A/B 相接线或改计数符号车横移时原地打转vy 项符号错按 4.1 节第四步验证对调两个轮子的左右旋位置或改符号原地转角度差很多k 值标定不准转十圈看里程计角度偏差重新标定 k同时修改逆解和正解走 3 米偏 10cm轮径标定不准或机械不对称分别测量四个轮子的实际滚动距离重新标定 R检查轮子气压/磨损位置控制停不下来位置环积分没清或 Kp 太大观察接近目标点时的速度曲线加到达判定清积分减小 Kp低速时车点头死区补偿没做或标定不准逐步加大占空比看电机从几伏开始转重新测量死区阈值或加前馈串口数据偶尔乱码波特率偏差或者丢字节空闲时看是否正常跑起来才乱检查时钟配置加帧头帧尾和校验烧录后程序不跑启动文件选错型号看是否卡在 HardFault确认用的是startup_stm32f10x_md.s串口这一条我多讲两句。速度环调试的时候数据量不小四个轮子的目标值和实际值加时间戳5ms 一次就是 9 个 float如果每个都用文本格式打印波特率 115200 根本不够而且文本转换本身很耗时。我的做法是二进制协议帧头两字节 0xAA、0x55一个字节长度然后是数据体最后两个字节的简单校验和。上位机用 Python 解析成波形比文本快十倍不止。实测下来 5ms 周期发 40 字节115200 波特率占用不到 40%还有余量。还有个小技巧调试期间把关键状态打到 OLED 上。有时候现场没带电脑OLED 能显示当前速度、PID 输出、编码器计数这些值效率比连串口高。我一般留一页显示四个轮子的目标/实际速度一页显示里程计位姿出现问题扫一眼就知道是哪一层的问题。这台底盘我从建工程到跑通位置控制前后折腾了大概三周其中一半时间花在标定和排查硬件上真正写控制算法的代码量其实不大——轮速 PID 三十行运动学解算二十行里程计三十行加起来不到两百行核心代码。难度不在代码在于把每一层的误差控制在可接受的范围内让它们叠加起来还能满足要求。后续如果想把精度再往上推我的建议是加一个下视光流模块直接测地面位移和里程计做融合能把打滑造成的误差干掉一大半或者上 ROS 做多传感器融合代价是开发复杂度直线上升看你的实际需求值不值。