find the constrain settings' problem, i.e. the output need to be the predicted one, not the current one. Then added current linear and angular speed as constrains.

MPC/src/mpc/TowBotMPC.cpp

@@ -56,20 +56,20 @@ public:
     {
 
         // std::cout << "max size: " << naccth2 + K - 1 << std::endl;
-        double v1 = m_data[0];
-        double v2 = m_data[1];
-        double vth1 = m_data[2];
-        double vth2 = m_data[3];
+        // double v1 = m_data[0];
+        // double v2 = m_data[1];
+        // double vth1 = m_data[2];
+        // double vth2 = m_data[3];
         double ref_v = m_data[4];
         double dt = m_data[5];
-        
+
         /// 两车初始位姿差   前车相对后车坐标系
-        double dx = m_data[6]; 
+        double dx = m_data[6];
         double dy = m_data[7];
         double dth = m_data[8]; ///两坐标系角度
 
         ///前车 cost weight
-        const int y_cost_weight = 8000;
+        const int y_cost_weight = 2000;
         const int th_cost_weight = 1000;
         const int v_cost_weight = 20;
 
@@ -79,7 +79,7 @@ public:
 
         fg[0] = 0;
         ///后车 cost weight
-        const int back_y_cost_weight = 8000;
+        const int back_y_cost_weight = 2000;
         const int back_th_cost_weight = 1000;
         const int back_v_cost_weight = 20;
 
@@ -100,12 +100,12 @@ public:
             fg[0] += v_cost_weight * CppAD::pow(vars[nv1 + t] - ref_v, 2);
             fg[0] += vth_cost_weight * CppAD::pow(vars[nvth1 + t], 2);
 
-            // 在前车坐标系下，后车y坐标差
+            // 在后车坐标系下，后车y坐标差
             AD<double> xt2 = vars[nx2 + t];
             AD<double> fx2 = coeffs_b[0] + coeffs_b[1] * xt2 + coeffs_b[2] * pow(xt2, 2) + coeffs_b[3] * pow(xt2, 3);
             fg[0] += back_y_cost_weight * CppAD::pow(vars[ny2 + t] - fx2, 2);
 
-            // 在前车坐标系下，后车theta cost & v cost
+            // 在后车坐标系下，后车theta cost & v cost
             AD<double> fth2 = CppAD::atan(coeffs_b[1] + 2 * coeffs_b[2] * xt2 + 3 * coeffs_b[3] * pow(xt2, 2));
             fg[0] += back_th_cost_weight * CppAD::pow(vars[nth2 + t] - fth2, 2);
             fg[0] += back_v_cost_weight * CppAD::pow(vars[nv2 + t] - ref_v, 2);
@@ -115,7 +115,7 @@ public:
         // Costs for steering (delta) and acceleration (a)
         for (int t = 0; t < K - 1; t++)
         {
-            fg[0] += a_cost_weight * CppAD::pow(vars[nacc1+ t], 2);
+            fg[0] += a_cost_weight * CppAD::pow(vars[nacc1 + t], 2);
             fg[0] += ath_cost_weight * CppAD::pow(vars[naccth1 + t], 2);
 
             fg[0] += back_a_cost_weight * CppAD::pow(vars[nacc2 + t], 2);
@@ -123,9 +123,9 @@ public:
         }
 
         ///添加两车距离损失
-        const int distance_weight = 800;
-        const int delta_th_weight = 10;
-        char buf[255]={0};
+        const int distance_weight = 600;
+        const int delta_th_weight = 50;
+        // char buf[255]={0};
         AD<double> dtheta = vars[nth1] - vars[nth2] + dth;
         for (int t = 0; t < K; ++t)
         {
@@ -140,24 +140,24 @@ public:
 
             fg[0] += distance_weight * CppAD::pow(CppAD::sqrt(CppAD::pow(distance_x, 2) + CppAD::pow(distance_y, 2)) - ref_dist, 2);
             fg[0] += delta_th_weight * CppAD::pow(CppAD::atan(y1_in_back / x1_in_back), 2);
-            sprintf(buf+strlen(buf), " %.4f ", sqrt(pow(distance_x,2)+pow(distance_y,2)));
+            // sprintf(buf+strlen(buf), " %.4f ", sqrt(pow(distance_x,2)+pow(distance_y,2)));
             // // 两车最短距离约束
             // fg[1 + relative_pose_contrains + t - 4*(K-1)] = CppAD::sqrt(CppAD::pow(distance_x, 2) + CppAD::pow(distance_y, 2));
         }
-        std::cout<<buf<<std::endl;
+        // std::cout<<buf<<std::endl;
 
         // x, y, theta 路径约束
-        fg[1 + nx1] = vars[nx1];// - vars[nv1] * dt;
+        fg[1 + nx1] = vars[nx1]; // - vars[nv1] * dt;
         fg[1 + ny1] = vars[ny1];
-        fg[1 + nth1] = vars[nth1];// - vars[nvth1] * dt;
-        fg[1 + nv1] = vars[nv1];
-        fg[1 + nvth1] = vars[nvth1];
+        fg[1 + nth1] = vars[nth1]; // - vars[nvth1] * dt;
+        fg[1 + nv1] = vars[nv1];// + vars[nacc1] * dt;
+        fg[1 + nvth1] = vars[nvth1];// + vars[naccth1] * dt;
 
-        fg[1 + nx2 - 2*(K-1)] = vars[nx2];// - vars[nv2] * dt;
-        fg[1 + ny2 - 2*(K-1)] = vars[ny2];
-        fg[1 + nth2 - 2*(K-1)] = vars[nth2];// - vars[nvth2] * dt;
-        fg[1 + nv2 - 2*(K-1)] = vars[nv2];
-        fg[1 + nvth2 - 2*(K-1)] = vars[nvth2];
+        fg[1 + nx2 - 2 * (K - 1)] = vars[nx2]; // - vars[nv2] * dt;
+        fg[1 + ny2 - 2 * (K - 1)] = vars[ny2];
+        fg[1 + nth2 - 2 * (K - 1)] = vars[nth2]; // - vars[nvth2] * dt;
+        fg[1 + nv2 - 2 * (K - 1)] = vars[nv2];// + vars[nacc2] * dt;
+        fg[1 + nvth2 - 2 * (K - 1)] = vars[nvth2];// + vars[naccth2] * dt;
 
         // The rest of the constraints
         for (int t = 1; t < K; t++)
@@ -178,9 +178,9 @@ public:
             AD<double> facc0 = vars[nacc1 + t - 1];
             AD<double> faccth0 = vars[naccth1 + t - 1];
 
-            fg[1 + nx1 + t] = fx1 - (fx0 + fv0 * CppAD::cos(fth0) * dt);
-            fg[1 + ny1 + t] = fy1 - (fy0 + fv0 * CppAD::sin(fth0) * dt);
-            fg[1 + nth1 + t] = fth1 - (fth0 + fvth0 * dt);
+            fg[1 + nx1 + t] = fx1 - (fx0 + (fv0 + 0.5 * facc0 * dt) * CppAD::cos(fth0 + 0.5 * fvth0 * dt) * dt);
+            fg[1 + ny1 + t] = fy1 - (fy0 + (fv0 + 0.5 * facc0 * dt) * CppAD::sin(fth0 + 0.5 * fvth0 * dt) * dt);
+            fg[1 + nth1 + t] = fth1 - (fth0 + (fvth0 + 0.5 * faccth0 * dt) * dt);
             fg[1 + nv1 + t] = fv1 - (fv0 + facc0 * dt);
             fg[1 + nvth1 + t] = fvth1 - (fvth0 + faccth0 * dt);
 
@@ -200,11 +200,11 @@ public:
             AD<double> bacc0 = vars[nacc2 + t - 1];
             AD<double> baccth0 = vars[naccth2 + t - 1];
 
-            fg[1 + nx2 + t - 2*(K-1)] = bx1 - (bx0 + bv0 * CppAD::cos(bth0) * dt);
-            fg[1 + ny2 + t - 2*(K-1)] = by1 - (by0 + bv0 * CppAD::sin(bth0) * dt);
-            fg[1 + nth2 + t - 2*(K-1)] = bth1 - (bth0 + bvth0 * dt);
-            fg[1 + nv2 + t - 2*(K-1)] = bv1 - (bv0 + bacc0 * dt);
-            fg[1 + nvth2 + t - 2*(K-1)] = bvth1 - (bvth0 + baccth0 * dt);
+            fg[1 + nx2 + t - 2 * (K - 1)] = bx1 - (bx0 + (bv0 + 0.5 * bacc0 * dt) * CppAD::cos(bth0 + 0.5 * bvth0 * dt) * dt);
+            fg[1 + ny2 + t - 2 * (K - 1)] = by1 - (by0 + (bv0 + 0.5 * bacc0 * dt) * CppAD::sin(bth0 + 0.5 * bvth0 * dt) * dt);
+            fg[1 + nth2 + t - 2 * (K - 1)] = bth1 - (bth0 + (bvth0 + 0.5 * baccth0 * dt) * dt);
+            fg[1 + nv2 + t - 2 * (K - 1)] = bv1 - (bv0 + bacc0 * dt);
+            fg[1 + nvth2 + t - 2 * (K - 1)] = bvth1 - (bvth0 + baccth0 * dt);
         }
     }
 };
