企业官网建设流程全解析

写铰接式车辆轨迹优化这件事，最早是被一台矿用铰接自卸车逼出来的。第一次在实际巷道里调试时，我用的还是传统阿克曼模型做规划，结果车辆在窄弯处后车体直接蹭到了挡墙。那一次之后我才意识到，复杂环境中的轮式铰接车辆轨迹优化，跟常规四轮小车完全不是一回事，从运动学模型、约束构造到Matlab代码实现，每一步都需要重新设计。

这篇文章就从那次翻车经历说起。目标很明确：在已知地图、起点和目标点的情况下，用Matlab代码实现一条既要避开障碍物、又要满足铰接车辆运动学约束、还得平滑可执行的轨迹。内容会覆盖模型推导、优化问题构建、避障约束处理、求解器选型和实战踩坑，适合正在做矿卡、农机、港口机械、叉车或者研究非完整系统轨迹规划的朋友参考。

1. 复杂环境下铰接车辆轨迹优化的问题定位与技术路线

1.1 铰接车辆到底难在哪

普通乘用车是前轮转向，阿克曼几何在低速下能把车辆简化成单轨自行车模型，前轮转角直接决定横摆角速度。但轮式铰接车辆不一样，它的前后车体之间有一个铰接机构，转向发生在车体正中，前车体和后车体相对转过一个角度，形成所谓的铰接转向角。这个动作不像阿克曼车那样由前轮指向改变，而是整个前车体带着后车体“折过”一个角度。

这类车辆的显著特点是转弯半径小、巷道内机动灵活，但代价是运动学模型复杂。规划算法不能只盯着车头，因为后车体是沿着前车体留下的轨迹“抄近道”的。在倒车入库或者狭窄巷道掉头场景下，车头明明躲过了障碍，后车体却可能扫到墙沿。很多初次上手的朋友会下意识地把车辆当成一个圆形，然后用单点模型去做避障，这样做出来的轨迹放到仿真里看着还行，一上实车就露馅。

另一个难点在于，复杂环境往往包含多个障碍物、狭窄通道、直角弯、甚至需要原地折返。轨迹优化又是一个高维度非线性问题，状态量加控制量动辄上百个，约束里面既有动力学等式约束又有避障不等式约束，求解起来收敛性、实时性都容易出问题。所以这条路不是简单调用一个规划库就能解决的。

1.2 技术路线：从场景输入到可行轨迹

我采用的完整技术路线大致可以分为五步。

第一步是环境预处理。把传感器或者先验地图转换成占用栅格，再对其做距离变换，生成一张欧氏距离场地图。这张图里的每一个像素值，代表该位置到最近障碍物的距离，后面构造避障约束时可以直接使用。

第二步是车辆运动学建模。根据前后车体的尺寸和铰接点位置，写出状态方程，确定包括位置、航向角、铰接角以及速度、转向速率在内的状态和控制量。

第三步是初始参考轨迹生成。先用A或者RRT在栅格地图上寻找一条全局无碰撞路径，再把这条路径平滑、插值，转换成可以给优化器热启动的初始猜测。不要小看这一步，好的初始解能让非线性求解器速度快好几倍。

第四步是构建并求解最优控制问题。我会用直接法把连续时间系统离散化，写成非线性规划问题，然后用CasADi的nlpsol接口去求解，或者用Matlab优化工具箱做轻量验证。

第五步是后处理与仿真验证。求解得到的状态和控制序列要重新带入动力学方程做正向仿真，确认轨迹可靠的，再把车辆轮廓画出来逐帧检查是否碰撞，最后根据效果调整权重重新求解。

1.3 为什么选择轨迹优化而不是单纯路径规划

很多人会问，用路径规划不行吗？先用A*找路，再用纯跟踪或者MPC跟踪不就行了。实际做下来会发现，这种思路在铰接车上有一个大问题：全局路径本身就是无运动学约束的，跟踪层为了满足铰接角度的限制，往往会偏离路径很远。越是在复杂的窄通道里，这种偏离越致命。

轨迹优化直接把运动学约束、避障约束、执行器限制放到一个优化问题里，输出的不是一条几何线，而是带时间戳的位姿序列和控制序列。它在理论上就保证了“这条轨迹车辆能开出来”。代价是求解难度高，但换来的安全性完全值得。我的建议是：路径规划仍然需要用，但把它当作给优化器提供初始猜测的工具，而不是最终结果。

