基于pydrake的倒立摆吸引域（RoA）分析求解失败问题咨询

核心错误点 #

• 黎卡提解S是平衡点[pi, 0]处线性化系统求解得到的，对应的Lyapunov候选函数必须以状态相对于平衡点的偏移量为输入，你当前直接用全局状态x计算V = x.dot(S.dot(x))，会导致V在平衡点处取值不为0，整个RoA分析的前提不成立。
• 动力学中用错了控制输入：你定义了带参考点偏移的控制量u2，但实际代入动力学fn的是针对全局原点设计的u，控制器根本不能稳定[pi, 0]这个平衡点，自然不可能满足Vdot负定的条件。

修正方案 #

1. 统一用状态偏移量计算所有项：将状态x定义为相对于[pi, 0]的偏移，即全局角度为x[0] + pi，全局角速度为x[1]，此时平衡点对应偏移量x=[0,0]，所有运算都基于偏移后的x完成。
2. 控制输入改用偏移量计算，不要用全局状态计算的u。
3. 可将rho设为优化变量最大化取值，不用手动固定测试。

修正后的核心代码片段 #

# 常量定义和LQR求解部分保持不变，以下是动力学和SOS约束部分修改
prog = MathematicalProgram()
# x为相对于平衡点[pi,0]的偏移量：x[0] = theta - pi, x[1] = thetadot - 0
x = prog.NewIndeterminates(2, "x")
x1_offset = x[0]
x2_offset = x[1]

# 三阶泰勒展开，此时展开点为偏移量0，对应全局theta=pi
Tsin = -x1_offset + (x1_offset**3)/6  
# 控制输入基于偏移量计算，等价于原来的u2
u = -K.dot(x)[0] 

# 非线性动力学（偏移量下的形式）
fn = [
    x2_offset,
    (u - d*x2_offset - Tsin * m * g * l) / (m * l**2)
]

# Lyapunov函数直接用偏移量x计算，平衡点处V=0
V = x.dot(S.dot(x))
Vdot = Jacobian([V], x).dot(fn)[0]

# 拉格朗日乘子定义不变
lambda_ = prog.NewSosPolynomial(Variables(x), 2)[0].ToExpression()

# 把rho设为优化变量，最大化其取值
rho = prog.NewContinuousVariables(1, "rho")[0]
prog.AddCost(-rho)

# SOS约束保持逻辑不变
prog.AddSosConstraint(-Vdot + lambda_*(V - rho))
prog.AddSosConstraint(V)
prog.AddSosConstraint(lambda_)
# 加一个rho的下界约束避免 trivial 解
prog.AddBoundingBoxConstraint(0, 10, rho)

# 求解
result = Solve(prog)
if result.is_success():
    rho_sol = result.GetSolution(rho)
    print(f"Verified RoA: V < {rho_sol:.3f}")
else:
    print("求解失败")

额外排查方向 #

如果修正后仍然存在数值求解问题，可以尝试：

• 降低拉格朗日乘子的多项式次数，或者使用齐次多项式
• 给SOS约束加小的数值裕度，比如约束右边加1e-6 * x.dot(x)避免数值病态
• 切换到Mosek等商用SDP求解器，精度比默认求解器更高

内容的提问来源于stack exchange，提问作者Zheng Cheng