AL-iLQR实践指南 4：各种约束如何建模

目标：理解如何用统一接口描述各种约束，并封装为"带约束的最优控制问题"

4.1 为什么需要约束

在 Chapter 3 中，iLQR 能求解无约束最优控制问题。但如果没有约束，优化器可能产生物理上不可能或不安全的结果——比如加速度无穷大、穿过障碍物、冲出车道。

现实中的约束大致分为三类：

类别	例子	约束方式
物理约束	方向盘最大转角、加速度极限	控制量上下界
安全约束	不能撞障碍物、不能出车道、限速	状态约束
目标约束	必须到达指定终点	终端约束

4.2 约束的数学形式

所有约束都统一为两种标准形式：

\text{等式约束（Equality）：} \quad c(x, u) = 0

\text{不等式约束（Inequality）：} \quad c(x, u) \leq 0

其中 $c: \mathbb{R}^n \times \mathbb{R}^m \to \mathbb{R}^p$ 是约束函数， $p$ 是约束的输出维度（一个约束函数可以同时输出多个约束值）。

关键理解：任何约束都能转化为这两种形式。比如"速度在 $[v_{\min}, v_{\max}]$ 之间"看起来是两个不等式，但可以写成一个输出 2 维向量的不等式约束：

c(x) = \begin{bmatrix} v_{\min} - v \\ v - v_{\max} \end{bmatrix} \leq 0

违约度（Violation）

衡量约束被违反的程度：

等式约束： $\|c(x, u)\|_\infty = \max_i |c_i|$ （任何一个分量不为零都是违约）
不等式约束： $\max(0, \max_i c_i)$ （只有正值算违约，负值表示满足）

代码实现（src/constraints/constraint_function.cpp）：

double MaxViolation(const ConstraintFunction& constraint, const Vector& values) {
  if (constraint.Type() == ConstraintType::kEquality) {
    return values.cwiseAbs().maxCoeff();     // max |c_i|
  }
  double max_violation = 0.0;
  for (int i = 0; i < values.size(); ++i) {
    max_violation = std::max(max_violation, values(i));
  }
  return std::max(0.0, max_violation);       // max(0, max c_i)
}

4.3 约束接口设计

本项目通过一个纯虚基类统一所有约束。不管是控制量限制还是障碍物碰撞，对外都是同一个接口：

class ConstraintFunction {
 public:
  virtual ConstraintType Type() const = 0;        // 等式 or 不等式
  virtual int StateDim() const = 0;               // 需要的状态维度
  virtual int ControlDim() const = 0;             // 需要的控制维度
  virtual int OutputDim() const = 0;              // 约束值的维度 p
  virtual std::string Name() const = 0;           // 用于调试
  virtual Vector Evaluate(const Vector& state,    // 核心：计算约束值
                          const Vector& control) const = 0;
};

所有具体约束都继承这个基类：

4.4 车辆外形建模：多圆近似

在讲解具体约束之前，需要先理解本项目如何表示车辆的外形。

4.4.1 为什么不能用一个点代表车辆？

最简单的做法是把车辆当成一个点（后轴中心），用欧几里得距离判断碰撞。但车辆实际上是一个有长度和宽度的矩形体

4.4.2 多圆近似：兼顾准确性和计算效率

本项目用多个圆沿车体纵轴排列来近似矩形车体。每个圆的半径等于车宽的一半，圆心均匀分布在前后边界之间。

圆的数量由配置项 collision_circle_count 控制（默认 2）。圆越多近似越准确，但约束数量也越多。

代码实现（src/autodrive/vehicle_circle_approximation.cpp）：

 
VehicleCircleApproximation::VehicleCircleApproximation(VehicleBodyConfig config)
    : config_(std::move(config)) {
  const double radius = 0.5 * config_.width;       // 半径 = 车宽/2
  const double rear_limit = -config_.rear_axle_to_rear + radius;
  const double front_limit = (config_.length - config_.rear_axle_to_rear) - radius;
 
  // 圆心沿纵轴均匀分布
  const double span = front_limit - rear_limit;
  const double step = span / static_cast<double>(config_.collision_circle_count - 1);
  for (int i = 0; i < config_.collision_circle_count; ++i) {
    circles_.push_back(VehicleCircle{rear_limit + step * i, 0.0, radius});
  }
}

4.4.3 从车体坐标到世界坐标

圆心在车体坐标系下是固定的（沿纵轴排列），但车辆在世界中会移动和转向。需要根据当前状态 $(x, y, \theta)$ 把圆心变换到世界坐标：

\begin{bmatrix} c_x^{\text{world}} \\ c_y^{\text{world}} \end{bmatrix} = \begin{bmatrix} x \\ y \end{bmatrix} + \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} c_x^{\text{body}} \\ c_y^{\text{body}} \end{bmatrix}

代码中的 EvaluateWorldCenters(state) 就是做这个变换：

std::vector<Vector> VehicleCircleApproximation::EvaluateWorldCenters(const Vector& state) const {
  const double x = state(0), y = state(1), yaw = state(2);
  const double cos_yaw = std::cos(yaw), sin_yaw = std::sin(yaw);
  
  std::vector<Vector> centers;
  for (const auto& circle : circles_) {
    Vector center(2);
    center(0) = x + cos_yaw * circle.center_x_body - sin_yaw * circle.center_y_body;
    center(1) = y + sin_yaw * circle.center_x_body + cos_yaw * circle.center_y_body;
    centers.push_back(center);
  }
  return centers;
}

4.5 具体约束实现

4.5.1 控制量上下界（ControlBoxConstraint）

物理含义：限制控制量在 $[u_{\min}, u_{\max}]$ 范围内。对自行车模型，控制量是 $u = [a, \delta]^T$ （加速度和前轮转角）。