2. 铰接车辆运动学模型与约束建模

2.1 车辆状态与铰接转向运动学方程

要写代码，先得把状态定下来。我这里的铰接车辆模型选取铰接点坐标(x, y)作为位置参考，状态向量是：

[ s = [x, y, \theta_f, \phi]^T ]

其中 (\theta_f) 是前车体航向角，(\phi) 是铰接角，也就是前车体航向角减去后车体航向角。控制量是前馈速度 (v) 和铰接角速度 (u = \dot{\phi})。

在低速、忽略轮胎侧偏的假设下，运动学方程可以近似写成：

[ \begin{aligned} \dot{x} &= v \cos\theta_f \ \dot{y} &= v \sin\theta_f \ \dot{\theta}_f &= \frac{v}{L_f} \sin\phi \ \dot{\phi} &= u \end{aligned} ]

这里 (L_f) 是铰接点到前车体上等效参考轴之间的距离。严格来说，完整的两节式模型还需要考虑后车体长度和后桥方向约束，但在低速规划场景下上述模型已经能抓住铰接车辆的核心特征：转向由铰接角驱动，车头方向和后车体方向不一致。如果你要做高精度倒车控制，可以把状态扩展到包含后车体航向角 (\theta_r)，并写成：

[ \theta_r = \theta_f - \phi ]

然后在优化结果输出时额外检查后车体轮廓是否越界。我的建议是开发分两步走：先用上述模型把框架跑通，再根据实际车型的轴距、铰点位置把系数修正掉。

2.2 离散化方式与模型精度权衡

把连续方程写进优化器之前，必须离散化。最简单的做法是前向欧拉法：

[ s_{k+1} = s_k + dt \cdot f(s_k, u_k) ]

欧拉法的代码写起来最清爽，但在步长较大时会引入数值误差。我用的是四阶Runge-Kutta离散，虽然每次计算动力学要多算几次右端函数，但在同样的精度要求下，可以选择更长的采样时间，优化变量规模更小、收敛也更快。Matlab实现起来也不复杂：

function s_next = rk4_step(s, u, dt, Lf) k1 = dyn(s, u, Lf); k2 = dyn(s + 0.5*dt*k1, u, Lf); k3 = dyn(s + 0.5*dt*k2, u, Lf); k4 = dyn(s + dt*k3, u, Lf); s_next = s + dt/6*(k1 + 2*k2 + 2*k3 + k4); end function sdot = dyn(s, u, Lf) x = s(1); y = s(2); theta = s(3); phi = s(4); v = u(1); dphi = u(2); sdot = [v*cos(theta); v*sin(theta); v*sin(phi)/Lf; dphi]; end

实际使用中可以把采样时间设置在0.3到0.8秒之间，轨迹总时长取决于从起点到目标点需要的空间距离。比如一个20米距离的泊车任务，采样时间0.5秒、总共40步，就能覆盖10秒的轨迹。总步数太少，轨迹会很生硬；总步数太多，优化变量太多，求解时间会显著上升。

2.3 转向速度、加速度和终端约束

运动学模型之外，必须把机械和执行器限制作为硬约束加入优化问题。对于铰接车辆，我一般会设置以下边界条件：

• 铰接角约束：(\phi_{min} \leq \phi \leq \phi_{max})，一般取正负0.5到0.8弧度，具体取决于车辆铰接限位。
• 转向速率约束：(\dot{\phi}{min} \leq u \leq \dot{\phi}{max})，用来模拟液压转向系统的动作上限。
• 速度约束：(v_{min} \leq v \leq v_{max})，前进、倒退都有上限。
• 加速度约束：可以通过控制量施加一阶惯性约束，或者通过相邻步速度差施加 (a_{min} \leq (v_{k+1} - v_k)/dt \leq a_{max})。

初始状态必须严格等于车辆当前位姿，终端状态不是简单让位置到达目标点就行，还希望车辆最终摆正、铰接角归零。具体表达式是：

[ x_N = x_{goal}, \quad y_N = y_{goal}, \quad \theta_{f,N} = \theta_{goal}, \quad \phi_N = 0 ]

