增加矩形防护，矩形方程带旋转指数8，功能待测试

MPC/loaded_mpc.cpp

@@ -16,7 +16,6 @@ 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:
@@ -36,16 +35,11 @@ class FG_eval_half_agv {
       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 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;
@@ -74,12 +68,8 @@ class FG_eval_half_agv {
         //目标速度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
@@ -107,8 +97,6 @@ class FG_eval_half_agv {
         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);
@@ -123,28 +111,38 @@ class FG_eval_half_agv {
         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);
+
+
+      if(m_statu.size()==2+8){
+        //与障碍物的距离
+        for(int i=0;i<4;++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_w,8);
+          }
+        }
       }
-    //转弯半径
-      /*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){
+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(){}
 
-bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,MPC_parameter mpc_param,
+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();
@@ -166,7 +164,7 @@ bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu
   if (filte_Path(pose_agv, target, trajectory, filte_poses, 10) == false)
   {
     printf( "filte path failed ...\n");
-    return false;
+    return failed;
   }
   select_traj= Trajectory(filte_poses);
 
@@ -182,7 +180,6 @@ bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu
   Eigen::VectorXd coef = fit_path(transform_poses);
 
   //优化
-  bool ok = true;
   typedef CPPAD_TESTVECTOR(double) Dvector;
 
   //根据当前点和目标点距离,计算目标速度
@@ -205,6 +202,7 @@ bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu
   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++)
   {
@@ -234,7 +232,14 @@ bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu
   }
 
   // Lower and upper limits for the constraints
-  size_t n_constraints = N * (5+1);
+  size_t n_constraints = N * 5;
+  /*
+   * 障碍物是否进入 碰撞检测范围内
+   */
+  float distance=Pose2d::distance(obs_relative_pose_,Pose2d(0,0,0));
+  if(distance<5){
+    n_constraints = N * (5+4);
+  }
   Dvector constraints_lowerbound(n_constraints);
   Dvector constraints_upperbound(n_constraints);
   for (int i = 0; i < n_constraints; i++)
@@ -254,13 +259,15 @@ bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu
     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;
+
+  // 与障碍物保持距离的约束
+  if(distance<5) {
+    for (int i = nobs; i < nobs + 4*N; ++i) {
+      constraints_lowerbound[i] = 1.0;
+      constraints_upperbound[i] = 1e19;
+    }
   }
+
   //限制最小转弯半径，
   /*for(int i=nwmg;i<nwmg+N;++i)
   {
@@ -268,34 +275,33 @@ bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu
     constraints_upperbound[i] = 1.0/(radius*radius);
   }*/
 
-
-  Eigen::VectorXd statu_velocity(4);
-  if(line_velocity>max_velocity_)
-  {
+  if(line_velocity>max_velocity_){
     line_velocity=max_velocity_;
-    //printf(" input +v limited\n");
   }
-  if(line_velocity<-max_velocity_)
-  {
+  if(line_velocity<-max_velocity_){
     line_velocity=-max_velocity_;
-    //printf(" input -v limited\n");
   }
-
-  if(angular>max_dlt)
-  {
+  if(angular>max_dlt){
     angular = max_dlt;
-    printf(" input +dlt limited\n");
   }
-  if(angular<-max_dlt)
-  {
+  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 statu_velocity(2);
+  if(distance<5){
+    statu_velocity=Eigen::VectorXd(2+8);
+    std::vector<Pose2d> vertexs=Pose2d::generate_rectangle_vertexs(obs_relative_pose_,2.5,1.3);
+    for(int i=0;i<vertexs.size();++i){
+      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(2);
-  condition << dt, ref_velocity;
+  Eigen::VectorXd condition(4);
+  condition << dt, ref_velocity,obs_w_,obs_h_;
   FG_eval_half_agv fg_eval(coef, statu_velocity, condition);
 
   // options for IPOPT solver
@@ -317,12 +323,14 @@ bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu
   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)
+  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) relative:(%f,%f) \n",solution.status,line_velocity,
            wmg,angular,obs_relative_pose_.x(),obs_relative_pose_.y());
-    return false;
+    if (solution.status == CppAD::ipopt::solve_result<Dvector>::invalid_number_detected)
+      return no_solution;
+    return failed;
   }
 
   // Cost
@@ -358,7 +366,7 @@ bool LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu
     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;
+  return success;
 }
 
 bool LoadedMPC::filte_Path(const Pose2d& point,Pose2d target,Trajectory trajectory,

MPC/loaded_mpc.h

@@ -7,7 +7,11 @@
 
 #include <Eigen/Core>
 #include "trajectory.h"
-
+enum MpcError{
+    success=0,
+    no_solution=1,
+    failed
+};
 class LoadedMPC
 {
 public:
@@ -18,9 +22,9 @@ public:
         float acc_angular;
     }MPC_parameter;
  public:
-    LoadedMPC(const Pose2d& obs,double min_velocity,double max_velocity);
+    LoadedMPC(const Pose2d& obs,double obs_w,double obs_h,double min_velocity,double max_velocity);
     virtual ~LoadedMPC();
-    virtual bool solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,MPC_parameter mpc_param,
+    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:
@@ -37,6 +41,8 @@ public:
     Pose2d obs_relative_pose_;    //障碍物相对小车的位姿
     double min_velocity_;
     double max_velocity_;
+    double obs_w_;
+    double obs_h_;
 
 };
 

MPC/navigation.cpp

@@ -65,7 +65,19 @@ double next_speed(double speed,double target_speed,double acc,double dt=0.1)
   }
 }
 