转化为标准形式 $c(x,u) \leq 0$ ：

c(x, u) = \begin{bmatrix} u_{\min} - u \\ u - u_{\max} \end{bmatrix} \leq 0

上半部分约束下界，下半部分约束上界。对于 $m$ 维控制量，输出维度 $p = 2m$ 。

示例： $a \in [-1, 1]$ ， $\delta \in [-0.45, 0.45]$

c = \begin{bmatrix} -1 - a \\ -0.45 - \delta \\ a - 1 \\ \delta - 0.45 \end{bmatrix} \leq 0 \quad \text{（4维输出）}

代码（src/constraints/control_box_constraint.cpp）：

Vector ControlBoxConstraint::Evaluate(const Vector& state, const Vector& control) const {
  Vector values(OutputDim());                    // 2m 维输出
  values.head(ControlDim()) = lower_bound_ - control;   // u_min - u ≤ 0
  values.tail(ControlDim()) = control - upper_bound_;   // u - u_max ≤ 0
  return values;
}

4.5.2 速度限制（SpeedLimitConstraint）

物理含义：限制车辆速度在 $[v_{\min}, v_{\max}]$ 范围内。

转化为标准形式：和控制量上下界完全相同的思路，只是约束的对象是状态中的速度分量 $v = x_3$ ：

c(x, u) = \begin{bmatrix} v_{\min} - v \\ v - v_{\max} \end{bmatrix} \leq 0 \quad \text{（2维输出）}

代码（src/autodrive/speed_limit_constraint.cpp）：

Vector SpeedLimitConstraint::Evaluate(const Vector& state, const Vector& control) const {
  Vector values(OutputDim());
  values(0) = min_speed_ - state(3);    // v_min - v ≤ 0
  values(1) = state(3) - max_speed_;    // v - v_max ≤ 0
  return values;
}

4.5.3 道路边界约束

道路边界约束有两种实现：

质点版（RoadBoundaryConstraint）

将车辆视为一个点（后轴中心），约束其横向偏差在道路范围内：

c(x, u) = \begin{bmatrix} e_{\text{lat,lower}} - e_{\text{lat}} \\ e_{\text{lat}} - e_{\text{lat,upper}} \end{bmatrix} \leq 0

其中 $e_{\text{lat}}$ 是车辆相对参考线的横向偏差，由 StraightReferenceLine::LateralError 计算：

e_{\text{lat}} = -(x - x_0)\sin\theta_{\text{ref}} + (y - y_0)\cos\theta_{\text{ref}}

代码（src/autodrive/road_boundary_constraint.cpp）：

Vector RoadBoundaryConstraint::Evaluate(const Vector& state, const Vector& control) const {
  const double lateral_error = reference_line_->LateralError(state);
  Vector values(OutputDim());
  values(0) = lateral_lower_bound_ - lateral_error;   // 下界约束
  values(1) = lateral_error - lateral_upper_bound_;   // 上界约束
  return values;
}

多圆版（MultiCircleRoadBoundaryConstraint）

质点版的问题是：后轴中心没出界，但车头或车尾可能已经伸出道路边界了。多圆版对每个近似圆单独检查，并扣除圆的半径：

每个圆产生 2 个约束值， $n$ 个圆共 $2n$ 个：

c_i(x, u) = \begin{bmatrix} (e_{\text{lower}} + r_i) - e_{\text{lat},i} \\ e_{\text{lat},i} - (e_{\text{upper}} - r_i) \end{bmatrix} \leq 0

代码（src/autodrive/multi_circle_road_boundary_constraint.cpp）：

Vector MultiCircleRoadBoundaryConstraint::Evaluate(const Vector& state,
                                                   const Vector& control) const {
  const auto world_centers = vehicle_geometry_.EvaluateWorldCenters(state);
  const auto& circles = vehicle_geometry_.Circles();
  const int n = vehicle_geometry_.CircleCount();
  
  Vector values(OutputDim());   // 2n 维
  for (int i = 0; i < n; ++i) {
    Vector center_state(2);
    center_state(0) = world_centers[i](0);
    center_state(1) = world_centers[i](1);
    const double lateral = reference_line_->LateralError(center_state);
    const double r = circles[i].radius;
    values(2 * i)     = (road_lower_bound_ + r) - lateral;   // 下界 + 半径
    values(2 * i + 1) = lateral - (road_upper_bound_ - r);   // 上界 - 半径
  }
  return values;
}

4.5.4 障碍物约束：三种建模方式

障碍物碰撞检测是约束建模中最复杂的部分。本项目提供三种方式，对应枚举 ObstacleType：

enum class ObstacleType {
  kCircle = 0,       // 多圆车 vs 圆形障碍物
  kPolygonLSE = 1,   // 多圆车 vs 多边形障碍物 (LSE 光滑近似)
  kPolygonExact = 2, // 矩形车 vs 多边形障碍物 (精确多边形距离)
};

方式一：多圆车 vs 圆形障碍物（MultiCircleVehicleObstacleConstraint）

这是最简单的方式：障碍物用一个圆表示（圆心 $o$ 、半径 $r_{\text{obs}}$ ），车辆用 $n$ 个圆近似（每个圆心 $c_i$ 、半径 $r_i$ ）。

碰撞条件：两圆相交 $\iff$ 圆心距 $< r_i + r_{\text{obs}}$

转化为标准形式：用距离的平方避免开方（更光滑）：

c_i = (r_i + r_{\text{obs}})^2 - \|c_i - o\|^2 \leq 0

$c_i > 0$ 表示第 $i$ 个车体圆与障碍物圆重叠（碰撞）。

代码（src/autodrive/multi_circle_vehicle_obstacle_constraint.cpp）：