硬约束如果导致求解器找不到可行解，我一般先拿出来作为终端代价项，用高权重惩罚逼近目标位姿。下面的小节会说怎么处理这些约束冲突。

3. 复杂环境建模与避障约束构造

3.1 障碍物和车辆形状的近似表达

环境建模这一步直接决定避障约束的质量。第一次做这个优化器的时候，我把整个车辆用一个大圆包住，结果在宽度只比车宽多40公分的通道里，算法直接宣告不可行。原因很简单：圆的外接半径太大，把原本能走的空间全给堵死了。

正确做法是把铰接车辆拆成两个车体分别处理。前车体和后车体各用一个或两个圆来覆盖，圆的直径要尽量贴合实际车宽，同时留出20到30厘米的安全余量。之所以用圆而不是用多边形，是因为圆的距离约束是光滑的解析表达式，交给梯度优化器处理时数值性质好得多。

障碍物侧同样可以有多种表示。对于简单场景，把每个障碍物膨胀成圆；对于比较复杂的巷道边界，我会直接把占用栅格转成距离场，用距离场数值来构造避障约束。距离场在Matlab里有现成函数可以算，非常方便。

3.2 避障约束的数学化处理

避开第k个障碍物时，我用的是如下形式的约束：

对于车辆第j个圆形覆盖体，其圆心坐标为 ((x_{c,j}, y_{c,j}))，覆盖半径为 (r_{v,j})，障碍物圆心坐标为 ((x_{o}, y_{o}))，障碍物膨胀半径为 (r_{o})，那么有：

[ \sqrt{(x_{c,j} - x_o)^2 + (y_{c,j} - y_o)^2} - (r_{v,j} + r_o) \geq d_{safe} ]

这里的 (d_{safe}) 是额外安全距离。所有前向仿真节点上的所有覆盖圆都要对每一个障碍物满足这种距离不等式。于是碰撞约束的数量就是：

[ \text{约束数量} = \text{仿真步数} \times \text{覆盖圆数量} \times \text{障碍物数量} ]

假设仿真步数50、覆盖圆4个、障碍物30个，那就是6000个约束。这些约束彼此独立，在构造稀疏雅可比时是很干净的结构。

如果使用距离场地图，避障约束可以写成：

[ D(x_{c,j}, y_{c,j}) - r_{v,j} \geq d_{safe} ]

其中 (D(\cdot)) 是距离场插值函数。需要注意的是，直接用Matlabbwdist生成的离散距离场在优化中作约束时，梯度不平滑，容易导致求解器振荡。我通常是先算距离场，再用三次样条插值函数包装一层，这样梯度才是连续的。

3.3 代价函数设计与权重标定

轨迹优化最容易忽视的就是代价函数设计。我踩过的坑是，一开始把目标设成“尽快到达”，结果求解器给出一条几乎贴着障碍物边缘、速度忽快忽慢的激进轨迹。

我目前使用的代价函数包含四部分：

[ J = J_{time} + J_{smooth} + J_{ref} + J_{term} ]

时间代价是总时长惩罚，可以用 (T = N \cdot dt) 的线性项表示；平滑代价包含控制量平方、相邻控制增量平方，用来抑制速度跳变和转向抖动；参考代价是轨迹与初始全局路径的偏离距离；终端代价是到达目标点时的位姿误差惩罚。

权重标定的经验顺序是：先固定时间代价权重，然后加大平滑代价，直到轨迹不再出现画龙现象。之后再松开终端代价，让目标点精度满足要求。不要同时调所有权重，那样很难收敛到一把合适的标定。

我常用的一组初值如下表所示：

这个权重组合不是通用的，但可以作为第一版调试起点。

4. 优化问题的构建与求解器选型

4.1 离散最优控制问题的标准形式

把所有章节串起来，整个优化问题可以写成标准的非线性规划形式：

[ \begin{aligned} \min_{X, U} \quad & \sum_{k=1}^{N} L(s_k, u_k) + \Phi(s_N) \ \text{s.t.} \quad & s_{k+1} = F(s_k, u_k), \quad k = 1, \dots, N-1 \ & s_1 = s_{start} \ & g_{dyn}(s_k, u_k) \leq 0 \ & g_{obs}(s_k) \leq 0 \end{aligned} ]

