4. 约束建模
轨迹规划的本质，是在一个由物理规律、任务要求和安全边界共同限定的可行域内，寻找一条既满足性能指标又不越界的运动曲线。约束建模就是把这个“可行域”翻译成优化器能够理解的语言。若约束写得过松，规划结果可能物理不可行；若写得过紧，问题可能无解或求解效率骤降。因此，约束建模既是数学表达问题，也是工程折中问题。
核心思想：任何一条轨迹都可以抽象为决策变量向量
x，约束就是一组关于 x 的等式或不等式关系。等式约束定义“必须精确满足”的硬关系，不等式约束定义“允许在一定范围内波动”的软边界，而边界约束、动力学约束、避障约束和时间约束则分别从变量范围、物理可行性、几何安全性和时序节奏四个维度对 x 进行限制。
一、约束的分类与统一数学形式
在连续时间最优控制中，轨迹通常表示为状态 x(t) 和控制 u(t) 的函数。约束可以统一写成如下形式：
min J(x(t), u(t), t)
s.t. ẋ(t) = f(x(t), u(t), t) // 动力学等式约束
g(x(t), u(t), t) ≤ 0 // 不等式约束
h(x(t), u(t), t) = 0 // 等式约束
x_min ≤ x(t) ≤ x_max // 状态边界约束
u_min ≤ u(t) ≤ u_max // 控制边界约束
t ∈ [t0, tf] // 时间约束
在离散化后，x(t) 和 u(t) 变成有限维向量，约束也随之转化为代数形式。下面这张图展示了六类约束在轨迹规划问题中的位置与相互关系。
二、六类约束的数学表达与工程含义
| 约束类型 | 典型数学形式 | 工程含义 | 常见误区 |
|---|---|---|---|
| 等式约束 | h(x,u,t)=0 |
闭环关系、配点条件、归一化约束 | 过度使用导致自由度耗尽 |
| 不等式约束 | g(x,u,t)≤0 |
速度上限、加速度上限、力矩饱和 | 忽略松弛变量导致无解 |
| 边界约束 | x_min≤x≤x_max |
关节限位、位置范围、控制量幅值 | 与不等式约束重复建模 |
| 动力学约束 | ẋ=f(x,u,t) |
车辆运动学、机械臂动力学 | 模型精度不足导致轨迹不可跟踪 |
| 避障约束 | dist(p,O)≥d_safe |
与障碍物保持安全距离 | 用点代替刚体，低估实际占用空间 |
| 时间约束 | t0≤t≤tf |
任务截止时间、时间同步 | 固定时间步长导致精度与效率矛盾 |
三、从连续到离散：约束的落地流程
实际求解时，连续约束需要离散化。以直接配点法为例，流程如下：
- 选择离散节点：将时间区间
[t0, tf]划分为 N 段，得到节点t0, t1, ..., tN。 - 变量参数化：在每个节点上定义状态
x_k和控制u_k，形成决策向量。 - 动力学离散：用梯形法或 Hermite-Simpson 法将
ẋ=f(x,u,t)转化为节点间的代数等式。 - 路径约束采样：在节点或配点上施加不等式、边界和避障约束。
- 时间约束处理：将
tf作为优化变量或固定值，并约束节点间距。
# 以梯形法离散动力学约束为例
for k in range(N):
x_next = x[k] + 0.5 * dt * (f(x[k], u[k]) + f(x[k+1], u[k+1]))
add_equality_constraint(x[k+1] - x_next == 0)
# 避障约束：对每个障碍物 O_j 和每个节点 k
for k in range(N+1):
for j in range(num_obstacles):
add_inequality_constraint(dist(x[k], O_j) - d_safe >= 0)
工程技巧：避障约束常用符号距离函数（SDF）表达，
dist(p,O) 可替换为 sdf(p,O)。对于凸多边形障碍物，SDF 可解析计算；对于复杂障碍物，可用支撑函数或采样近似。若优化器对非光滑敏感，可将 max(0, d_safe - dist) 作为惩罚项加入目标函数，形成软约束。
四、案例：移动机器人穿越窄通道
设机器人状态为 [x, y, θ, v, ω]，控制为 [a, α]。窄通道两侧墙壁为两条平行线段，障碍物为圆形。约束建模如下：
- 动力学约束：
ẋ = v cosθ, ẏ = v sinθ, θ̇ = ω, v̇ = a, ω̇ = α。 - 边界约束：
v ∈ [0, 1.5] m/s, ω ∈ [-1.0, 1.0] rad/s, a ∈ [-2, 2] m/s²。 - 避障约束：机器人外接圆半径
r_robot=0.35 m，安全距离d_safe=0.15 m，则dist((x,y), O_j) ≥ 0.5 m。 - 时间约束：总时间
tf ≤ 12 s，且每段dt ∈ [0.05, 0.5] s。 - 等式约束：起点和终点状态固定，
x(0)=x_start, x(tf)=x_goal。
若忽略机器人外接圆半径，仅用中心点做避障，规划出的轨迹在仿真中会与墙壁发生擦碰。若把 d_safe 设得过大，通道内可行域消失，优化器返回不可行。因此，避障约束的尺度必须与机器人几何尺寸和通道宽度匹配。
常见误区：把边界约束和不等式约束混为一谈。边界约束通常直接作用于决策变量的上下限，求解器可高效处理；而不等式约束可能涉及多个变量的非线性组合，计算代价更高。能写成边界约束的，不要写成一般不等式约束。
五、约束建模的复盘要点
约束建模完成后，建议从以下四个角度复盘：
- 必要性：每条约束是否对应真实的物理或任务限制？删除后轨迹是否仍然可用？
- 一致性：约束之间是否存在冲突？例如时间约束过紧与速度边界过小可能同时导致无解。
- 可微性：优化器是否需要梯度？若需要，避障约束中的
dist是否光滑？ - 松弛策略：是否为关键约束准备了软约束版本？当硬约束导致不可行时，能否降级求解？
约束建模不是一次性工作，而是随着场景复杂度、模型精度和求解器能力不断迭代的过程。把六类约束的数学表达写清楚、写准确，是后续实时优化能够稳定运行的前提。