AGV/MPC/loaded_mpc.cpp

//
// Created by zx on 22-12-1.
//

#include "loaded_mpc.h"
#include <chrono>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>

size_t N = 10;                  //优化考虑后面多少步

size_t nx = 0;
size_t ny = nx + N;
size_t nth = ny + N;
size_t nv = nth + N;
size_t ndlt = nv + N;

size_t nobs = ndlt + N;

class FG_eval_half_agv {
public:
    // Fitted polynomial coefficients
    Eigen::VectorXd m_coeffs;                 //曲线方程
    Eigen::VectorXd m_statu;                  //当前状态
    Eigen::VectorXd m_condition;              //搜索条件参数
    FG_eval_half_agv(Eigen::VectorXd coeffs, Eigen::VectorXd statu, Eigen::VectorXd condition) {
        m_coeffs = coeffs;
        m_statu = statu;
        m_condition = condition;
    }

    typedef CPPAD_TESTVECTOR(CppAD::AD<double>) ADvector;

    void operator()(ADvector &fg, const ADvector &vars) {

        fg[0] = 0;
        double dt = m_condition[0];
        double ref_v = m_condition[1];
        double obs_w = m_condition[2];
        double obs_h = m_condition[3];
        double target_x = m_condition[4];
        double target_y = m_condition[5];

        double v = m_statu[0];
        double delta = m_statu[1];

        // Weights for how "important" each cost is - can be tuned
        const double y_cost_weight = 1000;
        const double th_cost_weight = 4000;
        const double v_cost_weight = 5000;
        const double vth_cost_weight = 1000;
        const double a_cost_weight = 1;
        const double ath_cost_weight = 10;
        const double obs_distance_weight = 5000.0;

        // Cost for CTE, psi error and velocity
        for (int t = 0; t < N; t++) {
            CppAD::AD<double> xt = vars[nx + t];
            CppAD::AD<double> fx = m_coeffs[0] + m_coeffs[1] * xt + m_coeffs[2] * pow(xt, 2) + m_coeffs[3] * pow(xt, 3);
            fg[0] += y_cost_weight * CppAD::pow(vars[ny + t] - fx, 2);
            //fg[0]+=v_cost_weight*(CppAD::pow(vars[nx+t]-target_x,2)+CppAD::pow(vars[ny+t]-target_y,2));
            //目标速度loss
            fg[0] += v_cost_weight * CppAD::pow(vars[nv + t] - ref_v, 2);


        }

        for (int t = 0; t < N - 1; t++) {
            //速度，加速度，前轮角 weight loss
            fg[0] += vth_cost_weight * CppAD::pow(vars[ndlt + t], 2);
            fg[0] += a_cost_weight * CppAD::pow(vars[nv + t + 1] - vars[nv + t], 2);
            fg[0] += ath_cost_weight * CppAD::pow(vars[ndlt + t + 1] - vars[ndlt + t], 2);
        }

        /////////////////////
        fg[1 + nx] = vars[nx] - vars[nv] * dt;
        fg[1 + ny] = vars[ny];
        //CppAD::AD<double> w0=vars[nv]/wheelbase*CppAD::tan(vars[ndlt]);
        fg[1 + nth] = vars[nth] - vars[ndlt] * dt;

        //位姿约束
        for (int t = 1; t < N; t++) {
            // State at time t + 1
            CppAD::AD<double> x1 = vars[nx + t];
            CppAD::AD<double> y1 = vars[ny + t];
            CppAD::AD<double> th1 = vars[nth + t];

            // State at time t
            CppAD::AD<double> x0 = vars[nx + t - 1];
            CppAD::AD<double> y0 = vars[ny + t - 1];
            CppAD::AD<double> th0 = vars[nth + t - 1];
            CppAD::AD<double> v0 = vars[nv + t - 1];

            // Setting up the rest of the model constraints
            fg[1 + nx + t] = x1 - (x0 + v0 * CppAD::cos(th0) * dt);
            fg[1 + ny + t] = y1 - (y0 + v0 * CppAD::sin(th0) * dt);
            fg[1 + nth + t] = th1 - (th0 + vars[ndlt + t - 1] * dt);

        }
        //加速度和dlt约束
        fg[1 + nv] = (vars[nv] - v) / dt;
        fg[1 + ndlt] = (vars[ndlt] - delta) / dt;
        for (int t = 1; t < N; ++t) {
            fg[1 + nv + t] = (vars[nv + t] - vars[nv + t - 1]) / dt;
            fg[1 + ndlt + t] = (vars[ndlt + t] - vars[ndlt + t - 1]) / dt;
        }

        if (m_statu.size() == 2 + 3) {
            float obs_x = m_statu[2];
            float obs_y = m_statu[3];
            float obs_theta = m_statu[4];
            Pose2d obs_relative_pose(obs_x, obs_y, obs_theta);
            std::vector<Pose2d> obs_vertexes = Pose2d::generate_rectangle_vertexs(obs_relative_pose, obs_w * 2,
                                                                                  obs_h * 2);
            for (auto t = 0; t < N; ++t) {
                CppAD::AD<double> min_distance = 1e19;
                for (auto i = 0; i < 8; ++i) {
                    CppAD::AD<double> ra = (obs_vertexes[i].x() - vars[nx + t]) * CppAD::cos(vars[nth + t]) -
                                           (obs_vertexes[i].y() - vars[ny + t]) * CppAD::sin(vars[nth + t]);
                    CppAD::AD<double> rb = (obs_vertexes[i].x() - vars[nx + t]) * CppAD::sin(vars[nth + t]) +
                                           (obs_vertexes[i].y() - vars[ny + t]) * CppAD::cos(vars[nth + t]);
                    CppAD::AD<double> distance = CppAD::pow(ra / obs_w, 8) + CppAD::pow(rb / obs_h, 8);
                    if (distance < min_distance) min_distance = distance;
                }
                fg[nobs + t] = min_distance;
            }
//            std::cout << " fg[n_obs]: " << fg[n_obs] << " | __LINE__:" << __LINE__ << std::endl;
        }
//        if (m_statu.size() == 2 + 3) {
//            //与障碍物的距离
//            for (int i = 0; i < 8; ++i) {
//                double ox = m_statu[2 + i * 2];
//                double oy = m_statu[2 + i * 2 + 1];
//                for (int t = 0; t < N; ++t) {
//                    /*第i点的矩形方程为:   {[(x-xi)cos(Θi)-(y-yi)sin(Θi)]/1.35}**8  +
//                     *                  {[(x-xi)sin(Θi)+(y-yi)cos(Θi)]/0.75}**8  = 1.0
//                     *
//                     * */
//                    CppAD::AD<double> ra = (ox - vars[nx + t]) * CppAD::cos(vars[nth + t]) -
//                                           (oy - vars[ny + t]) * CppAD::sin(vars[nth + t]);
//                    CppAD::AD<double> rb = (ox - vars[nx + t]) * CppAD::sin(vars[nth + t]) +
//                                           (oy - vars[ny + t]) * CppAD::cos(vars[nth + t]);
//                    fg[1 + nobs + N * i + t] = CppAD::pow(ra / obs_w, 8) + CppAD::pow(rb / obs_h, 8);
//                }
//            }
//        }
    }
};

