前几天我把一个搁置了挺久的仿真项目重新翻出来打磨复杂环境中轮式铰接车辆的精确轨迹优化研究全程用Matlab实现。这个课题一句话概括就是让一辆轮式铰接车辆在存在静态障碍物、窄通道、目标位姿约束的场地里自己算出一条既能通过、又能精确停到目标点的轨迹。听起来不复杂但真要在Matlab里把建模、优化、代码、可视化全串起来坑还是不少。这篇文章不是教科书式的公式堆砌而是我从实际调试角度做的完整复盘。适合正在做工程机械、特种车辆、轮式机器人轨迹规划课题的同学也适合要接手铰接式车辆控制项目的工程师。如果你只是听说过“铰接角”“轨迹优化”“fmincon”这些词还没真正跑通过一次那这篇文章可以帮你省掉很多试错时间。1. 这个课题到底卡在哪儿1.1 铰接车辆为什么比普通小车难规划轮式铰接车辆常见的有装载机、铰接式矿卡、铰接式叉车、机场牵引车等。它和普通乘用车最大的区别在于转向方式普通车靠前轮或后轮主动转向而铰接车是靠前后的车架通过一个垂直铰接销连接再由液压缸推动改变前车架相对后车架的夹角这个角就是“铰接角”。这个结构带来了一个很麻烦的特性车辆转弯时前车架和后车架会“错开”车身轮廓不是一个固定矩形而是随着铰接角不断变化。换句话说你规划轨迹时不能简单把车当成一个方框在平面上移动必须同时考虑前后车身的位置和朝向。再加上铰接式车辆普遍强调低速重载、大扭矩、在狭窄矿山巷道或工地里工作留给规划算法的安全余量通常很小后面的车厢还可能“摆尾”这就让轨迹规划问题比常规AGV难了一个量级。更关键的是铰接车也是一种典型的非完整约束系统。它不能像全向移动平台那样横着走、原地转只能通过速度方向与航向的约束关系来运动。这类约束没法简单通过几何路径独立求解必须在优化模型里变成等式约束和轨迹本身一起解出来。1.2 “精确轨迹优化”在复杂环境里指的是什么标题里“精确”两个字不是指优化算法算出来的数值精度高而是指轨迹要在几种互相矛盾的需求之间做权衡。首先轨迹要保证车辆能安全通过复杂环境比如绕过障碍物、钻进窄门、倒车入库。其次到达终点时不能只是“大概到了”航向、铰接角、位置都要满足指定要求比如矿卡的轮胎对准装载点。第三轨迹还要让执行机构能干得动。如果轨迹本身很平滑但铰接角变化率忽大忽小或者车速要求瞬间跳变真实液压系统根本跟不住仿真里看着好看的轨迹落不了地。所以“精确”与“优化”的组合本质上是在路径长度、跟踪精度、控制平滑性、机械约束、实时性之间找平衡点。这也是我建议每个做类似课题的人先把目标函数和约束条件拆清楚再做代码的原因。2. 先搭运动学模型再谈优化2.1 铰接车运动学方程的一种常用形式做轨迹优化第一步不是急着写fmincon而是先把车辆模型定下来。我在这个项目里用的是工程上很常见的低速运动学模型。状态量取x、y后车架中心或后轴中心在地面坐标系中的位置θ后车架航向角γ前车架相对后车架的铰接角控制量取v纵向车速ω铰接角变化率对应的运动学方程可以写成x_dot v * cos(theta); y_dot v * sin(theta); theta_dot v * sin(gamma) / L; gamma_dot omega_gamma;其中 L 是等效轴距。这个模型的物理含义是把铰接转向近似成自行车模型里的前轮转角只是这里的“前轮转角”变成了铰接角。它成立的前提是低速、轮胎侧偏可以忽略这个假设在装载机、矿卡这类作业场景里基本合理。如果后续要做高速工况就需要再加轮胎力、侧偏角、质量转移等动力学项模型复杂度会上升一截但轨迹优化的基本框架不变。有的文献会把前后车架拆成两个刚体用速度约束反推出更精确的转向关系。这种模型更接近真实铰接运动但用于轨迹规划时它和上述简化模型在结果上的差别通常远小于障碍物建模、初值质量对轨迹的影响。所以我建议第一版代码先用这个简洁模型跑通再根据实际需要升级。2.2 状态量与参数的Matlab定义这个项目在Matlab里推荐用一个结构体统一定义车辆参数不要散落在脚本各个角落。我习惯这样写veh.L 3.2; % 等效轴距单位m veh.dt 0.1; % 离散时间间隔单位s veh.Npts 50; % 轨迹离散点数状态点数量 veh.vMin -0.8; % 最小车速单位m/s允许小范围倒车 veh.vMax 2.0; % 最大车速单位m/s veh.gammaMax deg2rad(35); % 最大铰接角单位rad veh.gammaDotMax deg2rad(40); % 最大铰接角变化率单位rad/s veh.dSafe 0.6; % 障碍物安全距离单位m veh.circleR 1.2; % 车身圆形包络半径单位m这些参数不是我随手填的而是参考某工程车辆样机参数后拍的一个合理初始值。实际项目里最大铰接角通常受机械限位和液压缸行程影响最大车速和最大铰接角变化率则受液压系统流量影响。调试时先用保守值等轨迹能稳定跑通后再逐步放宽观察优化结果怎么变化。状态量和控制量在优化变量里需要展平成一维向量。假设有 Npts 个状态点那么状态变量就是 Npts×4 个控制变量是 (Npts-1)×2 个。为了后面写约束函数方便我写了一个索引拆分函数function [zs, us] unpackX(X, veh) zs reshape(X(1:veh.Npts*4), 4, veh.Npts); us reshape(X(veh.Npts*41:end), 2, veh.Npts-1); end这样在目标函数、约束函数、可视化脚本里都能直接用统一的拆包方式避免索引写错。很多人做优化卡住的第一个点就是优化变量拆包和组合时维度不匹配提前封装好这个函数能省很大力气。3. 把轨迹优化变成一个可求解的数学问题3.1 目标函数路径、效率和平滑性怎么加权轨迹优化本质上是一个带约束的非线性规划问题。我在项目里把目标函数拆成三个部分跟踪精度项终点位置、航向角与目标位姿的误差用二次型描述。控制平滑项车速变化、铰接角变化率尽量小避免轨迹抖动。路径效率项路径总长度尽量短但不能为了短而牺牲平滑性和避障裕度。定义成公式大概是这样function cost trajectory_cost(X, veh, target) [zs, us] unpackX(X, veh); % 终点误差 zN zs(end,:); cost_terminal 10 * ((zN(1)-target(1))^2 (zN(2)-target(2))^2) ... 5 * (wrapToPi(zN(3)-target(3))^2) ... 3 * (zN(4)-target(4))^2; % 控制平滑 cost_effort 0.5 * sum(sum(us.^2, 2)) 1.0 * sum(diff(us(:,1)).^2); % 路径长度 diff_z diff(zs(:,1:2)); cost_length 0.2 * sum(sqrt(sum(diff_z.^2, 2))); cost cost_terminal cost_effort cost_length; end这里的权重 10、5、3、0.5、1.0、0.2 都是我调过的初始值不是固定答案。我的经验是如果终点误差权重太高优化器会宁可贴着障碍物走过去也要把终点对准结果轨迹变得很“险”。如果路径长度权重太高轨迹会尽量拉直线避障能力下降。正确做法是先固定权重跑出一条能收敛的轨迹再用小步长调整其中一个权重观察轨迹形态变化。3.2 约束条件动力学、机械臂展、避障约束条件是这个项目里真正耗时间的地方。我把它们分成四类约束类型数学表达说明运动学等式约束z_{k1} f(z_k, u_k)通过离散运动学方程作为ceq返回控制输入边界vMin ≤ v ≤ vMax|ω| ≤ γDotMaxbox约束直接写到优化变量上下限机械状态边界|γ| ≤ γMax铰接角限位约束障碍物不等式约束dist(车身, 障碍物) ≥ dSafe用车辆包络圆与障碍物距离表达运动学等式约束不能用之前的欧拉离散我建议用中点法离散稳定性和精度都更好function z_next articulated_dyn_mid(z, u, veh) dz1 articulated_dyn(z, u, veh); z_mid z 0.5 * veh.dt * dz1; dz2 articulated_dyn(z_mid, u, veh); z_next z veh.dt * dz2; end障碍物约束我采用的是“车身包络圆”近似。具体做法是在车辆后车架中心、铰接点附近、前车架中心放置三个圆形包络用这些圆与障碍物的距离来判断是否碰撞。只需要在约束函数里循环每个离散点和每个障碍物即可。好处是几何计算简单、求导稳定坏处是包络比真实车身保守一些但工程上通常能接受。3.3 求解器选择fmincon的参数配置技巧纯Matlab环境里最直接的选择就是fmincon。这不是因为它是最先进的优化器而是因为它在优化工具箱里开箱即用能直接处理等式约束、不等式约束、边界约束。我用的求解器配置如下options optimoptions(fmincon, ... Algorithm, sqp, ... Display, iter, ... MaxFunctionEvaluations, 1e6, ... MaxIterations, 2000, ... ConstraintTolerance, 1e-5, ... OptimalityTolerance, 1e-6, ... StepTolerance, 1e-8, ... SpecifyConstraintGradient, false, ... SpecifyObjectiveGradient, false);关于算法选择我的看法是interior-point 在处理大规模问题上稳定性不错但在这个中等规模的非线性问题上收敛速度偏慢sqp 在轨迹优化这类问题里通常更干脆而且对初值没有想象中那么敏感。如果每次迭代都会出现约束振荡我再切回interior-point试一轮。需要注意我这里是让fmincon用有限差分求梯度。问题规模不大时这个方案省事一旦离散点数超过100建议还是手工推导或使用符号工具箱生成梯度解析式否则求解时间会非常难看。4. Matlab代码实现的完整流程4.1 环境与障碍物表达复杂环境第一步是定义障碍物。这个项目里我用了统一的地图数据结构每个障碍物用圆形表示圆心坐标和半径写在一张表格里方便后续扩展成任何传感器输出的障碍物列表。obs [ 3.0, 4.0, 0.8; 7.5, 6.0, 1.2; 12.0, 8.5, 0.6; 15.0, 4.5, 0.9; ];圆形障碍物虽然简单但真实场景里也有墙壁、料堆、设备等不规则障碍物。我的处理方法是先用多个圆形去“填充”不规则障碍物包围盒只要覆盖得足够密约束就足够表达。尤其是窄通道我会故意把通道两侧的障碍物半径加大0.2m作为安全缓冲这个做法比单纯依赖避障约束更有效。4.2 初值生成先规划骨架再做精优化初值质量直接决定fmincon会不会陷入局部最优。我的流程是先不用优化器直接在栅格地图上做一次A*路径搜索得到一条从起点到目标点的粗略路径再把它插值成优化变量需要的状态序列。% 生成A*粗路径 gridMap create_grid_map(mapSize, obs, veh.circleR); coarsePath astar_path(gridMap, startGrid, goalGrid); % 插值到Npts个状态点 initPathXY interp1(1:size(coarsePath,1), coarsePath, linspace(1,size(coarsePath,1), veh.Npts)); % 组装初始状态x,y,theta,gamma thetaInit atan2(gradient(initPathXY(:,2)), gradient(initPathXY(:,1))); gammaInit zeros(veh.Npts, 1); uInit repmat([1.0; 0.0], veh.Npts-1, 1); X0 [reshape([initPathXY(:,1), initPathXY(:,2), thetaInit, gammaInit], [], 1); uInit];这里有一种常见误区以为A路径已经足够好直接按路径点走就行。实际上A路径是折线由很多短直线段组成完全没有考虑车辆最小转弯半径和铰接角限制车辆根本跟踪不上。它的用途是给优化器提供一个“大致走向正确”的初值真正精细的平滑、避障、动态可行性都是fmincon后续完成的。如果环境特别简单比如起点到终点之间没有障碍物也可以直接生成一条直线作为初值再用目标函数里的平滑项去修正。但一旦进入复杂环境A*初值的优势会非常明显它能减少大量无效迭代。4.3 优化循环与可视化核心优化调用是这样组织的[X_opt, fval] fmincon((X) trajectory_cost(X, veh, target), X0, ... [], [], [], [], lb, ub, ... (X) trajectory_constraints(X, obs, veh, target), options);对应的约束函数需要返回等式约束和不等式约束。等式约束主要是运动学方程残差我写成下面这种形式function [c, ceq] trajectory_constraints(X, obs, veh, target) [zs, us] unpackX(X, veh); c []; ceq zeros((veh.Npts-1)*4, 1); for k 1:veh.Npts-1 z_next articulated_dyn_mid(zs(k,:), us(k,:), veh); ceq((k-1)*41:k*4) zs(k1,:) - z_next; end % 避障不等式约束 c obstacle_constraints(zs, obs, veh); end可视化部分这个项目里比较关键的是要把铰接后的车身画出来。如果只画质点轨迹很难发现后车架摆尾扫过障碍物的问题。我的做法是每次绘制时计算前后车架四个角点的位置用patch函数画出两个四边形再按50ms间隔刷新生成类似动画的效果。这一步对调试帮助特别大因为很多轨迹问题一眼就能从车身相对障碍物的位置看出来。5. 调参踩坑记录5.1 铰接角约束引发的“伪失败”我最早跑通模型时只加了铰接角边界没加铰接角变化率边界。结果优化器给出的轨迹铰接角变化率非常夸张几乎是一步从0度跳到35度再跳回0度。实际上液压缸根本做不到这个速度最后会变成“仿真能过、实车必翻”。这个问题的解决方式有两个层面一是在控制量上下限里限制ω二是把控制平滑权重调大一点。如果两者冲突应该优先保控制量的硬约束因为权重再大也只是软约束极端情况下优化器可能会为了满足其他硬约束而牺牲它。5.2 避障约束导致局部最优另一个典型的坑是避障约束让优化器把轨迹逼到某个死角结果收敛到一条贴着障碍物绕远路的路径。出现这种情况时我一般先看A初值有没有被障碍物穿模。如果A路径本身就贴着某两个障碍物的缝隙那fmincon大概率会把轨迹卡在缝隙里而不是主动选择一个更宽的绕行路线。我的应对方法是做多起点扰动把初始铰接角改成若干组随机值或对A*路径做左右偏移后再优化最后取目标函数最小的结果。这个方法简单粗暴但在轨迹优化里非常有效。还有一个更轻量的办法是在目标函数里增加一个障碍物距离的软惩罚项让优化器在硬约束满足的前提下主动把轨迹往更开阔的位置推。5.3 计算时间与实时性的矛盾在Matlab里当离散点数到50、障碍物数量到8个以上时fmincon单次求解时间经常要几十秒甚至几分钟。如果你做的是离线仿真这个时间可以接受。但如果你准备把它往实车规划算法方向推这个时间肯定扛不住。我的经验是这样拆解需求离线计算场景比如矿卡固定路径学习可以直接离线算在线重规划场景比如车辆遇到动态障碍物需要把离散点数降到20左右放宽约束精度同时考虑在C环境里重新实现。Matlab在这个阶段最大的价值是快速验证算法逻辑而不是做实时部署。6. 从仿真往工程走还能怎么扩展6.1 模型复杂度与落地成本怎么权衡我在这个项目里最终用的是运动学模型加圆形包络避障这套组合非常适合第一版验证。但工程落地时有几个问题会逐渐浮现出来首先是轮胎侧偏矿区路面条件差车辆满载时侧偏不可忽略其次是地形坡度如果轨迹要穿越坡道坡度会同时影响牵引力和稳定性最后是液压执行机构延迟铰接角的实际响应并非理想的阶跃响应。这些问题的处理原则是不要一开始就把模型做到最复杂。准确的做法是先把运动学模型加上轨迹优化跑通确认算法框架、约束体系、可视化调试工具全部稳定再逐步增加动力学修正项。每加一项都用仿真数据对比前后结果看看是否真的改善了轨迹可行性而不是让自己掉进参数标定的无底洞。6.2 轨迹优化之后的控制衔接轨迹优化计算出的是一条包含位置、航向、铰接角、车速、铰接角变化率的完整状态轨迹。要让车辆真正跟上去还需要一个轨迹跟踪控制器。我见过不少项目在“优化出漂亮轨迹”后就觉得大功告成结果一上实车跟踪误差大得离谱。针对铰接车辆我习惯在优化结果下接一个基于模型预测控制思想的跟踪控制层用当前状态和目标轨迹的偏差做滚动优化。这样规划层可以按较低频率运行控制层按较高频率修正。如果你不想引入MPC也可以用比例积分控制加前馈补偿但前提是轨迹本身已经足够平滑否则跟踪性能会差很多。6.3 可迁移的应用场景这个课题的应用范围其实比标题看起来要大。除了矿山装载机、铰接式运输车很多无人化港口的水平运输车、机场行李牵引车、仓库里的铰接式叉车都会遇到类似的轨迹优化问题。只要车辆是前后车架铰接转向低速非完整约束作业环境有静态障碍物和目标位姿要求这套“运动学建模 直接配置法 fmincon求解 可视化调试”的流程就都能迁移过去。你甚至可以用同一套代码框架去处理普通前轮转向车辆的轨迹规划只需要把运动学方程里的铰接角改成前轮转角约束和优化结构几乎不变。这也是我把代码封装成函数而不是写成一个大脚本的原因留出复用接口后续要换车型、换场景只需要改参数和障碍物列表。最后说一个我自己的体会。很多第一次跑这个课题的人看到fmincon报“Converged to an infeasible point”就以为代码写错了其实大部分情况是模型、约束、初值三者没匹配好。我后来养成的习惯是先把约束逐步放宽比如先不限制铰接角变化率然后看轨迹是在哪个位置开始“耍赖”再针对性地修改初值或障碍物表达。这个方法虽然土但比闷头调权重有效得多。如果你也在做类似轨迹优化建议先跑通最简单的空旷环境再慢慢加障碍物每加一类障碍物都看一遍车身包络圆与障碍物的距离变化曲线这样很多问题会在早期暴露出来而不是等优化器剧烈挣扎时才发现。
