1.优化MPC旋转调整输入

MPC/rotate_mpc.cpp

@@ -68,13 +68,13 @@ public:
             fg[n_angular + t] = theta1 - (theta0 + angular_velocity0 * dt);
         }
 
-        /// 角加速度不等式约束 N+1~2N
+        /// 角加速度等式约束 N+1~2N
         fg[n_angular_acceleration] = (vars[0 + N] - angular_velocity) / dt;
         for (int t = 1; t < N; ++t) {
             fg[n_angular_acceleration + t] = (vars[t + N] - vars[t + N - 1]) / dt;
         }
 
-        /// 障碍物距离不等式约束 2N+1~3N
+        /// 障碍物距离等式约束 2N+1~3N
         if (m_statu.size() == 1 + 3) {
             float obs_x = m_statu[1];
             float obs_y = m_statu[2];
@@ -125,6 +125,7 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
     double max_angular_acceleration_velocity = 3.0 / 180 * M_PI;//mpc_param.acc_angular; //
     /// 调整输入
     if (angular_velocity > max_angular_velocity) angular_velocity = max_angular_velocity;
+    if (angular_velocity < -max_angular_velocity) angular_velocity = -max_angular_velocity;
 
     // 优化
     typedef CPPAD_TESTVECTOR(double) Dvector;