做视觉定位加PID控制的小车机器人最头疼的不是电机转不动也不是单片机跑不起来而是你辛辛苦苦把代码写完、把PID参数填上去之后小球还是像喝了假酒一样满屏乱跑。这个项目我前前后后改了三版结构从OpenMV到USB摄像头从单级PID换成级联PID中间踩的坑比熬夜次数还多。今天把这些经验整理出来从相机怎么选、坐标怎么提、控制周期怎么定、级联PID怎么调一次性讲清楚给正在做或者打算做这个项目的朋友一个可以抄作业的全流程参考。1. 整体架构与方案选型动工之前先把整个系统拆开来看。自主平衡球机器人说白了就是让一个球在一块平台上保持中心位置平台偏了、球滚了视觉模块发现后告诉控制器控制器转动电机让平台反向修正。听起来很简单但真正的难点在于“发现”和“修正”这两个动作是异步的视觉慢、控制快怎么把这种快慢节奏协调好是整台机器人能不能站稳的核心。1.1 系统架构总览我把这个项目分成三层感知层、决策层、执行层。感知层负责看——摄像头采集图像、提取小球坐标决策层负责想——根据坐标偏差计算平台应该如何倾斜执行层负责做——电机、驱动器、机械平台把控制量变成实际位移。感知层我用的是普通USB摄像头加OpenCV跑在上位机我用的是一块香橙派其实树莓派、笔记本也都行。坐标算出来后通过串口发给下位机下位机是一块STM32F405负责真正闭环控制。为什么不用单片机直接做视觉不是说不行是性价比太低了。STM32算一个640x480的HSV滤波加轮廓提取帧率能掉到个位数还占着CPU大量时间。而用上位机几行OpenCV就能轻松跑30帧省下的精力都放在控制逻辑上。决策层执行层的架构是典型的“裸机PID控制”方案——STM32不开系统、不跑RTOS就靠定时器中断和主循环轮询裸跑PID。这套方案的好处是实时性完全可以预测中断来了就是来了不会像操作系统那样出现线程调度抖动。控制周期我定的是内环2ms、外环20ms这个节奏后面会细说为什么这样搭配。1.2 视觉方案选型与取舍视觉这块我建议直接选一个800万像素以上的普通USB摄像头别买那些几十块钱的鱼眼玩具镜头畸变会让你调参数调到怀疑人生。我第一批买的一个廉价广角头小球在画面边缘时坐标直接偏出去好几个像素后来换了焦距稍长一些、无明显广角畸变的镜头才稳定。目标检测就两个选择OpenCV的HSV阈值分割或者深度学习目标检测。我强烈建议用前者。网络上很多教程一上来就给你YOLO我的建议是不要上来就搞那些。小球是单色、刚体、光照基本可控的场景一个HSV阈值框就能解决还快、还稳。深度学习留到以后需要识别多目标再考虑现阶段纯粹是给自己找麻烦。HSV阈值分割还有一个隐性好处它天生不受灰度影响、对光照变化有一定鲁棒性。操作时先把RGB帧转成HSV再用inRange筛出小球颜色的像素区域之后做形态学开运算去噪最后画轮廓、求质心得到的就是一个带像素坐标的输出结果。1.3 机械结构选择与理由机械上常见的有两种方案一种是平台固定、小球在上面滚动靠平台倾斜来调整球的位置另一种是平台本身装在万向轮或球支撑结构上做跟随运动。我的项目用的是第一种——平台固定、云台倾斜方案。电机带动两个方向的倾斜机构X轴和Y轴相当于把平台变成一个二自由度的“盘子”盘子倾斜了小球自然会滚动。这种结构和常见的“倒立摆”不同它没有复杂的运动学耦合两个轴的PID调试完全独立可以把更多精力放在控制逻辑调试上。电机选择上我用的带减速箱的直流电机加增量式编码器这样既能测速形成速度内环又有足够扭矩扛住频烦往复。2. 视觉定位模块让机器人“看见”小球视觉定位听起来高大上其实本质就是三个步骤拿到图、找到球、算坐标。难的是每一步都有一点小坑尤其在你实际调试时灯光一换、球稍微滚到阴影区域程序和坐标就开始飘。所以这一章我把涉及颜色提取、坐标平滑、坐标转换的细节都铺开讲。2.1 颜色空间转换与二值化相机输出默认是BGROpenCV默认读图顺序新手常犯的错误是直接用RGB范围调阀值结果会发现深色阴影和浅色球体根本分不开。正确做法是转成HSV颜色空间因为它把“颜色”和“明暗”分开了阈值对光照变化的敏感度低很多。import cv2 import numpy as np cap cv2.VideoCapture(0) cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) cap.set(cv2.CAP_PROP_AUTO_WB, 0) # 关闭自动白平衡这点很关键 # 小球为亮绿色测量HSV范围后填写 lower np.array([35, 80, 80]) upper np.array([85, 255, 255]) while True: ret, frame cap.read() if not ret: break hsv cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) mask cv2.inRange(hsv, lower, upper) mask cv2.morphologyEx(mask, cv2.MORPH_OPEN, np.ones((5, 5), np.uint8)) mask cv2.morphologyEx(mask, cv2.MORPH_CLOSE, np.ones((5, 5), np.uint8)) cnts, _ cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) if cnts: # 取最大轮廓防噪点干扰 c max(cnts, keycv2.contourArea) area cv2.contourArea(c) if area 30: M cv2.moments(c) if M[m00] 0: cx int(M[m10] / M[m00]) cy int(M[m01] / M[m00]) cv2.imshow(mask, mask) if cv2.waitKey(1) 0xFF ord(q): break这段代码里CAP_PROP_AUTO_WB关闭自动白平衡那行我建议必加。因为自动白平衡会不停改图像色偏HSV阈值可能前几秒还好好的突然整个画面偏了小球就被滤没了。你宁可固定在一个适合现场光照的白平衡值上换场景时手动调一次也不要让它自己漂移。2.2 坐标平滑与防抖处理就算阈值稳定了实际输出坐标还是会有一点随机跳变因为相机的传感器本身有噪声小球低帧率状态下的滚动也会带来差值。直接用这个坐标驱动控制你会发现电机在跟着微小抖动一起抖整个平台像筛糠一样。我试过三种平滑方法平均值滤波、中值滤波、一阶低通滤波。平均值滤波简单但滞后明显中值滤波能干掉脉冲噪声但如果采样点少输出变化很生硬最后真正用得顺手的是一阶低通滤波公式如下filtered_x alpha * raw_x (1 - alpha) * filtered_x_oldalpha 我取到0.3到0.5之间大家可以根据实时性要求调。alpha越大响应越快、alpha越小越平滑。实测0.4对于30fps视觉来说滞后大概在20ms以内控制环路完全能接受。还有个细节过滤后要做限速单帧坐标变化量超过一定像素比如30就认为有噪声或误检直接丢弃这一帧。2.3 像素坐标与物理坐标转换视觉输出的只是像素坐标控制环需要的是“小球相对平台中心偏移的物理距离”。最简单的方法是用一个固定高度、固定角度的摄像头拍摄一个已知尺寸的棋盘算出像素/毫米比例。比如我在平台中心贴了一张A4纸测出纸的边缘对应的像素坐标差然后mm_per_pixel_x paper_width_mm / (x_right - x_left) mm_per_pixel_y paper_height_mm / (y_bottom - y_top)这样每次视觉回来就做一个线性缩放offset_x_mm (cx - image_center_x) * mm_per_pixel_x offset_y_mm (cy - image_center_y) * mm_per_pixel_y这里有个隐含前提镜头与平台近似平行、畸变不大。如果镜头安装歪了测出来的比例会在画面不同位置不一致需要在标定时尽量保证镜头与平台平行。我第一版就是没注意安装角度结果画面左侧比例是2.1mm/px右侧变成1.7mm/px控制效果自然很差。后来把相机装正、重新标定效果立刻好一个量级。到这里视觉模块输出的就是X、Y两个方向的偏移量了通过串口每20ms发给下位机。串口协议我用的非常简单帧头0xAA、数据长度、X坐标、Y坐标、校验和。校验和将前面所有字节相加取低8位避免中间脏数据。裸机端用串口接收中断解析状态机判断帧头完整一帧接收后才更新坐标变量。3. PID控制设计从原理到级联实现这一章是整个项目的灵魂。你必须先理解为什么一个普通PID不够用再去动手写代码。直接把单级PID填上去球经常会晃来晃去或者直接跑飞不是公式错了是结构就不适合这个系统。级联PID才是这类自主平衡项目的正解。3.1 为什么单级PID不够用如果你试图直接用视觉偏移量 → PID → PWM → 电机整个环路里面有一大堆滞后视觉滞后20ms、电机响应滞后、机械惯性滞后。而外环PID看到偏差时实际平台早超调了。更麻烦的是单环PID的输出直接是PWM它没有“目标速度”这个中间量电机无法被主动纠正为“速度刚刚好”球到了中央时可能电机还在以较大速度回转等惯性把球推偏PID才反应过来于是一直来回晃。拿打方向盘类比单级PID就是开车时只看车离车道中心多远然后猛打方向盘不关心当前方向盘角度和车速级联PID呢是先规划“期望车速”再通过速度环让方向盘平稳达到那个车速。后者天然带阻尼不容易震荡。3.2 级联PID的结构与原理级联PID的标准结构如下位置环外环: 读取视觉偏移量 offset_x 目标速度 PID_outer(offset_x) 速度环内环: 读取编码器速度 actual_speed PWM输出 PID_inner(目标速度 - actual_speed)为什么要先给速度环、再给位置环核心原因是响应速度不一样。位置反馈的采样率20ms远低于速度反馈2ms且位置环存在视觉坐标的噪声和通信延迟。用速度环把内层做快、做稳相当于在外环看来“我想让平台以某个速度转它就差不多能实现”系统的开环特性变得可预测外环PID也好调很多。我实现的时候X轴和Y轴各一套独立级联PID。两个轴结构相同参数因为机械重心、摩擦不同会略有差异。实际测下来X轴和Y轴参数大概相差10%左右别指望一套参数通吃。3.3 PID参数解析Kp、Ki、Kd到底怎么用初学PID的容易把三个参数背得滚瓜烂熟但真到了现场还是不懂调。我用一个比较接地气的方式来解释Kp比例你看到球偏了施加多大力气去纠正。Kp太小球偏了反应迟钝慢吞吞晃回去Kp太大反应极其生猛球直接冲过头然后来回振荡。Ki积分长期累积的小偏差靠它消掉。平台因为重心偏一点、摩擦阻力略大回不到正中就用积分一点一点磨掉。但这家伙放大了容易“积分饱和”——偏差积累太多撤掉偏差后输出还在满冲小车直接飞车。Kd微分观察偏差变化的趋势趋势太快就提前“刹车”。它增加阻尼、抑制超调但如果你信号噪声大微分放大噪声能力比P还可怕。调参的顺序我个人推荐先P后D再I也有人先P后I再D但机器人系统我强烈建议把I放最后因为积分会掩盖一部分比例不足的问题等P、D调完再加积分修正静差会清晰很多。还有一个重度建议一定要给PID输出加限幅以及给积分加限幅这个在裸机实现时是必须处理的。3.4 裸机PID实现与运行周期稳定性我在STM32上的实现是用定时器中断驱动的。TIM3产生2ms中断执行速度环内环在循环里计数到第10次也就是20ms执行位置环外环。串口接收中断单独跑用于接收视觉坐标。代码核心示意增量式PID速度环用typedef struct { float Kp, Ki, Kd; float target; float integral; float last_err; float out; float out_max; // 输出限幅 float integral_max; // 积分限幅 } PID_TypeDef; float PID_Update(PID_TypeDef *pid, float feedback) { float err pid-target - feedback; pid-integral pid-Ki * err; if (pid-integral pid-integral_max) pid-integral pid-integral_max; if (pid-integral -pid-integral_max) pid-integral -pid-integral_max; float d pid-Kd * (err - pid-last_err); pid-last_err err; pid-out pid-Kp * err pid-integral d; 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; }注意微分这里我只对误差做差分没有对反馈信号做滤波。但前面也说了如果反馈信号本身有噪声这个D会非常容易引发高频抖动。所以内环速度反馈我用了编码器读取后的滑动平均取最近4次求平均外环位置反馈就在上位机做了低通。这属于“硬件上没有传感器滤波软件上必须做”的实际情况。还有一个裸机项目的关键点控制周期必须稳定。不要在循环里做HAL_UART_Receive这种阻塞调用串口接收用DMA中断。否则一旦某次接收耗时过长CPU没能在2ms定时器中断里及时完成计算下一个中断一来控制周期就变了PID的积分和微分全部失真。这个问题在快速调D参数时会直接体现在球体高频抖动上。4. 联调与参数整定现场实录代码写完只是万里长征第一步真正的困难在调参。我这边调试用了三个晚上熬到凌晨才把整台机器稳定住。这里把调试顺序、方法、遇到的坑都记录下来。4.1 联调顺序建议我不建议一上来就把视觉控制全跑起来因为你不知道球跑飞是视觉算错坐标、还是速度环没跟上、还是位置环出了饱和。每一层都要单独验证开环测试电机发一个固定PWM确认电机转动方向正确、编码器读值方向正确。方向反了是新手最容易忽略的一上来就发现球被推得更远。开环测试视觉屏幕上打印小球坐标看它能不能稳定跟踪。手推动球确认坐标值变化方向与物理方向一致。内环速度闭环先把位置环去掉给目标速度固定值用串口把实际速度打印出来确认速度能快速响应且没有低频振荡。外环位置闭环把视觉坐标接入外环球放偏一点看它能不能慢慢回中然后再一步步加大扰动。顺序上强烈建议卡死内环不稳绝不接外环。速度环只要有一点收敛慢位置环会来回打摆子。4.2 参数整定方法与实践数据我用的是经典的临界比例度法先把Ki和Kd都设成0Kp从一个很小值开始加直到输出开始等幅振荡记下这个Kp为临界增益Kc振荡周期为Tc。然后参考经验公式P控制器Kp 0.5 * KcPI控制器Kp 0.45 * Kc, Ki 0.054 * Kc / TcPID控制器Kp 0.6 * Kc, Ki 0.12 * Kc / Tc, Kd 0.075 * Kc * Tc注意这只是起步点。我实测速度环合随后初始参数取Kp7.5、Ki0.02、Kd0.25随后做微调。等速度环那套稳定之后再接位置环位置环也是先只用PKp大约0.8左右球开始来回摆动后加Kd抑制超调最后再加小积分。最终大体参数如下会随电机和机械变化仅供参考控制环KpKiKd输出限幅控制周期速度环内环7.50.050.20±800PWM2ms位置环外环0.90.030.15±300mm/s20ms这里有个调参工具上的经验把PID内部变量误差、目标、输出通过串口实时打印出来在电脑上用串口绘图工具我用的是SerialPlot看波形。很多人调不好PID很大原因是光靠眼睛看球反应这太慢了。波形图能看到偏差衰减曲线是收敛还是振荡还可以看出超调量是多少一目了然。建议调试时直接把几条曲线实时显示出来比肉眼判断高效很多。4.3 联调中的系统性现象与对策调试过程中记录了一些“症状”和对应策略小球高频抖动、发出滋滋声多半是微分项或者视觉抖动放大。先检查视觉坐标是否稳定再降低Kd最后确认PWM频率低频PWM几百赫兹会让电机啸叫我后来把PWM频率提到了20kHz直接没声音了。小球回中极慢像没吃饭一样通常是外环Kp太小或者视觉滞后太大。先把坐标平滑系数调低一点增加响应性再适当提高外环Kp。来回大范围振荡、停不下来不要盲目加D先看是不是限幅饱和。如果速度环输出经常顶到限幅说明内环跟不上外环目标速度这时候把外环目标速度限幅调低些比多加Kd有效。小球在中心附近小幅徘徊积分太小或死区设置不合理增加积分或者把死区宽度减少到±2mm以内。5. 常见问题与排查技巧实录我自己整理了一个排查速查表写在下面。以后出问题先对着看能省大量时间。现象可能原因解决对策视觉坐标偶尔跳变很大光照变化、阈值漂移关闭自动白平衡、加遮光罩、形态学滤波、限制单帧跳动范围视觉一直找不到球球色超出HSV范围用摄像头实时取HSV值检查下界上界是否过窄换高饱和球电机转向导致球越跑越远PID正反作用错误将期望方向与实际偏转方向对比调整目标为负或反馈为负平台高频抖动Kd过大、控制周期不稳定、PWM频率太低导数滤波、确保定时器中断不受阻塞、PWM频率设置≥20kHz球反复居中又超过位置环Kp过大或速度环响应慢先调内环再减小外环Kp必要时调高外环限幅系统有静态偏差到不了中心积分不足增加Ki同时设置积分限幅防饱和球一加速就猛偏机械间隙或编码器滑移检查减速齿轮是否打滑编码器连接是否紧固最后分享一个排查思路先确认“感知→决策→执行”哪一环掉链子再下手改。测试时在串口助手里同时看视觉坐标、目标速度、实际PWM和编码器速度一个环节一个环节地排除。我自己最常犯的错误是一看到球不稳定就直接去调PID结果改了半天发现是视觉坐标的偶发跳变在捣乱纯属白费功夫。所以别偷懒把数据打出来一眼就能定位问题。
