电赛双车跟随系统：从PID控制到多传感器融合的实战指南

在准备电赛控制类题目时双车跟随系统一直是极具挑战性和代表性的赛题。它综合考察了机械结构、运动控制、传感器融合和通信协同等多方面的能力。本文将以2022年电赛B题为蓝本系统性地拆解一套能够冲击满分的“新版双车跟随”解决方案。我们将从核心思想、硬件选型、软件算法到调试技巧进行全流程的实战讲解旨在帮助参赛队伍构建一个稳定、快速且鲁棒性强的跟随系统。1. 双车跟随系统核心概念与赛题分析1.1 什么是双车跟随系统双车跟随系统通常指由一辆领航车主车和一辆跟随车从车组成的协同运动系统。跟随车需要实时感知领航车的状态如位置、速度、航向并自主控制自身的运动以保持与领航车之间特定的相对关系如固定距离、固定角度或形成特定队形。在电赛背景下这不仅仅是简单的“追着跑”而是对控制精度、响应速度和抗干扰能力的综合考验。1.2 2022年电赛B题关键要求与难点回顾虽然具体赛题要求每年略有调整但核心挑战通常集中在以下几点高精度相对定位跟随车如何准确获取自身与领航车的相对位置和角度这是所有控制的基础。快速且稳定的运动控制如何将定位信息转化为精准的电机控制指令使跟随车能平滑、无超调地跟踪领航车的轨迹变化复杂场景适应性领航车可能进行直线、转弯、加减速甚至急停等操作跟随系统需要在各种动态场景下保持稳定。抗干扰与容错传感器数据可能存在噪声通信可能延迟或中断系统需要具备一定的鲁棒性。启动与停止的协调如何实现两车的同步启动和平稳停止避免碰撞。理解这些难点是我们设计“满分套餐”的出发点。2. 系统总体设计与硬件选型一套优秀的硬件平台是算法得以施展的基础。我们的设计遵循模块化、高可靠性和易调试原则。2.1 核心控制器主控芯片STM32F4系列如F407、F429是当前电赛的主流选择。其拥有较高的主频168MHz以上、丰富的定时器用于电机PWM和编码器采集以及足够的SRAM和Flash能够流畅运行浮点运算和较为复杂的控制算法。相较于F1系列F4的硬件FPU浮点运算单元对提升控制循环速度至关重要。备选方案对于预算有限或追求极致性价比的队伍STM32F1系列如F103C8T6经过精心优化代码后也能胜任但需注意算法复杂度。2.2 感知模块跟随车的“眼睛”这是决定系统性能上限的关键。我们推荐多传感器融合的方案不把鸡蛋放在一个篮子里。绝对定位核心UWB超宽带模块作用提供跟随车与领航车之间高精度、低延迟的相对距离和角度信息。它是实现稳定跟随的“定海神针”。选型建议选择市面上成熟的电赛专用UWB定位套件如基于DW1000芯片的方案。通常包含一个基站安装在领航车和至少一个标签安装在跟随车。注意选择支持“双边双向测距TWR”和“到达角AoA”功能的模块以同时获得距离和角度。优点精度高可达厘米级不受光照影响穿透能力强。缺点成本较高在金属多径环境下可能受干扰。辅助与容错视觉传感器/激光测距作用在UWB信号短暂失效或受到干扰时提供备份数据或在特定赛题要求如识别特定标记时作为主传感器。OpenMV/MT9V034全局快门摄像头适合进行颜色跟踪、二维码/AprilTag识别。可以用于测量领航车尾部标识的横向偏移和距离作为PID控制的输入。全局快门避免运动拖影。激光测距模块如TOF提供前方障碍物或领航车尾部的单点距离信息简单可靠可作为距离校验。自身状态感知编码器与IMU光电编码器/霍尔编码器安装在电机上用于测量跟随车自身的轮子转速和计算里程odometry实现闭环速度控制并为某些融合算法提供本体航向变化。IMU惯性测量单元如MPU6050提供跟随车自身的加速度和角速度通过融合如互补滤波、卡尔曼滤波可以估算出较为准确的实时航向角Yaw。在UWB角度信息跳变时IMU的短时精度很高可用于平滑数据。2.3 执行模块电机与驱动推荐使用直流减速电机配合电机驱动模块如TB6612、DRV8833、RT7272。这些驱动芯片集成度高支持PWM调速和正反转控制接口简单。确保驱动电流足够带动你的小车底盘。电源管理为单片机、传感器、电机驱动提供独立、稳定的电压。建议使用多个降压模块如LM2596、MP1584将电池电压如12V分别降至5V供主控、传感器和电机驱动所需电压。特别注意电机回路与单片机回路在电源上要尽可能隔离例如使用不同的稳压模块并在电源入口加装大容量电解电容和瓷片电容滤波以抑制电机启停造成的电源扰动。2.4 通信模块双车间通信用于传输UWB测量数据或简单的同步指令。如果UWB模块自带数据透传功能则可直接使用。否则可增加一对简单的无线串口模块如NRF24L01、ESP8266 STA/AP模式。通信协议务必设计得简单、高效包含校验位。调试通信预留USB转串口接口用于连接电脑输出调试信息如距离、角度、PID参数、电机输出等这是调试阶段的“生命线”。2.5 车体结构底盘结构坚固重心低两轮差速驱动是常见且灵活的选择。确保轮子不打滑编码器安装牢固。传感器布局UWB天线/模块应尽量安装在车体中心轴线上方远离金属和大电流线路以减少干扰。摄像头应向前视野开阔。3. 软件架构与核心算法实现软件是系统的灵魂。我们将系统划分为感知、决策、控制、执行四大层。3.1 程序主循环设计一个稳定、定时触发的控制循环是基础。// 伪代码示意主循环结构 int main(void) { // 硬件初始化时钟、GPIO、定时器、PWM、编码器、串口、IMU、UWB等 All_Init(); // 初始化控制参数 PID_Init(pid_distance, 2.0, 0.1, 0.5); // 距离PID参数示例 PID_Init(pid_angle, 3.0, 0.05, 0.3); // 角度PID参数示例 while(1) { // 1. 感知层定时采集数据例如每10ms一次 if (timer_10ms_arrived) { timer_10ms_arrived 0; // 读取UWB数据距离、角度 UWB_GetData(distance, angle); // 读取IMU数据航向角 IMU_GetYaw(self_yaw); // 读取编码器速度 Encoder_GetSpeed(left_speed, right_speed); // 2. 数据融合与处理 // 例如对UWB角度进行低通滤波或与IMU航向进行互补融合 filtered_angle LowPassFilter(angle, last_angle, 0.3); // 计算目标期望距离设为0.5米期望角度差设为0度正后方跟随 target_distance 0.5; // 米 target_angle_diff 0; // 度 // 3. 决策与控制层双环PID计算 // 外环位置环根据距离误差计算目标速度 distance_error distance - target_distance; target_linear_speed PID_Calculate(pid_distance, distance_error); // 外环角度环根据角度误差计算目标角速度 angle_error filtered_angle - target_angle_diff; target_angular_speed PID_Calculate(pid_angle, angle_error); // 4. 运动学解算将线速度和角速度转换为左右轮目标速度 // 差速轮模型V_left V_linear - (V_angular * L / 2) // V_right V_linear (V_angular * L / 2) // L为轮距 target_speed_left target_linear_speed - (target_angular_speed * WHEEL_BASE / 2); target_speed_right target_linear_speed (target_angular_speed * WHEEL_BASE / 2); // 5. 内环速度环PID控制电机达到目标轮速 speed_error_left target_speed_left - left_speed; speed_error_right target_speed_right - right_speed; pwm_left PID_Calculate(pid_speed_left, speed_error_left); pwm_right PID_Calculate(pid_speed_right, speed_error_right); // 6. 执行层输出PWM到电机驱动 Motor_SetPwm(MOTOR_LEFT, pwm_left); Motor_SetPwm(MOTOR_RIGHT, pwm_right); // 7. 调试信息输出可降低频率如每100ms一次 if (debug_timer_arrived) { printf([Dist:%.2f, Ang:%.1f, LPWM:%d, RPWM:%d]\r\n, distance, filtered_angle, pwm_left, pwm_right); } } // 其他后台任务如按键扫描、状态灯控制 Other_Tasks(); } }3.2 核心算法串级PID控制这是实现平滑跟随的关键。“串级”意味着一个PID控制器的输出是另一个PID控制器的设定值。外环位置环输入UWB测得的实际距离与目标距离的误差、实际角度与目标角度的误差。控制器两个独立的PID控制器分别处理距离误差和角度误差。输出目标线速度V_linear和目标角速度V_angular。注意角度环的输出是角速度而不是直接给电机差速这样更符合物理规律控制更平稳。内环速度环输入由外环计算出的目标轮速通过运动学解算得到左右轮目标速度与编码器反馈的实际轮速的误差。控制器两个独立的PID控制器分别控制左轮和右轮。输出直接驱动电机的PWM占空比。为什么用串级PID内环速度环可以快速响应负载变化如地面摩擦不同保证电机转速紧紧跟随设定值。外环位置环则专注于消除跟随车与领航车之间的位姿误差。这种结构将快速扰动电机特性、地面和慢速扰动领航车运动分开处理系统动态性能更好超调更小。3.3 数据滤波与融合原始传感器数据不能直接使用。UWB数据滤波距离和角度数据可能存在毛刺。使用一阶低通滤波或滑动平均滤波进行平滑。// 一阶低通滤波示例 float LowPassFilter(float new_value, float old_value, float alpha) { return alpha * new_value (1 - alpha) * old_value; // alpha为滤波系数 (0alpha1, 越小越平滑) }IMU数据融合使用互补滤波或卡尔曼滤波融合加速度计和陀螺仪的数据得到稳定的航向角。MPU6050的DMP库可以简化这一步。传感器融合在UWB信号良好时以其为主当UWB信号丢失或跳变巨大时可以短暂切换到以IMU航向积分和编码器里程计推算的位置并缓慢修正。4. 系统调试与参数整定实战调试是电赛中最耗时也最重要的环节。遵循“先独立后联合先内环后外环”的原则。4.1 硬件单体测试电机与编码器编写测试程序让电机以固定PWM正反转观察编码器计数是否正常增减。确保电机接线正确编码器脉冲数能正确读取。UWB模块固定基站和标签通过串口助手查看输出的距离和角度数据是否稳定、符合预期。移动标签观察数据变化是否连续。IMU静止放置读取航向角观察是否漂移缓慢旋转观察角度变化是否灵敏且连续。4.2 控制回路调试步骤一内环速度环PID整定目标让小车能精准、快速地达到并保持指定的轮子转速。方法将外环断开直接给左右轮一个目标速度如每秒10个编码器脉冲。先将Kp,Ki,Kd全部设为0。调Kp逐渐增大Kp直到电机开始出现震荡然后回调至震荡消失的80%。此时电机应能基本跟上速度变化但可能有静差稳态误差。调Ki逐渐增大Ki用于消除静差。注意Ki太大会引起积分饱和导致系统反应迟钝或超调震荡。调Kd如果系统响应有超调或震荡可适当加入Kd进行抑制。Kd对噪声敏感通常需要配合低通滤波使用。验证让小车在空载和负载用手施加阻力情况下都能快速稳定到设定速度。步骤二外环位置环PID整定前提内环已调好且UWB/视觉等定位传感器工作正常。方法固定领航车让跟随车从不同位置启动尝试跟随到一个固定点固定距离和角度。距离环先调Kp使小车能向目标移动再调Kd抑制接近目标时的超调可能会“撞”上去最后微调Ki消除残余误差。角度环同理。关键技巧外环的Kp和Kd通常需要配合。Kp负责“拉动”小车Kd负责“刹车”。可以想象成开车接近停车线。验证领航车静止时跟随车能稳定停在目标位姿且响应迅速无震荡。步骤三联合调试与动态测试低速直线跟随领航车缓慢直线行驶观察跟随车是否平稳。转弯测试领航车进行定圆或8字行驶观察跟随车轨迹是否平滑能否保持队形。加减速测试领航车突然加速或减速观察跟随车是否及时响应且不过冲。通信中断测试短暂遮挡UWB信号观察跟随车是否进入安全模式如缓停或保持最后指令。5. 常见问题与故障排查清单在调试和比赛过程中你会遇到各种各样的问题。下表列出了典型问题及排查思路问题现象可能原因排查思路与解决方案跟随车完全不动1. 电机电源未接通或电压不足。2. 电机驱动使能信号未拉高。3. PWM输出引脚配置错误或未输出。4. 主控芯片“死机”程序跑飞。1. 用万用表测量电机驱动板输入电压和电机两端电压。2. 检查驱动芯片的STBY/ENABLE引脚电平。3. 用示波器或LED检查PWM引脚是否有波形/电平变化。4. 检查看门狗简化程序排查数组越界、除零等错误。小车“抽搐”或剧烈震荡1. 电源功率不足电机启动时拉低单片机电压导致复位。2. PID参数过激尤其是Kp或Kd太大。3. 编码器接线松动或受到干扰速度反馈异常。4. 传感器数据如UWB角度噪声大且未滤波。1. 加强电源滤波电机与单片机电源分离。2. 重新整定PID参数降低Kp和Kd。3. 检查编码器接线尝试给编码器信号线加屏蔽或上拉电阻。4. 对传感器数据施加低通滤波增大滤波系数。跟随车总是跑偏或画圈1. 左右轮电机机械差异或负载不同。2. 左右轮编码器分辨率校准不一致。3. 车体安装不对称重心偏移。4. UWB天线安装位置不在车体中心线上。1. 分别测试左右轮空载转速在代码中对PWM输出做微调补偿。2. 重新校准编码器确保每转脉冲数准确。3. 调整电池等重物位置使重心居中。4. 重新安装UWB天线确保其物理中心与车体旋转中心对齐。远距离或拐弯时跟随丢失1. UWB通信距离达到极限或信号被遮挡。2. 领航车速度过快超出跟随车动力学极限。3. 外环PID参数在高速/大误差下不适应。1. 检查UWB模块功率和天线避免金属遮挡。考虑增加中继或换用更高功率模块。2. 限制领航车最大速度或提高跟随车电机功率。3. 考虑使用变参数PID根据误差大小动态调整Kp等参数。上位机显示数据正常但小车不跟1. 控制周期不稳定或太慢。2. 数据单位混淆如角度制与弧度制。3. 运动学解算公式错误或轮距L参数不对。1. 确保控制中断定时准确避免在中断内进行复杂运算。2. 统一所有计算环节的角度单位推荐使用弧度。3. 反复检查运动学正逆解公式实测轮距并代入。6. 满分冲刺优化与工程实践要冲击满分需要在稳定性的基础上优化性能、增加智能和提升鲁棒性。参数自适应与模糊控制简单的PID在复杂轨迹下可能不够用。可以尝试模糊PID根据距离误差和角度误差的大小动态调整PID参数。例如误差大时用大的Kp快速接近误差小时用小的Kp和适当的Kd防止超调。运动预测与前馈控制如果通信允许让领航车将其速度、角速度信息也发送给跟随车。跟随车可以利用这些信息进行前馈控制提前做出调整减少滞后。例如知道领航车正在加速跟随车可以提前增加电机输出。状态机与异常处理设计完善的系统状态机如初始化、就绪、跟随、丢失目标、错误等。在丢失目标状态跟随车不应急停可能导致翻车而应缓慢减速或沿最后已知的领航车轨迹惯性行驶一段同时尝试重新获取信号。性能优化将浮点运算转换为定点运算以提升速度尤其在F1系列上。优化代码结构确保控制循环时间固定且尽可能短如5-10ms。使用DMA传输传感器数据释放CPU资源。赛场调试技巧参数固化调试出几组针对不同场景直线、弯道、低速、高速的PID参数在比赛时通过拨码开关或蓝牙指令快速切换。快速标定赛前编写一个简单的标定程序快速测量轮子直径、轮距、UWB安装偏移角等关键物理参数。冗余设计准备一套备用的传感器方案如纯视觉或纯编码器IMU在主传感器失效时能降级运行拿到部分分数。从硬件选型、软件架构到算法调试双车跟随系统是一个典型的软硬件结合工程。成功的关键在于模块化测试、数据驱动调试善用printf和上位机绘图和对物理原理的深刻理解。不要盲目调参要观察现象、分析数据、理解每个参数改变背后的物理意义。