-
+bool Navigation::PoseTimeout(){
+  if(timedPose_.timeout()== false && timedBrotherPose_.timeout()==false){
+    return false;
+  }else{
+    if(timedPose_.timeout()){
+      printf(" current pose is timeout \n");
+    }
+    if(timedBrotherPose_.timeout()){
+      printf(" brother pose is timeout \n");
+    }
+    return true;
+  }
+}
 
 bool Navigation::Init(const NavParameter::Navigation_parameter& parameter)
 {
@@ -399,7 +411,7 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
       //发送暂停消息
       continue;
     }
-    if (timedPose_.timeout() || timedV_.timeout() || timedA_.timeout()) {
+    if (PoseTimeout() || timedV_.timeout() || timedA_.timeout()) {
       printf(" navigation Error:Pose/v/a is timeout \n");
       break;
     }
@@ -600,7 +612,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
     std::cout << "execute enter_space failed ,Action miss some infos:" << action.DebugString() << std::endl;
     return false;
   }
-  if (timedPose_.timeout() == true) {
+  if (PoseTimeout() == true) {
     printf(" inout_space failed type:%d : current pose is timeout\n", action.type());
     return false;
   }
@@ -651,19 +663,18 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
     return false;
   }
 
-  if (action.type() == 1) //1,进库
-    if (execute_adjust_action(begin, begin_accuracy, adjust_x, adjust_h, adjust_angular, false) == false)
+  if (action.type() == 1) //1,进库,先调整到库位前方（起点）
+    if (execute_adjust_action(begin, begin_accuracy, adjust_x, adjust_h, adjust_angular, true) == false)
       return false;
 
-  //if (action.type() == 2) //2，出库
-    if (execute_along_action(begin, target, target_accuracy, mpc_velocity, false) == false)
+  if (execute_along_action(begin, target, target_accuracy, mpc_velocity, true) == false)
       return false;
   return true;
 }
 
 
 bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& out,bool directY) {
-  if (timedPose_.timeout() == true) {
+  if (PoseTimeout() == true) {
     printf(" MPC once Error:Pose is timeout \n");
     return false;
   }
@@ -680,11 +691,6 @@ bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& o
   double velocity = timedV_.Get();    //从plc获取状态
   double angular = timedA_.Get();
 
-  Pose2d brother = timedBrotherPose_.Get();
-  Pose2d relative = Pose2d::relativePose(brother, pose);//计算另一节在当前小车坐标系下的坐标
-  if (move_mode_ == Monitor_emqx::ActionMode::eMain)
-    relative = Pose2d(-1e8, -1e8, 0);
-
   Eigen::VectorXd statu = Eigen::VectorXd::Zero(5);//agv状态
 
   statu[0] = pose.x();
@@ -696,15 +702,26 @@ bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& o
   Trajectory optimize_trajectory;
   Trajectory selected_trajectory;
   NavParameter::MPC_parameter mpc_parameter=parameter_.x_mpc_parameter();
+  double obs_w=1.35;
+  double obs_h=0.75;
+
   if (directY == true) {
     //将车身朝向旋转90°，车身朝向在(-pi,pi]
     statu[2] += M_PI / 2;
     if (statu[2] > M_PI)
       statu[2] -= 2 * M_PI;
     mpc_parameter=parameter_.y_mpc_parameter();
+    obs_w=0.75;
+    obs_h=1.35;
   }
-  bool ret = false;
-  LoadedMPC MPC(relative, limit_v.min, limit_v.max);
+
+  Pose2d brother = timedBrotherPose_.Get();
+  Pose2d relative = Pose2d::relativePose(brother.rotate(brother.x(),brother.y(),M_PI/2), pose.rotate(pose.x(),pose.y(),M_PI/2));//计算另一节在当前小车坐标系下的坐标
+  if (move_mode_ == Monitor_emqx::ActionMode::eMain)
+    relative = Pose2d(-1e8, -1e8, 0);
+
+  MpcError ret = failed;
+  LoadedMPC MPC(relative, obs_w,obs_h,limit_v.min, limit_v.max);
   LoadedMPC::MPC_parameter parameter={mpc_parameter.shortest_radius(),mpc_parameter.dt(),
                                      mpc_parameter.acc_velocity(),mpc_parameter.acc_angular()};
   //printf(" r:%f  dt:%f acc:%f  acc_a:%f\n",mpc_parameter.shortest_radius(),
@@ -713,9 +730,14 @@ bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& o
                   out, selected_trajectory, optimize_trajectory);
 
   //std::cout<<" traj size %d  %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
+  if(ret==no_solution) {
+    printf(" mpc solve no solution set v/w = 0\n");
+    out[0]=0;
+    out[1]=0;
+  }
   predict_traj_.reset(optimize_trajectory, 1);
   selected_traj_.reset(selected_trajectory, 1);
-  return ret;
+  return ret==success || ret==no_solution;
 }
 
 bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
