陆拾贰號1. 轨迹优化基础概念解析
轨迹优化是机器人学和自动驾驶领域的核心技术之一。简单来说，它解决的问题是：给定一个系统的初始状态和目标状态，如何找到一条最优的运动路径，使得某种代价（如能量消耗、时间、偏差等）最小化。
想象一下你开车从家到公司的场景。你希望：
- 准时到达（终端代价）
- 行驶平稳（控制代价）
- 燃油经济（阶段代价）
- 避开障碍（约束条件）
这就是一个典型的轨迹优化问题。在数学上，我们可以用两种方式描述这个问题：
1.1 连续时间表述
在连续时间域，轨迹优化问题可以表示为：
minx(t), u(t)ℓf(x(tf))+∫0tfℓ(x(t),u(t)) dt
s.t.x˙=f(x,u),x(0)=x0
其中：
- x(t) ∈ ℝⁿ 是状态向量（如位置、速度等）
- u(t) ∈ ℝᵐ 是控制向量（如加速度、转向角等）
- f(x,u) 是系统动力学模型
- ℓ(x,u) 是阶段代价函数
- ℓf(x(tf)) 是终端代价函数
1.2 离散时间表述
由于计算机处理的是离散数据，我们通常将问题离散化为N个时间步：
minu0,…,uN-1ℓN(xN)+∑k=0N-1ℓk(xk,uk)
s.t.xk+1=f(xk,uk,Δt),k=0,1,…,N-1
离散化后，轨迹由一系列"节点"(knot points)组成，每个节点包含状态和控制量：
- 状态序列：x₀, x₁, ..., x_N（共N+1个）
- 控制序列：u₀, u₁, ..., u_{N-1}（共N个）
提示：时域长度(horizon)N的选择很重要——太短可能导致无法到达目标，太长会增加计算负担。通常需要根据具体问题进行调整。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 核心数据结构与实现
2.1 基础类型定义
项目使用Eigen库进行线性代数运算，定义了两个基础类型：
cpp复制// include/core/types.hpp
using Vector = Eigen::VectorXd; // 动态大小的列向量
using Matrix = Eigen::MatrixXd; // 动态大小的矩阵
选择Eigen的原因：
- 高性能的矩阵运算
- 丰富的线性代数功能
- 表达式模板优化
- 广泛用于机器人学和控制系统
2.2 轨迹容器实现
Trajectory类是项目的核心数据结构，用于存储和管理轨迹数据：
cpp复制// include/core/trajectory.hpp
class Trajectory {
public:
Trajectory(int state_dim, int control_dim, int horizon);
// 访问接口
Vector& State(int k); // k ∈ [0, N]
Vector& Control(int k); // k ∈ [0, N-1]
// 维度查询
int Horizon() const; // 返回N
int StateDim() const; // 状态维度n
int ControlDim() const; // 控制维度m
private:
std::vector<Vector> states_; // 大小N+1
std::vector<Vector> controls_; // 大小N
double dt_; // 时间步长
};
关键设计考虑：
- 使用
std::vector存储状态和控制序列，便于动态调整大小 - 提供维度检查，防止越界访问
- 封装时间步长信息，便于统一管理
3. 动力学模型详解
3.1 动力学模型抽象接口
所有动力学模型都继承自基类DynamicsModel：
cpp复制// include/dynamics/dynamics_model.hpp
class DynamicsModel {
public:
virtual int StateDim() const = 0; // 状态维度n
virtual int ControlDim() const = 0; // 控制维度m
// 核心接口：计算下一状态
virtual Vector NextState(const Vector& state,
const Vector& control,
double dt) const = 0;
};
3.2 常见动力学模型实现
3.2.1 线性点质量模型
最简单的动力学模型，适用于直线运动物体：
状态：x = [p, v]ᵀ （位置，速度）
控制：u = [a] （加速度）
动力学方程：
p_{k+1} = p_k + Δt·v_k
v_{k+1} = v_k + Δt·a_k
3.2.2 独轮车模型(Unicycle)
常用于移动机器人：
状态：x = [x, y, θ]ᵀ （位置x,y，朝向θ）
控制：u = [v, ω]ᵀ （线速度，角速度）
动力学方程：
x_{k+1} = x_k + Δt·v_k·cos(θ_k)
y_{k+1} = y_k + Δt·v_k·sin(θ_k)
θ_{k+1} = θ_k + Δt·ω_k
3.2.3 运动学自行车模型
自动驾驶常用模型：
状态：x = [x, y, θ, v]ᵀ （位置x,y，朝向θ，速度v）
控制：u = [a, δ]ᵀ （加速度，前轮转角）
动力学方程：
x_{k+1} = x_k + Δt·v_k·cos(θ_k)
y_{k+1} = y_k + Δt·v_k·sin(θ_k)
θ_{k+1} = θ_k + Δt·(v_k·tan(δ_k)/L)
v_{k+1} = v_k + Δt·a_k
其中L是轴距（wheelbase）。
实现代码：
cpp复制Vector KinematicBicycleModel::NextState(const Vector& state,
const Vector& control,
double dt) const {
const double x = state(0);
const double y = state(1);
const double yaw = state(2);
const double velocity = state(3);
const double acceleration = control(0);
const double steering = control(1);
Vector next_state(StateDim());
next_state(0) = x + dt * velocity * std::cos(yaw);
next_state(1) = y + dt * velocity * std::sin(yaw);
next_state(2) = yaw + dt * velocity * std::tan(steering) / wheelbase_;
next_state(3) = velocity + dt * acceleration;
return next_state;
}
注意事项：自行车模型假设无滑动，实际应用中需要考虑轮胎动力学和滑动影响。当速度较高或地面摩擦较小时，这个简化模型可能不够准确。
4. 代价函数设计与实现
4.1 代价函数抽象接口
cpp复制// include/cost/cost_function.hpp
class CostFunction {
public:
virtual int StateDim() const = 0;
virtual int ControlDim() const = 0;
virtual double StageCost(const Vector& state,
const Vector& control) const = 0;
virtual double TerminalCost(const Vector& state) const = 0;
};
4.2 二次代价函数实现
最常用的代价函数形式是二次型：
阶段代价：
ℓ_k(x_k, u_k) = ½(x_k - x_ref)ᵀQ(x_k - x_ref) + ½(u_k - u_ref)ᵀR(u_k - u_ref)
终端代价：
ℓ_N(x_N) = ½(x_N - x_ref)ᵀQ_f(x_N - x_ref)
其中：
- Q, Q_f是半正定状态权重矩阵
- R是正定控制权重矩阵
- x_ref, u_ref是参考状态和控制
实现代码：
cpp复制double QuadraticCost::StageCost(const Vector& state,
const Vector& control) const {
const Vector dx = state - state_reference_;
const Vector du = control - control_reference_;
return 0.5 * dx.dot(Q_ * dx) + 0.5 * du.dot(R_ * du);
}
double QuadraticCost::TerminalCost(const Vector& state) const {
const Vector dx = state - state_reference_;
return 0.5 * dx.dot(Qf_ * dx);
}
权重矩阵设计技巧：
- 对角线元素对应各状态/控制量的权重
- 非对角线元素可以表示状态/控制量之间的耦合惩罚
- Q_f通常比Q大，强调终端精度
- R的大小影响控制量的激进程度
5. 最优控制问题封装
5.1 OptimalControlProblem类
将动力学模型、代价函数和问题参数封装在一起：
cpp复制// include/problems/optimal_control_problem.hpp
class OptimalControlProblem {
public:
OptimalControlProblem(std::shared_ptr<DynamicsModel> dynamics,
std::shared_ptr<CostFunction> cost,
int horizon, double dt);
// 前向仿真
Trajectory Rollout(const Vector& initial_state,
const std::vector<Vector>& control_sequence) const;
// 计算总代价
double TotalCost(const Trajectory& trajectory) const;
private:
std::shared_ptr<DynamicsModel> dynamics_;
std::shared_ptr<CostFunction> cost_;
int horizon_;
double dt_;
};
5.2 前向仿真(Rollout)实现
Rollout是指给定初始状态和控制序列，通过动力学模型生成完整轨迹的过程：
cpp复制Trajectory OptimalControlProblem::Rollout(
const Vector& initial_state,
const std::vector<Vector>& control_sequence) const {
Trajectory trajectory(StateDim(), ControlDim(), horizon_);
trajectory.SetTimeStep(dt_);
trajectory.State(0) = initial_state;
for (int k = 0; k < horizon_; ++k) {
trajectory.Control(k) = control_sequence[k];
trajectory.State(k + 1) =
dynamics_->NextState(trajectory.State(k),
trajectory.Control(k),
dt_);
}
return trajectory;
}
5.3 总代价计算
cpp复制double OptimalControlProblem::TotalCost(
const Trajectory& trajectory) const {
double total_cost = 0.0;
// 累加阶段代价
for (int k = 0; k < horizon_; ++k) {
total_cost += cost_->StageCost(trajectory.State(k),
trajectory.Control(k));
}
// 加上终端代价
total_cost += cost_->TerminalCost(trajectory.State(horizon_));
return total_cost;
}
6. 实践建议与常见问题
6.1 参数选择经验
-
时间步长Δt：
- 太大：仿真精度低
- 太小：计算负担重
- 经验值：通常取系统最快动态的1/10~1/20
-
时域长度N：
- 应覆盖系统主要动态响应时间
- 可通过试验调整：从较小值开始，逐步增加直到性能不再明显改善
-
权重矩阵：
- 先设置对角线元素，再考虑耦合项
- 通过Bryson规则归一化：Q_ii = 1/(允许的最大x_i²)
- 调试时可以先设大R保证稳定，再逐步减小
6.2 常见问题排查
-
仿真发散：
- 检查动力学模型实现是否正确
- 验证Δt是否过大
- 检查控制量是否超出物理限制
-
优化结果不理想：
- 检查代价函数权重设置
- 确认参考轨迹是否可行
- 尝试增加时域长度N
-
计算速度慢：
- 检查是否有不必要的内存拷贝
- 考虑使用更高效的线性代数库
- 评估是否可以增大Δt或减小N
6.3 扩展思考
-
模型不确定性处理：
- 可以扩展为鲁棒或随机最优控制
- 加入扰动估计和补偿
-
实时性优化：
- 考虑模型预测控制(MPC)框架
- 实现热启动和并行计算
-
复杂约束处理：
- 加入障碍物避免约束
- 考虑控制输入和状态约束
在实际自动驾驶项目中，我曾遇到自行车模型在高速过弯时预测不准的问题。解决方案是加入简单的轮胎侧偏模型，在不显著增加计算负担的情况下提高了轨迹预测精度。这提醒我们，模型复杂度需要根据应用场景仔细权衡。