1. 两轮差速机器人轨迹跟踪到底难在哪两轮差速机器人是移动机器人里最经典的构型之一左右两个驱动轮独立控制转速靠速度差实现转向。听起来简单但真正让它沿着一条期望轨迹跑起来中间要跨过的坑比想象中多得多。我自己第一次做轨迹跟踪的时候觉得不就是两个轮子转嘛结果仿真一跑机器人画出来的轨迹跟喝醉了酒一样要么画龙要么直接飞出去。这个项目的核心目标很明确在Simulink环境下从零搭建一个两轮差速机器人的运动学模型设计PID控制器让机器人能够跟踪给定的参考轨迹直线、圆弧、任意曲线并且通过调参让跟踪误差收敛到可接受的范围。适合正在做机器人课程设计的学生、刚接触移动机器人控制的工程师以及想从Simulink仿真入手理解PID在非线性系统中应用的技术爱好者。为什么选Simulink而不是纯代码因为Simulink的图形化建模能让你直观看到信号怎么流、误差怎么算、控制量怎么给。尤其是调参阶段改一个Kp值直接跑仿真看轨迹变化比在代码里改参数再编译烧录快太多了。而且Simulink支持从模型直接生成C代码后续要部署到STM32或者其他嵌入式平台路径是通的。但这里有个关键认知两轮差速机器人是一个典型的非完整约束系统。什么意思就是它不能横向平移只能沿着车头方向前进和绕自身旋转。这个约束导致它的运动学模型是非线性的不是简单的一个传递函数就能描述。所以PID控制在这里的应用跟控制一个直流电机或者温控系统完全不是一回事。你需要先把运动学关系理清楚才能设计出合理的控制结构。我见过太多人上来就搭PID误差直接乘Kp给轮速结果怎么调都调不好。问题出在控制结构上不是参数上。后面我会详细拆解正确的控制架构应该怎么搭。2. 运动学建模把机器人的位姿关系搞清楚2.1 差速运动学方程的推导逻辑两轮差速机器人的位姿用三个量描述x坐标、y坐标、航向角θ。这三个量构成状态向量。机器人的线速度v和角速度ω由左右轮速决定。设左轮线速度为vL右轮线速度为vR轮半径为r两轮间距为L。则机器人线速度 v (vL vR) / 2机器人角速度 ω (vR - vL) / L这两个公式是整个运动学模型的基础。为什么角速度是(vR - vL)/L你可以这样理解如果左轮不动右轮转机器人绕左轮画圆圆的半径就是L角速度就是右轮线速度除以半径。如果两轮反向转机器人绕两轮中点旋转半径是L/2角速度是(vR - vL)/L。这个推导在纸上画个图就清楚了。然后位姿更新方程是dx/dt v * cos(θ)dy/dt v * sin(θ)dθ/dt ω这三个微分方程就是差速机器人的运动学模型。注意cos(θ)和sin(θ)的存在说明系统是非线性的。θ变化会直接影响x和y方向的速度分量这就是为什么不能用简单的线性PID直接控制位置。在Simulink里搭建这个模型有两种方式。一种是用Integrator模块直接积分这三个微分方程另一种是封装成MATLAB Function模块。我推荐用MATLAB Function因为代码写起来直观而且后续要改逻辑方便。2.2 Simulink建模的具体操作步骤打开Simulink新建一个Blank Model。从Library Browser里拖入以下模块两个Constant模块分别给vL和vR赋值后续会替换成控制器的输出一个MATLAB Function模块写运动学方程三个Integrator模块分别积分x、y、θ一个Mux模块把x、y、θ合成一个向量输出Scope模块用于观察轨迹MATLAB Function里的代码大概长这样function [x_dot, y_dot, theta_dot] kinematics(vL, vR, theta) r 0.05; % 轮半径单位米 L 0.2; % 轮间距单位米 v (vL vR) / 2; omega (vR - vL) / L; x_dot v * cos(theta); y_dot v * sin(theta); theta_dot omega; end这里有个细节要注意θ的积分结果会不断增大如果不做处理cos和sin虽然能算但数值会越来越大。建议在MATLAB Function里对θ做mod(θ, 2*pi)处理保持角度在0到2π之间。这个坑我踩过仿真跑久了θ变成几千虽然cos值没问题但看着难受而且后续做误差计算时容易出问题。另外轮半径和轮间距的单位要统一。我习惯用米和秒因为标准单位制下不容易出错。如果你用厘米那速度单位就是厘米每秒积分出来的位置就是厘米最后画图的时候注意坐标轴标注。2.3 参考轨迹的生成方法轨迹跟踪需要一条参考轨迹。最简单的做法是参数化生成。比如直线轨迹t 0:0.01:10; x_ref 0.5 * t; y_ref 0.3 * t; theta_ref atan2(0.3, 0.5) * ones(size(t));圆弧轨迹R 2; omega_ref 0.5; x_ref R * cos(omega_ref * t); y_ref R * sin(omega_ref * t); theta_ref omega_ref * t pi/2;注意theta_ref的初值。如果圆弧从(2,0)开始逆时针转初始航向角应该是π/2因为速度方向是切线方向。这个细节很容易搞错导致一开始误差就很大。在Simulink里可以用MATLAB Function模块根据仿真时间t生成参考位姿也可以用From Workspace模块导入预先算好的数据。我推荐用MATLAB Function因为改轨迹方便不用重新生成数据。参考轨迹的导数也很重要。有些控制算法需要参考速度v_ref和ω_ref这些可以通过对参考位姿求导得到。对于圆弧轨迹v_ref R * omega_refω_ref就是常数。对于更复杂的轨迹可以用数值差分近似。3. PID控制器设计从单环到级联的演进3.1 为什么直接对位置误差做PID效果不好最朴素的想法是计算x方向误差ex x_ref - xy方向误差ey y_ref - y然后分别做PID输出给左右轮。我试过这个方案结果很糟糕。问题在于x和y的误差跟左右轮速之间没有直接的解耦关系。你增大左轮速机器人既会前进也会转向x和y都会变。两个PID互相耦合调好了一个另一个就乱了。而且当机器人航向角偏离很大时x方向的误差可能通过转向来消除更高效但位置PID不知道这件事。更合理的思路是把控制目标分解为“前进速度控制”和“航向控制”。机器人先对准目标点然后前进。这就是所谓的“朝向目标点”策略。3.2 级联PID控制结构的设计我最终采用的是一种双环结构外环控制位置误差生成参考速度内环控制轮速跟踪参考速度。具体来说外环计算当前位姿与参考位姿的误差在机器人局部坐标系下表示。设全局误差为(ex, ey)转换到局部坐标系ex_local cos(theta) * ex sin(theta) * ey; ey_local -sin(theta) * ex cos(theta) * ey;ex_local是纵向误差前进方向ey_local是横向误差垂直于前进方向。然后线速度指令 v_cmd Kp_v * ex_local角速度指令 ω_cmd Kp_omega * ey_local Kd_omega * (ey_local - ey_local_prev) / dt这里角速度控制用到了横向误差和它的变化率相当于PD控制。为什么不用积分项因为积分项在轨迹跟踪里容易导致超调和振荡尤其是轨迹曲率变化时。我试过加积分效果反而变差后来去掉了。内环把v_cmd和ω_cmd转换为左右轮速指令vL_cmd v_cmd - omega_cmd * L / 2; vR_cmd v_cmd omega_cmd * L / 2;然后左右轮各用一个PID控制器跟踪轮速指令。如果电机模型是简单的惯性环节内环PID可以调得比较快让轮速快速跟上指令。这个结构的优势在于外环负责“往哪走”内环负责“走多快”职责清晰调参有章可循。外环的Kp_v决定机器人向目标点靠近的速度Kp_omega决定纠正横向偏差的力度Kd_omega抑制横向振荡。3.3 参数整定的实操方法调参顺序很重要。我的习惯是第一步先把内环调好。给左右轮一个阶跃速度指令看轮速响应。如果超调大就减小Kp如果响应慢就增大Kp。内环的Ki一般给很小的值或者不给因为轮速控制不需要消除稳态误差外环会处理。第二步调外环的Kp_v。先给一个较小的值比如0.5让机器人慢慢向目标点移动。观察纵向误差是否收敛。如果收敛太慢就增大如果冲过头就减小。第三步调外环的Kp_omega。这个参数决定航向纠正的力度。太小了机器人画龙太大了机器人会剧烈摆动。我一般从1.0开始试根据轨迹效果调整。第四步加一点Kd_omega抑制振荡。Kd_omega的值一般是Kp_omega的0.1到0.5倍。可以用试凑法先给Kp_omega的0.2倍看效果再微调。这里给一组我实测可用的参数轮半径0.05m轮间距0.2m机器人最大线速度1m/s参数数值作用Kp_v2.0纵向速度增益Kp_omega3.0航向角速度增益Kd_omega0.5航向角速度微分增益内环Kp10轮速比例增益内环Ki1轮速积分增益这组参数在直线和圆弧轨迹下都能跑出不错的效果。但要注意参数跟机器人物理参数强相关换一个轮间距或者轮半径参数需要重新调。注意调参时一定要把仿真步长设小一点比如0.001秒。步长太大微分项计算不准而且积分会累积误差。我用0.01秒步长时Kd_omega稍微大一点就振荡改成0.001秒后稳定多了。4. Simulink仿真实操全流程4.1 模型整体架构搭建整个Simulink模型分为四个部分参考轨迹生成、误差计算、控制器、机器人运动学模型。信号流向是参考轨迹和当前位姿比较得到误差误差经过控制器输出轮速指令轮速指令驱动运动学模型更新位姿位姿反馈回去继续比较。具体模块清单Clock模块提供仿真时间MATLAB Function参考轨迹根据时间生成x_ref, y_ref, theta_refMATLAB Function误差计算计算局部坐标系下的误差MATLAB Function外环控制器输出v_cmd和ω_cmdMATLAB Function轮速分配输出vL_cmd和vR_cmd两个PID Controller模块内环轮速控制MATLAB Function运动学模型更新位姿三个Integrator模块积分x, y, θScope模块显示轨迹和误差曲线这里有个技巧把外环控制器和轮速分配写在一个MATLAB Function里减少模块数量模型看起来清爽。但如果你要单独调外环参数分开写更方便。我一般分开写因为调参时经常要单独禁用某一环。4.2 关键模块的参数配置PID Controller模块的配置要注意几点。采样时间设为-1表示继承上游信号的采样时间或者直接设成仿真步长。积分器初始条件设为0除非你有特殊需求。输出限幅一定要设比如轮速指令限制在-10到10 rad/s之间防止控制器输出过大导致仿真崩溃。Integrator模块的初始条件就是机器人的初始位姿。我一般设x0, y0, θ0。如果你想测试机器人从偏离轨迹的位置开始跟踪可以把初始值设成偏离值比如x-1, y-0.5, θ0.5。这样能看出控制器的收敛能力。MATLAB Function模块里要注意数据类型。Simulink默认是double但如果你后续要生成嵌入式代码可能需要改成single。仿真阶段用double就行精度高。4.3 仿真运行与结果分析设置仿真时间10秒步长0.001秒运行。打开Scope看轨迹。理想情况下机器人的实际轨迹应该跟参考轨迹重合。如果一开始有偏差应该能看到实际轨迹逐渐向参考轨迹靠拢。我通常会画三个图一个是x-y平面轨迹图一个是纵向误差随时间变化一个是横向误差随时间变化。轨迹图看整体效果误差曲线看收敛速度和稳态误差。如果轨迹图显示机器人绕参考轨迹画圈说明Kp_omega太大或者Kd_omega太小。如果机器人慢慢偏离参考轨迹说明Kp_v太小或者Kp_omega太小。如果机器人一开始偏差大但很快收敛说明参数基本合适。还有一个常见现象机器人到达参考轨迹后沿着轨迹走但有小幅振荡。这是Kd_omega不够导致的。增大Kd_omega可以抑制但太大会引入高频噪声。折中一下Kd_omega取Kp_omega的0.3倍左右比较合适。提示仿真跑完后用plot命令把参考轨迹和实际轨迹画在一起比在Scope里看更清楚。可以用plot(x_ref, y_ref, r--, x_actual, y_actual, b-)红色虚线是参考蓝色实线是实际。5. 常见问题与排查技巧实录5.1 仿真报错与数值问题排查问题一仿真跑着跑着就卡住或者报错“Derivative input is Inf or NaN”。这个通常是因为控制器输出过大导致积分饱和或者运动学模型里除了零。检查PID输出限幅是否设置检查轮间距L是否为零。还有一种可能是θ积分到很大后cos和sin计算出现数值问题加mod处理。问题二轨迹跟踪效果时好时坏换一条轨迹就不行了。这是参数鲁棒性的问题。PID参数是在特定轨迹下调出来的换轨迹后误差特性变了。解决办法是降低Kp_v和Kp_omega牺牲一点收敛速度换取鲁棒性。或者改用增益调度根据误差大小动态调整参数。问题三内环轮速跟踪不上指令。检查内环PID参数是否太小或者电机模型的时间常数是否太大。如果电机模型是一阶惯性环节时间常数0.1秒内环Kp至少要到10才能让轮速在0.1秒内跟上。如果电机模型有延迟内环需要加前馈。5.2 轨迹跟踪效果不佳的调参速查表现象可能原因调整方法机器人画龙Kp_omega太大或Kd_omega太小减小Kp_omega增大Kd_omega收敛太慢Kp_v或Kp_omega太小适当增大超调严重Kp_v太大或Kd_omega太小减小Kp_v增大Kd_omega稳态误差大缺少积分项或积分增益太小加小积分增益高频振荡仿真步长太大或Kd_omega太大减小步长减小Kd_omega初始偏差大时失控控制器输出限幅太宽收紧限幅5.3 从仿真到嵌入式部署的注意事项Simulink模型调好后如果要生成C代码部署到STM32或者其他MCU上有几个坑要注意。第一MATLAB Function里的代码要符合代码生成规范。不能用动态内存分配不能用递归变量要预定义类型。我一般会在MATLAB Function里加%#codegen指令让MATLAB检查代码生成兼容性。第二PID Controller模块生成代码时积分项和微分项的实现方式要确认。Simulink默认的PID是理想型还是实际型积分限幅怎么处理这些都要在配置里看清楚。我习惯用实际型PID带积分限幅和微分滤波更适合嵌入式。第三仿真步长和实际控制周期要匹配。仿真用0.001秒实际控制周期如果是0.01秒参数需要重新调。因为微分项对步长敏感步长变了微分效果就变了。我一般会把仿真步长设成跟实际控制周期一样这样参数可以直接用。第四浮点运算和定点运算的差异。Simulink仿真默认是doubleSTM32如果是M3内核没有浮点单元用float会慢用定点又需要额外处理。建议先在Simulink里用single类型仿真一遍看效果是否可接受。注意生成代码前一定要做一次Simulink Test用测试用例覆盖直线、圆弧、大偏差初始条件等场景。我吃过亏仿真看着好好的实际跑起来遇到一个没测过的场景就发散了。6. 进阶方向从PID到更高级的控制策略PID在轨迹跟踪上能跑出不错的效果但它的局限性也很明显。当轨迹曲率变化剧烈时PID的跟踪误差会明显增大。当机器人速度提高时PID的相位滞后会导致跟踪不上。这时候可以考虑几个进阶方向。第一个方向是前馈加反馈。在PID反馈的基础上把参考速度v_ref和ω_ref直接前馈给轮速指令PID只负责补偿误差。这样能大幅减小跟踪滞后。前馈项的推导需要参考轨迹的导数对于参数化轨迹很容易得到。第二个方向是模型预测控制。用运动学模型预测未来若干步的位姿优化控制量使预测误差最小。MPC处理约束和曲率变化的能力比PID强但计算量大适合算力充足的平台。第三个方向是滑模控制。滑模对扰动和参数不确定性鲁棒性强但抖振问题需要处理。我试过在Simulink里搭滑模控制器跟踪效果确实好但抖振导致轮速指令高频切换实际电机可能受不了。第四个方向是Simulink和CarSim联合仿真。CarSim提供高精度的车辆动力学模型Simulink负责控制算法。这样能验证控制算法在更真实模型下的效果。不过CarSim是商业软件学习成本不低。我个人建议先把PID吃透理解清楚每个参数对跟踪效果的影响再往高级算法走。PID调不好的问题换MPC大概率也调不好因为底层的问题是一样的误差怎么定义、控制结构怎么设计、参数怎么整定。这些基本功扎实了换什么算法都是水到渠成的事。最后分享一个我在实际项目中总结的小技巧调参时不要只盯着一条轨迹调。准备三条轨迹——直线、圆弧、S形曲线每次调完参数三条都跑一遍。如果三条都能跑好参数才算真正可用。只调一条轨迹很容易过拟合换条轨迹就露馅了。这个习惯帮我省了很多返工的时间。