1.修改旋转控制方式为MPC

CMakeLists.txt

@@ -72,7 +72,9 @@ add_executable(${PROJECT_NAME}
 		controller.cpp
 		define/TimerRecord.cpp
 		MPC/parameter.pb.cc
+		MPC/custom_type.h
 		MPC/loaded_mpc.cpp
+		MPC/rotate_mpc.cpp
 		MPC/trajectory.cpp
 		MPC/pose2d.cpp
 		MPC/navigation.cpp

MPC/custom_type.h

@@ -0,0 +1,27 @@
+//
+// Created by gf on 23-7-21.
+// 自定义类型
+//
+
+#ifndef NAVIGATION_CUSTOM_TYPE_H
+#define NAVIGATION_CUSTOM_TYPE_H
+
+//////////////////////////////////////////////////
+/// MPC返回值
+enum MpcError {
+    success = 0,
+    no_solution = 1,
+    failed
+};
+//////////////////////////////////////////////////
+
+//////////////////////////////////////////////////
+/// MPC Rotate 可选方向
+enum MpcRotateDirection{
+    eForward = 0x0001,
+    eBackward= 0x0002,
+    eLeft    = 0x0004,
+    eRight   = 0x0008,
+};
+//////////////////////////////////////////////////
+#endif //NAVIGATION_CUSTOM_TYPE_H

MPC/loaded_mpc.cpp

@@ -15,431 +15,418 @@ size_t nth = ny + N;
 size_t nv = nth + N;
 size_t ndlt = nv + N;
 
-size_t nobs=ndlt+N;
+size_t nobs = ndlt + N;
 
 class FG_eval_half_agv {
- public:
+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;
+    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+16){
-        //与障碍物的距离
-        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);
-          }
+
+    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 + 16) {
+            //与障碍物的距离
+            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(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
-  size_t n_constraints = N * 5;
-  /*
-   * 障碍物是否进入 碰撞检测范围内
-   */
-  float distance=Pose2d::distance(obs_relative_pose_,Pose2d(0,0,0));
-  bool find_obs=false;
-  if(distance<20){
-    //printf(" mpc find obs ,w=%f,h=%f\n",obs_w_,obs_h_);
-    find_obs=true;
-  }
-  if(find_obs) n_constraints = N * (5+8);
-
-  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 + 8*N; ++i) {
-      constraints_lowerbound[i] = 1.0;
-      constraints_upperbound[i] = 1e19;
+
+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;
     }
-  }
-
-  //限制最小转弯半径，
-  /*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);
-  if(find_obs){
-    statu_velocity=Eigen::VectorXd(2+16);
-
-    std::vector<Pose2d> vertexs=Pose2d::generate_rectangle_vertexs(obs_relative_pose_,obs_w_*2,obs_h_*2);
-    for(int i=0;i<vertexs.size();++i){
-      //std::cout<<"vetex:"<<vertexs[i]<<std::endl;
-      statu_velocity[2+i*2]=vertexs[i].x();
-      statu_velocity[2+i*2+1]=vertexs[i].y();
+    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;
     }
-  }
-  statu_velocity[0]= line_velocity;
-  statu_velocity[1]=angular;
-
-  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;
-
-  solution.status == CppAD::ipopt::solve_result<Dvector>::success; // 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;
-}
+    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
+    size_t n_constraints = N * 5;
+    /*
+     * 障碍物是否进入 碰撞检测范围内
+     */
+    float distance = Pose2d::distance(obs_relative_pose_, Pose2d(0, 0, 0));
+    bool find_obs = false;
+    if (distance < 20) {
+        //printf(" mpc find obs ,w=%f,h=%f\n",obs_w_,obs_h_);
+        find_obs = true;
+    }
+    if (find_obs) n_constraints = N * (5 + 8);
 