@@ -216,13 +216,13 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
     // State vector holds all current values neede for vars below
 
     // std::cout<<"state: "<<state<<std::endl;
+
     double v1 = state[0];
     double v2 = state[1];
     double vth1 = state[2];
     double vth2 = state[3];
-    double ref_v = state[4];
+    // double ref_v = state[4];
     double dt = state[5];
-
     // double dx = state[6]; /// 两车初始位姿差   前车相对后车坐标系
     // double dy = state[7];
     // double dth = state[8]; ///两坐标系角度
@@ -235,14 +235,14 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
     {
         vars[i] = 0.0;
     }
-    for (int i = 0; i < K; ++i)
-    {
-        vars[nv1 + i] = v1;
-        vars[nv2 + i] = v2;
+    // for (int i = 0; i < 1; ++i)
+    // {
+    //     vars[nv1 + i] = v1;
+    //     vars[nv2 + i] = v2;
 
-        vars[nvth1 + i] = vth1;
-        vars[nvth2 + i] = vth2;
-    }
+    //     vars[nvth1 + i] = vth1;
+    //     vars[nvth2 + i] = vth2;
+    // }
 
     Dvector vars_lowerbound(n_vars);
     Dvector vars_upperbound(n_vars);
@@ -273,13 +273,16 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
     {
         //// limit acc1 acc2
         vars_lowerbound[nacc1 + i] = -5.0;
+        vars_upperbound[nacc1 + i] = 5.0;
+        vars_lowerbound[nacc2 + i] = -5.0;
         vars_upperbound[nacc2 + i] = 5.0;
 
         //// limit accth1 accth2
-        vars_lowerbound[naccth1 + i] = -0.05;//3度
-        vars_upperbound[naccth2 + i] = 0.05;
+        vars_lowerbound[naccth1 + i] = -0.05; //3度
+        vars_upperbound[naccth1 + i] = 0.05; //3度
+        vars_lowerbound[naccth2 + i] = -0.05;
+        vars_upperbound[naccth2 + i] = 0.05; //3度
     }
