修改模型为汽车前轮角与后轮驱动模式

MPC/loaded_mpc.cpp

@@ -12,7 +12,7 @@ size_t nx = 0;
 size_t ny = nx + N;
 size_t nth = ny + N;
 size_t nv = nth + N;
-size_t nvth = nv + N;
+size_t ndlt = nv + N;
 
 class FG_eval_half_agv {
  public:
@@ -36,7 +36,9 @@ class FG_eval_half_agv {
       double dt=m_condition[0];
       double ref_v=m_condition[1];
       double v=m_statu[0];
-      double v_th=m_statu[1];
+      double delta=m_statu[1];
+      double wheelbase=m_statu[2];
+
       // Reference State Cost
       // Below defines the cost related the reference state and
       // any anything you think may be beneficial.
@@ -55,26 +57,32 @@ class FG_eval_half_agv {
         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);
 
+        //目标速度loss
         fg[0]+=v_cost_weight*CppAD::pow(vars[nv+t]-ref_v,2);
 
       }
 
       // Costs for steering (delta) and acceleration (a)
+
       for (int t = 0; t < N-1; t++) {
-        fg[0] += vth_cost_weight * CppAD::pow(vars[nvth + t], 2);
+        //速度，加速度，前轮角 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[nvth + t+1]-vars[nvth+t], 2);
+        fg[0] += ath_cost_weight * CppAD::pow(vars[ndlt+t+1]-vars[ndlt+t], 2);
       }
+      fg[0]*=0.01;
 
       /////////////////////
       fg[1 + nx] = vars[nx]-vars[nv]*dt;
       fg[1 + ny] = vars[ny];
-      fg[1 + nth] = vars[nth]-vars[nvth]*dt;
+      CppAD::AD<double> w0=vars[nv]/wheelbase*CppAD::tan(vars[ndlt]);
+      fg[1 + nth] = vars[nth]-w0*dt;
 
-      // The rest of the constraints
+      //位姿约束
       for (int t = 1; t < N; t++) {
         // State at time t + 1
         CppAD::AD<double> x1 = vars[nx + t];
@@ -86,29 +94,30 @@ class FG_eval_half_agv {
         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> vth0 = vars[nvth + t-1];
 
-        // Actuator constraints at time t only
+        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 + vth0 * dt);
+        fg[1 + nth + t] = th1 - (th0 + w_0 * dt);
 
       }
-
+      //加速度和dlt约束
       fg[1 + nv]=(vars[nv]-v)/dt;
-      fg[1+nvth]=(vars[nvth]-v_th)/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+nvth+t]=(vars[nvth+t]-vars[nvth+t-1])/dt;
+        fg[1+ndlt+t]=(vars[ndlt+t]-vars[ndlt+t-1])/dt;
       }
 
     }
 };
 
-LoadedMPC::LoadedMPC(){}
+LoadedMPC::LoadedMPC(double wheelbase){
+  wheelbase_=wheelbase;
+}
 LoadedMPC::~LoadedMPC(){}
 
 bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::VectorXd statu,
@@ -150,19 +159,17 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   float gradient=fabs(target.x()-pose_agv.x())>1e-6?(target.y()-pose_agv.y())/(target.x()-pose_agv.x()):500.0;
   double direction=gradient2theta(gradient,target.x()>=pose_agv.x());
 
-  //float k=180.0/M_PI;
-  //printf("ref :%.3f   agv theta: %.3f   derect: %.3f  abs: %.3f\n",ref_velocity,
   //        pose_agv.theta()*k,direction*k,k*fabs(pose_agv.theta()-direction));
   if (fabs(direction-pose_agv.theta()) >M_PI/2.0)
     ref_velocity = -ref_velocity;
 
 
   double dt = 0.1;
-  double min_velocity = 0.1;
+  double min_velocity = 0.05;
   double max_velocity = 1.9;
-  double max_angular_velocity=5*M_PI/180.0;
+  double max_dlt=5*M_PI/180.0;
   double max_acc_line_velocity=1.9/(N*dt);
-  double max_acc_angular_velocity=5*M_PI/180.0;
+  double max_acc_dlt=5*M_PI/180.0;
 
   size_t n_vars = N * 5;
   size_t n_constraints = N * 5;
@@ -185,15 +192,15 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   //// limit  v
   for (int i = nv; i < nv + N; i++)
   {
-    vars_lowerbound[i] = -max_velocity;
+    vars_lowerbound[i] = min_velocity;
     vars_upperbound[i] = max_velocity;
   }
 
-  ////limint vth
-  for (int i = nvth; i < nvth + N; i++)
+  ////limint dlt
+  for (int i = ndlt; i < ndlt + N; i++)
   {
-    vars_lowerbound[i] = -max_angular_velocity;
-    vars_upperbound[i] = max_angular_velocity;
+    vars_lowerbound[i] = -max_dlt;
+    vars_upperbound[i] = max_dlt;
   }
 
   // Lower and upper limits for the constraints
