two bot mpc can successfully steer at the target.

src/MPC/src/mpc/TowBotMPC.cpp

@@ -73,14 +73,14 @@ public:
         double target_dy = m_data[10];
         double slowdown_dist = m_data[11];
 
-        const int brake_weight = 200;
+        const int brake_weight = 5000;
         ///前车 cost weight
         const int y_cost_weight = 15000;
         const int th_cost_weight = 1000;
         const int v_cost_weight = 20;
 
         const int vth_cost_weight = 100;
-        const int a_cost_weight = 1;
+        const int a_cost_weight = 10;
         const int ath_cost_weight = 10;
 
         fg[0] = 0;
@@ -90,21 +90,21 @@ public:
         const int back_v_cost_weight = 20;
 
         const int back_vth_cost_weight = 100;
-        const int back_a_cost_weight = 1;
+        const int back_a_cost_weight = 10;
         const int back_ath_cost_weight = 10;
 
         // x, y, theta 路径约束
         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 + nv1] = vars[nv1] - v1;
+        fg[1 + nvth1] = vars[nvth1] - vth1;
 
         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 + nv2 - 2*(K-1)] = vars[nv2] - v2;
+        fg[1 + nvth2 - 2*(K-1)] = vars[nvth2] - vth2;
 
         AD<double> accumulate_fx = vars[nx1];
         AD<double> accumulate_fy = vars[ny1];
@@ -173,7 +173,9 @@ public:
         // 预测末端与目标点偏移
         // AD<double> final_dist = CppAD::sqrt(final_dx + final_dy);
 
-        // 减速模式权重,越接近目标,该权重越大,最终减速到达目标
+        // 控制 当前距离/减速距离 值(0,2),越接近目标,越趋近0
+        double slowdown_ratio = 2.0 / (1.0 + exp(10.0 * sqrt(current_dist / pow(slowdown_dist, 2)) - 5.0));
+        // 减速模式权重(0,1),越接近目标ratio越小,该权重越大,最终减速到达目标
         double slowdown_weight = 1.0 / (1.0 + exp(10.0 * sqrt(current_dist) - 5.0));
         // char disp[255];
         // memset(disp, 0, 255);
@@ -218,7 +220,7 @@ public:
         }
 
         ///添加两车距离损失
-        const int distance_weight = 800;
+        const int distance_weight = 5000;
         const int delta_th_weight = 10;
         char buf[255]={0};
         AD<double> dtheta = vars[nth1] - vars[nth2] + dth;
@@ -288,6 +290,9 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
         vars_upperbound[i] = 1.0e19;
     }
 
+    double max_vth = 0.15;
+    double max_acc = 2.5;
+    double max_accth = 0.5;
     for (int i = 0; i < K; i++)
     {
         //// limit  v1 v2
@@ -298,27 +303,27 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
         vars_upperbound[nv2 + i] = max_v;
 
         //// limit  vth1 2
-        vars_lowerbound[nvth1 + i] = -0.25;
-        vars_upperbound[nvth1 + i] = 0.25;
+        vars_lowerbound[nvth1 + i] = -max_vth;
+        vars_upperbound[nvth1 + i] = max_vth;
 
-        vars_lowerbound[nvth2 + i] = -0.25;
-        vars_upperbound[nvth2 + i] = 0.25;
+        vars_lowerbound[nvth2 + i] = -max_vth;
+        vars_upperbound[nvth2 + i] = max_vth;
     }
     for (int i = 0; i < K - 1; i++)
     {
         //// limit acc1 acc2
-        vars_lowerbound[nacc1 + i] = -1.0;
-        vars_upperbound[nacc1 + i] = 1.0;
+        vars_lowerbound[nacc1 + i] = -max_acc;
+        vars_upperbound[nacc1 + i] = max_acc;
 
-        vars_lowerbound[nacc2 + i] = -1.0;
-        vars_upperbound[nacc2 + i] = 1.0;
+        vars_lowerbound[nacc2 + i] = -max_acc;
+        vars_upperbound[nacc2 + i] = max_acc;
 
         //// limit accth1 accth2
-        vars_lowerbound[naccth1 + i] = -0.05;//3度
-        vars_upperbound[naccth1 + i] = 0.05;
+        vars_lowerbound[naccth1 + i] = -max_accth;//3度
+        vars_upperbound[naccth1 + i] = max_accth;
 
-        vars_lowerbound[naccth2 + i] = -0.05;//3度
-        vars_upperbound[naccth2 + i] = 0.05;
+        vars_lowerbound[naccth2 + i] = -max_accth;//3度
+        vars_upperbound[naccth2 + i] = max_accth;
     }
     
 
@@ -331,23 +336,24 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
         constraints_upperbound[i] = 0;
     }
 
-    for (int i = 0; i < 1; 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;
-
-        //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;
-
-        // constraints_lowerbound[relative_pose_contrains + i - 4*(K-1)] = min_dist;
-        // constraints_upperbound[relative_pose_contrains + i - 4*(K-1)] = max_dist;
-    }
+    // for (int i = 0; i < 1; 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;
+
+    //     //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;
+
+    //     // 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-0.1;
     // constraints_upperbound[nv1]=v1+0.1;
@@ -396,10 +402,10 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
     // {
     //     speed2=speed2>0?0.1:-0.1;
     // }
-    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;
@@ -415,7 +421,7 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
         double interval = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - last_time).count() * 0.001;
         double acc = (solution.x[nv1] - velo) / dt;
         // printf("is ok? %d, cost: %.3f\n", ok, cost);
-        printf("Cost: %.3f\tv1:%.3f\tvth1:%.3f\t||\tnv1:%.3f\tnvth1:%.3f\tnacc1:%.3f\tinterval:%.3f\n", solution.obj_value, v1, vth1, solution.x[nv1], solution.x[nvth1], acc, interval);
+        printf("Cost: %.3f v1:%.3f vth1:%.3f || nv1:%.3f nvth1:%.3f nacc1:%.3f interval:%.3f\n", solution.obj_value, v1, vth1, solution.x[nv1], solution.x[nvth1], acc, interval);
         
         // printf("Cost : %.3f\tref:%.3f\tcv : %.3f\tcvth : %.3f\t||\tv2:%.3f\tth2:%.3f\n",solution.obj_value,
         //     ref_v,v2,vth2,solution.x[nv2],solution.x[nvth2]);