LoadedMPC::LoadedMPC(const Pose2d &obs, double obs_w, double obs_h, double min_velocity, double max_velocity) {
    min_velocity_ = min_velocity;
    max_velocity_ = max_velocity;
    obs_relative_pose_ = obs;
    obs_w_ = obs_w;
    obs_h_ = obs_h;
}

LoadedMPC::~LoadedMPC() {}

MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd statu, MPC_parameter mpc_param,
                          std::vector<double> &out, Trajectory &select_traj, Trajectory &optimize_trajectory) {
    auto start = std::chrono::steady_clock::now();
    // State vector holds all current values neede for vars below
    Pose2d pose_agv = Pose2d(statu[0], statu[1], statu[2]);
    double line_velocity = statu[3];
    double wmg = statu[4];

    //纠正角速度/线速度，使其满足最小转弯半径
    double angular = wmg;
    double radius = mpc_param.shortest_radius * (1.0 / sqrt(line_velocity * line_velocity + 1e-10));
    if ((line_velocity * line_velocity) / (wmg * wmg + 1e-9) < (radius * radius)) {
        angular = fabs(line_velocity) / radius;
        if (wmg < 0) angular = -angular;
    }
    double max_wmg = 0.5 / radius;//fabs(line_velocity) / radius;

    std::vector<Pose2d> filte_poses;
    if (filte_Path(pose_agv, target, trajectory, filte_poses, 10) == false) {
        printf("filte path failed ...\n");
        return failed;
    }
    select_traj = Trajectory(filte_poses);

    //将选中点移动到小车坐标系
    std::vector<Pose2d> transform_poses;
    for (int i = 0; i < filte_poses.size(); i++) {
        double x = filte_poses[i].x() - pose_agv.x();
        double y = filte_poses[i].y() - pose_agv.y();
        transform_poses.push_back(Pose2d(x, y, 0).rotate(-pose_agv.theta()));
    }

    Eigen::VectorXd coef = fit_path(transform_poses);

    //优化
    typedef CPPAD_TESTVECTOR(double) Dvector;

    //根据当前点和目标点距离,计算目标速度

    double ref_velocity = Pose2d::distance(pose_agv, target) / 2.5;
    if (ref_velocity < 0.1)
        ref_velocity = 0.1;
    //目标点与起点的连线 朝向与启动朝向 > M_PI/2.0

    Pose2d targetPoseInAGV = Pose2d::relativePose(target, pose_agv);
    //std::cout<<"target:"<<target<<", agv:"<<pose_agv<<", relative:"<<targetPoseInAGV<<std::endl;
    if (targetPoseInAGV.x() < 0)
        ref_velocity = -ref_velocity;

    double dt = mpc_param.dt;

    //printf("min_v:%f   max_v:%f\n",min_velocity_,max_velocity_);
    double max_dlt = max_wmg;//5*M_PI/180.0;
    double max_acc_line_velocity = mpc_param.acc_velocity;
    double max_acc_dlt = mpc_param.acc_angular * M_PI / 180.0;

    size_t n_vars = N * 5;

    Dvector vars(n_vars);
    for (int i = 0; i < n_vars; i++) {
        vars[i] = 0.0;
    }

    Dvector vars_lowerbound(n_vars);
    Dvector vars_upperbound(n_vars);
    for (int i = 0; i < n_vars; i++) {
        vars_lowerbound[i] = -1.0e19;
        vars_upperbound[i] = 1.0e19;
    }

    //// limit  v
    for (int i = nv; i < nv + N; i++) {
        vars_lowerbound[i] = -max_velocity_;
        vars_upperbound[i] = max_velocity_;
    }

    ////limint dlt
    for (int i = ndlt; i < ndlt + N; i++) {
        vars_lowerbound[i] = -max_dlt;
        vars_upperbound[i] = max_dlt;
    }

    // Lower and upper limits for the constraints
    /*
     * 障碍物是否进入 碰撞检测范围内
     */
    float distance = Pose2d::distance(obs_relative_pose_, Pose2d(0, 0, 0));
    bool find_obs = distance < 20;
    if (find_obs) printf(" find obs ..............................\n");
    size_t n_constraints = 5 * N + find_obs * N;

    Dvector constraints_lowerbound(n_constraints);
    Dvector constraints_upperbound(n_constraints);
    for (int i = 0; i < n_constraints; i++) {
        constraints_lowerbound[i] = 0;
        constraints_upperbound[i] = 0;
    }
    //// acc v
    for (int i = nv; i < nv + N; i++) {
        constraints_lowerbound[i] = -max_acc_line_velocity;
        constraints_upperbound[i] = max_acc_line_velocity;
    }
    //// acc ndlt
    for (int i = ndlt; i < ndlt + N; i++) {
        constraints_lowerbound[i] = -max_acc_dlt;
        constraints_upperbound[i] = max_acc_dlt;
    }

    // 与障碍物保持距离的约束
    if (find_obs) {
        for (int i = nobs; i < nobs + N; ++i) {
            constraints_lowerbound[i] = 1.0;
            constraints_upperbound[i] = 1e19;
        }
    }

    //限制最小转弯半径，
    /*for(int i=nwmg;i<nwmg+N;++i)
    {
      constraints_lowerbound[i] = 0;
      constraints_upperbound[i] = 1.0/(radius*radius);
    }*/

    if (line_velocity > max_velocity_) {
        line_velocity = max_velocity_;
    }
    if (line_velocity < -max_velocity_) {
        line_velocity = -max_velocity_;
    }
    if (angular > max_dlt) {
        angular = max_dlt;
    }
    if (angular < -max_dlt) {
        angular = -max_dlt;
    }

    Eigen::VectorXd statu_velocity(2 + 3*find_obs);
    statu_velocity[0] = line_velocity;
    statu_velocity[1] = angular;
    if (find_obs) {
        statu_velocity[2]= obs_relative_pose_.x();
        statu_velocity[3] = obs_relative_pose_.y();
        statu_velocity[4] = obs_relative_pose_.theta();
    }

    Eigen::VectorXd condition(6);
    condition << dt, ref_velocity, obs_w_, obs_h_, targetPoseInAGV.x(), targetPoseInAGV.y();
    FG_eval_half_agv fg_eval(coef, statu_velocity, condition);

    // options for IPOPT solver
    std::string options;
    // Uncomment this if you'd like more print information
    options += "Integer print_level  0\n";
    options += "Sparse  true        forward\n";
    options += "Sparse  true        reverse\n";
    options += "Numeric max_cpu_time          0.5\n";
    // place to return solution
    CppAD::ipopt::solve_result<Dvector> solution;

    // solve the problem
    CppAD::ipopt::solve<Dvector, FG_eval_half_agv>(
            options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
            constraints_upperbound, fg_eval, solution);

    auto now = std::chrono::steady_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(now - start);
    double time =
            double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;

    // invalid_number_detected
    if (solution.status != CppAD::ipopt::solve_result<Dvector>::success) {
        printf(" mpc failed statu : %d  input: %.4f  %.5f(%.5f) max_v:%f\n", solution.status, line_velocity,
               wmg, angular, max_velocity_);
        if (solution.status == CppAD::ipopt::solve_result<Dvector>::local_infeasibility)
            return no_solution;
        return failed;
    }

    // Cost
    auto cost = solution.obj_value;

    out.clear();

    double solve_velocity = solution.x[nv];
    double solve_angular = solution.x[ndlt];

    //纠正角速度/线速度，使其满足最小转弯半径
    double correct_angular = solve_angular;
    if ((solve_velocity * solve_velocity) / (solve_angular * solve_angular + 1e-9) < (radius * radius)) {
        correct_angular = fabs(line_velocity) / radius;
        if (solve_angular < 0) correct_angular = -correct_angular;
    }
    ///-----
    printf("input:%.4f %.5f(%.5f) output:%.4f %.5f(%.5f) ref:%.3f max_v:%f time:%.3f\n",
           line_velocity, wmg, angular, solve_velocity, solve_angular, correct_angular,
           ref_velocity, max_velocity_, time);

    /*if(solve_velocity>=0 && solve_velocity<min_velocity_)
      solve_velocity=min_velocity_;
    if(solve_velocity<0 && solve_velocity>-min_velocity_)
      solve_velocity=-min_velocity_;*/

    out.push_back(solve_velocity);
    out.push_back(correct_angular);


    //计算预测轨迹
    optimize_trajectory.clear();
    for (int i = 0; i < N; ++i) {
        Pose2d pose(solution.x[nx + i], solution.x[ny + i], solution.x[nth + i]);
        optimize_trajectory.push_point(pose_agv + pose.rotate(pose_agv.theta()));
    }
    return success;
}

