optimizer.cpp - master - Logrus/CopterControl

optimizer.cpp
301 строка11 KB
Nikolay Nosorev
Initial commit
04 окт 2025, 16:17
04 окт 2025, 16:1799ef28c
#include "pch.h"
#include "optimizer.h"
#include "config.h"
#include <nlopt.hpp>
#include <iostream>
#include <cmath>
#include <iomanip>
#define MAXEVAL 400
// Определение границ параметров
const std::vector<double> PIDOptimizer::bounds_min = {
    0.1,      // Kp_pos_xy, Kd_pos_xy
    0.1, 0.01, // Kp_pos_z, Ki_pos_z, Kd_pos_z
    0.1, 0.1,     // Kp_vel_xy, Kd_vel_xy  
    0.1, 0.1,     // Kp_vel_z, Kd_vel_z
    2.0, 2.0, 2.0, // kp_phi, kp_theta, kp_psi
    0.5, 0.5, 0.5  // kd_phi, kd_theta, kd_psi
};

const std::vector<double> PIDOptimizer::bounds_max = {
    5.0,      // Kp_pos_xy, Kd_pos_xy
    5.0, 1.0,  // Kp_pos_z, Ki_pos_z, Kd_pos_z
    5.0, 3.0,     // Kp_vel_xy, Kd_vel_xy  
    5.0, 3.0,     // Kp_vel_z, Kd_vel_z
    15.0, 15.0, 15.0, // kp_phi, kp_theta, kp_psi
    5.0, 5.0, 5.0     // kd_phi, kd_theta, kd_psi
};

PIDOptimizer::PIDOptimizer(MinimumSnapTrajectory& trajectory, double dt) 
    : trajectory(trajectory), dt(dt) {}

double PIDOptimizer::objective(const std::vector<double>& params_vec) {
	 if (params_vec.size() != 13) {
        std::cerr << "❌ params size = " << params_vec.size() << std::endl;
        return 1e10;
    }
    double t_final = trajectory.get_t_points().back();
    //std::cout << "DEBUG: t_final = " << t_final << std::endl; // ← ДОБАВЬТЕ ЭТО
    if (t_final <= 0) {
        std::cerr << "❌ Invalid trajectory duration!" << std::endl;
        return 1e10;
    }
    try {
        Params params = Params::fromVector(params_vec);
        Controller controller(params, dt);
        Quadcopter quad;

        double total_error = 0.0;
        double t_final = trajectory.get_t_points().back();
        std::vector<double> t_list;
        for (double t = 0; t <= t_final + dt; t += dt) {
            t_list.push_back(t);
        }

        // Для отслеживания движения
        Eigen::Vector3d initial_pos = quad.pos;
        double max_movement = 0.0;
        std::vector<Eigen::Vector3d> positions_actual;
        std::vector<Eigen::Vector3d> positions_desired;
			double F_total; Eigen::Vector3d M;
        for (double t : t_list) {
            auto [pos_des, vel_des, acc_des, psi_des] = trajectory.evaluate(t);
            
            // Вычисление команд управления
				controller.compute_control(
                quad.pos, quad.vel, quad.angles, quad.ang_vel,
                pos_des, vel_des, acc_des, psi_des,
					 F_total,M
            );

            // Проверка стабильности
            if (quad.pos.array().isNaN().any() || quad.pos.array().isInf().any() ||
                std::isnan(F_total) || std::isinf(F_total) ||
                F_total > 2 * cfg.F_MAX || F_total < -cfg.F_MAX) {
                std::cout << "⚠️ Unstable on t=" << t << std::endl;
                return 1e10;
            }

            quad.update_state(F_total, M, dt);

            // Отслеживание движения
            double movement = (quad.pos - initial_pos).norm();
            max_movement = std::max(max_movement, movement);

            positions_actual.push_back(quad.pos);
            positions_desired.push_back(pos_des);

            double pos_error = (pos_des - quad.pos).norm();
            total_error += pos_error * pos_error;
        }

        double mse = total_error / t_list.size();

        // Штраф за отсутствие движения
        if (max_movement < 0.1) {
            std::cout << "⚠️ Not moving! movement=" << max_movement << std::endl;
            return 1e10 + (0.1 - max_movement) * 1e6;
        }

        // Штраф за слишком большое отклонение
        if (max_movement > 50.0) {
            return 1e10;
        }

        if (std::isnan(mse) || std::isinf(mse)) {
            return 1e10;
        }

        // Вывод информации о параметрах
        /*std::cout << "Params: ";
        for (double p : params_vec) {
            std::cout << std::fixed << std::setprecision(2) << p << " ";
        }
        std::cout << "| MSE: " << std::setprecision(6) << mse 
                  << " | Movement: " << std::setprecision(2) << max_movement << "m" << std::endl;
			*/
        return mse;

    } catch (const std::exception& e) {
        std::cout << "❌ Error: " << e.what() << std::endl;
        return 1e10;
    }
}