@@ -210,15 +217,43 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
     constraints_lowerbound[i] = -max_acc_line_velocity;
     constraints_upperbound[i] = max_acc_line_velocity;
   }
-  //// acc vth
-  for (int i = nvth; i < nvth + N; i++)
+  //// acc ndlt
+  for (int i = ndlt; i < ndlt + N; i++)
   {
-    constraints_lowerbound[i] = -max_acc_angular_velocity;
-    constraints_upperbound[i] = max_acc_angular_velocity;
+    constraints_lowerbound[i] = -max_acc_dlt;
+    constraints_upperbound[i] = max_acc_dlt;
   }
 
-  Eigen::VectorXd statu_velocity(2);
-  statu_velocity << line_velocity, angular_velocity;
+
+  Eigen::VectorXd statu_velocity(3);
+  if(line_velocity>max_velocity)
+  {
+    line_velocity=max_velocity;
+    printf(" input +v limited\n");
+  }
+  if(line_velocity<min_velocity)
+  {
+    line_velocity=min_velocity;
+    printf(" input -v limited\n");
+  }
+
+  double omga=0;
+  if(abs(line_velocity)>0.000001)
+  {
+    omga=angular_velocity*wheelbase_/tan(line_velocity);
+  }
+  if(omga>max_dlt)
+  {
+    omga = max_dlt;
+    printf(" input +dlt limited\n");
+  }
+  if(omga<-max_dlt)
+  {
+    omga = -max_dlt;
+    printf(" input -dlt limited\n");
+  }
+
+  statu_velocity << line_velocity, omga,wheelbase_;
 
   Eigen::VectorXd condition(2);
   condition << dt, ref_velocity;
@@ -243,7 +278,6 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   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)
   {
@@ -264,10 +298,10 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
   }*/
 
   out.push_back(solve_velocity);
-  out.push_back(solution.x[nvth]);
-
+  double omg=solve_velocity/wheelbase_*tan(solution.x[ndlt]);
+  out.push_back(omg);
   printf(" input : %.4f  %.4f   output : %.4f  %.4f  ref : %.3f time:%.3f\n",line_velocity,
-         angular_velocity,solve_velocity,solution.x[nvth],ref_velocity,time);
+         angular_velocity,solve_velocity,omg,ref_velocity,time);
 
   optimize_trajectory.clear();
   for (int i = 0; i < N; ++i)

MPC/loaded_mpc.h

@@ -11,7 +11,7 @@
 class LoadedMPC
 {
  public:
-    LoadedMPC();
+    LoadedMPC(double wheelbase);
     ~LoadedMPC();
     virtual bool solve(const Trajectory& trajectory, const Pose2d& target,Eigen::VectorXd statu,
                        std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory);
@@ -27,6 +27,7 @@ class LoadedMPC
      * 根据路径点，拟合曲线
      */
     static Eigen::VectorXd fit_path(const std::vector<Pose2d>& trajectory);
+    double wheelbase_;
 
 };
 

MPC/navigation.cpp

@@ -198,7 +198,7 @@ bool Navigation::mpc_once(const Trajectory& traj,std::vector<double>& out)
   double velocity=timedV_.Get();    //从plc获取状态
   double angular=timedA_.Get();
 
-  LoadedMPC MPC;
+  LoadedMPC MPC(2.7);
   Eigen::VectorXd statu=Eigen::VectorXd::Zero(5);//agv状态
 
   statu[0]=pose.x();

main.cpp

@@ -238,7 +238,7 @@ int main(int argc, char** argv)
     return -3;
 
   Monitor::get_instance()->set_statucallback(monitor_statu_callback);
-  Monitor::get_instance()->plc_socket_init("192.168.2.180",30001);
+  Monitor::get_instance()->plc_socket_init("127.0.0.1",30001);
 
   PangolinViewer* viewer=PangolinViewer::CreateViewer();
   viewer->SetCallbacks(pangolinSpinOnce,OnStartBtn,OnStopBtn,OnFreqChanged,OnDijkstraBtn);
@@ -351,8 +351,8 @@ bool InitMap(std::string config_file)
 
 bool InitDijkstraMap()
 {
-  DijkstraMap.AddVertex(1, 2.3, 3.1);
-  DijkstraMap.AddVertex(2, 19.2, 3.1);
+  DijkstraMap.AddVertex(1, 0, 0);
+  DijkstraMap.AddVertex(2, 19.2, 0);
   DijkstraMap.AddVertex(3, 59.7, 3.3);
   DijkstraMap.AddVertex(4, 98.4, 3.3);
   DijkstraMap.AddVertex(5, 98.5, -13);