bool LoadedMPC::filte_Path(const Pose2d &point, Pose2d target, Trajectory trajectory,
                           std::vector<Pose2d> &poses, int point_num) {
    double gradient = 0;
    if (fabs((target - point).x()) == 0)
        gradient = 200.0;
    else
        gradient = (target - point).y() / (target - point).x();

    double theta = gradient2theta(gradient, target.x() - point.x() >= 0);

    if (trajectory.size() < 2)
        return false;

    poses.clear();
    for (int i = 0; i < trajectory.size(); i++) {
        // 平移加反向旋转到小车坐标系
        Pose2d offset = trajectory[i] - point;
        double x = offset.x();
        double y = offset.y();
        double trans_x = x * cos(-theta) - y * sin(-theta);
        double trans_y = x * sin(-theta) + y * cos(-theta);
        if (trans_x >= 0 && poses.size() < point_num) {
            // 旋转到原坐标系
            float nx = trans_x * cos(theta) - trans_y * sin(theta);
            float ny = trans_x * sin(theta) + trans_y * cos(theta);
            Pose2d pose(nx + point.x(), ny + point.y(), trajectory[i].theta());
            poses.push_back(pose);
        }
    }
    int size = poses.size();
    if (size < point_num) {
        //一个点都没有，则以当前点开始，反向取点
        float dl = -0.1;
        Pose2d last = point;
        if (size >= 1) {
            //从最后点到最后一点均匀选取
            last = poses[size - 1]; //
            dl = 0.1;
        }
        float dx = cos(last.theta()) * dl;
        float dy = sin(last.theta()) * dl;
        for (int i = 1; i < point_num - size + 1; ++i)
            poses.push_back(Pose2d(last.x() + dx * i, last.y() + dy * i, last.theta()));
    }
    return true;
}

Eigen::VectorXd LoadedMPC::fit_path(const std::vector<Pose2d> &trajectory) {
    int order = 3;
    assert(order >= 1 && order <= trajectory.size() - 1);
    Eigen::MatrixXd A(trajectory.size(), order + 1);
    Eigen::VectorXd yvals(trajectory.size());
    for (int i = 0; i < trajectory.size(); i++) {
        A(i, 0) = 1.0;
        yvals[i] = trajectory[i].y();
    }

    for (int j = 0; j < trajectory.size(); j++) {
        for (int i = 0; i < order; i++) {
            A(j, i + 1) = A(j, i) * trajectory[j].x();
        }
    }


    auto Q = A.householderQr();
    auto result = Q.solve(yvals);
    return result;
}