Configuración del Controlador Principal

El proceso de inicialización del controlador getsiona la creación de los componentes clave para el control predictivo por modelo (MPC). Aquí se definen la dinámica del vehículo, la estructura de la malla temporal y el motor de optimización.

void ControladorMPC::inicializarComponentes() {
  // Definición del modelo cinemático (ej. Uniciclo)
  modelo_dinamico = std::make_shared<ModeloUniciclo>();

  // Configuración de la malla de discretización (Multiple Shooting)
  // Almacena estados, entradas y puntos de referencia
  malla_temporal = configurarMalla(nodo_nh);  

  // Inicialización del motor de resolución numérica
  optimizador = configurarSolver(nodo_nh);

  // Construcción del Problema de Control Óptimo (OCP)
  // Integra la dinámica, la malla y el optimizador en un grafo estructurado
  problema_estructurado = configurarOCP(nodo_nh, lista_obstaculos, modelo_robot, waypoints);
}

Definición de la Malla de Discretización

La malla permite transformar el problema continuo en uno discreto. Se pueden utilizar mallas fijas o variables según la necesidad de adaptación temporal.

corbo::DiscretizationGridInterface::Ptr ControladorMPC::configurarMalla(const rclcpp::Node::SharedPtr& nh) {
  FiniteDifferencesGridSE2::Ptr malla_se2;
  
  if (usar_malla_variable) {
    malla_se2 = std::make_shared<FiniteDifferencesVariableGridSE2>();
  } else {
    malla_se2 = std::make_shared<FiniteDifferencesGridSE2>();
  }

  // Selección del método de colocación (Diferencias hacia adelante)
  malla_se2->setFiniteDifferencesCollocationMethod(std::make_shared<ForwardDiffCollocationSE2>());

  // Regla de integración para el costo (Suma por la izquierda)
  if (metodo_integracion == "left_sum") {
    malla_se2->setCostIntegrationRule(FullDiscretizationGridBaseSE2::CostIntegrationRule::LeftSum);
  }
  return malla_se2;
}

Integración con el Optimizador IPOPT

El solver utiliza la librería IPOPT para resolver problemas de programación no lineal (NLP). Se configuran parámetros críticos como tolernacias y límites de tiempo de cómputo.

void ControladorMPC::configurarSolver() {
  // SolverIpopt encapsula IpoptWrapper (interfaz NLP) e IpoptApplication
  auto solver_ipopt = std::make_shared<corbo::SolverIpopt>();
  solver_ipopt->initialize();

  // Parámetros de ejecución
  solver_ipopt->setIterations(max_iteraciones); 
  solver_ipopt->setMaxCpuTime(tiempo_limite_cpu);

  // Configuración interna de IPOPT: estrategia de barrera y tolerancias
  // Equivale a manipular Options() en la API nativa de Ipopt
  solver_ipopt->setIpoptOptionNumeric("tol", 1e-6);
  solver_ipopt->setIpoptOptionString("mu_strategy", "adaptive");
}

Formulación del Problema de Control Óptimo (OCP)

En esta etapa se definen las funciones de costo cuadráticas y las restricciones físicas que el robot debe respetar durante la planificación.

void ControladorMPC::configurarOCP() {
  // Creación de un hipergrafo para representar el problema de optimización
  auto grafo_base = std::make_shared<corbo::HyperGraphOptimizationProblemEdgeBased>();

  // Estructura del OCP vinculando la malla y el solver
  auto ocp_instancia = std::make_shared<corbo::StructuredOptimalControlProblem>(malla_temporal, modelo_dinamico, grafo_base, optimizador);

  // Límites de las variables de control (velocidad lineal y angular)
  ocp_instancia->setControlBounds(Eigen::Vector2d(min_v, min_w), Eigen::Vector2d(max_v, max_w));

  // Función de costo de etapa (Stage Cost) y costo terminal
  ocp_instancia->setStageCost(std::make_shared<QuadraticFormCostSE2>(matriz_Q, matriz_R, forma_integral, solver_lsq));
  ocp_instancia->setFinalStageCost(std::make_shared<QuadraticFinalStateCostSE2>(matriz_Qf, solver_lsq));

  // Restricciones de desigualdad: Obstáculos y Footprint
  auto restricciones = std::make_shared<StageInequalitySE2>();
  restricciones->setObstacleVector(lista_obstaculos);
  restricciones->setRobotFootprintModel(perfil_robot);
  restricciones->setMinimumDistance(distancia_seguridad);
  
  // Límites de aceleración
  restricciones->setControlDeviationBounds(Eigen::Vector2d(dec_max, acc_w_max), Eigen::Vector2d(acc_max, acc_w_max));

  ocp_instancia->setStageInequalityConstraint(restricciones);
}

Ciclo de Ejecución (Step)

La función step se encarga de recibir el estado actual y generar la trayectoria óptima. Si el objetivo cambia significativamente, la malla se reinicia para evitar inconsistencias.

bool ControladorMPC::ejecutarPaso(const std::vector<PoseStamped>& plan_global, const Twist& vel_actual, double dt) {
  // Conversión de objetivos y estados a formatos vectoriales
  modelo_dinamico->extraerEstado(objetivo_final, x_final);
  modelo_dinamico->extraerEstado(pose_actual, x_actual);

  // Reinicio de la malla si el cambio de objetivo es brusco
  if (verificarCambioObjetivo(x_final)) {
    malla_temporal->clear();
  }

  // Generación de trayectoria inicial si la malla está vacía (interpolación lineal)
  if (malla_temporal->isEmpty()) {
    generarTrayectoriaReferencia(x_actual, x_final, plan_global);
  }

  // Llamada al motor de cómputo predictivo
  return ocp_instancia->compute(x_actual, referencia_estatica, entrada_cero, dt, tiempo_actual, seq_u, seq_x);
}

Interfaz del Optimizador y Jacobianos

El IpoptWrapper actúa como puente, mapeando los vectores de parámetros del problema de optimización a las funciones de evaluación que requiere el solver para calcular el gradiente y la curvatura.

// Evaluación de la función objetivo
void IpoptWrapper::eval_f(int n, const double* x, bool new_x, double& obj_value) {
  if (new_x) {
    Eigen::Map<const Eigen::VectorXd> vector_x(x, n);
    problema_base->setParameterVector(vector_x); // Actualiza vértices en la malla
  }
  obj_value = problema_base->computeValueObjective();
}

// Evaluación de restricciones
void IpoptWrapper::eval_g(int n, const double* x, bool new_x, int m, double* g) {
  if (new_x) {
    Eigen::Map<const Eigen::VectorXd> vector_x(x, n);
    problema_base->setParameterVector(vector_x);
  }
  Eigen::Map<Eigen::VectorXd> vector_g(g, m);
  // Separa restricciones de igualdad y desigualdad
  problema_base->computeValuesEquality(vector_g.head(dim_igualdad));
  problema_base->computeValuesInequality(vector_g.tail(dim_desigualdad));
}