Vector MultiCircleVehicleObstacleConstraint::Evaluate(const Vector& state,
                                                      const Vector& control) const {
  const auto centers = vehicle_geometry_.EvaluateWorldCenters(state);
  Vector values(OutputDim());   // n 维，每个圆一个约束
  for (int i = 0; i < OutputDim(); ++i) {
    const double dx = centers[i](0) - obstacle_center_x_;
    const double dy = centers[i](1) - obstacle_center_y_;
    const double safe_distance = obstacle_radius_ + vehicle_geometry_.Circles()[i].radius;
    values(i) = safe_distance * safe_distance - (dx * dx + dy * dy);
  }
  return values;
}

方式二：多圆车 vs 多边形障碍物 — LSE 光滑近似（LSEPolygonObstacleConstraint）

现实中障碍物往往不是圆形而是矩形（车辆、建筑等）。用多边形建模更精确，但需要计算点到凸多边形的距离。

凸多边形的半平面表示

一个凸多边形可以表示为若干半平面的交集。每条边定义一个半平面，用外法线 $n_j$ 和偏移 $b_j$ 表示：

\text{点 } p \text{ 在多边形内部} \iff n_j \cdot p - b_j \leq 0, \quad \forall j

点到多边形边界的"穿透深度"可以用最大违反量来衡量：

d(p) = \max_j (n_j \cdot p - b_j)

$d(p) > 0$ 表示点在多边形外部， $d(p) < 0$ 表示在内部。

为什么需要 Log-Sum-Exp？

$\max$ 函数在多个分量相等时不可微（有"尖角"）。iLQR 的 backward pass 需要计算代价的导数，不可微的约束函数会导致数值差分不准确。

Log-Sum-Exp (LSE) 是 $\max$ 函数的光滑近似：

\text{LSE}_\alpha(z_1, \ldots, z_k) = \frac{1}{\alpha} \ln \sum_{j=1}^{k} e^{\alpha z_j} \approx \max_j z_j

参数 $\alpha > 0$ 控制近似精度： $\alpha$ 越大越接近真实 $\max$ ，但梯度越"尖锐"。项目默认 $\alpha = 20$ 。

LSE 约束的具体公式

对车辆的第 $i$ 个近似圆（圆心 $c_i$ ，半径 $r_i$ ），计算它到障碍物多边形的 LSE 距离：

d_{\text{LSE}}(c_i) = \frac{1}{\alpha} \ln \sum_{j=1}^{k} e^{\alpha (n_j \cdot c_i - b_j)}

约束条件为"圆心到多边形边界的距离 ≥ 圆的半径"：

c_i = r_i - d_{\text{LSE}}(c_i) \leq 0

代码（src/autodrive/lse_polygon_obstacle_constraint.cpp）：

Vector LSEPolygonObstacleConstraint::Evaluate(const Vector& state,
                                              const Vector& control) const {
  const auto centers = vehicle_geometry_.EvaluateWorldCenters(state);
  const auto& circles = vehicle_geometry_.Circles();
  Vector values(OutputDim());
  for (int i = 0; i < OutputDim(); ++i) {
    const double lse = obstacle_polygon_.LogSumExpDistance(centers[i], alpha_);
    values(i) = circles[i].radius - lse;   // r - d_LSE ≤ 0
  }
  return values;
}

LogSumExpDistance 的实现使用了数值稳定的 "减最大值" 技巧避免指数溢出：

double ConvexPolygon::LogSumExpDistance(const Vector& point, double alpha) const {
  double max_val = -infinity;
  for (int i = 0; i < FaceCount(); ++i) {
    double val = outward_normals_[i].dot(point) - offsets_[i];
    max_val = std::max(max_val, val);
  }
  double sum_exp = 0.0;
  for (int i = 0; i < FaceCount(); ++i) {
    double val = outward_normals_[i].dot(point) - offsets_[i];
    sum_exp += std::exp(alpha * (val - max_val));   // 减 max_val 防溢出
  }
  return max_val + std::log(sum_exp) / alpha;
}

方式三：精确多边形碰撞（PolygonCollisionConstraint）

前两种方式都用多圆近似车辆，存在近似误差。第三种方式直接用车辆的真实矩形轮廓和障碍物的凸多边形轮廓计算精确距离。

车辆多边形的构造

根据状态 $(x, y, \theta)$ 和车体尺寸，构造车辆的四个角点（在世界坐标系下）：

ConvexPolygon MakeVehiclePolygon(double x, double y, double yaw,
                                 double length, double width,
                                 double rear_axle_to_rear) {
  const double c = std::cos(yaw), s = std::sin(yaw);
  const double hw = 0.5 * width;
  const double body[][2] = {   // 车体坐标系下的四个角点
    {-rear_axle_to_rear, -hw},          // 左后
    {length - rear_axle_to_rear, -hw},  // 右后
    {length - rear_axle_to_rear, hw},   // 右前
    {-rear_axle_to_rear, hw},           // 左前
  };
  // 旋转 + 平移到世界坐标
  for (const auto& corner : body) {
    v(0) = x + c * corner[0] - s * corner[1];
    v(1) = y + s * corner[0] + c * corner[1];
  }
  return ConvexPolygon(vertices);
}

多边形对之间的距离：SAT 算法

两个凸多边形之间的距离用分离轴定理（Separating Axis Theorem, SAT） 计算。核心思想：如果两个凸多边形不相交，则必存在一条"分离轴"（取自两多边形的某条边的法线方向），使得两多边形在该轴上的投影不重叠。

代码中 ConvexPolygonPairDistance 实现了完整的 SAT 检测：

遍历两个多边形的所有边法线作为候选分离轴
计算两多边形在每个轴上的投影区间
如果某个轴上投影不重叠：多边形分离，返回正距离（通过顶点-边距离计算精确欧几里得距离）
如果所有轴上投影都重叠：多边形碰撞，返回负值（穿透深度）

