时间戳

首页 > 解决方案 > MATLAB 中的局部不可行性

gekko - MATLAB 中的局部不可行性

问题描述

我需要一个动态优化问题的帮助，该问题包括具有此最优控制问题的无人机的消耗能量优化。

在此处输入图像描述

在此处输入图像描述

我的代码是这个

教育：

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

最后得到的答案是：

在此处输入图像描述

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

标签： gekko

解决方案

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

  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)

这样，如果优化器无法达到摆问题中讨论的终端约束，优化器将不会报告不可行的解决方案。我对您的模型和脚本进行了一些其他修改，以实现成功的解决方案。这是一个摘要：

  • 将终端约束转换为目标函数（软约束）
  • 参数t1-t4应该是变量
  • w1通过制作-w4变量和w1p-变量来修复自由度问题w4p。w1-w4是差分状态。
  • w1p在-10和 10 之间添加了约束以w4p帮助求解器收敛
  • 添加了初始化步骤以在优化之前模拟模型。本文中有关初始化策略的更多详细信息：Safdarnejad, SM, Hedengren, JD, Lewis, NR, Haseltine, E., Initialization Strategies for Optimization of Dynamic Systems , Computers and Chemical Engineering, 2015, Vol. 78，第 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
 ---------------------------------------------------

作为下一步，我建议您增加时间点的数量或允许最终时间更改以满足终端约束。您可能还想考虑切换到使用与 APM MATLAB 相同的底层引擎的Python Gekko 。在这种情况下，建模语言与 Python 完全集成。

推荐阅读