MATLAB中的局部不可行性

Question

我需要一个动态优化问题的帮助，这个动态优化问题存在于无人机的消耗能量优化与这个最优控制问题。

enter image description here

enter image description here

我的代码是

Ecuations：

Parameters
tf

#Velocidad de rotores rad/s
#Las condiciones iniciales permiten igualar la acción de la gravedad
#Se tomo 4000rad/s como la velocidad maxima de los rotores
w1 = 912.32, >=0, <=3000
w2 = 912.32, >=0, <=3000
w3 = 912.32, >=0, <=3000
w4 = 912.32, >=0, <=3000
t1 = 0, >=0
t2 = 0, >=0
t3 = 0, >=0
t4 = 0, >=0

Constants
!----------------COEFICIENTES DEL MODELO-----------------!
#Gravedad
g = 9.81 !m/s^2
pi = 3.14159265359

#Motor Coefficients
 J = 4.1904e-5 !kg*m^2
 kt = 0.0104e-3 !N*m/A
 kv = 96.342 !rad/s/volt
 Dv = 0.2e-3 !N*m*s/rad
 R = 0.2 !Ohms

#Battery parameters
 Q = 1.55 !Ah
 Rint = 0.02 !Ohms
 E0 = 1.24 !volt
 K = 2.92e-3 !volt
 A = 0.156
 B =2.35

#Quadrotor parameters
  l = 0.175 !m
  m = 1.3 !kg
  Ix = 0.081 !kg*m^2
  Iy = 0.081 !kg*m^2
  Iz = 0.142 !kg*m^2
  kb = 3.8305e-6 !N/rad/s
  ktau = 2.2518e-8 !(N*m)/rad/s

#Parametrizacion del polinomio
  a1 = -1.72e-5
  a2 = 1.95e-5
  a3 = -6.98e-6
  a4 = 4.09e-7
  b1 = 0.014
  b2 = -0.0157
  b3 = 5.656e-3
  b4 = -3.908e-4
  c1 = -0.8796
  c2 = 0.3385
  c3 = 0.2890
  c4 = 0.1626

Variables
!------------------CONDICONES INICIALES------------------!
 x = 0
 xp = 0
 y = 0
 yp = 0
 z = 0
 zp = 0
 pitch = 0, >=-pi/2, <=pi/2   !theta - restricciones
 pitchp = 0
 roll = 0, >=-pi/2, <=pi/2    !phi - restricciones
 rollp = 0
 yaw = 0                      !psi
 yawp = 0%, >=-200/180, <=200/180

#Función objetivo
  of = 0 !condición inicial de la función objetivo
Intermediates

#Motor 1
  aw1 = a1*w1^2 + b1*w1 + c1
  bw1 = a2*w1^2 + b2*w1 + c2
  cw1 = a3*w1^2 + b3*w1 + c3
  dw1 = a4*w1^2 + b4*w1 + c4
#Motor 2
  aw2 = a1*w2^2 + b1*w2 + c1
  bw2 = a2*w2^2 + b2*w2 + c2
  cw2 = a3*w2^2 + b3*w2 + c3
  dw2 = a4*w2^2 + b4*w2 + c4 
#Motor 3
  aw3 = a1*w3^2 + b1*w3 + c1
  bw3 = a2*w3^2 + b2*w3 + c2
  cw3 = a3*w3^2 + b3*w3 + c3
  dw3 = a4*w3^2 + b4*w3 + c4
#Motor 4
  aw4 = a1*w4^2 + b1*w4 + c1
  bw4 = a2*w4^2 + b2*w4 + c2
  cw4 = a3*w4^2 + b3*w4 + c3
  dw4 = a4*w4^2 + b4*w4 + c4
#frj(wj(t),Tj(t))
  fr1=aw1*t1^3 + bw1*t1^2 + cw1*t1 + dw1
  fr2=aw2*t2^3 + bw2*t2^2 + cw2*t2 + dw2
  fr3=aw3*t3^3 + bw3*t3^2 + cw3*t3 + dw3
  fr4=aw4*t4^3 + bw4*t4^2 + cw4*t4 + dw4