-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)
+    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 + 8 * N; ++i) {
+            constraints_lowerbound[i] = 1.0;
+            constraints_upperbound[i] = 1e19;
+        }
+    }
+
+    //限制最小转弯半径，
+    /*for(int i=nwmg;i<nwmg+N;++i)
     {
-      // 旋转到原坐标系
-      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);
+      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);
+    if (find_obs) {
+        statu_velocity = Eigen::VectorXd(2 + 16);
+
+        std::vector<Pose2d> vertexs = Pose2d::generate_rectangle_vertexs(obs_relative_pose_, obs_w_ * 2, obs_h_ * 2);
+        for (int i = 0; i < vertexs.size(); ++i) {
+            //std::cout<<"vetex:"<<vertexs[i]<<std::endl;
+            statu_velocity[2 + i * 2] = vertexs[i].x();
+            statu_velocity[2 + i * 2 + 1] = vertexs[i].y();
+        }
+    }
+    statu_velocity[0] = line_velocity;
+    statu_velocity[1] = angular;
+
+    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;
+
+    solution.status == CppAD::ipopt::solve_result<Dvector>::success; // 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;
     }
-  }
-  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;
+    ///-----
+    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()));
     }
-    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;
+    return success;
 }
 
-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();
+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;
+    auto Q = A.householderQr();
+    auto result = Q.solve(yvals);
+    return result;
 }

MPC/loaded_mpc.h

@@ -7,37 +7,38 @@
 
 #include <Eigen/Core>
 #include "trajectory.h"
-enum MpcError{
-    success=0,
-    no_solution=1,
-    failed
-};
-class LoadedMPC
-{
+#include "custom_type.h"
+
+class LoadedMPC {
 public:
     typedef struct {
         float shortest_radius;
         float dt;
         float acc_velocity;
         float acc_angular;
-    }MPC_parameter;
- public:
-    LoadedMPC(const Pose2d& obs,double obs_w,double obs_h,double min_velocity,double max_velocity);
+    } MPC_parameter;
+public:
+    LoadedMPC(const Pose2d &obs, double obs_w, double obs_h, double min_velocity, double max_velocity);
+
     virtual ~LoadedMPC();
-    virtual MpcError solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,MPC_parameter mpc_param,
-                       std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory);
 
- protected:
+    virtual MpcError solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd statu, MPC_parameter mpc_param,
+                           std::vector<double> &out, Trajectory &select_traj, Trajectory &optimize_trajectory);
+
+
+protected:
     /*
      * 根据当前点和轨迹，选择前方 point_num
      * point:当前点，target：目标点
      */
-    static bool filte_Path(const Pose2d& point,Pose2d target,
-                           Trajectory trajectory,std::vector<Pose2d>& poses,int point_num=10);
+    static bool filte_Path(const Pose2d &point, Pose2d target,
+                           Trajectory trajectory, std::vector<Pose2d> &poses, int point_num = 10);
+
     /*
      * 根据路径点，拟合曲线
      */
-    static Eigen::VectorXd fit_path(const std::vector<Pose2d>& trajectory);
+    static Eigen::VectorXd fit_path(const std::vector<Pose2d> &trajectory);
+
     Pose2d obs_relative_pose_;    //障碍物相对小车的位姿
     double min_velocity_;
     double max_velocity_;

MPC/navigation.h

@@ -4,6 +4,7 @@
 
 #ifndef LIO_LIVOX_CPP_MPC_NAVIGATION_H_
 #define LIO_LIVOX_CPP_MPC_NAVIGATION_H_
+
 #include <glog/logging.h>
 #include "../define/timedlockdata.hpp"
 #include "pose2d.h"
@@ -14,16 +15,14 @@
 #include <queue>
 #include <thread>
 