约束公式

c(x, u) = d_{\text{safety}} - d_{\text{polygon}}(V, O) \leq 0

其中 $V$ 是车辆多边形， $O$ 是障碍物多边形， $d_{\text{safety}}$ 是安全余量（默认 0.1m）。

输出维度始终为 1（一对多边形只产生一个距离值），这是与多圆方式的重要区别。

代码（src/autodrive/polygon_collision_constraint.cpp）：

Vector PolygonCollisionConstraint::Evaluate(const Vector& state,
                                            const Vector& control) const {
  // 根据当前状态构造车辆矩形
  ConvexPolygon vehicle = MakeVehiclePolygon(
      state(0), state(1), state(2),
      vehicle_length_, vehicle_width_, rear_axle_to_rear_);
  // 计算车辆多边形与障碍物多边形的精确距离
  double dist = ConvexPolygonPairDistance(vehicle, obstacle_polygon_);
  Vector values(1);
  values(0) = safety_margin_ - dist;   // safety - dist ≤ 0
  return values;
}

4.5.5 终端目标约束（TerminalGoalConstraint）

物理含义：要求终端状态精确等于目标状态。这是一个等式约束。

c(x_N) = x_N - x_{\text{target}} = 0

输出维度等于状态维度（对自行车模型为 4）。终端约束不依赖控制量。

代码（src/constraints/terminal_goal_constraint.cpp）：

Vector TerminalGoalConstraint::Evaluate(const Vector& state, const Vector& control) const {
  return state - target_state_;   // x_N - x_target = 0
}

4.6 带约束的最优控制问题

ConstrainedOptimalControlProblem

有了各种约束实现后，需要一个容器把它们组装到最优控制问题上。ConstrainedOptimalControlProblem 在 OptimalControlProblem 的基础上，为每个 knot point 管理一组约束：

约束按 knot point 独立管理——每个时间步可以有不同的约束集合。比如动态障碍物在不同时间步的位置不同，每步的 MultiCircleVehicleObstacleConstraint 使用不同的障碍物中心坐标。

约束添加方式（demo_scenario.cpp 中的实现）

以下是 CreateScenarioSkeleton 中组装约束的代码：

// 为每个时间步添加三类公共约束
for (int k = 0; k < scenario.base_problem->Horizon(); ++k) {
  // 1. 控制量上下界
  scenario.constrained_problem->AddStageConstraint(
      k, std::make_shared<ControlBoxConstraint>(state_dim, u_lb, u_ub));
  
  // 2. 多圆道路边界
  scenario.constrained_problem->AddStageConstraint(
      k, std::make_shared<MultiCircleRoadBoundaryConstraint>(
             reference_line, vehicle_geometry, road_lower, road_upper));
 
  // 3. 速度限制
  scenario.constrained_problem->AddStageConstraint(
      k, std::make_shared<SpeedLimitConstraint>(min_speed, max_speed));
}

障碍物约束根据 ObstacleType 选择不同的实现：

// AddObstacleConstraints: 根据类型选择障碍物约束
for (int k = 0; k < horizon; ++k) {
  switch (scenario->obstacle_type) {
    case ObstacleType::kCircle:
      // 多圆车 vs 圆形障碍物
      AddStageConstraint(k, MultiCircleVehicleObstacleConstraint(...));
      break;
    case ObstacleType::kPolygonLSE:
      // 多圆车 vs 多边形障碍物 (LSE 光滑距离)
      AddStageConstraint(k, LSEPolygonObstacleConstraint(..., alpha=20));
      break;
    case ObstacleType::kPolygonExact:
      // 精确矩形 vs 多边形 (SAT 精确距离)
      AddStageConstraint(k, PolygonCollisionConstraint(..., safety_margin=0.1));
      break;
  }
}

4.7 约束维度计数

理解约束维度对后续的增广拉格朗日方法至关重要——每个约束值都需要一个 $\lambda$ （拉格朗日乘子）和一个 $\mu$ （惩罚参数）。

4.8 本章小结

本章建立了约束建模的完整框架：

统一接口：所有约束都实现 ConstraintFunction，对外只有 Evaluate(state, control) 一个方法
标准形式：任何约束都转化为 $c(x,u) \leq 0$ （不等式）或 $c(x,u) = 0$ （等式）
车辆外形：用多圆近似矩形车体，兼顾准确性和计算效率
三种障碍物建模： - 多圆 vs 圆：最简单，处处可微 - 多圆 vs 多边形 (LSE)：更精确，通过 Log-Sum-Exp 保持光滑 - 精确多边形 vs 多边形 (SAT)：最精确，但边界处不光滑

此时我们能建模约束，但还不能求解带约束问题——iLQR 只能处理无约束优化。Chapter 5 将介绍增广拉格朗日方法，它的核心思想是把约束"塞进"代价函数，让 iLQR 无需修改就能间接处理约束。

目标：理解如何用统一接口描述各种约束，并封装为"带约束的最优控制问题"

4.1 为什么需要约束

现实中的约束大致分为三类：

类别	例子	约束方式
物理约束	方向盘最大转角、加速度极限	控制量上下界
安全约束	不能撞障碍物、不能出车道、限速	状态约束
目标约束	必须到达指定终点	终端约束

4.2 约束的数学形式

所有约束都统一为两种标准形式：

\text{等式约束（Equality）：} \quad c(x, u) = 0

\text{不等式约束（Inequality）：} \quad c(x, u) \leq 0

其中 $c: \mathbb{R}^n \times \mathbb{R}^m \to \mathbb{R}^p$ 是约束函数， $p$ 是约束的输出维度（一个约束函数可以同时输出多个约束值）。