!---------------------CONTROL INPUTS---------------------!
  T = kb * (w1^2 + w2^2 + w3^2 + w4^2)
  u1 = kb * (w2^2 - w4^2)
  u2 = kb * (w3^2 - w1^2)
  u3 = ktau * (w1^2 - w2^2 + w3^2 - w4^2)
  wline = w1 - w2 + w3 - w4
!-------------------ENERGIA POR ROTOR--------------------!
  Ec1 = ((J*$w1 + ktau*w1^2 + Dv*w1)/fr1)*w1
  Ec2 = ((J*$w2 + ktau*w2^2 + Dv*w2)/fr2)*w2
  Ec3 = ((J*$w3 + ktau*w3^2 + Dv*w3)/fr3)*w3
  Ec4 = ((J*$w4 + ktau*w4^2 + Dv*w4)/fr4)*w4
  Ectotal = Ec1 + Ec2 + Ec3 + Ec4
Equations
!---------------MINIMIZAR FUNCIÓN OBJETIVO---------------!
  minimize tf * of
!-----------------RELACION DE VARIABLES------------------!
  xp = $x
  yp = $y
  zp = $z
  pitchp = $pitch
  rollp = $roll
  yawp = $yaw
!-----------------CONDICONES DE FRONTERA-----------------!
#Condiciones finales del modelo
  tf * x = 4
  tf * y = 5
  tf * z = 6
  tf * xp = 0
  tf * yp = 0
  tf * zp = 0
  tf * roll = 0
  tf * pitch = 0
  tf * yaw = 0
!-----------------TORQUE DE LOS MOTORES------------------!
  t1 = J*$w1 + ktau*w1^2 + Dv*w1
  t2 = J*$w2 + ktau*w2^2 + Dv*w2
  t3 = J*$w3 + ktau*w3^2 + Dv*w3
  t4 = J*$w4 + ktau*w4^2 + Dv*w4
!------------------------SUJETO A------------------------!
#Modelo aerodinámico del UAV
  m*$xp = (cos(roll)*sin(pitch)*cos(yaw) + sin(roll)*sin(yaw))*T
  m*$yp = (cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw))*T
  m*$zp = (cos(roll)*cos(pitch))*T-m*g
  Ix*$rollp = ((Iy - Iz)*pitchp*yawp + J*pitchp*wline + l*u1)
  Iy*$pitchp = ((Iz - Ix)*rollp*yawp - J*rollp*wline + l*u2)
  Iz*$yawp = ((Ix - Iy)*rollp*pitchp + u3)
!--------------------FUNCIÓN OBJETIVO--------------------!
  $of = Ectotal

MATLAB：

clear all; close all; clc

server = 'http://127.0.0.1';
app = 'traj_optima';

addpath('C:/Program Files/MATLAB/apm_matlab_v0.7.2/apm')
apm(server,app,'clear all');
apm_load(server,app,'ecuaciones_mod.apm');
csv_load(server,app,'tiempo2.csv');

apm_option(server,app,'apm.max_iter',200);
apm_option(server,app,'nlc.nodes',3);
apm_option(server,app,'apm.rtol',1);
apm_option(server,app,'apm.otol',1);
apm_option(server,app,'nlc.solver',3);
apm_option(server,app,'nlc.imode',6);
apm_option(server,app,'nlc.mv_type',1);


costo=1e-5;%1e-5
%VARIABLES CONTROLADAS
%Velocidades angulares
apm_info(server,app,'MV','w1');
apm_option(server,app,'w1.status',1);
apm_info(server,app,'MV','w2');
apm_option(server,app,'w2.status',1);
apm_info(server,app,'MV','w3'); 
apm_option(server,app,'w3.status',1);
apm_info(server,app,'MV','w4');
apm_option(server,app,'w4.status',1);

% Torques
apm_info(server,app,'MV','t1');
apm_option(server,app,'t1.status',1);
apm_info(server,app,'MV','t2');
apm_option(server,app,'t2.status',1);
apm_info(server,app,'MV','t3');
apm_option(server,app,'t3.status',1);
apm_info(server,app,'MV','t4');
apm_option(server,app,'t4.status',1);