-    
 
     ////   constraints
     Dvector constraints_lowerbound(n_constraints);
@@ -293,20 +296,32 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
     for (int i = 0; i < K; i++)
     {
         ////acc v1 v2
-        constraints_lowerbound[nv1 + i] = -3.0;
-        constraints_upperbound[nv1 + i] = 3.0;
-        constraints_lowerbound[nv2 + i - 2*(K-1)] = -3.0;
-        constraints_upperbound[nv2 + i - 2*(K-1)] = 3.0;
+        // constraints_lowerbound[nv1 + i] = -0.5;
+        // constraints_upperbound[nv1 + i] = 0.5;
+        // constraints_lowerbound[nv2 + i - 2 * (K - 1)] = -0.5;
+        // constraints_upperbound[nv2 + i - 2 * (K - 1)] = 0.5;
 
-        //acc th 1 2
-        constraints_lowerbound[nvth1 + i] = -0.1;
-        constraints_upperbound[nvth1 + i] = 0.1;
-        constraints_lowerbound[nvth2 + i - 2*(K-1)] = -0.1;
-        constraints_upperbound[nvth2 + i - 2*(K-1)] = 0.1;
+        // //acc th 1 2
+        // constraints_lowerbound[nvth1 + i] = -0.05;
+        // constraints_upperbound[nvth1 + i] = 0.05;
+        // constraints_lowerbound[nvth2 + i - 2 * (K - 1)] = -0.05;
+        // constraints_upperbound[nvth2 + i - 2 * (K - 1)] = 0.05;
 
         // constraints_lowerbound[relative_pose_contrains + i - 4*(K-1)] = min_dist;
         // constraints_upperbound[relative_pose_contrains + i - 4*(K-1)] = max_dist;
     }