其中 (X = [s_1, s_2, \dots, s_N]) 是状态序列，(U = [u_1, u_2, \dots, u_{N-1}]) 是控制序列。这里的核心是把连续最优控制问题转成有限维优化问题，术语叫直接法或者直接多重打靶法。它的好处是不需要推导两点边值问题，约束的稀疏结构也很直观。

4.2 用Matlab加CasADi求解非线性规划

我的主力工具是Matlab结合CasADi。CasADi提供符号化节点和高效自动微分，特别适合这种需要大量约束梯度计算的非线性规划问题。求解器一般用IPOPT，它对中等规模非线性约束问题收敛稳定。核心流程如下：

import casadi.* opti = casadi.Opti(); % 优化变量 X = opti.variable(4, N); % 状态 U = opti.variable(2, N-1); % 控制 % 目标函数 J = 0; for k = 1:N-1 J = J + w1 * sum(U(:,k).^2); J = J + w2 * sumsqr(U(:,k) - U(:,max(k-1,1))); end opti.minimize(J); % 动力学约束 for k = 1:N-1 s_next = rk4_casadi(X(:,k), U(:,k), dt, Lf); opti.subject_to(X(:,k+1) == s_next); opti.subject_to(opti.bounded(phi_min, X(4,k), phi_max)); opti.subject_to(opti.bounded(v_min, U(1,k), v_max)); end % 边界条件 opti.subject_to(X(:,1) == x_start); opti.subject_to(X(1:2,N) == x_goal); opti.subject_to(X(4,N) == 0); % 求解 opti.solver('ipopt', struct('print_level', 0)); sol = opti.solve();

这里需要注意的点是，动力学函数需要用CasADi的符号变量重新写一遍，而不能直接调用我前面写的普通Matlab函数。最简单的办法是把离散动力学公式用CasADi运算逐行写出，保证符号图完整构建。

4.3 初始轨迹生成对求解速度的影响

同一个优化问题，初始猜测给得不好，IPOPT可能在几步之后就因为不可行解退出。我常用三种初始猜测来源：

第一种，A*路线加密后直接线性插值。在地图不大、障碍物不复杂的场景下，这个方法最快。

第二种，Dubins路径平滑。把始末位姿用三段圆弧和直线拟合，虽然不严格满足铰接约束，但形态上已经接近车辆可行驶轨迹。

第三种，上一轮优化结果作为本轮热启动。在迭代调试或滚动优化中，这个技巧能让求解时间缩短一半以上。

用CasADi给初始猜测赋值的方法是在求解前设置初值，例如opti.set_initial(X, X0)和opti.set_initial(U, U0)。这里X0和U0需要预先构造好，维度不能错，否则CasADi报错信息会让你怀疑人生。

4.4 基于Matlab优化工具箱的轻量验证方案

如果你不想引入CasADi这种外部依赖，完全可以用Matlab自带的fmincon做小规模验证。我把车辆覆盖圆数量降到1到2个、障碍物数量控制在10个以内，预测时长30步，然后构造目标函数和非线性约束函数，用SQP算法求解。简单的倒库测试没问题，复杂地图会慢很多，但用来检查建模逻辑和约束写法的合理性已经够用。

options = optimoptions('fmincon', ... 'Algorithm', 'sqp', ... 'Display', 'iter', ... 'MaxFunctionEvaluations', 1e6, ... 'ConstraintTolerance', 1e-4); [z_opt, fval] = fmincon(@costFun, z0, [], [], [], [], lb, ub, @constraintFun, options);

这种方案下，z是所有状态和控制量摊平后的大向量，costFun和constraintFun需要自己维护索引。代码会啰嗦一些，但好处是不依赖第三方库，在演示或者学生实验环境非常友好。

5. Matlab代码实现流程与关键代码

5.1 代码整体结构和输入输出约定

下面是我在实际项目中常用的一套Matlab工程目录，供参考：

articulated_planner/ |-- config | -- vehicle_params.m |-- src | -- dynamics.m | -- costFunction.m | -- collisionConstraints.m | -- interpMap.m |-- script | -- run_planner.m |-- utils | -- load_map.m | -- initTrajectory.m | -- plotResult.m