Params PIDOptimizer::optimize_LBFGSB(const Params& initial_params) {
    std::vector<double> x = initial_params.toVector();
    
    // LD_LBFGS - градиентный метод, аналогичный L-BFGS-B
    // Но NLopt не имеет точного аналога L-BFGS-B с границами, поэтому используем LD_SLSQP
    // который хорошо работает с границами и похож на L-BFGS
    nlopt::opt opt(nlopt::LD_SLSQP, 13);
    
    // Установка границ
    opt.set_lower_bounds(bounds_min);
    opt.set_upper_bounds(bounds_max);
    
    // Установка целевой функции
    opt.set_min_objective(objective_wrapper, this);
    
    // Параметры, аналогичные Python версии
    opt.set_ftol_rel(1e-8);  // ftol
    //opt.set_ftol_abs(1e-6);
    opt.set_xtol_rel(5e-9);  // gtol
    opt.set_maxeval(MAXEVAL);     // maxiter
    
    try {
        double minf;
        nlopt::result result = opt.optimize(x, minf);
        
        std::cout << "OPtimization result: ";
        switch(result) {
            case nlopt::SUCCESS: std::cout << "SUCCESS"; break;
            case nlopt::FTOL_REACHED: std::cout << "FTOL_REACHED"; break;
            case nlopt::XTOL_REACHED: std::cout << "XTOL_REACHED"; break;
            case nlopt::MAXEVAL_REACHED: std::cout << "MAXEVAL_REACHED"; break;
            default: std::cout << "Code " << result;
        }
        std::cout << std::endl;
        std::cout << "Minimum value of MSE: "<<std::setprecision(6)<< minf << std::endl;
        
        return Params::fromVector(x);
        
    } catch (std::exception& e) {
        std::cout << "Optimization Error: " << e.what() << std::endl;
        return initial_params;
    }
}

// Альтернативная реализация с использованием LD_LBFGS (но без поддержки границ)
// Params PIDOptimizer::optimize_LBFGS(const Params& initial_params) {
    // std::vector<double> x = initial_params.toVector();
    
    // nlopt::opt opt(nlopt::LD_LBFGS, 13);
    
    // // LD_LBFGS не поддерживает явные границы, но мы можем добавить ограничения
    // opt.set_min_objective(objective_wrapper, this);
    
    // // Добавляем ограничения для имитации границ
    // for (size_t i = 0; i < bounds_min.size(); i++) {
        // opt.add_inequality_constraint(
            // [i, this](const std::vector<double>& x, std::vector<double>& grad) {
                // return x[i] - this->bounds_min[i];  // x[i] >= bounds_min[i]
            // }, 
            // nullptr, 1e-8
        // );
        // opt.add_inequality_constraint(
            // [i, this](const std::vector<double>& x, std::vector<double>& grad) {
                // return this->bounds_max[i] - x[i];  // x[i] <= bounds_max[i]
            // }, 
            // nullptr, 1e-8
        // );
    // }
    
    // opt.set_ftol_rel(1e-8);
    // opt.set_xtol_rel(5e-9);
    // opt.set_maxeval(MAXEVAL);
    
    // try {
        // double minf;
        // nlopt::result result = opt.optimize(x, minf);
        // std::cout << "LBFGS optimization finished, MSE: "<<std::setprecision(6)<< minf << std::endl;
        // return Params::fromVector(x);
    // } catch (std::exception& e) {
        // std::cout << "Error on LBFGS optimization: " << e.what() << std::endl;
        // return initial_params;
    // }
