基于Gekko的双球摆哈密顿系统优化问题求助

问题背景 #

我正在$\mathbb{R}^3$空间中优化一组带弹簧摆的模拟方程，摆之间、摆与初始向量间均设有弹簧。系统本质为矩阵向量形式，现拆分为独立方程便于查看，当前仅包含两个摆：

• q[0:3]和q[3:6]表示摆的方向，摆长固定为1，因此q可直接代表整个摆
• p表示两个摆端点的速度

z方向有向下的力F作用于末端点，末端点定义为Q=l[0]*q[:3]+l[1]q[3:]，该力已纳入两个p的z方向方程中。

k*num/den项为弹簧对偏转角的反作用力，形式为m.acos(q1@q2)/m.sqrt(1-(q1@q2)**2)。当摆同向时会出现0/0问题，初始状态曾为此情况，已通过微扰初始x值修复。

摆受约束方程限制长度为1，因此p方程中包含q*lam项（约束哈密顿系统的标准结构）。

故障现象 #

无控制输入u时，系统可正常求解微分方程演化；但添加u后出现以下问题：

• 使用m.options.SOLVER 1和3时，触发最大迭代次数错误
• 使用SOLVER 2时，触发矩阵奇异错误（提示信息：MATRIX IS SINGULAR. RANK= 135）

简化代码 #

from gekko import GEKKO
import numpy as np
#%% Functions
def get_M(n, d, ml, l):
    M = np.zeros((n*d, n*d))
    for i in range(n):
        for j in range(n):
            M[d*i:d*(i+1), d*j:d*(j+1)] = sum(ml[max(i, j):]) * l[i] * l[j] * np.eye(d)
    return M
def get_q(theta, phi):
    theta = theta*np.pi/180
    phi = phi*np.pi/180
    x = np.sin(theta)*np.cos(phi)
    y = np.sin(theta)*np.sin(phi)
    z = np.cos(theta)
    return np.array([x, y, z])
#%% Run
m = GEKKO()

n = 2; d = 3; #n number of pendulums, d dimension
k = np.ones(n)*1e3; ml = np.ones(n); l = np.ones(n)  #k list of spring constants, ml list of masses, l list of lengths
F = 1 # Force
N = 3 # Number of steps + 1
Minv = np.linalg.inv(get_M(n=n, d=d, ml=ml, l=l)) # Inverted mass matrix

m.time = np.linspace(0, 0.01, N)

q = m.Array(m.Var, n*d, value=0)
q[0].value, q[1].value, q[2].value = get_q(90, 90.001) # Initial values of pendulum 1
q[3].value, q[4].value, q[5].value = get_q(90, 89.999) # Initial values of pendulum 2

p = m.Array(m.Var, n*d, value=0)

lam = m.Array(m.Var, n, value=0)

sv = np.zeros(3)
sv[1] = 1 # [0., 1., 0.] Initial vector, first spring is between this and the first pendulum

# control variables
u = m.MV(value=0)
u.STATUS = 1

# q equations
m.Equations(q[i].dt() == (Minv@p)[i] for i in range(n*d))

# spring terms
s1 = m.Var(lb=0)
m.Equation(s1 >= 1-(q[:3]@sv)**2)
m.Minimize(s1)
num1 = m.Intermediate(m.acos(q[:3]@sv))
den1 = m.Intermediate(m.sqrt(s1))

s2 = m.Var(lb=0)
m.Equation(s2 >= 1-(q[:3]@q[3:])**2)
m.Minimize(s2)
num2 = m.Intermediate(m.acos(q[:3]@q[3:]))
den2 = m.Intermediate(m.sqrt(s2))

# p equations
m.Equation(p[0].dt() ==                                + k[1]*num2/den2*q[3] - q[0]*lam[0]) # x
m.Equation(p[1].dt() ==             k[0]*num1/den1     + k[1]*num2/den2*q[4] - q[1]*lam[0]) # y
m.Equation(p[2].dt() == - F +  u                       + k[1]*num2/den2*q[5] - q[2]*lam[0]) # z
m.Equation(p[3].dt() ==                                + k[1]*num2/den2*q[0] - q[3]*lam[1]) # x
m.Equation(p[4].dt() ==                                + k[1]*num2/den2*q[1] - q[4]*lam[1]) # y
m.Equation(p[5].dt() == - F +  u                       + k[1]*num2/den2*q[2] - q[5]*lam[1]) # z

# constraints
m.Equation(0.5 * (q[:3]@q[:3] - 1) == 0)
m.Equation(0.5 * (q[3:]@q[3:] - 1) == 0)

m.options.IMODE = 6
m.options.SOLVER = 3

m.Obj((q[5]-2*q[2])**2)
m.solve(disp=1)

求助说明 #

刚接触Gekko，代码可能存在明显疏漏，恳请帮忙排查问题。

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