Scipy.optimize fmin 不优化四轴飞行器的功能

如何解决Scipy.optimize fmin 不优化四轴飞行器的功能

我正在尝试使用 scipy 的 fmin 函数找到我的四轴飞行器的工作点。我的四轴飞行器模型的动力学看起来像下面的代码。

def stateDerivative(x,W):
    kf,km = 0.2,0.125
    length = 10
    g = 9.81
    m = 0.1
    Ixx = 0.1
    Iyy = 0.1
    Izz = 0.9*(Ixx + Iyy)
    ub = x[0] #VeLocity in the x direction 
    vb = x[1] #VeLocity in the y direction
    wb = x[2] #VeLocity in the z direction
    p = x[3] #Angular acceleration in the x direction
    q = x[4] #Angular acceleration y direction
    r = x[5] #Angular acceleration z direction
    psi = x[6] #Roll angle
    theta = x[7] #Pitch angle
    phi = x[8] #Yaw angle
    xE = x[9] #X coordinate of the quadcopter
    yE = x[10] #Y coordinate 
    zE = x[11] #Z coordinate 
    W = np.clip(W,10) #Propeller speed should be between 0 and 10
    F1,F2,F3,F4 = [kf * w**2 for w in W ]
    M1,M2,M3,M4 = [km * w**2 for w in W]
    Fz = F1 + F2 + F3 + F4
    #Fz = u[0]
    L = length*(F2-F4)
    M = length*(F3-F1)
    N = M1 - M2 + M3 - M4

    #L,M,N = u[1],u[2],u[3]
    cpsi = np.cos(psi); spsi = np.sin(psi)
    ctheta = np.cos(theta); stheta = np.sin(theta)
    cphi = np.cos(phi); sphi = np.sin(phi)

    xdot = np.zeros(12)

    xdot[0] = -g * stheta + r * vb - q * wb  # = udot
    xdot[1] = g * sphi*ctheta - r * ub + p * wb # = vdot
    xdot[2] = 1/m * (-Fz) + g*cphi*ctheta + q * ub - p * vb # = wdot
    xdot[3] = 1/Ixx * (L + (Iyy - Izz) * q * r)  # = pdot
    xdot[4] = 1/Iyy * (M + (Izz - Ixx) * p * r)  # = qdot
    xdot[5] = 1/Izz * (N + (Ixx - Iyy) * p * q)  # = rdot
    xdot[6] = p + (q*sphi + r*cphi) * stheta / ctheta  # = phidot
    xdot[7] = q * cphi - r * sphi  # = thetadot
    xdot[8] = (q * sphi + r * cphi) / ctheta  # = psidot
    xdot[9] =ctheta*cpsi*ub+(-cphi*spsi+sphi*stheta*cpsi)*vb+(sphi*spsi+cphi*stheta*cpsi)*wb#= xEdot
    xdot[10] =ctheta*spsi*ub+(cphi*cpsi+sphi*stheta*spsi)*vb +(-sphi*cpsi+cphi*stheta*spsi)*wb#= yEdot
    xdot[11] = -1*(-stheta * ub + sphi*ctheta * vb + cphi*ctheta * wb) # = hEdot
    return xdot

应该最小化的成本函数如下。请注意，我的约束是我希望 x 方向上的速度等于 2 m/s。

def cost(z_ini):
    x = z_ini[:12]
    u = z_ini[12:]
    xdot = stateDerivative(x,u)
    Q = np.append(xdot,(x[0]-2)).reshape(1,-1) # veLocity constraint. 

    H = np.diag(np.ones((1,15))).reshape(1,-1)
    return np.dot(np.dot(H,Q),Q.T)

下一部分实现 fmin 函数。

z_ini = np.zeros((1,16)) #Intial guess 
z_ini[0,11] = 2 # Initial guess
minimum = fmin(cost,z_ini,maxiter=50000,maxfun=50000,xtol=0.0001,ftol=0.0001)
print(minimum)

代码在不满足如下约束的情况下成功运行。

Optimization terminated successfully.
         Current function value: 10.832437
         Iterations: 143
         Function evaluations: 578
[ 1.54479663e+00 -5.40596482e-09 -3.61909532e-08  1.24255826e-07
 -1.29738201e+00 -2.17997571e-08  2.06902429e+00 -2.23037510e-09
  8.63633640e-09  7.05479172e+00  4.26959916e-01 -3.12357411e+02
  1.31965608e+00 -3.32139581e-01  1.31965608e+00 -1.09420133e+00]

x 方向的速度是 1.544 m/s 而不是 2 m/s。为什么会发生这种情况？