-class Navigation
-{
+class Navigation {
 public:
-    typedef struct{
+    typedef struct {
         double min;
         double max;
-    }stLimit;
-    enum ActionType
-    {
-        eReady=0,
+    } stLimit;
+    enum ActionType {
+        eReady = 0,
         eRotation,
         eHorizon,
         eVertical,
@@ -31,117 +30,150 @@ public:
         eClampClose
     };
     //舵轮位置
-    enum RWheelPosition{
-        eUnknow=0,
+    enum RWheelPosition {
+        eUnknow = 0,
         eX,   //超前
         eY,     //横移
         eR      //旋转位
     };
-    enum ClampStatu
-    {
-        eInvalid=0,
-        eClosed=1,
-        eOpened=2
+    enum ClampStatu {
+        eInvalid = 0,
+        eClosed = 1,
+        eOpened = 2
     };
 
-    enum MpcResult{
-        eMpcSuccess=0,
+    enum MpcResult {
+        eMpcSuccess = 0,
         eMpcFailed,
         eImpossibleToTarget
     };
 
 
- public:
+public:
     Navigation();
+
     virtual ~Navigation();
     //连接AGV emqx服务
 
-    bool Init(const NavParameter::Navigation_parameter& parameter);
+    bool Init(const NavParameter::Navigation_parameter &parameter);
+
+    virtual void ResetPose(const Pose2d &pose);
+
+    void ResetStatu(double v, double a);
 
-    virtual void ResetPose(const Pose2d& pose);
-    void ResetStatu(double v,double a);
     virtual void ResetClamp(ClampStatu statu);
-    TimedLockData<Pose2d>& RealTimePose(){
-      return timedPose_;
+
+    TimedLockData<Pose2d> &RealTimePose() {
+        return timedPose_;
     }
-    TimedLockData<double>& RealTimeV(){
-      return timedV_;
+
+    TimedLockData<double> &RealTimeV() {
+        return timedV_;
     }
-    TimedLockData<double>& RealTimeA(){
-      return timedA_;
+
+    TimedLockData<double> &RealTimeA() {
+        return timedA_;
     }
-    virtual void Start(const NavMessage::NavCmd& cmd,NavMessage::NavResponse& response);
-    void Cancel(const NavMessage::NavCmd& cmd,NavMessage::NavResponse& response);
+
+    virtual void Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response);
+
+    void Cancel(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response);
+
     bool Stop();
+
     bool SlowlyStop();
-    virtual void SwitchMode(Monitor_emqx::ActionMode mode,float wheelBase);
+
+    virtual void SwitchMode(Monitor_emqx::ActionMode mode, float wheelBase);
+
     bool PoseTimeout();
 
- protected:
-    virtual void HandleAgvStatu(const MqttMsg& msg);
-    virtual void SendMoveCmd(Monitor_emqx::ActionMode mode,Monitor_emqx::ActionType type,
-                             double v,double angular);
+protected:
+    virtual void HandleAgvStatu(const MqttMsg &msg);
 
-    static void RobotPoseCallback(const MqttMsg& msg,void* context);
-    static void RobotSpeedCallback(const MqttMsg& msg,void* context);
-    static void BrotherAgvStatuCallback(const MqttMsg& msg,void* context);
+    virtual void SendMoveCmd(Monitor_emqx::ActionMode mode, Monitor_emqx::ActionType type,
+                             double v, double angular);
+
+    static void RobotPoseCallback(const MqttMsg &msg, void *context);
+
+    static void RobotSpeedCallback(const MqttMsg &msg, void *context);
+
+    static void BrotherAgvStatuCallback(const MqttMsg &msg, void *context);
 
 
     bool IsArrived(NavMessage::PathNode targetNode);
-    bool PossibleToTarget(NavMessage::PathNode targetNode,bool directY);
+
+    bool PossibleToTarget(NavMessage::PathNode targetNode, bool directY);
 
     /*
      * 移动到目标点，不能到达则旋转
      */
-    bool MoveToTarget(NavMessage::PathNode node,stLimit limit_v,stLimit limit_w,bool autoDirect,bool enable_rotate);
+    bool MoveToTarget(NavMessage::PathNode node, stLimit limit_v, stLimit limit_w, bool autoDirect, bool enable_rotate);
+
     /*
      * autoDirect:自动选择 纵向还是横向巡线
      */
-    MpcResult MpcToTarget(NavMessage::PathNode node,stLimit limit_v,bool autoDirect);
+    MpcResult MpcToTarget(NavMessage::PathNode node, stLimit limit_v, bool autoDirect);
+
     /*
      * 旋转摆正，朝向目标点,
      * anyDirect: 任意一个朝向对准目标点
      */
-    virtual bool RotateReferToTarget(const Pose2d& target,stLimit limit_rotate,bool anyDirect);
+    virtual bool RotateReferToTarget(const Pose2d &target, stLimit limit_rotate, bool anyDirect);
+
+    virtual MpcResult RotateReferToTarget(NavMessage::PathNode target, stLimit limit_rotate, int directions);
+
     /*
      * 按照路径点导航
      */
     bool execute_nodes(NavMessage::NewAction action);
+
     /*
      * 进出库
      */
     bool execute_InOut_space(NavMessage::NewAction action);
+
     /*
      * 根据自身和配对车辆状态，调整入库姿态，先到的进入车位1点，后车先到的倒车入库，后到的车与先到的车保持朝向一致
      * space:输入车位点,
      * target:输出目标点
      */
-    virtual bool RotateBeforeEnterSpace(NavMessage::PathNode space,double wheelbase,NavMessage::PathNode& target);
+    virtual bool RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target);
 
     virtual bool clamp_close();
+
     virtual bool clamp_open();
-    bool mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& out,bool directY=false);
 
-    virtual bool CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu);
+    bool mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, bool directY = false);
+
+    /* 原地旋转mpc
+     *
+     * */
+    bool
+    Rotation_mpc_once(Pose2d current_to_target, stLimit limit_wmg, std::vector<double> &out, bool all_direct = true);
+
+    virtual bool CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu);
+
     /*
      * 计算以当前点到目标点的曲率
      */
-    float compute_cuv(const Pose2d& target);
+    float compute_cuv(const Pose2d &target);
+
     /*
      * 发布导航模块状态
      */
-    static void pubStatuThreadFuc(void* p);
-    virtual void publish_statu(NavMessage::NavStatu& statu);
-
- protected:
-    std::mutex  mtx_;
-    bool exit_=false;
-    ActionType actionType_=eReady;
-    std::thread* pubthread_= nullptr;
-    bool inited_=false;
-    Monitor_emqx* monitor_= nullptr;
-    Terminator_emqx* terminator_= nullptr;  //本机emqx连接
-    Terminator_emqx* brotherEmqx_= nullptr;  //本机emqx连接
+    static void pubStatuThreadFuc(void *p);
+
+    virtual void publish_statu(NavMessage::NavStatu &statu);
+
+protected:
+    std::mutex mtx_;
+    bool exit_ = false;
+    ActionType actionType_ = eReady;
+    std::thread *pubthread_ = nullptr;
+    bool inited_ = false;
+    Monitor_emqx *monitor_ = nullptr;
+    Terminator_emqx *terminator_ = nullptr;  //本机emqx连接
+    Terminator_emqx *brotherEmqx_ = nullptr;  //本机emqx连接
 
     TimedLockData<Pose2d> timedPose_;           //当前位姿
     TimedLockData<double> timedV_;              //底盘数据
@@ -156,11 +188,11 @@ public:
 
     NavMessage::NavCmd global_navCmd_; //导航任务,包含一系列动作
     std::queue<NavMessage::NewAction> newUnfinished_cations_; //未完成的动作
-    bool running_=false;
-    bool pause_=false;
-    bool cancel_=false;
+    bool running_ = false;
+    bool pause_ = false;
+    bool cancel_ = false;
 
-    Monitor_emqx::ActionMode move_mode_=Monitor_emqx::eSingle;
+    Monitor_emqx::ActionMode move_mode_ = Monitor_emqx::eSingle;
 
     bool isInSpace_; //是否在车位或者正在进入车位
     std::string space_id_;
@@ -168,9 +200,13 @@ public:
     TimedLockData<Trajectory> predict_traj_;
 
     NavParameter::Navigation_parameter parameter_;
-    RWheelPosition  RWheel_position_;
+    RWheelPosition RWheel_position_;
 };
-double limit_gause(double x,double min,double max);
-double limit(double x,double min,double max);
-double next_speed(double speed,double target_speed,double acc,double dt=0.1);
+
+double limit_gause(double x, double min, double max);
+
+double limit(double x, double min, double max);
+
+double next_speed(double speed, double target_speed, double acc, double dt = 0.1);
+
 #endif //LIO_LIVOX_CPP_MPC_NAVIGATION_H_

MPC/pose2d.h

@@ -33,6 +33,13 @@ class Pose2d
       return *this;
     }
 
+    /*
+     * 单独设置属性
+     */
+    void set_theta(float theta){    m_theta = theta;    }
+    void set_x(float x){    m_x = x;    }
+    void set_y(float y){    m_y = y;    }
+
     /*
      * 两位姿相减，角度在-PI ,PI
      */

MPC/rotate_mpc.cpp

@@ -0,0 +1,242 @@
+//
+// Created by gf on 23-7-21.
+//
+
+#include "rotate_mpc.h"
+#include <chrono>
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+
+size_t mpc_rotate_steps = 25;                  //优化考虑后面多少步
+#define N mpc_rotate_steps
+
+class FG_eval_rotate {
+public:
+    Eigen::VectorXd m_statu;                  //当前状态
+    Eigen::VectorXd m_condition;              //搜索条件参数
+    FG_eval_rotate(Eigen::VectorXd status, Eigen::VectorXd condition) {
+        m_statu = status;
+        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 target_theta = m_condition[1];
+        double obs_w = m_condition[2];
+        double obs_h = m_condition[3];
+
+        double angular_velocity = m_statu[0];
+
+        // Weights for how "important" each cost is - can be tuned
+        const double y_cost_weight = 1000;
+        const double angular_cost_weight = 4000;
+        const double v_cost_weight = 5000;
+        const double angular_velocity_cost_weight = 1000;
+        const double acceleration_cost_weight = 1;
+        const double angular_acceleration_cost_weight = 10;
+        const double obs_distance_weight = 5000.0;
+
+        // 代价函数计算
+        for (int t = 0; t < N - 1; ++t) {
+            /// 角速度，角加速度weight loss
+            //fg[0] += angular_velocity_cost_weight * CppAD::pow(vars[N + t], 2);
+            fg[0] += angular_acceleration_cost_weight * CppAD::pow(vars[N + t + 1] - vars[N + t], 2);
+            /// 目标角度 loss
+            fg[0] += angular_cost_weight * CppAD::pow(target_theta - vars[t], 2);
+        }
+
+        // 约束条件
+        int n_angular = 1;
+        int n_angular_acceleration = n_angular + N;
+        int n_obs = n_angular_acceleration + N;
+
+        /// 角度等式约束 1~N
+        fg[n_angular] = vars[0] - angular_velocity * dt;
+        for (int t = 1; t < N; ++t) {
+            // State at time t + 1
+            CppAD::AD<double> theta1 = vars[t];
+
+            // State at time t
+            CppAD::AD<double> theta0 = vars[t - 1];
+            CppAD::AD<double> angular_velocity0 = vars[N + t - 1];
+
+            // Setting up the rest of the model constraints
+            fg[n_angular + t] = theta1 - (theta0 + angular_velocity0 * dt);
+        }
+
+        /// 角加速度不等式约束 N+1~2N
+        fg[n_angular_acceleration] = (vars[0 + N] - angular_velocity) / dt;
+        for (int t = 1; t < N; ++t) {
+            fg[n_angular_acceleration + t] = (vars[1 + t + N] - vars[t + N]) / dt;
+        }
+
+        /// 障碍物距离不等式约束 2N+1~3N
+        if (m_statu.size() == 1 + 3) {
+            float obs_x = m_statu[1];
+            float obs_y = m_statu[2];
+            float obs_theta = m_statu[3];
+            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 < 7; ++i) {
+                    CppAD::AD<double> ra = obs_x * CppAD::cos(vars[t]) -
+                                           obs_y * CppAD::sin(vars[t]);
+                    CppAD::AD<double> rb = obs_x * CppAD::sin(vars[t]) +
+                                           obs_y * CppAD::cos(vars[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[n_obs + t] = min_distance;
+            }
+//            std::cout << " fg[n_obs]: " << fg[n_obs] << " | __LINE__:" << __LINE__ << std::endl;
+        }
+    }
+};
+
+RotateMPC::RotateMPC(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;
+}
+
+RotateMPC::~RotateMPC() {}
+
+MpcError
+RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter mpc_param, std::vector<double> &out) {
+    auto start = std::chrono::steady_clock::now();
+    // State vector holds all current values needed for vars below
+    Pose2d pose_agv = Pose2d(statu[0], statu[1], statu[2]);
+    double line_velocity = statu[3];
+    double angular_velocity = statu[4];
+
+    /// 定义最大最小角度,角速度,角加速度(不区分方向)
+    double max_angular = M_PI;
+    double max_angular_velocity = 10.0 / 180 * M_PI;
+    double min_angular_velocity = 0.1 / 180 * M_PI;
+    double max_angular_acceleration_velocity = 8.0 / 180 * M_PI;//mpc_param.acc_angular; //
+
+    // 优化
+    typedef CPPAD_TESTVECTOR(double) Dvector;
+
+    /// 步长
+    double dt = mpc_param.dt;
+
+    /// 初始化参数
+    size_t n_vars = N * 2; // 0-N为角度，N-2N为角速度
+    Dvector vars(n_vars); // 0-N为角度，N-2N为角速度
+
+    for (int i = 0; i < n_vars; i++) vars[i] = 0.0;
+    vars[N] = angular_velocity;
+
+    // 参数上下限
+    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 of angular velocity
+    for (auto i = N; i < 2 * N; i++) {
+        vars_lowerbound[i] = -max_angular_velocity;
+        vars_upperbound[i] = max_angular_velocity;
+    }
+
+    // Lower and upper limits for the constraints
+    /// 障碍物是否进入 碰撞检测范围内
+    float distance = Pose2d::distance(obs_relative_pose_, Pose2d(0, 0, 0));
+    bool find_obs = distance < 5;
+    size_t n_constraints = 2 * N + find_obs * N;// 0~N角度，N~2N角加速度, 2N~3N障碍物约束
+    Dvector constraints_lowerbound(n_constraints);
+    Dvector constraints_upperbound(n_constraints);
+    for (auto i = 0; i < n_constraints; ++i) {
+        constraints_lowerbound[i] = 0;
+        constraints_upperbound[i] = 0;
+    }
+    /// limit of angular acceleration
+    for (auto i = N; i < 2 * N; ++i) {
+        constraints_lowerbound[i] = -max_angular_acceleration_velocity;
+        constraints_upperbound[i] = max_angular_acceleration_velocity;
+    }
+    /// limit of obs distance
+    if (find_obs)
+        for (auto i = 2 * N; i < 3 * N; ++i) {
+            constraints_lowerbound[i] = 1.0;
+            constraints_upperbound[i] = 1e19;
+        }
+
+    // fg_eval for IPOPT solver
+    size_t n_fg_status = 1 + 3*find_obs;
+    Eigen::VectorXd fg_status(n_fg_status);
+    fg_status[0] = angular_velocity;
+    if (find_obs){
+        fg_status[1] = obs_relative_pose_.x();
+        fg_status[2] = obs_relative_pose_.y();
+        fg_status[3] = obs_relative_pose_.theta();
+    }
+//    std::cout << " size of fg_status: " << fg_status.size() << " | __LINE__: " << __LINE__ << std::endl;
+
+
+    Pose2d targetAngularInAGV = current_to_target;
+    /// 调整到适合的目标角度
+    if (targetAngularInAGV.theta() > max_angular_velocity * N * dt)
+        targetAngularInAGV.set_theta(max_angular_velocity * N * dt);
+
+    Eigen::VectorXd condition(4);
+    condition << dt, targetAngularInAGV.theta(), obs_w_, obs_h_;
+
+    FG_eval_rotate fg_eval(fg_status, 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_rotate>(
+            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 status : %d  input: %.5f max_wmg:%.5f\n", solution.status,
+               angular_velocity, max_angular_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_angular_velocity = solution.x[N];
+
+    out.push_back(solve_angular_velocity);
+
+    return success;
+}
+

MPC/rotate_mpc.h

@@ -0,0 +1,40 @@
+//
+// Created by gf on 23-7-21.
+//
+
+#ifndef LIO_LIVOX_CPP_MPC_ROTATE_MPC_H_
+#define LIO_LIVOX_CPP_MPC_ROTATE_MPC_H_
+
+#include <Eigen/Core>
+#include "trajectory.h"
+#include "custom_type.h"
+
+class RotateMPC {
+public:
+    typedef struct {
+        float shortest_radius;
+        float dt;
+        float acc_velocity;
+        float acc_angular;
+    } MPC_parameter;
+public:
+    RotateMPC(const Pose2d &obs, double obs_w, double obs_h, double min_velocity, double max_velocity);
+
+    virtual ~RotateMPC();
+
+    /*
+     * 原地旋转MPC
+     */
+    virtual MpcError
+    solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter mpc_param, std::vector<double> &out);
+
+protected:
+    Pose2d obs_relative_pose_;    //障碍物相对小车的位姿
+    double min_velocity_;
+    double max_velocity_;
+    double obs_w_;
+    double obs_h_;
+
+};
+
+#endif //LIO_LIVOX_CPP_MPC_ROTATE_MPC_H_

config/webots/navigation.prototxt

@@ -5,7 +5,7 @@ rpc_ipport:"127.0.0.1:9091"
 Agv_emqx
 {
 	NodeId:"agv-Child"
-	ip:"127.0.0.1"
+	ip:"192.168.2.179"
 	port:1883
 	pubSpeedTopic:"ChildPLC/speedcmd"
 	subPoseTopic:"lidar_odom/child"
@@ -34,14 +34,14 @@ x_mpc_parameter
 {
 	shortest_radius:26
 	dt:0.1
-	acc_velocity:1.0
+	acc_velocity:2.0
 	acc_angular:10
 }
 y_mpc_parameter
 {
 	shortest_radius:24.5
 	dt:0.1
-	acc_velocity:1.0
+	acc_velocity:2.0
 	acc_angular:10
 }
 
@@ -53,7 +53,7 @@ InOutVLimit
 NodeVelocityLimit
 {
     min:0.03
-    max:3
+    max:1.5
 }
 NodeAngularLimit
 {

config/webots/navigation_main.prototxt

@@ -4,7 +4,7 @@ rpc_ipport:"127.0.0.1:9090"
 Agv_emqx
 {
 	NodeId:"agv-main"
-	ip:"127.0.0.1"
+	ip:"192.168.2.179"
 	port:1883
 	pubSpeedTopic:"MainPLC/speedcmd"
 	subPoseTopic:"lidar_odom/main"
@@ -33,14 +33,14 @@ x_mpc_parameter
 {
 	shortest_radius:26
 	dt:0.1
-	acc_velocity:1.0
+	acc_velocity:2.0
 	acc_angular:10
 }
 y_mpc_parameter
 {
 	shortest_radius:24.5
 	dt:0.1
-	acc_velocity:1.0
+	acc_velocity:2.0
 	acc_angular:10
 }
 
@@ -52,7 +52,7 @@ InOutVLimit
 NodeVelocityLimit
 {
     min:0.03
-    max:3
+    max:1.5
 }
 NodeAngularLimit
 {