// }
// Статическая функция-ограничение для нижней границы
static double lower_bound_constraint(const std::vector<double>& x, std::vector<double>& grad, void* data) {
    // data указывает на структуру с индексом и указателем на объект
    struct ConstraintData {
        size_t idx;
        PIDOptimizer* obj;
    };
    auto* cdata = static_cast<ConstraintData*>(data);
    size_t i = cdata->idx;
    PIDOptimizer* self = cdata->obj;

    if (!grad.empty()) {
        std::fill(grad.begin(), grad.end(), 0.0);
        grad[i] = 1.0; // градиент: d/dx_i (x_i - min) = 1
    }
    return x[i] - self->bounds_min[i];
}

// Аналогично для верхней границы
static double upper_bound_constraint(const std::vector<double>& x, std::vector<double>& grad, void* data) {
    struct ConstraintData {
        size_t idx;
        PIDOptimizer* obj;
    };
    auto* cdata = static_cast<ConstraintData*>(data);
    size_t i = cdata->idx;
    PIDOptimizer* self = cdata->obj;

    if (!grad.empty()) {
        std::fill(grad.begin(), grad.end(), 0.0);
        grad[i] = -1.0; // градиент: d/dx_i (max - x_i) = -1
    }
    return self->bounds_max[i] - x[i];
}
Params PIDOptimizer::optimize_LBFGS(const Params& initial_params) {
    std::vector<double> x = initial_params.toVector();
    nlopt::opt opt(nlopt::LD_LBFGS, 13);

    opt.set_min_objective(objective_wrapper, this);

    // Создаём временные данные для каждого ограничения
    struct ConstraintData {
        size_t idx;
        PIDOptimizer* obj;
    };

    std::vector<ConstraintData> lower_data(bounds_min.size());
    std::vector<ConstraintData> upper_data(bounds_min.size());
	std::cout<<"Adding constraints\n";
    for (size_t i = 0; i < bounds_min.size(); ++i) {
        lower_data[i] = {i, this};
        upper_data[i] = {i, this};

        opt.add_inequality_constraint(
            lower_bound_constraint,
            &lower_data[i],
            1e-8
        );

        opt.add_inequality_constraint(
            upper_bound_constraint,
            &upper_data[i],
            1e-8
        );
    }

    // ВАЖНО: lower_data и upper_data должны жить до вызова opt.optimize()
    double minf;
    nlopt::result result = opt.optimize(x, minf);

    return Params::fromVector(x);
}
// Версия с BOBYQA (безградиентный метод) - оставлена как альтернатива
Params PIDOptimizer::optimize_BOBYQA(const Params& initial_params) {
    std::vector<double> x = initial_params.toVector();
    
    nlopt::opt opt(nlopt::LN_BOBYQA, 13);
    opt.set_lower_bounds(bounds_min);
    opt.set_upper_bounds(bounds_max);
    opt.set_min_objective(objective_wrapper, this);
    opt.set_ftol_rel(1e-8);
    opt.set_xtol_rel(5e-9);
    opt.set_maxeval(MAXEVAL);
    
    try {
        double minf;
        nlopt::result result = opt.optimize(x, minf);
        std::cout << "BOBYQA optimization finished, MSE: "<<std::setprecision(6) << minf << std::endl;
        return Params::fromVector(x);
    } catch (std::exception& e) {
        std::cout << "Errrof of BOBYQA optimize: " << e.what() << std::endl;
        return initial_params;
    }
}