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 = 20;                  //优化考虑后面多少步

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;
size_t nwmg=nobs+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 v=m_statu[0];
      double delta=m_statu[1];
      double obs_x=m_statu[2];
      double obs_y=m_statu[3];

      double obs_distance=sqrt(obs_x*obs_x+obs_y*obs_y);

      // Reference State Cost
      // Below defines the cost related the reference state and
      // any anything you think may be beneficial.

      // Weights for how "important" each cost is - can be tuned
      const double y_cost_weight = 5000;
      const double th_cost_weight = 4000;
      const double v_cost_weight = 1000;
      const double vth_cost_weight = 1000;
      const double a_cost_weight = 1;
      const double ath_cost_weight=100;
      const double obs_distance_weight=1000.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);

        //朝向loss
        //CppAD::AD<double> fth = CppAD::atan(m_coeffs[1] + 2*m_coeffs[2]*xt + 3*m_coeffs[3]*pow(xt,2));
        //fg[0] += th_cost_weight * CppAD::pow(vars[nth + t]-fth, 2);
        /*CppAD::AD<double> gy = CppAD::atan(m_coeffs[1] + 2*m_coeffs[2]*xt + 3*m_coeffs[3]*pow(xt,2));
        CppAD::AD<double> cx=CppAD::cos(vars[nth + t]);
        CppAD::AD<double> cy=CppAD::sin(vars[nth + t]);
        CppAD::AD<double> dot=(cx+cy*gy)/CppAD::sqrt(gy*gy+1);
        fg[0] += th_cost_weight * CppAD::pow(CppAD::acos(dot),2);*/

        //目标速度loss
        fg[0]+=v_cost_weight*CppAD::pow(vars[nv+t]-ref_v,2);

        //loss 加上车位姿与障碍物的距离
        /*if(obs_distance<4.0)
          fg[0]+=obs_distance_weight*(16.0-(CppAD::pow(vars[nx+t]-obs_x,2)));*/

      }
      // Costs for steering (delta) and acceleration (a)

      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];

        //CppAD::AD<double> w_0=vars[nv+t-1]/wheelbase*CppAD::tan(vars[ndlt+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;
      }
      //与障碍物的距离
      for(int t=0;t<N;++t)
      {
        fg[1+nobs+t]=CppAD::pow(vars[nx+t]-obs_x,2)+CppAD::pow(vars[ny+t]-obs_y,2);
      }
    //转弯半径
      /*for(int t=0;t<N;++t)
      {
        fg[1+nwmg+t]=CppAD::pow(vars[ndlt+t],2)/(CppAD::pow(vars[nv+t],2)+1e-10);
      }*/

    }
};

LoadedMPC::LoadedMPC(const Pose2d& obs,double min_velocity,double max_velocity){
  min_velocity_=min_velocity;
  max_velocity_=max_velocity;
  obs_relative_pose_=obs;
}
LoadedMPC::~LoadedMPC(){}

bool 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 false;
  }
  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);

  //优化
  bool ok = true;
  typedef CPPAD_TESTVECTOR(double) Dvector;

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

  double ref_velocity = Pose2d::distance(pose_agv, target) / 2.5;
  if(ref_velocity<0.05)
    ref_velocity=0.05;
  //目标点与起点的连线 朝向与启动朝向 > 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
  size_t n_constraints = N * (5+1);
  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;
  }
  // 与障碍物保持距离
  double dobs=1.6;
  for(int i=nobs;i<nobs+N;++i)
  {
    constraints_lowerbound[i] = dobs*dobs;//pow(float(nobs+N-i)/float(N)*dobs,2);
    constraints_upperbound[i] = 1e19;
  }
  //限制最小转弯半径，
  /*for(int i=nwmg;i<nwmg+N;++i)
  {
    constraints_lowerbound[i] = 0;
    constraints_upperbound[i] = 1.0/(radius*radius);
  }*/


  Eigen::VectorXd statu_velocity(4);
  if(line_velocity>max_velocity_)
  {
    line_velocity=max_velocity_;
    //printf(" input +v limited\n");
  }
  if(line_velocity<-max_velocity_)
  {
    line_velocity=-max_velocity_;
    //printf(" input -v limited\n");
  }

  if(angular>max_dlt)
  {
    angular = max_dlt;
    printf(" input +dlt limited\n");
  }
  if(angular<-max_dlt)
  {
    angular = -max_dlt;
    printf(" input -dlt limited\n");
  }

  statu_velocity << line_velocity, angular,obs_relative_pose_.x(),obs_relative_pose_.y();

  Eigen::VectorXd condition(2);
  condition << dt, ref_velocity;
  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;

  ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
  if (ok == false)
  {
    printf(" mpc failed statu : %d  input: %.4f  %.5f(%.5f) relative:(%f,%f) \n",solution.status,line_velocity,
           wmg,angular,obs_relative_pose_.x(),obs_relative_pose_.y());
    return false;
  }

  // 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 relative:(%f,%f) time:%.3f\n",
         line_velocity,wmg,angular,solve_velocity,solve_angular,correct_angular,
         ref_velocity,targetPoseInAGV.x(),targetPoseInAGV.y(),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 true;
}

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() < point_num)
    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;
}