c(x) = \begin{bmatrix} v_{\min} - v \\ v - v_{\max} \end{bmatrix} \leq 0

违约度（Violation）

衡量约束被违反的程度：

等式约束： $\|c(x, u)\|_\infty = \max_i |c_i|$ （任何一个分量不为零都是违约）
不等式约束： $\max(0, \max_i c_i)$ （只有正值算违约，负值表示满足）

代码实现（src/constraints/constraint_function.cpp）：

double MaxViolation(const ConstraintFunction& constraint, const Vector& values) {
  if (constraint.Type() == ConstraintType::kEquality) {
    return values.cwiseAbs().maxCoeff();     // max |c_i|
  }
  double max_violation = 0.0;
  for (int i = 0; i < values.size(); ++i) {
    max_violation = std::max(max_violation, values(i));
  }
  return std::max(0.0, max_violation);       // max(0, max c_i)
}

4.3 约束接口设计

本项目通过一个纯虚基类统一所有约束。不管是控制量限制还是障碍物碰撞，对外都是同一个接口：

class ConstraintFunction {
 public:
  virtual ConstraintType Type() const = 0;        // 等式 or 不等式
  virtual int StateDim() const = 0;               // 需要的状态维度
  virtual int ControlDim() const = 0;             // 需要的控制维度
  virtual int OutputDim() const = 0;              // 约束值的维度 p
  virtual std::string Name() const = 0;           // 用于调试
  virtual Vector Evaluate(const Vector& state,    // 核心：计算约束值
                          const Vector& control) const = 0;
};

所有具体约束都继承这个基类：

4.4 车辆外形建模：多圆近似

在讲解具体约束之前，需要先理解本项目如何表示车辆的外形。

4.4.1 为什么不能用一个点代表车辆？

最简单的做法是把车辆当成一个点（后轴中心），用欧几里得距离判断碰撞。但车辆实际上是一个有长度和宽度的矩形体

4.4.2 多圆近似：兼顾准确性和计算效率

本项目用多个圆沿车体纵轴排列来近似矩形车体。每个圆的半径等于车宽的一半，圆心均匀分布在前后边界之间。

圆的数量由配置项 collision_circle_count 控制（默认 2）。圆越多近似越准确，但约束数量也越多。

代码实现（src/autodrive/vehicle_circle_approximation.cpp）：

 
VehicleCircleApproximation::VehicleCircleApproximation(VehicleBodyConfig config)
    : config_(std::move(config)) {
  const double radius = 0.5 * config_.width;       // 半径 = 车宽/2
  const double rear_limit = -config_.rear_axle_to_rear + radius;
  const double front_limit = (config_.length - config_.rear_axle_to_rear) - radius;
 
  // 圆心沿纵轴均匀分布
  const double span = front_limit - rear_limit;
  const double step = span / static_cast<double>(config_.collision_circle_count - 1);
  for (int i = 0; i < config_.collision_circle_count; ++i) {
    circles_.push_back(VehicleCircle{rear_limit + step * i, 0.0, radius});
  }
}

4.4.3 从车体坐标到世界坐标

圆心在车体坐标系下是固定的（沿纵轴排列），但车辆在世界中会移动和转向。需要根据当前状态 $(x, y, \theta)$ 把圆心变换到世界坐标：

\begin{bmatrix} c_x^{\text{world}} \\ c_y^{\text{world}} \end{bmatrix} = \begin{bmatrix} x \\ y \end{bmatrix} + \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} c_x^{\text{body}} \\ c_y^{\text{body}} \end{bmatrix}

代码中的 EvaluateWorldCenters(state) 就是做这个变换：

std::vector<Vector> VehicleCircleApproximation::EvaluateWorldCenters(const Vector& state) const {
  const double x = state(0), y = state(1), yaw = state(2);
  const double cos_yaw = std::cos(yaw), sin_yaw = std::sin(yaw);
  
  std::vector<Vector> centers;
  for (const auto& circle : circles_) {
    Vector center(2);
    center(0) = x + cos_yaw * circle.center_x_body - sin_yaw * circle.center_y_body;
    center(1) = y + sin_yaw * circle.center_x_body + cos_yaw * circle.center_y_body;
    centers.push_back(center);
  }
  return centers;
}

4.5 具体约束实现

4.5.1 控制量上下界（ControlBoxConstraint）

物理含义：限制控制量在 $[u_{\min}, u_{\max}]$ 范围内。对自行车模型，控制量是 $u = [a, \delta]^T$ （加速度和前轮转角）。

转化为标准形式 $c(x,u) \leq 0$ ：

c(x, u) = \begin{bmatrix} u_{\min} - u \\ u - u_{\max} \end{bmatrix} \leq 0

上半部分约束下界，下半部分约束上界。对于 $m$ 维控制量，输出维度 $p = 2m$ 。

示例： $a \in [-1, 1]$ ， $\delta \in [-0.45, 0.45]$

c = \begin{bmatrix} -1 - a \\ -0.45 - \delta \\ a - 1 \\ \delta - 0.45 \end{bmatrix} \leq 0 \quad \text{（4维输出）}

代码（src/constraints/control_box_constraint.cpp）：

Vector ControlBoxConstraint::Evaluate(const Vector& state, const Vector& control) const {
  Vector values(OutputDim());                    // 2m 维输出
  values.head(ControlDim()) = lower_bound_ - control;   // u_min - u ≤ 0
  values.tail(ControlDim()) = control - upper_bound_;   // u - u_max ≤ 0
  return values;
}

4.5.2 速度限制（SpeedLimitConstraint）

物理含义：限制车辆速度在 $[v_{\min}, v_{\max}]$ 范围内。

