修改限制速度 标准差

MPC/loaded_mpc.cpp

@@ -150,7 +150,7 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
 
   //纠正角速度/线速度，使其满足最小转弯半径
   double angular=wmg;
-  double radius=25.0*1.3 * (1.0/sqrt(line_velocity*line_velocity+1e-10));
+  double radius=20.0*1.3 * (1.0/sqrt(line_velocity*line_velocity+1e-10));
   if( (line_velocity*line_velocity)/(wmg*wmg+1e-9) < (radius*radius)) {
     angular = fabs(line_velocity) / radius;
     if (wmg < 0) angular = -angular;
@@ -287,7 +287,7 @@ bool LoadedMPC::solve(const Trajectory& trajectory, const Pose2d& target,Eigen::
     printf(" input -dlt limited\n");
   }
 
-  statu_velocity << line_velocity, -angular,obs_relative_pose_.x(),obs_relative_pose_.y();
+  statu_velocity << line_velocity, angular,obs_relative_pose_.x(),obs_relative_pose_.y();
 
   Eigen::VectorXd condition(2);
   condition << dt, ref_velocity;

MPC/navigation.cpp

@@ -32,7 +32,7 @@ Navigation::~Navigation()
  */
 double limit(double x,double min,double max){
   double r=x>=0?1.0:-1.0;
-  double delta=max/3.0;
+  double delta=max/1.0;
   return (max-(max-min)*exp(-x*x/(delta*delta)))*r;
 
 }