%Salida
output = apm(server,app,'solve');
disp(output)
y = apm_sol(server,app); 
z = y.x;

tiempo2.csv

time,tf
0,0
0.001,0
0.2,0
0.4,0
0.6,0
0.8,0
1,0
1.2,0
1.4,0
1.6,0
1.8,0
2,0
2.2,0
2.4,0
2.6,0
2.8,0
3,0
3.2,0
3.4,0
3.6,0
3.8,0
4,0
4.2,0
4.4,0
4.6,0
4.8,0
5,0
5.2,0
5.4,0
5.6,0
5.8,0
6,0
6.2,0
6.4,0
6.6,0
6.8,0  
7,0
7.2,0
7.4,0
7.6,0
7.8,0
8,0
8.2,0
8.4,0 
8.6,0
8.8,0
9,0
9.2,0
9.4,0
9.6,0
9.8,0
10,1

最后得到的答案是：

enter image description here

我需要帮助来解决这个本地不可行的问题。

Answer 1

不可行解是由终端约束引起的：

  tf * z = 4
  tf * z = 5
  tf * z = 6

如果为tf=0，则会将约束求值为0=4、0=5、0=6，并且求解器会报告求解器无法满足这些条件。相反，您可以将约束设置为：

  tf * (x-4) = 0
  tf * (y-5) = 0
  tf * (z-6) = 0

这样，约束在最后一次执行tf=0和tf=1时有效。一种潜在的更好的方法是使用f=1000将终端约束转换为客观条件，例如：

  minimize f*tf*((x-4)^2 + (y-5)^2 + (z-6)^2)
  minimize f*tf*(xp^2 + yp^2 + zp^2)
  minimize f*tf*(roll^2 + pitch^2 + yaw^2)

这样，如果优化器不能达到终端约束discussed in the pendulum problem，它就不会报告不可行的解决方案。我对您的模型和脚本进行了其他一些修改，以实现一个成功的解决方案。以下是摘要：

将终端约束转换为目标函数(soft constraints)

• Parameters t1-t4应该是通过生成w1-w4变量和w1p-w4p变量的自由度问题)。states.

• Added - converge

• Added是在-10和10之间对w1-w4p的差分states.

• Added约束，以帮助求解器converge

• Added初始化步骤在优化之前模拟模型。本文中有更多关于初始化策略的详细信息: Safdarnejad，S.M.，Hedengren，J.D.，Lewis，N.R.，Haseltine，E.，Initialization Strategies for Optimization of Dynamic Systems，Computers and Chemical，2015，Vol.78，pp.39-50，DOI: 10.1016/j.compchemeng.2015.04.016

模型

Parameters
tf

 w1p = 0 > -10 < 10
 w2p = 0 > -10 < 10
 w3p = 0 > -10 < 10
 w4p = 0 > -10 < 10


Constants
!----------------COEFICIENTES DEL MODELO-----------------!
#Gravedad
g = 9.81 !m/s^2
pi = 3.14159265359

#Motor Coefficients
 J = 4.1904e-5 !kg*m^2
 kt = 0.0104e-3 !N*m/A
 kv = 96.342 !rad/s/volt
 Dv = 0.2e-3 !N*m*s/rad
 R = 0.2 !Ohms

#Battery parameters
 Q = 1.55 !Ah
 Rint = 0.02 !Ohms
 E0 = 1.24 !volt
 K = 2.92e-3 !volt
 A = 0.156
 B =2.35

#Quadrotor parameters
  l = 0.175 !m
  m = 1.3 !kg
  Ix = 0.081 !kg*m^2
  Iy = 0.081 !kg*m^2
  Iz = 0.142 !kg*m^2
  kb = 3.8305e-6 !N/rad/s
  ktau = 2.2518e-8 !(N*m)/rad/s

