RobotLocomotion/drake项目中的三次多项式系统吸引域计算分析

引言：非线性系统稳定性分析的挑战

在机器人学和控制系统领域，非线性动力系统的稳定性分析一直是核心挑战。传统线性系统分析方法在面对非线性特性时往往失效，特别是当系统存在多个平衡点时。三次多项式系统 ẋ = -x + x³ 作为一个典型的非线性系统案例，完美展示了这种复杂性——它拥有三个平衡点（-1, 0, 1），其中只有原点（0）是稳定的。

关键问题场景：工程师在设计控制器时，需要确切知道从哪些初始状态出发，系统能够收敛到稳定平衡点。这个"安全区域"就是吸引域（Region of Attraction, ROA）。传统的试错方法不仅耗时，而且无法提供理论保证。

吸引域计算的数学基础

Lyapunov稳定性理论

Lyapunov第二方法为非线性系统稳定性分析提供了强有力的工具。其核心思想是：如果存在一个正定的Lyapunov函数V(x)，且其沿系统轨迹的导数V̇(x)负定，那么系统是渐近稳定的。

对于我们的三次多项式系统：

• 系统动力学：ẋ = -x + x³
• 候选Lyapunov函数：V(x) = x²
• 时间导数：V̇(x) = 2x·ẋ = 2x(-x + x³) = -2x² + 2x⁴

Sum-of-Squares（SOS）规划方法

SOS规划将Lyapunov稳定性条件转化为可计算的优化问题。基本思路是寻找最大的ρ，使得：

• 在边界 V(x) = ρ 上，V̇(x) ≤ 0
• 在区域 V(x) ≤ ρ 内，V̇(x) < 0（除原点外）

flowchart TD
    A[非线性系统<br>ẋ = f(x)] --> B[选择候选Lyapunov函数 V(x)]
    B --> C[计算时间导数 V̇(x) = ∇V·f(x)]
    C --> D[构建SOS优化问题]
    D --> E[使用SOS约束表达稳定性条件]
    E --> F[求解凸优化问题]
    F --> G[获得吸引域估计 V(x) ≤ ρ]
    G --> H[验证结果精度]

Drake实现详解

系统建模

Drake使用模板化的VectorSystem类来定义动力系统：

template <typename T>
class CubicPolynomialSystem : public systems::VectorSystem<T> {
 public:
  CubicPolynomialSystem() : systems::VectorSystem<T>(0, 0, false) {
    this->DeclareContinuousState(1);  // 单状态变量
  }

 private:
  void DoCalcVectorTimeDerivatives(
      const systems::Context<T>& context,
      const Eigen::VectorBlock<const VectorX<T>>& input,
      const Eigen::VectorBlock<const VectorX<T>>& state,
      Eigen::VectorBlock<VectorX<T>>* derivatives) const {
    unused(context, input);
    using std::pow;
    (*derivatives)(0) = -state(0) + pow(state(0), 3.0);
  }
};

SOS优化问题构建

void ComputeRegionOfAttraction() {
  // 创建符号系统
  CubicPolynomialSystem<Expression> system;
  auto context = system.CreateDefaultContext();
  auto derivatives = system.AllocateTimeDerivatives();

  solvers::MathematicalProgram prog;
  const VectorX<Variable> xvar{prog.NewIndeterminates<1>("x")};
  const VectorX<Expression> x = xvar.cast<Expression>();

  // 设置系统状态并计算导数
  context->get_mutable_continuous_state_vector().SetFromVector(x);
  system.CalcTimeDerivatives(*context, derivatives.get());

  // 定义Lyapunov函数和时间导数
  const Expression V = x.dot(x);
  const Expression Vdot = (Jacobian(Vector1<Expression>(V), xvar) * 
                         derivatives->CopyToVector())[0];

  // SOS约束：最大化ρ，使得(V(x)-ρ)x² - λ(x)V̇(x)是SOS
  const Variable rho{prog.NewContinuousVariables<1>("rho").coeff(0)};
  const Expression lambda{
      prog.NewSosPolynomial(Variables(xvar), 4).first.ToExpression()};

  prog.AddSosConstraint((V - rho) * x.dot(x) - lambda * Vdot);
  prog.AddLinearCost(-rho);  // 最大化ρ
}

数学原理深度解析

SOS规划的形式化描述

吸引域计算问题可表述为：

$$\begin{aligned} &\max_{\rho, \lambda(x)} \rho \\ &\text{subject to:} \\ &(V(x) - \rho)x^2 - \lambda(x)\dot{V}(x) \in \Sigma[x] \\ &\lambda(x) \in \Sigma[x] \end{aligned}$$

其中 $\Sigma[x]$ 表示平方和多项式集合。

稳定性条件验证

对于三次多项式系统：

• $V(x) = x^2$ 是正定的
• $\dot{V}(x) = -2x^2 + 2x^4 = 2x^2(x^2 - 1)$
• 当 $|x| < 1$ 时，$\dot{V}(x) < 0$（除原点）
• 理论上最大吸引域应为 $V(x) < 1$

计算结果与分析

运行示例程序后，Drake输出：

Verified that x^2 < 1.0 is in the region of attraction.

这个结果与理论分析完全一致，验证了SOS方法的正确性。

性能对比

方法	计算复杂度	精度	适用性
传统数值积分	高	中等	通用但耗时
SOS规划	中等	高	多项式系统
线性化方法	低	低	仅局部

实际应用场景

机器人控制设计

在机器人控制中，吸引域分析帮助工程师：

1. 控制器参数整定：确保控制器在期望区域内稳定
2. 安全边界确定：避免系统进入不稳定区域
3. 故障恢复策略：设计从扰动中恢复的机制

相关领域应用

动态系统经常面临非线性特性，吸引域分析可用于：

• 系统恢复策略验证
• 大范围运动稳定性保证
• 控制系统冗余设计

扩展与进阶应用

高维系统处理

对于多变量系统，Drake支持：

// 多变量SOS多项式构造
const Expression lambda_multi{
    prog.NewSosPolynomial(Variables(xvar), 6).first.ToExpression()};

混合整数规划扩展

对于包含离散变量的混合系统：

// 添加整数约束
prog.AddConstraint(lambda, 0, 1); // 二进制变量

最佳实践与注意事项

计算效率优化

1. 多项式阶数选择：过高的阶数增加计算负担，过低则影响精度
2. 稀疏性利用：利用多项式稀疏结构加速计算
3. 并行计算：Drake支持多线程SOS规划求解

数值稳定性

// 添加数值稳定性约束
prog.AddConstraint(rho >= 0.001); // 避免数值问题
prog.AddConstraint(lambda >= 0.001); // 保证正定性

结论与展望

Drake的三次多项式系统吸引域计算示例展示了现代控制理论中SOS规划的强大能力。这种方法：

1. 提供理论保证：严格的数学证明替代经验猜测
2. 计算高效：将非线性问题转化为凸优化
3. 扩展性强：适用于各类多项式系统

未来发展方向包括：

• 更高维系统的有效处理
• 实时吸引域计算
• 与机器学习方法的结合

通过掌握这些技术，工程师能够为复杂的机器人系统设计出既安全又高性能的控制策略，推动自动化、智能系统等领域的创新发展。

实践建议：读者可以修改系统参数（如将 -x + x³ 改为 -x + 0.5x³），观察吸引域的变化，深入理解非线性系统的稳定性特性。