转化为标准形式：和控制量上下界完全相同的思路，只是约束的对象是状态中的速度分量 $v = x_3$ ：

c(x, u) = \begin{bmatrix} v_{\min} - v \\ v - v_{\max} \end{bmatrix} \leq 0 \quad \text{（2维输出）}

代码（src/autodrive/speed_limit_constraint.cpp）：

Vector SpeedLimitConstraint::Evaluate(const Vector& state, const Vector& control) const {
  Vector values(OutputDim());
  values(0) = min_speed_ - state(3);    // v_min - v ≤ 0
  values(1) = state(3) - max_speed_;    // v - v_max ≤ 0
  return values;
}

4.5.3 道路边界约束

道路边界约束有两种实现：

质点版（RoadBoundaryConstraint）

将车辆视为一个点（后轴中心），约束其横向偏差在道路范围内：

c(x, u) = \begin{bmatrix} e_{\text{lat,lower}} - e_{\text{lat}} \\ e_{\text{lat}} - e_{\text{lat,upper}} \end{bmatrix} \leq 0

其中 $e_{\text{lat}}$ 是车辆相对参考线的横向偏差，由 StraightReferenceLine::LateralError 计算：

e_{\text{lat}} = -(x - x_0)\sin\theta_{\text{ref}} + (y - y_0)\cos\theta_{\text{ref}}

代码（src/autodrive/road_boundary_constraint.cpp）：

Vector RoadBoundaryConstraint::Evaluate(const Vector& state, const Vector& control) const {
  const double lateral_error = reference_line_->LateralError(state);
  Vector values(OutputDim());
  values(0) = lateral_lower_bound_ - lateral_error;   // 下界约束
  values(1) = lateral_error - lateral_upper_bound_;   // 上界约束
  return values;
}

多圆版（MultiCircleRoadBoundaryConstraint）

质点版的问题是：后轴中心没出界，但车头或车尾可能已经伸出道路边界了。多圆版对每个近似圆单独检查，并扣除圆的半径：

每个圆产生 2 个约束值， $n$ 个圆共 $2n$ 个：

c_i(x, u) = \begin{bmatrix} (e_{\text{lower}} + r_i) - e_{\text{lat},i} \\ e_{\text{lat},i} - (e_{\text{upper}} - r_i) \end{bmatrix} \leq 0

代码（src/autodrive/multi_circle_road_boundary_constraint.cpp）：

Vector MultiCircleRoadBoundaryConstraint::Evaluate(const Vector& state,
                                                   const Vector& control) const {
  const auto world_centers = vehicle_geometry_.EvaluateWorldCenters(state);
  const auto& circles = vehicle_geometry_.Circles();
  const int n = vehicle_geometry_.CircleCount();
  
  Vector values(OutputDim());   // 2n 维
  for (int i = 0; i < n; ++i) {
    Vector center_state(2);
    center_state(0) = world_centers[i](0);
    center_state(1) = world_centers[i](1);
    const double lateral = reference_line_->LateralError(center_state);
    const double r = circles[i].radius;
    values(2 * i)     = (road_lower_bound_ + r) - lateral;   // 下界 + 半径
    values(2 * i + 1) = lateral - (road_upper_bound_ - r);   // 上界 - 半径
  }
  return values;
}

4.5.4 障碍物约束：三种建模方式

障碍物碰撞检测是约束建模中最复杂的部分。本项目提供三种方式，对应枚举 ObstacleType：

enum class ObstacleType {
  kCircle = 0,       // 多圆车 vs 圆形障碍物
  kPolygonLSE = 1,   // 多圆车 vs 多边形障碍物 (LSE 光滑近似)
  kPolygonExact = 2, // 矩形车 vs 多边形障碍物 (精确多边形距离)
};

方式一：多圆车 vs 圆形障碍物（MultiCircleVehicleObstacleConstraint）

这是最简单的方式：障碍物用一个圆表示（圆心 $o$ 、半径 $r_{\text{obs}}$ ），车辆用 $n$ 个圆近似（每个圆心 $c_i$ 、半径 $r_i$ ）。

碰撞条件：两圆相交 $\iff$ 圆心距 $< r_i + r_{\text{obs}}$

转化为标准形式：用距离的平方避免开方（更光滑）：

c_i = (r_i + r_{\text{obs}})^2 - \|c_i - o\|^2 \leq 0

$c_i > 0$ 表示第 $i$ 个车体圆与障碍物圆重叠（碰撞）。

代码（src/autodrive/multi_circle_vehicle_obstacle_constraint.cpp）：

Vector MultiCircleVehicleObstacleConstraint::Evaluate(const Vector& state,
                                                      const Vector& control) const {
  const auto centers = vehicle_geometry_.EvaluateWorldCenters(state);
  Vector values(OutputDim());   // n 维，每个圆一个约束
  for (int i = 0; i < OutputDim(); ++i) {
    const double dx = centers[i](0) - obstacle_center_x_;
    const double dy = centers[i](1) - obstacle_center_y_;
    const double safe_distance = obstacle_radius_ + vehicle_geometry_.Circles()[i].radius;
    values(i) = safe_distance * safe_distance - (dx * dx + dy * dy);
  }
  return values;
}

方式二：多圆车 vs 多边形障碍物 — LSE 光滑近似（LSEPolygonObstacleConstraint）

现实中障碍物往往不是圆形而是矩形（车辆、建筑等）。用多边形建模更精确，但需要计算点到凸多边形的距离。

凸多边形的半平面表示

一个凸多边形可以表示为若干半平面的交集。每条边定义一个半平面，用外法线 $n_j$ 和偏移 $b_j$ 表示：

\text{点 } p \text{ 在多边形内部} \iff n_j \cdot p - b_j \leq 0, \quad \forall j