+    //acc v1 v2
+    constraints_lowerbound[nv1] = v1;// + vars_lowerbound[nacc1] * dt;
+    constraints_upperbound[nv1] = v1;// + vars_upperbound[nacc1] * dt;
+    constraints_lowerbound[nv2 - 2 * (K - 1)] = v2;// + vars_lowerbound[nacc2] * dt;
+    constraints_upperbound[nv2 - 2 * (K - 1)] = v2;// + vars_upperbound[nacc2] * dt;
+
+    //acc th 1 2
+    constraints_lowerbound[nvth1] = vth1;// + vars_lowerbound[naccth1] * dt;
+    constraints_upperbound[nvth1] = vth1;// + vars_upperbound[naccth1] * dt;
+    constraints_lowerbound[nvth2 - 2 * (K - 1)] = vth2;// + vars_lowerbound[naccth2] * dt;
+    constraints_upperbound[nvth2 - 2 * (K - 1)] = vth2;// + vars_upperbound[naccth2] * dt;
+    // std::cout<<"state:"<<state<<std::endl;
 
     // object that computes objective and constraintsv
     FG_eval_2 fg_eval(coeffs, coeffs_b, state);
@@ -333,10 +348,10 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
     out.clear();
     m_trajectory_f.clear();
     m_trajectory_b.clear();
-    out.push_back(solution.x[nv1]);
-    out.push_back(solution.x[nvth1]);
-    out.push_back(solution.x[nv2]);
-    out.push_back(solution.x[nvth2]);
+    out.push_back(solution.x[nv1+1]);
+    out.push_back(solution.x[nvth1+1]);
+    out.push_back(solution.x[nv2+1]);
+    out.push_back(solution.x[nvth2+1]);
     for (int i = 0; i < K; ++i)
     {
         tf::Pose pose1, pose2;
@@ -349,6 +364,9 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
     // printf("Cost : %.3f ref:%.3f  cv : %.3f  cvth : %.3f  ||  v2:%.3f  th2:%.3f\n",solution.obj_value,
     //     ref_v,v2,vth2,solution.x[nv2],solution.x[nvth2]);
 
+    printf("(参考速度: %.3f) 前车速度: %.3f  后车速度: %.3f \n",
+           state[4], solution.x[nv1], solution.x[nv2]);
+
     return ok;
 }
 

