1.前中后三段角速度权重1：3：6

MPC/loaded_mpc.cpp

@@ -81,14 +81,14 @@ public:
         for (int t = 0; t < N - 1; t++) {
             //角速度，加速度，角加速度 weight loss
             fg[0] += vth_cost_weight * CppAD::pow(vars[ndlt + t], 2);
-            if(t > N / 3){//前中后三段角速度权重1：2：3
-                fg[0] += vth_cost_weight * CppAD::pow(vars[ndlt + t], 2);
+            if(t > N / 3){//前中后三段角速度权重1：3：6
+                fg[0] += 2*vth_cost_weight * CppAD::pow(vars[ndlt + t], 2);
             }
-            if(t > N / 3 * 2){//前中后三段角速度权重1：2：3
-                fg[0] += vth_cost_weight * CppAD::pow(vars[ndlt + t], 2);
+            if(t > N / 3 * 2){//前中后三段角速度权重1：3：6
+                fg[0] += 5*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[0] += ath_cost_weight * CppAD::pow(vars[ndlt + t + 1] - vars[ndlt + t], 2);
         }
 
         /////////////////////

MPC/navigation.cpp

@@ -1341,7 +1341,7 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
 
     MpcError ret = failed;
     LoadedMPC MPC(relative, obs_w, obs_h, limit_v.min, limit_v.max);
-    // 巡线最大角速度固定0.1745
+    // 巡线最大角加速度固定0.1745
     LoadedMPC::MPC_parameter parameter = {mpc_parameter.shortest_radius(), mpc_parameter.dt(),
                                           mpc_parameter.acc_velocity(), 0.1745};
     //printf(" r:%f  dt:%f acc:%f  acc_a:%f\n",mpc_parameter.shortest_radius(),