#Parametrizacion del polinomio
  a1 = -1.72e-5
  a2 = 1.95e-5
  a3 = -6.98e-6
  a4 = 4.09e-7
  b1 = 0.014
  b2 = -0.0157
  b3 = 5.656e-3
  b4 = -3.908e-4
  c1 = -0.8796
  c2 = 0.3385
  c3 = 0.2890
  c4 = 0.1626

Variables
!------------------CONDICONES INICIALES------------------!
 x = 0
 xp = 0
 y = 0
 yp = 0
 z = 0
 zp = 0
 pitch = 0, >=-pi/2, <=pi/2   !theta - restricciones
 pitchp = 0
 roll = 0, >=-pi/2, <=pi/2    !phi - restricciones
 rollp = 0
 yaw = 0                      !psi
 yawp = 0  %, >=-200/180, <=200/180

#Velocidad de rotores rad/s
#Las condiciones iniciales permiten igualar la acción de la gravedad
#Se tomo 4000rad/s como la velocidad maxima de los rotores
w1 = 912.32, >=0, <=3000
w2 = 912.32, >=0, <=3000
w3 = 912.32, >=0, <=3000
w4 = 912.32, >=0, <=3000

 t1 = 0, >=0
 t2 = 0, >=0
 t3 = 0, >=0
 t4 = 0, >=0

#Función objetivo
  of = 0 !condición inicial de la función objetivo
Intermediates

#Motor 1
  aw1 = a1*w1^2 + b1*w1 + c1
  bw1 = a2*w1^2 + b2*w1 + c2
  cw1 = a3*w1^2 + b3*w1 + c3
  dw1 = a4*w1^2 + b4*w1 + c4
#Motor 2
  aw2 = a1*w2^2 + b1*w2 + c1
  bw2 = a2*w2^2 + b2*w2 + c2
  cw2 = a3*w2^2 + b3*w2 + c3
  dw2 = a4*w2^2 + b4*w2 + c4 
#Motor 3
  aw3 = a1*w3^2 + b1*w3 + c1
  bw3 = a2*w3^2 + b2*w3 + c2
  cw3 = a3*w3^2 + b3*w3 + c3
  dw3 = a4*w3^2 + b4*w3 + c4
#Motor 4
  aw4 = a1*w4^2 + b1*w4 + c1
  bw4 = a2*w4^2 + b2*w4 + c2
  cw4 = a3*w4^2 + b3*w4 + c3
  dw4 = a4*w4^2 + b4*w4 + c4
#frj(wj(t),Tj(t))
  fr1=aw1*t1^3 + bw1*t1^2 + cw1*t1 + dw1
  fr2=aw2*t2^3 + bw2*t2^2 + cw2*t2 + dw2
  fr3=aw3*t3^3 + bw3*t3^2 + cw3*t3 + dw3
  fr4=aw4*t4^3 + bw4*t4^2 + cw4*t4 + dw4
!---------------------CONTROL INPUTS---------------------!
  T = kb * (w1^2 + w2^2 + w3^2 + w4^2)
  u1 = kb * (w2^2 - w4^2)
  u2 = kb * (w3^2 - w1^2)
  u3 = ktau * (w1^2 - w2^2 + w3^2 - w4^2)
  wline = w1 - w2 + w3 - w4
!-------------------ENERGIA POR ROTOR--------------------!
  Ec1 = ((J*$w1 + ktau*w1^2 + Dv*w1)/fr1)*w1
  Ec2 = ((J*$w2 + ktau*w2^2 + Dv*w2)/fr2)*w2
  Ec3 = ((J*$w3 + ktau*w3^2 + Dv*w3)/fr3)*w3
  Ec4 = ((J*$w4 + ktau*w4^2 + Dv*w4)/fr4)*w4
  Ectotal = Ec1 + Ec2 + Ec3 + Ec4

! scaling factor for terminal constraint
  f = 1000

Equations
!---------------MINIMIZAR FUNCIÓN OBJETIVO---------------!
  minimize tf * of
!-----------------RELACION DE VARIABLES------------------!
  xp = $x
  yp = $y
  zp = $z
  pitchp = $pitch
  rollp = $roll
  yawp = $yaw

  w1p = $w1
  w2p = $w2
  w3p = $w3
  w4p = $w4
