个人主页：
wengqidaifeng
✨ 永远在路上，永远向前走
个人专栏：
数据结构
C语言
嵌入式小白启动！
重要OJ算法题详解
蓝桥杯备战
C++从菜鸟到强手
python启航
AI大模型Agent：拥抱未来，赋能自己
文章目录
-
- 博客系列总目录与阅读路线
- 推荐阅读顺序
- 全系列知识地图
- 总览
- 仓库资料边界
- 开源说明
- [第四篇：编码器速度环、差速运动学、MPU6050 航向环与直线稳定性](#第四篇：编码器速度环、差速运动学、MPU6050 航向环与直线稳定性)
-
- [1. 直线蛇形为什么不是单纯的循迹 P 过大](#1. 直线蛇形为什么不是单纯的循迹 P 过大)
- [2. 编码器速度估算](#2. 编码器速度估算)
- [3. 速度 PID、前馈和静摩擦补偿](#3. 速度 PID、前馈和静摩擦补偿)
- [4. 差速底盘运动学](#4. 差速底盘运动学)
- [5. 循迹 PID 与速度环的级联关系](#5. 循迹 PID 与速度环的级联关系)
- [6. MPU6050 航向辅助](#6. MPU6050 航向辅助)
- [7. 从半圆进入直线的特殊问题](#7. 从半圆进入直线的特殊问题)
- [8. 目标速度斜坡和车体姿态](#8. 目标速度斜坡和车体姿态)
- [9. 用 VOFA 诊断直线不稳](#9. 用 VOFA 诊断直线不稳)
- [10. 与往年小车调参方法的比较](#10. 与往年小车调参方法的比较)
- [11. 本篇结论](#11. 本篇结论)
- [12. 进一步的模型与标定方法](#12. 进一步的模型与标定方法)
- 直线稳定的闭环结构
博客系列总目录与阅读路线
本系列记录 MSPM0G3507 双主控小车从赛题分析、硬件搭建、底层驱动、循迹调试，到摆杆滚球控制和最终测评复盘的完整过程。文章中的路径均为仓库相对路径，代码、测试脚本和模块封装可以直接从公开仓库取得。
推荐阅读顺序
- 赛题拆解、总体方案与往年控制类小车赛题比较
- 硬件选型、接线与上电验证
- 软件架构、SysConfig 与模块化设计
- 编码器速度环、差速运动学、MPU6050 航向环与直线稳定
- 8 路灰度循迹、直线/半圆分段与 S1/S2/S3 状态机
- 摆杆机械结构、MaxiCamPro 测量、步进执行器与三层 PID
- MaxiCamPro、双主控通信、VOFA+ 遥测与可复现实验方法
- 完整测评流程、失败案例复盘、比赛策略与开源交付
全系列知识地图
#mermaid-svg-pPIl4BxOEUWLvCvk{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-pPIl4BxOEUWLvCvk .error-icon{fill:#552222;}#mermaid-svg-pPIl4BxOEUWLvCvk .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-pPIl4BxOEUWLvCvk .marker{fill:#333333;stroke:#333333;}#mermaid-svg-pPIl4BxOEUWLvCvk .marker.cross{stroke:#333333;}#mermaid-svg-pPIl4BxOEUWLvCvk svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-pPIl4BxOEUWLvCvk p{margin:0;}#mermaid-svg-pPIl4BxOEUWLvCvk .edge{stroke-width:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .section--1 rect,#mermaid-svg-pPIl4BxOEUWLvCvk .section--1 path,#mermaid-svg-pPIl4BxOEUWLvCvk .section--1 circle,#mermaid-svg-pPIl4BxOEUWLvCvk .section--1 polygon,#mermaid-svg-pPIl4BxOEUWLvCvk .section--1 path{fill:hsl(240, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .section--1 text{fill:#ffffff;}#mermaid-svg-pPIl4BxOEUWLvCvk .node-icon--1{font-size:40px;color:#ffffff;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-edge--1{stroke:hsl(240, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-depth--1{stroke-width:17;}#mermaid-svg-pPIl4BxOEUWLvCvk .section--1 line{stroke:hsl(60, 100%, 86.2745098039%);stroke-width:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled circle,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:lightgray;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:#efefef;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-0 rect,#mermaid-svg-pPIl4BxOEUWLvCvk .section-0 path,#mermaid-svg-pPIl4BxOEUWLvCvk .section-0 circle,#mermaid-svg-pPIl4BxOEUWLvCvk .section-0 polygon,#mermaid-svg-pPIl4BxOEUWLvCvk .section-0 path{fill:hsl(60, 100%, 73.5294117647%);}#mermaid-svg-pPIl4BxOEUWLvCvk .section-0 text{fill:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .node-icon-0{font-size:40px;color:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-edge-0{stroke:hsl(60, 100%, 73.5294117647%);}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-depth-0{stroke-width:14;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-0 line{stroke:hsl(240, 100%, 83.5294117647%);stroke-width:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled circle,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:lightgray;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:#efefef;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-1 rect,#mermaid-svg-pPIl4BxOEUWLvCvk .section-1 path,#mermaid-svg-pPIl4BxOEUWLvCvk .section-1 circle,#mermaid-svg-pPIl4BxOEUWLvCvk .section-1 polygon,#mermaid-svg-pPIl4BxOEUWLvCvk .section-1 path{fill:hsl(80, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .section-1 text{fill:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .node-icon-1{font-size:40px;color:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-edge-1{stroke:hsl(80, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-depth-1{stroke-width:11;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-1 line{stroke:hsl(260, 100%, 86.2745098039%);stroke-width:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled circle,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:lightgray;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:#efefef;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-2 rect,#mermaid-svg-pPIl4BxOEUWLvCvk .section-2 path,#mermaid-svg-pPIl4BxOEUWLvCvk .section-2 circle,#mermaid-svg-pPIl4BxOEUWLvCvk .section-2 polygon,#mermaid-svg-pPIl4BxOEUWLvCvk .section-2 path{fill:hsl(270, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .section-2 text{fill:#ffffff;}#mermaid-svg-pPIl4BxOEUWLvCvk .node-icon-2{font-size:40px;color:#ffffff;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-edge-2{stroke:hsl(270, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-depth-2{stroke-width:8;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-2 line{stroke:hsl(90, 100%, 86.2745098039%);stroke-width:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled circle,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:lightgray;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:#efefef;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-3 rect,#mermaid-svg-pPIl4BxOEUWLvCvk .section-3 path,#mermaid-svg-pPIl4BxOEUWLvCvk .section-3 circle,#mermaid-svg-pPIl4BxOEUWLvCvk .section-3 polygon,#mermaid-svg-pPIl4BxOEUWLvCvk .section-3 path{fill:hsl(300, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .section-3 text{fill:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .node-icon-3{font-size:40px;color:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-edge-3{stroke:hsl(300, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-depth-3{stroke-width:5;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-3 line{stroke:hsl(120, 100%, 86.2745098039%);stroke-width:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled circle,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:lightgray;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:#efefef;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-4 rect,#mermaid-svg-pPIl4BxOEUWLvCvk .section-4 path,#mermaid-svg-pPIl4BxOEUWLvCvk .section-4 circle,#mermaid-svg-pPIl4BxOEUWLvCvk .section-4 polygon,#mermaid-svg-pPIl4BxOEUWLvCvk .section-4 path{fill:hsl(330, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .section-4 text{fill:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .node-icon-4{font-size:40px;color:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-edge-4{stroke:hsl(330, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-depth-4{stroke-width:2;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-4 line{stroke:hsl(150, 100%, 86.2745098039%);stroke-width:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled circle,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:lightgray;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:#efefef;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-5 rect,#mermaid-svg-pPIl4BxOEUWLvCvk .section-5 path,#mermaid-svg-pPIl4BxOEUWLvCvk .section-5 circle,#mermaid-svg-pPIl4BxOEUWLvCvk .section-5 polygon,#mermaid-svg-pPIl4BxOEUWLvCvk .section-5 path{fill:hsl(0, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .section-5 text{fill:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .node-icon-5{font-size:40px;color:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-edge-5{stroke:hsl(0, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-depth-5{stroke-width:-1;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-5 line{stroke:hsl(180, 100%, 86.2745098039%);stroke-width:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled circle,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:lightgray;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:#efefef;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-6 rect,#mermaid-svg-pPIl4BxOEUWLvCvk .section-6 path,#mermaid-svg-pPIl4BxOEUWLvCvk .section-6 circle,#mermaid-svg-pPIl4BxOEUWLvCvk .section-6 polygon,#mermaid-svg-pPIl4BxOEUWLvCvk .section-6 path{fill:hsl(30, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .section-6 text{fill:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .node-icon-6{font-size:40px;color:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-edge-6{stroke:hsl(30, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-depth-6{stroke-width:-4;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-6 line{stroke:hsl(210, 100%, 86.2745098039%);stroke-width:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled circle,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:lightgray;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:#efefef;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-7 rect,#mermaid-svg-pPIl4BxOEUWLvCvk .section-7 path,#mermaid-svg-pPIl4BxOEUWLvCvk .section-7 circle,#mermaid-svg-pPIl4BxOEUWLvCvk .section-7 polygon,#mermaid-svg-pPIl4BxOEUWLvCvk .section-7 path{fill:hsl(90, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .section-7 text{fill:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .node-icon-7{font-size:40px;color:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-edge-7{stroke:hsl(90, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-depth-7{stroke-width:-7;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-7 line{stroke:hsl(270, 100%, 86.2745098039%);stroke-width:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled circle,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:lightgray;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:#efefef;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-8 rect,#mermaid-svg-pPIl4BxOEUWLvCvk .section-8 path,#mermaid-svg-pPIl4BxOEUWLvCvk .section-8 circle,#mermaid-svg-pPIl4BxOEUWLvCvk .section-8 polygon,#mermaid-svg-pPIl4BxOEUWLvCvk .section-8 path{fill:hsl(150, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .section-8 text{fill:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .node-icon-8{font-size:40px;color:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-edge-8{stroke:hsl(150, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-depth-8{stroke-width:-10;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-8 line{stroke:hsl(330, 100%, 86.2745098039%);stroke-width:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled circle,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:lightgray;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:#efefef;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-9 rect,#mermaid-svg-pPIl4BxOEUWLvCvk .section-9 path,#mermaid-svg-pPIl4BxOEUWLvCvk .section-9 circle,#mermaid-svg-pPIl4BxOEUWLvCvk .section-9 polygon,#mermaid-svg-pPIl4BxOEUWLvCvk .section-9 path{fill:hsl(180, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .section-9 text{fill:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .node-icon-9{font-size:40px;color:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-edge-9{stroke:hsl(180, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-depth-9{stroke-width:-13;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-9 line{stroke:hsl(0, 100%, 86.2745098039%);stroke-width:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled circle,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:lightgray;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:#efefef;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-10 rect,#mermaid-svg-pPIl4BxOEUWLvCvk .section-10 path,#mermaid-svg-pPIl4BxOEUWLvCvk .section-10 circle,#mermaid-svg-pPIl4BxOEUWLvCvk .section-10 polygon,#mermaid-svg-pPIl4BxOEUWLvCvk .section-10 path{fill:hsl(210, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .section-10 text{fill:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .node-icon-10{font-size:40px;color:black;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-edge-10{stroke:hsl(210, 100%, 76.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .edge-depth-10{stroke-width:-16;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-10 line{stroke:hsl(30, 100%, 86.2745098039%);stroke-width:3;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled circle,#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:lightgray;}#mermaid-svg-pPIl4BxOEUWLvCvk .disabled text{fill:#efefef;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-root rect,#mermaid-svg-pPIl4BxOEUWLvCvk .section-root path,#mermaid-svg-pPIl4BxOEUWLvCvk .section-root circle,#mermaid-svg-pPIl4BxOEUWLvCvk .section-root polygon{fill:hsl(240, 100%, 46.2745098039%);}#mermaid-svg-pPIl4BxOEUWLvCvk .section-root text{fill:#ffffff;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-root span{color:#ffffff;}#mermaid-svg-pPIl4BxOEUWLvCvk .section-2 span{color:#ffffff;}#mermaid-svg-pPIl4BxOEUWLvCvk .icon-container{height:100%;display:flex;justify-content:center;align-items:center;}#mermaid-svg-pPIl4BxOEUWLvCvk .edge{fill:none;}#mermaid-svg-pPIl4BxOEUWLvCvk .mindmap-node-label{dy:1em;alignment-baseline:middle;text-anchor:middle;dominant-baseline:middle;text-align:center;}#mermaid-svg-pPIl4BxOEUWLvCvk :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} H题车载平衡滚球
赛题与指标
赛道圈时
启停判据
滚球目标点
硬件系统
MSPM0G3507
TB6612与编码器
灰度与MPU6050
MaxiCamPro与步进
软件控制
速度环
航向环
灰度状态机
三层滚球PID
工程方法
接线验证
VOFA遥测
回归测试
测评复盘
总览
这道题不是单独写一个循迹 PID，而是把机械结构、电源、传感器、实时调度、速度闭环、航向辅助、视觉通信、双主控安全和赛道状态机组合成一个可测量、可恢复、可验收的系统。调试时坚持"每次只改变一个因素"，并用串口遥测、主机回归测试和实车视频记录证据。
仓库资料边界
- 小车运动主控工程：公开仓库根目录
smartcar_keil_diag/ - 通用模块封装：公开仓库中的
ModulePackage/ - 摆杆/滚球控制参考工程：
BalanceController/ - 硬件与接线说明：
Hardware/ - 主机测试与回归脚本：
Tests/ - 主控、蓝牙、VOFA+ 和双主控协议说明：仓库根目录下相应的
*.md文件
主控采用 LCKFB 天猛星 MSPM0G3507，车体约为 30 cm × 24 cm，车轮直径约 65 mm。实际使用的主要器件包括 JGB37-520 直流减速电机、TB6612 驱动器、MPU6050、8 路灰度传感器、MaxiCamPro 和无编码器步进执行器。涉及具体电气参数、安装尺寸和控制增益时，应以实测记录和仓库中的配置为准，不要直接照搬示例数值。
开源说明
完整工程、博客、测试脚本和模块封装均发布在 GitHub：
https://github.com/wengqidaifeng/smartcar_keil_diag
建议按 01→08 的顺序阅读；需要查代码时从 User/、Source/、ModulePackage/ 和 Tests/ 进入。文章中的结论区分为"源码已实现""主机测试已验证"和"必须在实车/机构上复测"三类，便于后来者复现实验并继续改进。
第四篇：编码器速度环、差速运动学、MPU6050 航向环与直线稳定性
1. 直线蛇形为什么不是单纯的循迹 P 过大
实车调试中最常见的现象是小车在直线上左右摆动：黑线出现在右侧，控制器向左拉；拉回后又越过中心，黑线出现在左侧，再向右拉，振荡逐渐放大直至丢线。直观上像是循迹 P 太大，但真正的原因可能包括左右电机特性不同、速度反馈量化、控制延迟、车体残余角速度、灰度安装偏心、状态误判和目标速度阶跃。
如果左右轮直接使用开环 PWM，即使数值相同，实际速度也可能不同。电机减速箱摩擦、轮胎直径、负载和驱动压降都会产生偏差。循迹外环看到车辆缓慢偏离后持续增加差速，等偏差传感器反映出来时，车体已经有角速度；若控制器继续按位置偏差用力，就会越过黑线。于是，稳定直线首先要求左右轮速度闭环，再要求方向控制具有阻尼和航向约束。
2. 编码器速度估算
在固定采样周期 dt 内，编码器计数增量为 ΔN，每个车轮一圈的软件计数为 Nrev，轮径为 D，则线速度可估算为：
text
v = ΔN / Nrev × πD / dt本车轮径约 65 mm，周长约 204.2 mm。Nrev 必须通过实测确定，包含减速比和软件边沿倍频。若 10 ms 内计数较少，速度会呈阶梯状，直接微分会产生明显量化噪声。可使用一阶低通、滑动窗口或基于多个周期的估算，但滤波越强，延迟越大。工程中采用适度滤波，并通过主机测试检查固定目标下反馈波动。
左右编码器前进方向应统一为正。目标从正变负时，机械惯性会使短时间内实测仍为正，不能立刻判定"编码器方向反了"；应设置合理的方向反转宽限。相反，如果目标持续为正而反馈长时间为负，则应锁存速度方向故障并停机。超速保护同样需要连续确认，避免单个计数毛刺触发。
3. 速度 PID、前馈和静摩擦补偿
速度控制可写为 u = PID(v_target-v_measured)+u_ff。纯 PID 在低速时容易遇到电机死区：PWM 小于某阈值时轮子不动，积分逐渐累积，一旦克服静摩擦又突然冲出。加入与目标方向一致的启动补偿可以缩短死区，但补偿过大也会让低速不平顺。
调参时先架空单轮，关闭循迹。将 Ki、Kd 置零，逐步增加 Kp，使速度能跟随但不明显振荡；再加入很小 Ki 消除稳态误差，并设置抗饱和；若反馈噪声可控，再用少量 D 或测量滤波改善瞬态。随后分别测试 10、20、30 cm/s 和方向反转，记录目标、实测、PWM 和电池电压。
左右轮不一定使用完全相同的补偿，但不能用大量左右常数去掩盖机械故障。若相同目标下实测长期相差超过约 1～1.5 cm/s，应检查轮胎、减速箱、编码器和前馈；若实测基本一致而车仍偏，则问题更可能在灰度安装、轮距或航向零点。
4. 差速底盘运动学
对于左右轮中心距为 B 的差速底盘，左右线速度分别为 vL、vR，车体中心前进速度和角速度近似为：
text
v = (vL + vR) / 2
ω = (vR - vL) / B循迹控制输出的"转向量"本质是左右速度半差。若定义 left=forward-steering、right=forward+steering，正负号必须通过实车确认。因为电机安装镜像、驱动方向和传感器偏差符号可能改变最终关系，不能只凭公式假定。
外轮速度受最大 PWM 和电机能力限制。若在高速直线仍要求很大差速，外轮可能饱和，内轮继续降低，导致前进速度突然下降；状态切换时又恢复高速，车辆会猛冲。因此混控器需要在保持差速的同时进行整体缩放或外轮限幅，速度模式切换还要经过斜坡。
5. 循迹 PID 与速度环的级联关系
循迹环输入是灰度偏差，输出是左右速度差；速度环输入是各轮目标与实测之差，输出 PWM。外环不能比内环快得多，否则循迹每拍改变目标，而速度环尚未建立，车体响应滞后。10 ms 更新下，循迹转向量应设置每拍变化上限，直线比半圆更温和。
直线时不建议使用较大积分。长期偏差可能来自传感器物理偏心或左右轮尺度误差，少量积分可补偿，但积分在穿越黑线后仍保留，会推动车辆继续越界。应使用积分分离、条件积分或在重捕获/状态切换时清零。微分项应基于滤波误差或车体角速度，否则数字灰度跳变会产生尖峰。
6. MPU6050 航向辅助
MPU6050 的陀螺仪 Z 轴提供偏航角速度，积分得到相对航向。它不能替代灰度：陀螺会漂移，且赛道黑线才是最终横向基准。但在直线短时间内，IMU 能看见灰度尚未明显偏离时的转动趋势；在半圆中，它能判断是否接近 180°；出弯后，它能帮助消除残余角速度。
上电后需要静止采集足够样本估计零偏。每次启动前可保持车辆静止一段时间建立本次航向参考，但不应在运动中随意把偏航"学成零"。曾经出现中心探头仍看到黑线时，融合算法逐渐吞掉真实 5°偏航，结果航向纠正消失，车辆斜着走直到压到边缘。修复方法是直线锁期间保持起步航向参考，只有明确的状态转换才重置目标。
直线方向控制可组合灰度偏差、航向误差和角速度阻尼：
text
steering = K_line·e_line + K_heading·e_yaw - K_rate·gyroZ灰度项把车拉回黑线，航向项让车头平行于直线，角速度项抑制正在发生的转动。三项符号必须通过低速实验确认。航向项只在可信直线阶段启用或限幅，半圆则使用目标角速度/目标航向逻辑，不能强行把车拉回起始角。
7. 从半圆进入直线的特殊问题
第二段直线比第一段难，原因是车辆刚从半圆出来，车体仍可能带着同向角速度和切线角误差。若半圆控制突然归零，车辆会靠惯性继续向外；若直线看到边缘后用很大反向 P 拉回，又会横穿黑线形成振荡。
稳定出弯需要三个动作。第一，在接近 180°时逐渐卸载圆弧前馈，而不是一步清零。第二，进入短暂恢复窗口，用航向误差和角速度阻尼继续修正，但限制最大差速。第三，当灰度回到中心、角速度足够小并经过一定距离后，才转入普通直线低增益。仅凭编码器相位退出会受轮径误差影响，仅凭灰度中心退出又可能在车头仍歪时过早结束，两者应与 IMU 共同判断。
8. 目标速度斜坡和车体姿态
速度阶跃不仅影响轮速，也会改变车体俯仰并扰动钢球。S1 高速模式可以较快建立速度，S2 带球模式需要更缓的斜坡，S3 当前使用约 27 cm/s 和每 10 ms 约 0.20 cm/s 的上升限制。斜坡应作用于前进目标，而转向量也有独立斜坡，否则虽然车速平滑，左右差速仍可突变。
弯道通常需要降低前进速度并增加差速。注意"提前减速"和"提前转弯"不是一回事：可以在 BC/DA 前一段距离降低 forward，给底盘时间稳定，但转向仍保持直线轮廓，直到弯道证据成立。这样既避免高速冲入半圆，也避免在直线末端提前摆动。
9. 用 VOFA 诊断直线不稳
一次有效的直线测试至少同步输出：左右目标速度、左右实测速度、左右 PWM、灰度位图、偏差、yaw、gyroZ、当前赛道相位和转向量。观察顺序如下：先看目标是否稳定，再看实测是否跟随，再看 PWM 是否饱和，最后看灰度和 IMU 的相位关系。
若目标本身左右大幅摆动，问题在循迹/状态机；若目标稳定而实测摆动，问题在速度环或编码器；若实测一致但 yaw 持续变化，检查轮径、轮距、机械安装和 IMU；若 yaw 已转正而灰度仍偏，检查传感器安装偏心。视频可证明现象发生，但只有遥测能把原因分层。
10. 与往年小车调参方法的比较
低速、短轴距的传统循迹车有时只用 P 就能跑完，因为车速低、传感器宽、机械响应快。但本项目车体约 30 cm × 24 cm、上层机构较重，惯量和负载差异更明显；正式赛道包含长直线和半径固定的半圆，单一 P 很难同时满足。相比"偏差大就加大 P"，更有效的方法是先建立速度闭环，按直线/半圆分段，加入角速度阻尼和目标斜坡。
现代控制类赛题的共同趋势是任务复杂度增加，调参不能再只看单个波形。编码器、IMU、视觉和状态机共同提供上下文，控制器需要利用上下文改变增益和限幅，但状态切换必须有滞回和证据，不能让每个传感器一变就切模式。
11. 本篇结论
直线稳定是一条完整链路：机械对称性提供基础，编码器速度环消除左右轮差异，循迹低增益修正横向偏差，MPU6050 提供航向与角速度阻尼，斜坡和限幅控制瞬态，状态机保证直线与半圆不混淆。只有逐层验证，才能避免在最外层不断增加 P，把一个速度或状态问题变成更严重的振荡。
12. 进一步的模型与标定方法
如果时间允许，可以用实测阶跃数据估计电机一阶模型。给定若干固定 PWM，记录速度上升曲线和稳态速度，得到左右轮的死区、增益和时间常数；再建立目标速度到基础 PWM 的分段前馈表，PID 只修正剩余误差。相比单一常数补偿，分段前馈能改善低速与高速的差异，但表格必须在相近电池电压和负载下获得。
轮距也需要动态标定。仅用卡尺测外形宽度不等于有效轮距，可以命令左右轮等速反向转一圈，根据编码器行程和 MPU 累计角度反推。轮胎打滑会影响结果，因此要多次、低速、在正式地面上测试。有效轮距准确后，圆弧几何前馈和里程角度关系会更可靠。
MPU 零偏不是一个永久常数。温度变化和安装应力都会改变陀螺偏置，可在每次启动前静止估计，并记录温度与偏置。运动中只允许非常慢且有严格静止条件的偏置跟踪，不能因为灰度居中就把当前角速度当零。若需要长时间绝对航向，应增加磁力计或视觉参考，但本题一圈时间较短，相对航向配合黑线通常足够。
这些模型化方法不要求复杂辨识软件，VOFA 数据和简单拟合就能完成。关键仍是单位一致、实验条件可重复和一次只改变一个因素。模型用于给控制器更好的初值和前馈，不应成为忽略传感器反馈的理由。
直线稳定的闭环结构
#mermaid-svg-DV9xjTglYjw0q6dl{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-DV9xjTglYjw0q6dl .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-DV9xjTglYjw0q6dl .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-DV9xjTglYjw0q6dl .error-icon{fill:#552222;}#mermaid-svg-DV9xjTglYjw0q6dl .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-DV9xjTglYjw0q6dl .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-DV9xjTglYjw0q6dl .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-DV9xjTglYjw0q6dl .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-DV9xjTglYjw0q6dl .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-DV9xjTglYjw0q6dl .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-DV9xjTglYjw0q6dl .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-DV9xjTglYjw0q6dl .marker{fill:#333333;stroke:#333333;}#mermaid-svg-DV9xjTglYjw0q6dl .marker.cross{stroke:#333333;}#mermaid-svg-DV9xjTglYjw0q6dl svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-DV9xjTglYjw0q6dl p{margin:0;}#mermaid-svg-DV9xjTglYjw0q6dl .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-DV9xjTglYjw0q6dl .cluster-label text{fill:#333;}#mermaid-svg-DV9xjTglYjw0q6dl .cluster-label span{color:#333;}#mermaid-svg-DV9xjTglYjw0q6dl .cluster-label span p{background-color:transparent;}#mermaid-svg-DV9xjTglYjw0q6dl .label text,#mermaid-svg-DV9xjTglYjw0q6dl span{fill:#333;color:#333;}#mermaid-svg-DV9xjTglYjw0q6dl .node rect,#mermaid-svg-DV9xjTglYjw0q6dl .node circle,#mermaid-svg-DV9xjTglYjw0q6dl .node ellipse,#mermaid-svg-DV9xjTglYjw0q6dl .node polygon,#mermaid-svg-DV9xjTglYjw0q6dl .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-DV9xjTglYjw0q6dl .rough-node .label text,#mermaid-svg-DV9xjTglYjw0q6dl .node .label text,#mermaid-svg-DV9xjTglYjw0q6dl .image-shape .label,#mermaid-svg-DV9xjTglYjw0q6dl .icon-shape .label{text-anchor:middle;}#mermaid-svg-DV9xjTglYjw0q6dl .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-DV9xjTglYjw0q6dl .rough-node .label,#mermaid-svg-DV9xjTglYjw0q6dl .node .label,#mermaid-svg-DV9xjTglYjw0q6dl .image-shape .label,#mermaid-svg-DV9xjTglYjw0q6dl .icon-shape .label{text-align:center;}#mermaid-svg-DV9xjTglYjw0q6dl .node.clickable{cursor:pointer;}#mermaid-svg-DV9xjTglYjw0q6dl .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-DV9xjTglYjw0q6dl .arrowheadPath{fill:#333333;}#mermaid-svg-DV9xjTglYjw0q6dl .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-DV9xjTglYjw0q6dl .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-DV9xjTglYjw0q6dl .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-DV9xjTglYjw0q6dl .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-DV9xjTglYjw0q6dl .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-DV9xjTglYjw0q6dl .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-DV9xjTglYjw0q6dl .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-DV9xjTglYjw0q6dl .cluster text{fill:#333;}#mermaid-svg-DV9xjTglYjw0q6dl .cluster span{color:#333;}#mermaid-svg-DV9xjTglYjw0q6dl div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-DV9xjTglYjw0q6dl .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-DV9xjTglYjw0q6dl rect.text{fill:none;stroke-width:0;}#mermaid-svg-DV9xjTglYjw0q6dl .icon-shape,#mermaid-svg-DV9xjTglYjw0q6dl .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-DV9xjTglYjw0q6dl .icon-shape p,#mermaid-svg-DV9xjTglYjw0q6dl .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-DV9xjTglYjw0q6dl .icon-shape .label rect,#mermaid-svg-DV9xjTglYjw0q6dl .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-DV9xjTglYjw0q6dl .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-DV9xjTglYjw0q6dl .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-DV9xjTglYjw0q6dl :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 直线目标速度
左右速度环
编码器速度
基础 PWM
灰度偏差
航向/横向修正
MPU6050 yaw/gyroZ
差速限幅与斜率限制
左右轮 PWM
| 工况 | 主要反馈 | P 增益 | 差速限幅 | 速度策略 |
|---|---|---|---|---|
| 第一段直线 | 灰度小偏差 + yaw | 低 | 小 | 平滑加速 |
| 出弯恢复 | 灰度边缘 + gyroZ | 中 | 中 | 先抑制横摆 |
| 半圆 | 灰度趋势 + 角速度 | 高 | 大 | 允许持续差速 |
| 丢线恢复 | 最后有效偏差 | 固定 | 受限 | 不突然反向 |