基于Simulink的全向底盘狭窄通道轨迹规划与闭环控制

全向底盘配上狭窄通道这个组合听起来像是“优势拉满”的场景。麦轮或者全向轮的车子能横移、能斜着走转个弯不需要转弯半径按理说过个窄道应该很轻松才对。但真把车开到现场就会发现事情没这么简单通道太窄车体的姿态角稍微偏一点就可能蹭墙转向过程里轮速一旦配比不当车就会原地打转或者跑偏更别说某些通道还带着弯纯靠手动摇杆想一把开过去基本靠运气。这个项目的核心就是利用Simulink把“过狭窄通道”这件事从手忙脚乱的临场操作变成一条预先算好的、平滑且安全的轨迹再配合闭环控制让底盘沿着轨迹自动走完。本质上解决的是三个问题一是全向底盘运动学模型的建立与验证二是狭窄通道场景下的轨迹规划与优化三是Simulink环境下从仿真到参数整定的完整工作流。适合刚接触Simulink的初学者也适合已经在做AGV或者移动机器人项目、但苦于调轨迹不稳的开发者参考。全文会用一套完整的项目流程来讲模型怎么搭、约束怎么设、轨迹怎么优化、Simulink里怎么一步步实现以及我在调试中踩过的坑和排错经验。1. 项目整体设计与思路拆解1.1 为什么全向底盘也会卡在狭窄通道里很多人有一个误区全向底盘既然能平移那过窄通道就是简单地把车横着挪过去。但在真实场景里通道往往不是笔直的车在进入通道前需要一个姿态调整过程进入通道后要保持车身与通道走向平行出通道后可能还要立即转向对接下一个工位。这一连串的动作如果靠人工操作对摇杆的力度和时机要求极高。另外全向底盘的运动控制本身有一个特点它虽然自由度多但每个轮子的速度必须严格满足运动学约束。四个或者三个轮子的速度一旦配合不精准车体会出现内耗——两个轮子较劲虽然车没动但电机电流飙升。这在狭窄通道里是致命的因为车一旦在路上出现微小偏移再想纠偏就来不及了。所以这个项目真正要解决的不是“能不能过”而是“怎么平滑、稳定、安全地过”。这决定了我们需要一条几何上无碰撞的路径还需要一个可以实时跟踪这条路径的闭环控制系统。1.2 为什么选用Simulink来做这件事做轨迹优化的工具有很多可以直接写Python调优化库也可以用MATLAB脚本做数值求解。但在完整项目里轨迹优化只是起点后面还有运动学解算、闭环控制、电机执行、传感器反馈。如果全部用代码手写逻辑分散调试困难尤其涉及不同模块之间的数据交互时很容易出隐蔽的bug。Simulink的价值在于图形化建模和分层设计。你可以把整个系统拆成几个模块参考轨迹生成模块、运动学逆解模块、轨迹跟踪控制器、底盘模型、误差监视模块。每个模块负责单一功能中间用信号线连接数据流向一目了然。遇到问题的时候可以单独运行某个模块也可以随时用Scope查看任意节点的信号波形。更重要的是Simulink支持从模型直接生成C代码。这意味着你在Simulink里调试通过的控制逻辑后期可以直接部署到嵌入式控制器上不需要重新写一遍。对做实际项目的人来说这一步能省下大量时间。1.3 整体技术路线的分层设计在动手搭建之前我先梳理了整个项目的分层思路在这里分享给大家几何约束层描述通道尺寸、车辆尺寸、通道走向、起点和终点的位姿要求。轨迹生成层在满足几何约束的基础上通过参数化曲线生成候选轨迹并利用优化指标从中选出“最优”轨迹。控制层以轨迹为参考输入设计闭环控制器实时计算底盘各轮的目标转速。模型层建立底盘的Simulink运动学/动力学模型用于验证控制算法同时模拟传感器噪声等非理想因素。验证层通过多组仿真场景对比确认系统在典型工况下的跟踪精度、稳定性和安全性。这个分层思路的好处在于每一层都可以独立测试和替换。比如后期想换一种轨迹优化算法只需要改轨迹生成模块想换一种控制器只需要改动控制层。对整个项目来说不管是学习还是迭代都清晰很多。2. 全向底盘运动学模型是绕不开的第一道坎2.1 麦克纳姆轮底盘的三个核心运动方程我们这个项目用的是麦克纳姆轮底盘也就是大家常说的麦轮车。四个轮子按特定方式排列每个轮子的辊子轴线与轮子轴线成45度角。这种结构让底盘能实现前后直行、左右平移和原地旋转的自由组合。要规划轨迹、控制底盘首先得把“车体速度”和“四个轮子转速”之间的关系搞清楚。麦轮底盘的逆运动学可以写成矩阵形式但对初学的人可以先记住三个简化方程。我以常见的X型排布为例设轮距为2a轴距为2b轮子半径为R车体速度为(vx, vy, ω)左前轮转速w_FL (1/R) * (vx - vy - (a b) * ω)右前轮转速w_FR (1/R) * (vx vy (a b) * ω)左后轮转速w_RL (1/R) * (vx vy - (a b) * ω)右后轮转速w_RR (1/R) * (vx - vy (a b) * ω)注意正负号的约定可能与一些教材不同但本质是一样的。关键点是任意一个车体速度都能唯一对应一组轮速反过来给定轮速也能算出车体速度。这个映射关系在Simulink里可以写成矩阵乘法也可以写成MATLAB Function块。为了更直观我用一个简单的例子给大家演示。假设车体目标速度是“正前方1m/s没有横向速度不旋转”即(vx1, vy0, ω0)代入公式可以看到四个轮子转速是相同的这对应底盘直线前进。如果给的目标是“正右方1m/s不旋转”即(vx0, vy1, ω0)你会发现四个轮子里有两组正负号不同的转速这正是麦轮横向平移的实现方式。2.2 在Simulink里搭建运动学模型运动学模型在Simulink里的搭建方式不复杂但有几个细节值得注意。我用的是Simulink中的MATLAB Function块来封装逆运动学计算理由是这个公式本身很简单用Simulink基本模块拆开搭反而容易出错。具体步骤是新建一个Simulink模型命名清楚例如“Mecanum_Kinematics.slx”。添加一个MATLAB Function块双击进入编辑界面输入底盘参数轮距、轴距、轮子半径作为参数。定义输入为车体速度向量[vx; vy; omega]输出为四个轮子的角速度向量[w_FL; w_FR; w_RL; w_RR]。在函数体内直接写出2.1节中的公式注意矩阵运算时保持维度一致。再用一个MATLAB Function块实现正运动学用于仿真时从轮速反推车体速度方便后续对比。这样搭出来的模型看起来简单但它是整个仿真系统的基础后期所有控制逻辑都要依赖它。如果你一开始就在这里搞错坐标方向或者正负号后面整个项目的轨迹都会是歪的。2.3 运动学模型验证的两种方法模型搭好之后不能急着往下走你得先验证它是对的。我常用的方法有两种单元测试法直接在MATLAB脚本里调用运动学函数给一组已知的车体速度打印轮速结果人工验算是否符合预期。例如对于纯平移四个轮子速度一致对于纯旋转四个轮子速度大小相等、方向两两相反。仿真目测法在Simulink里把轮速接到积分器或者直接输出到动画模块给一段恒定速度输入观察底盘的位置变化轨迹。如果给的是纯直线运动指令底盘走出来的路线却是一条弧线那多半是轮子方向定义或者辊子角度配置错了。这两种方法都跑通之后才算有了讨论轨迹规划的基础。很多初学者上来就跳过去结果后面发现所有仿真结果全都不对再回头排查时间成本高得多。3. 狭窄通道轨迹优化到底在优化什么3.1 把“能过”变成数学约束狭窄通道通过性问题首先要变成几何约束。拿一个最简单的场景举例一条宽度为w的直线通道中心线是y0通道范围是[-w/2, w/2]。对于一个长为L、宽为W的矩形底盘如果沿中心线直行且姿态角为0那么车体边缘到通道壁的距离是W/2只要W/2 w/2就能过。但一旦姿态角不为0车体在通道宽度方向上的投影会变成Wcos(θ) Lsin(θ)。我在实际测试中算过一个数据底盘长度1.2米宽度0.7米通道宽度0.9米。姿态角0度时安全裕量有0.1米姿态角偏到5度时投影宽度变成约0.8米黏到极限偏到10度时投影宽度约0.9米刚好蹭着墙。这就说明在狭窄通道中车身姿态角是决定能否通过的关键因素光是“路径”正确还不够姿态也必须同时控制在容许范围内。所以在轨迹优化时约束条件必须同时包含位置约束和姿态约束位置约束车体几何中心到通道壁的距离必须小于安全距离。姿态约束车体长轴与通道中心线夹角必须保持在一定范围工程上通常控制在±3度以内。运动学约束轮速不能超过电机最大转速加速度不能超过电机最大转矩对应的值。3.2 轨迹参数化先用五次多项式把轨迹画出来约束有了接下来就是生成满足约束的轨迹。我采用的是基于五次多项式插值的参数化方法这是实际工程里很常见也比较稳的策略。五次多项式轨迹的含义是把车体横坐标变化与时间的关系用五阶多项式描述即x(t) a0 a1 t a2 t² a3 t³ a4 t⁴ a5 t⁵。为什么要五次而不是三次因为三次多项式只能保证位置和速度连续加速度会存在突变体现在轨迹上就是加速度曲线有“折角”。对于电机控制来说加速度突变意味着加加速度jerk无穷大实际执行时会产生冲击和振动。五次多项式给了我们足够的自由度可以同时约束起点和终点的位置、速度、加速度让整个轨迹平滑启动、平滑停止。举一个具体的例子。假设车辆起点在x0终点在x3起点和终点的速度与加速度都为0总时间T5秒。六