!-----------------CONDICONES DE FRONTERA-----------------!
#Condiciones finales del modelo
  #tf * (x-4) = 0
  #tf * (y-5) = 0
  #tf * (z-6) = 0
  #tf * xp = 0
  #tf * yp = 0
  #tf * zp = 0
  #tf * roll = 0
  #tf * pitch = 0
  #tf * yaw = 0

  minimize f*tf*((x-4)^2 + (y-5)^2 + (z-6)^2)
  minimize f*tf*(xp^2 + yp^2 + zp^2)
  minimize f*tf*(roll^2 + pitch^2 + yaw^2)
!-----------------TORQUE DE LOS MOTORES------------------!
  t1 = J*w1p + ktau*w1^2 + Dv*w1
  t2 = J*w2p + ktau*w2^2 + Dv*w2
  t3 = J*w3p + ktau*w3^2 + Dv*w3
  t4 = J*w4p + ktau*w4^2 + Dv*w4
!------------------------SUJETO A------------------------!
#Modelo aerodinámico del UAV
  m*$xp = (cos(roll)*sin(pitch)*cos(yaw) + sin(roll)*sin(yaw))*T
  m*$yp = (cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw))*T
  m*$zp = (cos(roll)*cos(pitch))*T-m*g
  Ix*$rollp = ((Iy - Iz)*pitchp*yawp + J*pitchp*wline + l*u1)
  Iy*$pitchp = ((Iz - Ix)*rollp*yawp - J*rollp*wline + l*u2)
  Iz*$yawp = ((Ix - Iy)*rollp*pitchp + u3)
!--------------------FUNCIÓN OBJETIVO--------------------!
  $of = Ectotal

MATLAB脚本

clear all; close all; clc

server = 'http://byu.apmonitor.com';
app = 'traj_optima';

addpath('apm')
apm(server,app,'clear all');
apm_load(server,app,'ecuaciones_mod.apm');
csv_load(server,app,'tiempo2.csv');

apm_option(server,app,'apm.max_iter',1000);
apm_option(server,app,'apm.nodes',3);
apm_option(server,app,'apm.rtol',1e-6);
apm_option(server,app,'apm.otol',1e-6);
apm_option(server,app,'apm.solver',3);
apm_option(server,app,'apm.imode',6);
apm_option(server,app,'apm.mv_type',1);


costo=1e-5;%1e-5
%VARIABLES CONTROLADAS
%Velocidades angulares
apm_info(server,app,'MV','w1p');
apm_option(server,app,'w1p.status',1);
apm_info(server,app,'MV','w2p');
apm_option(server,app,'w2p.status',1);
apm_info(server,app,'MV','w3p'); 
apm_option(server,app,'w3p.status',1);
apm_info(server,app,'MV','w4p');
apm_option(server,app,'w4p.status',1);

%Salida
disp('')
disp('------------- Initialize ----------------')
apm_option(server,app,'apm.coldstart',1);
output = apm(server,app,'solve');
disp(output)

disp('')
disp('-------------- Optimize -----------------')
apm_option(server,app,'apm.time_shift',0);
apm_option(server,app,'apm.coldstart',0);
output = apm(server,app,'solve');
disp(output)
y = apm_sol(server,app); 
z = y.x;

这给出了一个成功的解决方案，但不满足终端约束。求解器优化了w1p-w4p的使用，以最小化目标，但没有满足终端约束的解决方案。

 The solution was found.

 The final value of the objective function is    50477.4537378181     

 ---------------------------------------------------
 Solver         :  IPOPT (v3.12)
 Solution time  :    3.06940000000759      sec
 Objective      :    50477.4537378181     
 Successful solution
 ---------------------------------------------------

下一步，我建议您增加时间点或allow the final time to change的数量，以满足终端限制。您可能还想考虑切换到使用与APM MATLAB相同的底层引擎的Python Gekko。在这种情况下，建模语言与Python完全集成。