@@ -815,7 +837,7 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
     printf(" navigation has not inited\n");
     return false;
   }
-  if (timedPose_.timeout()) {
+  if (PoseTimeout()) {
     printf(" navigation Error:Pose is timeout \n");
     return false;
   }
@@ -841,7 +863,7 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
       //发送暂停消息
       continue;
     }
-    if (timedPose_.timeout()) {
+    if (PoseTimeout()) {
       printf(" navigation Error:Pose is timeout \n");
       return false;
     }
@@ -860,7 +882,7 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
     }
     /*
      * 判断周边是否有障碍物，半径r，当障碍物进入r内，停下等待，当障碍物远离 1.2*r，恢复
-     * */
+     *
     if(move_mode_==Monitor_emqx::eSingle) {
       double r = 2.4;
       static bool obs_stoped = false;
@@ -884,6 +906,7 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
 
       }
     }
+     */
 
     //一次变速
     std::vector<double> out;

MPC/navigation.h

@@ -44,6 +44,7 @@ public:
         eXY
     };
 
+
  public:
     Navigation();
     virtual ~Navigation();
@@ -68,7 +69,7 @@ public:
     void Pause();
     bool Stop();
     virtual void SwitchMode(Monitor_emqx::ActionMode mode,float wheelBase);
-
+    bool PoseTimeout();
 
  protected:
     virtual void HandleAgvStatu(const MqttMsg& msg);

MPC/pose2d.cpp

@@ -80,25 +80,11 @@ Pose2d Pose2d::rotate(float theta)const
   float new_theta=this->theta()+theta;
 
 
-  if(theta<0)
+  if(theta<=-M_PI)
     new_theta=new_theta+2*M_PI;
 
-  if(new_theta>=2*M_PI)
-    new_theta-=2*M_PI;
-
   if(new_theta>M_PI)
-    new_theta=new_theta-2*M_PI;
-
-  /*//转换到 [-pi/2, pi/2]下
-  int n=int(new_theta/(M_PI));
-  if(new_theta<-M_PI/2)
-  {
-    new_theta += M_PI*(n+1);
-  }
-  if(new_theta>M_PI/2)
-  {
-    new_theta -= M_PI*(n+1);
-  }*/
+    new_theta-=2*M_PI;
 
   return Pose2d(x,y,new_theta);
 }

MPC/pose2d.h

@@ -7,6 +7,7 @@
 
 #include <iostream>
 #include <atomic>
+#include <vector>
 #include <math.h>
 /*
  * 带direction的二维点,与x轴正方向逆时针为正方向
@@ -113,6 +114,15 @@ class Pose2d
     //返回值[0,2PI)
     static float vector2yaw(float x,float y);
 
+    static std::vector<Pose2d> generate_rectangle_vertexs(const Pose2d& pose,float w,float h){
+      std::vector<Pose2d> poses;
+      poses.push_back(pose*Pose2d(w/2,h/2,0));
+      poses.push_back(pose*Pose2d(w/2,-h/2,0));
+      poses.push_back(pose*Pose2d(-w/2,h/2,0));
+      poses.push_back(pose*Pose2d(-w/2,-h/2,0));
+      return poses;
+    }
+
  protected:
     float                     m_x;
     float                     m_y;