入口脚本run_planner.m负责设置地图、起点终点、车辆参数，然后调用优化求解部分。车辆参数文件集中管理轴距、铰接点位置、限位角、速度范围等等，这样换车型时只改配置文件即可。

5.2 主要参数配置与标定

我随便给一组车辆参数示例，方便理解：

% vehicle_params.m Lf = 1.65; % 铰点到前轴等效距离，单位m Lr = 1.35; % 铰点到后轴等效距离，单位m vehicle_width = 1.80; % 车体宽度 phi_max = 0.55; % 最大铰接角，约31度 dphi_max = 0.35; % 最大铰接角速度 rad/s v_max = 2.0; % 最大车速 m/s v_min = -1.5; % 最大倒车速度 m/s dt = 0.5; % 采样时间 s N = 40; % 优化步数

采样时间选择有一个原则：(N \cdot dt) 要覆盖整个机动过程。如果第一步就发现求解器被迫把速度压到很低来等时间窗结束，说明总时间不够，需要加大N或者dt。反过来，如果轨迹前半段几乎全是匀速巡航、后半段才开始猛打方向，说明总时间过于充裕，可以减小步数来降低求解压力。

5.3 动力学约束与避障约束实现细节

避障约束函数建议写成独立模块，便于检查。以障碍物圆形表示为例：