MPC/src/node2.cpp

@@ -34,6 +34,7 @@ int g_direction=1;      //朝向  向前为0  先后为1
 FourthTrajectory maker;
 mpc_tools mpc_tool;
 double g_max_vx;
+double g_ref_v;
 
 // 目标设置回调
 void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
@@ -213,6 +214,13 @@ bool MPC()
 
     tf::Vector3 length(dx, dy, 0);
     double direct_x = dx * cos(-theta) - dy * sin(-theta);
+    // 接近目标点减速
+    // if(length.length() < 1){
+    //     g_ref_v *= 0.5;
+    // }else{
+    //     g_ref_v = g_max_vx;
+    // }
+    // double ref_v = g_ref_v;
     double ref_v = length.length() / (g_max_vx * 1.5);
     ref_v = std::max(ref_v, 0.02);
     ref_v = std::min(ref_v, g_max_vx);
@@ -315,6 +323,7 @@ bool MPC()
         g_cmd_vel_f.linear.x,g_cmd_vel_f.angular.z,g_cmd_vel_b.linear.x,g_cmd_vel_b.angular.z);*/
 
     state << vx1, vx2, vth1, vth2, ref_v, dt, dx1, dy1, dth1;
+    // state << 0, 0, 0, 0, ref_v, dt, dx1, dy1, dth1;
     TowBotMPC mpc;
     std::vector<double> out;
     if (mpc.Solve(state, g_max_vx, coeffs, coeffs_b, out)) {

mpc_control/src/launch/default.rviz

@@ -9,7 +9,7 @@ Panels:
         - /Path2
         - /Axes1
       Splitter Ratio: 0.5
-    Tree Height: 390
+    Tree Height: 359
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -210,7 +210,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 7.42834616
+      Distance: 10.2860098
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1

mpc_control/src/mpc_two_vehicle/mpc_particle_controller_node.cpp

@@ -104,10 +104,13 @@ void mpc_particle_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_
   if (filterPath(m_current_pose, t_poses, t_partial_poses, 5, target_direction.dot(orient) > 0))
   {
     // 终点附近限制速度
-    if (target_direction.length() < 1)
+    if (target_direction.length() < 5.0)
     {
-      m_ref_vel *= target_direction.length();
-      m_ref_vel = fmax(fabs(m_ref_vel), 0.04);
+      m_ref_vel = target_direction.length() / (2.0 * 1.0);
+      m_ref_vel = std::max(m_ref_vel, 0.02);
+      m_ref_vel = std::min(m_ref_vel, 2.0);
+      // m_ref_vel *= target_direction.length();
+      // m_ref_vel = fmax(fabs(m_ref_vel), 0.04);
       if (target_direction.length() < 0.03)
       {
         m_ref_vel = 0;

mpc_control/src/mpc_two_vehicle/mpc_particle_model.cpp

@@ -269,16 +269,16 @@ bool Mpc_particle_model::Solve(double ref_velocity, std::vector<tf::Pose> ref_po
     constraints_upperbound[i] = 0;
   }
   // v
-  for (int i = v_start; i < v_start + N; i++)
+  for (int i = v_start; i < v_start + 1; i++)
   {
-    constraints_lowerbound[i] = -max_vel;
-    constraints_upperbound[i] = max_vel;
+    constraints_lowerbound[i] = v-0.1;
+    constraints_upperbound[i] = v+0.1;
   }
   // vth
-  for (int i = vth_start; i < vth_start + N; i++)
+  for (int i = vth_start; i < vth_start + 1; i++)
   {
-    constraints_lowerbound[i] = -M_PI;
-    constraints_upperbound[i] = M_PI;
+    constraints_lowerbound[i] = vth;
+    constraints_upperbound[i] = vth;
   }
 
   double theta_diff = curr_pose.getRotation().getAngle() - m_last_pose.getRotation().getAngle();