点到多边形边界的"穿透深度"可以用最大违反量来衡量：

d(p) = \max_j (n_j \cdot p - b_j)

$d(p) > 0$ 表示点在多边形外部， $d(p) < 0$ 表示在内部。

为什么需要 Log-Sum-Exp？

$\max$ 函数在多个分量相等时不可微（有"尖角"）。iLQR 的 backward pass 需要计算代价的导数，不可微的约束函数会导致数值差分不准确。

Log-Sum-Exp (LSE) 是 $\max$ 函数的光滑近似：

\text{LSE}_\alpha(z_1, \ldots, z_k) = \frac{1}{\alpha} \ln \sum_{j=1}^{k} e^{\alpha z_j} \approx \max_j z_j

参数 $\alpha > 0$ 控制近似精度： $\alpha$ 越大越接近真实 $\max$ ，但梯度越"尖锐"。项目默认 $\alpha = 20$ 。

LSE 约束的具体公式

对车辆的第 $i$ 个近似圆（圆心 $c_i$ ，半径 $r_i$ ），计算它到障碍物多边形的 LSE 距离：

d_{\text{LSE}}(c_i) = \frac{1}{\alpha} \ln \sum_{j=1}^{k} e^{\alpha (n_j \cdot c_i - b_j)}

约束条件为"圆心到多边形边界的距离 ≥ 圆的半径"：

c_i = r_i - d_{\text{LSE}}(c_i) \leq 0

代码（src/autodrive/lse_polygon_obstacle_constraint.cpp）：

Vector LSEPolygonObstacleConstraint::Evaluate(const Vector& state,
                                              const Vector& control) const {
  const auto centers = vehicle_geometry_.EvaluateWorldCenters(state);
  const auto& circles = vehicle_geometry_.Circles();
  Vector values(OutputDim());
  for (int i = 0; i < OutputDim(); ++i) {
    const double lse = obstacle_polygon_.LogSumExpDistance(centers[i], alpha_);
    values(i) = circles[i].radius - lse;   // r - d_LSE ≤ 0
  }
  return values;
}

LogSumExpDistance 的实现使用了数值稳定的 "减最大值" 技巧避免指数溢出：

double ConvexPolygon::LogSumExpDistance(const Vector& point, double alpha) const {
  double max_val = -infinity;
  for (int i = 0; i < FaceCount(); ++i) {
    double val = outward_normals_[i].dot(point) - offsets_[i];
    max_val = std::max(max_val, val);
  }
  double sum_exp = 0.0;
  for (int i = 0; i < FaceCount(); ++i) {
    double val = outward_normals_[i].dot(point) - offsets_[i];
    sum_exp += std::exp(alpha * (val - max_val));   // 减 max_val 防溢出
  }
  return max_val + std::log(sum_exp) / alpha;
}

方式三：精确多边形碰撞（PolygonCollisionConstraint）

前两种方式都用多圆近似车辆，存在近似误差。第三种方式直接用车辆的真实矩形轮廓和障碍物的凸多边形轮廓计算精确距离。

车辆多边形的构造

根据状态 $(x, y, \theta)$ 和车体尺寸，构造车辆的四个角点（在世界坐标系下）：

ConvexPolygon MakeVehiclePolygon(double x, double y, double yaw,
                                 double length, double width,
                                 double rear_axle_to_rear) {
  const double c = std::cos(yaw), s = std::sin(yaw);
  const double hw = 0.5 * width;
  const double body[][2] = {   // 车体坐标系下的四个角点
    {-rear_axle_to_rear, -hw},          // 左后
    {length - rear_axle_to_rear, -hw},  // 右后
    {length - rear_axle_to_rear, hw},   // 右前
    {-rear_axle_to_rear, hw},           // 左前
  };
  // 旋转 + 平移到世界坐标
  for (const auto& corner : body) {
    v(0) = x + c * corner[0] - s * corner[1];
    v(1) = y + s * corner[0] + c * corner[1];
  }
  return ConvexPolygon(vertices);
}

多边形对之间的距离：SAT 算法

代码中 ConvexPolygonPairDistance 实现了完整的 SAT 检测：

遍历两个多边形的所有边法线作为候选分离轴
计算两多边形在每个轴上的投影区间
如果某个轴上投影不重叠：多边形分离，返回正距离（通过顶点-边距离计算精确欧几里得距离）
如果所有轴上投影都重叠：多边形碰撞，返回负值（穿透深度）

约束公式

c(x, u) = d_{\text{safety}} - d_{\text{polygon}}(V, O) \leq 0

其中 $V$ 是车辆多边形， $O$ 是障碍物多边形， $d_{\text{safety}}$ 是安全余量（默认 0.1m）。

输出维度始终为 1（一对多边形只产生一个距离值），这是与多圆方式的重要区别。

代码（src/autodrive/polygon_collision_constraint.cpp）：

Vector PolygonCollisionConstraint::Evaluate(const Vector& state,
                                            const Vector& control) const {
  // 根据当前状态构造车辆矩形
  ConvexPolygon vehicle = MakeVehiclePolygon(
      state(0), state(1), state(2),
      vehicle_length_, vehicle_width_, rear_axle_to_rear_);
  // 计算车辆多边形与障碍物多边形的精确距离
  double dist = ConvexPolygonPairDistance(vehicle, obstacle_polygon_);
  Vector values(1);
  values(0) = safety_margin_ - dist;   // safety - dist ≤ 0
  return values;
}

4.5.5 终端目标约束（TerminalGoalConstraint）

物理含义：要求终端状态精确等于目标状态。这是一个等式约束。

c(x_N) = x_N - x_{\text{target}} = 0

输出维度等于状态维度（对自行车模型为 4）。终端约束不依赖控制量。

代码（src/constraints/terminal_goal_constraint.cpp）：