function g = collisionConstraints(X, obsC, obsR, vehR, safeDist, dt, N) % X: 4xN状态矩阵 % obsC: 障碍物圆心列表, 2xM % obsR: 障碍物半径列表, 1xM g = []; for k = 1:N x= X(1,k); y = X(2,k); phi = X(4,k); % 前后两个覆盖圆中心 cx_front = x + Lf * cos(phi); cy_front = y + Lf * sin(phi); cx_rear = x - Lr; cy_rear = y; centers = [cx_front, cy_front; cx_rear, cy_rear]; for j = 1:size(centers,1) for m = 1:size(obsC,2) dist = norm(centers(j,:) - obsC(:,m)'); g(end+1) = safeDist + vehR(j) + obsR(m) - dist; end end end end

上面这段代码只是示意，实际使用中还需要把状态向量摊平到优化变量上。特别提醒一点：覆盖圆中心坐标必须随着状态中铰接角的变化而更新，不能用固定车头位置替代，否则后车体扫掠区域完全没有保护。

5.4 求解与结果可视化

轨迹解出来之后，至少要画三张图才能判断结果有没有问题。第一张是地图上的车辆位姿序列图，把每个采样时刻的车辆矩形轮廓画出来，连续看有没有交叠；第二张是速度、铰接角随时间的变化曲线，重点检查有没有超出限位；第三张是障碍物距离曲线，确保整个过程中任何覆盖圆到障碍物的距离都大于安全阈值。

Matlab里画车辆轮廓的方式很多，我习惯用patch或者rectangle，将前车体、后车体分别建模成两个矩形，再根据状态做旋转平移。这一步虽然需要点几何计算，但非常值得，因为很多轨迹问题一眼看数值看不出毛病，画出轮廓马上就会暴露后车体扫掠问题。

6. 实操问题排查与避坑记录

6.1 初始值不当导致的不可行解

最典型的报错是IPOPT提示Infeasible Problem Detected。遇到这种情况，不要急着怀疑求解器，先检查初始轨迹是不是穿过了障碍物。如果初始轨迹本身就穿墙，那么避障约束在初始点就不满足，而IPOPT又很难在一次迭代里把数百个避障约束同时拉回可行域。

我的处理方法是先把避障约束临时去掉，求解一个纯运动学可行的轨迹，再逐步加入避障约束。这种“约束分层激活”技巧看着笨，实际调试效率非常高。

6.2 优化变量数量膨胀与计算耗时

复杂环境下最容易出现的问题就是步数N设得太大，导致优化变量成千上万，求解时间从几秒直接膨胀到几分钟。我先说说我的参数取舍经验：状态维数是4，配置N=60，控制维数是2，未知量就是 (4 \times 60 + 2 \times 59 = 358) 个，加上约束上千条，IPOPT通常几秒内能解完。但当障碍物数量超过50、碰撞约束数量到五千条以上时，求解时间会暴涨。

压缩计算量有两个方向。一是减少障碍物约束数量，把距离车辆太远的障碍物在每一轮迭代中提前剔除，只保留当前轨迹周边5米圆锥范围内的障碍物。二是缩短优化窗口，把长任务切成滚动窗口，每次只优化未来3到5秒。

6.3 权重参数的四两拨千斤

权重标定是这门手艺最容易卡住人的地方。时间权重太大会导致轨迹贴着障碍物走；平滑权重太大会导致车辆为了走直线，绕远路绕到天边去；终端权重太小又会导致最后两米位姿误差大得离谱。

我遇到过一个很隐蔽的问题：终端点附近出现小幅振荡，车辆像是打算“停车”但又来回调整。排查后确认是终端位姿权重过大、铰接角权重过小造成的。把铰接角惩罚从零调到一个相对大的值之后，这种“停车摆头”现象立刻消失了。如果你看到优化轨迹在终点附近呈锯齿状，先看看铰接角控制量的惩罚项有没有加上。

6.4 避障约束与数值病态的常见关系

障碍物距离约束在车辆恰好贴近障碍物时，梯度变化非常剧烈，甚至出现不可导点。这种情况下IPOPT容易在边界上来回抖动，表现为在迭代日志里看到约束残差反复跳动。

我常用的缓解措施是把硬间隔改成软约束，引入一个松弛变量：

[ g_{obs}(s_k) \leq \xi_k, \quad \xi_k \geq 0 ]

然后把 (|\xi|) 以高权重加入代价函数。这样求解器不需要强行在每一步满足所有约束，而是在安全性最差的几处投入较大的约束余量，收敛稳定得多。实际轨迹出来之后，再对松弛变量大于安全阈值的节点做专项检查。

6.5 故障排查速查表

7. 个人实践心得与后续扩展建议

7.1 先建模再优化，顺序不能乱

我刚开始做这个项目时，急着搭优化器和求解器，结果在模型上偷了懒，用了太简化的单车模型。后面所有调试时间都在和“轨迹看着合理但实际开不了”的问题缠斗，非常痛苦。现在我的习惯是先花两天时间把车辆模型标定清楚，用仿真历史数据和实车转向测试记录去拟合运动学参数，模型靠谱了再谈优化算法。

铰接车辆模型标定有一个非常小的细节值得单独说一下：不同负载和胎压下，前后车体的有效轴距有变化，会影响铰接角和转弯半径之间的关系。至少要把空载、半载、满载三组参数跑一遍，再把中间状态做插值，不能只靠理论几何尺寸。

7.2 从离线规划到在线重规划的扩展

这套Matlab轨迹优化目前比较适合离线规划场景，比如矿区固定线路、港口自动引导、厂内倒运。如果要做动态避障和实时重规划，就需要把CasADi求解器封装成可以反复调用的服务，并在前端加上感知障碍物更新。我做过的一个测试是，将N从60降到20，每1秒滚动重算一次，单次求解可以达到100毫秒左右，虽然离真正实时还有距离，但作为估算基准是可行的。

实时化还有一个组合拳：先用这条轨迹做控制跟踪，再用跟踪误差驱动下一轮优化，在代价函数中加入跟踪偏差项。这样既能在线避障，又不会因为频繁重规划造成轨迹突变。

7.3 结合仿真和实车验证的注意事项

最后说一条我反复提醒自己的经验：仿真里的轨迹优化结果只能说明“理论可执行”，不能说明“实际操作安全”。实车验证之前，要在仿真环境里把车辆轮廓扫掠动画放慢逐帧看，尤其注意倒车转向过程。车体转不过去的位置，往往是后车体转弯半径和铰接角极限协同作用的结果，不是优化器算不出来，而是约束没建模到位。

可以额外把铰接角限位做成一个带死区的软约束，让优化器在接近限位时自动留出安全余量。这样做虽然会让轨迹稍微保守一些，但换来的是对液压系统响应延迟和传感器噪声的鲁棒性。我在实车测试中靠这个改动，解决了多次莫名其妙的安全停车问题。

这套方案现在支撑着我们部门的自动倒运实验，后续如果把动态障碍物和预测性碰撞风险加进来，可能会更接近L4级别的作业需求。希望这篇用Matlab代码实现复杂环境铰接车辆轨迹优化的记录，能帮助你少走那些我已经踩过的弯路。