Vector TerminalGoalConstraint::Evaluate(const Vector& state, const Vector& control) const {
  return state - target_state_;   // x_N - x_target = 0
}

4.6 带约束的最优控制问题

ConstrainedOptimalControlProblem

约束添加方式（demo_scenario.cpp 中的实现）

以下是 CreateScenarioSkeleton 中组装约束的代码：

// 为每个时间步添加三类公共约束
for (int k = 0; k < scenario.base_problem->Horizon(); ++k) {
  // 1. 控制量上下界
  scenario.constrained_problem->AddStageConstraint(
      k, std::make_shared<ControlBoxConstraint>(state_dim, u_lb, u_ub));
  
  // 2. 多圆道路边界
  scenario.constrained_problem->AddStageConstraint(
      k, std::make_shared<MultiCircleRoadBoundaryConstraint>(
             reference_line, vehicle_geometry, road_lower, road_upper));
 
  // 3. 速度限制
  scenario.constrained_problem->AddStageConstraint(
      k, std::make_shared<SpeedLimitConstraint>(min_speed, max_speed));
}

障碍物约束根据 ObstacleType 选择不同的实现：

// AddObstacleConstraints: 根据类型选择障碍物约束
for (int k = 0; k < horizon; ++k) {
  switch (scenario->obstacle_type) {
    case ObstacleType::kCircle:
      // 多圆车 vs 圆形障碍物
      AddStageConstraint(k, MultiCircleVehicleObstacleConstraint(...));
      break;
    case ObstacleType::kPolygonLSE:
      // 多圆车 vs 多边形障碍物 (LSE 光滑距离)
      AddStageConstraint(k, LSEPolygonObstacleConstraint(..., alpha=20));
      break;
    case ObstacleType::kPolygonExact:
      // 精确矩形 vs 多边形 (SAT 精确距离)
      AddStageConstraint(k, PolygonCollisionConstraint(..., safety_margin=0.1));
      break;
  }
}

4.7 约束维度计数

理解约束维度对后续的增广拉格朗日方法至关重要——每个约束值都需要一个 $\lambda$ （拉格朗日乘子）和一个 $\mu$ （惩罚参数）。

4.8 本章小结

本章建立了约束建模的完整框架：

统一接口：所有约束都实现 ConstraintFunction，对外只有 Evaluate(state, control) 一个方法
标准形式：任何约束都转化为 $c(x,u) \leq 0$ （不等式）或 $c(x,u) = 0$ （等式）
车辆外形：用多圆近似矩形车体，兼顾准确性和计算效率
三种障碍物建模： - 多圆 vs 圆：最简单，处处可微 - 多圆 vs 多边形 (LSE)：更精确，通过 Log-Sum-Exp 保持光滑 - 精确多边形 vs 多边形 (SAT)：最精确，但边界处不光滑

4.1 为什么需要约束

4.2 约束的数学形式

违约度（Violation）

4.3 约束接口设计

4.4 车辆外形建模：多圆近似

4.4.1 为什么不能用一个点代表车辆？

4.4.2 多圆近似：兼顾准确性和计算效率

4.4.3 从车体坐标到世界坐标

4.5 具体约束实现

4.5.1 控制量上下界（ControlBoxConstraint）

4.5.2 速度限制（SpeedLimitConstraint）

4.5.3 道路边界约束

质点版（RoadBoundaryConstraint）

多圆版（MultiCircleRoadBoundaryConstraint）

4.5.4 障碍物约束：三种建模方式

方式一：多圆车 vs 圆形障碍物（MultiCircleVehicleObstacleConstraint）

方式二：多圆车 vs 多边形障碍物 — LSE 光滑近似（LSEPolygonObstacleConstraint）

凸多边形的半平面表示

为什么需要 Log-Sum-Exp？

LSE 约束的具体公式

方式三：精确多边形碰撞（PolygonCollisionConstraint）

车辆多边形的构造

多边形对之间的距离：SAT 算法

约束公式

4.5.5 终端目标约束（TerminalGoalConstraint）

4.6 带约束的最优控制问题

ConstrainedOptimalControlProblem

约束添加方式（demo_scenario.cpp 中的实现）

4.7 约束维度计数

4.8 本章小结

评论

加入讨论

0 条评论

AL-iLQR实践指南 4：各种约束如何建模

4.1 为什么需要约束

4.2 约束的数学形式

违约度（Violation）

4.3 约束接口设计

4.4 车辆外形建模：多圆近似

4.4.1 为什么不能用一个点代表车辆？

4.4.2 多圆近似：兼顾准确性和计算效率

4.4.3 从车体坐标到世界坐标

4.5 具体约束实现

4.5.1 控制量上下界（ControlBoxConstraint）

4.5.2 速度限制（SpeedLimitConstraint）

4.5.3 道路边界约束

质点版（RoadBoundaryConstraint）

多圆版（MultiCircleRoadBoundaryConstraint）

4.5.4 障碍物约束：三种建模方式

方式一：多圆车 vs 圆形障碍物（MultiCircleVehicleObstacleConstraint）

方式二：多圆车 vs 多边形障碍物 — LSE 光滑近似（LSEPolygonObstacleConstraint）

凸多边形的半平面表示

为什么需要 Log-Sum-Exp？

LSE 约束的具体公式

方式三：精确多边形碰撞（PolygonCollisionConstraint）

车辆多边形的构造

多边形对之间的距离：SAT 算法

约束公式

4.5.5 终端目标约束（TerminalGoalConstraint）

4.6 带约束的最优控制问题

ConstrainedOptimalControlProblem

约束添加方式（demo_scenario.cpp 中的实现）

4.7 约束维度计数

4.8 本章小结

评论

加入讨论

0 条评论