1.修复旋转时卡顿问题

2.限制下发角速度的最小角速度
3.修复整车入库时，位置在车库2号点问题
4.优化巡线MPC检测障碍物逻辑，缩短计算用时
5.障碍物检测框长宽各增加10cm

MPC/loaded_mpc.cpp

@@ -7,7 +7,7 @@
 #include <cppad/cppad.hpp>
 #include <cppad/ipopt/solve.hpp>
 
-size_t N = 10;                  //优化考虑后面多少步
+size_t N = 15;                  //优化考虑后面多少步
 
 size_t nx = 0;
 size_t ny = nx + N;
@@ -112,8 +112,8 @@ public:
             Pose2d obs_relative_pose(obs_x, obs_y, obs_theta);
             std::vector<Pose2d> obs_vertexes = Pose2d::generate_rectangle_vertexs(obs_relative_pose, obs_w * 2,
                                                                                   obs_h * 2);
-            for (auto t = 0; t < N; ++t) {
-                CppAD::AD<double> min_distance = 1e19;
+            for (auto t = 0; t < N/2; ++t) {
+                CppAD::AD<double> min_distance = 1e19-1;
                 for (auto i = 0; i < 8; ++i) {
                     CppAD::AD<double> ra = (obs_vertexes[i].x() - vars[nx + t]) * CppAD::cos(vars[nth + t]) -
                                            (obs_vertexes[i].y() - vars[ny + t]) * CppAD::sin(vars[nth + t]);
@@ -122,7 +122,7 @@ public:
                     CppAD::AD<double> distance = CppAD::pow(ra / obs_w, 8) + CppAD::pow(rb / obs_h, 8);
                     if (distance < min_distance) min_distance = distance;
                 }
-                fg[nobs + t] = min_distance;
+                fg[1 + nobs + t] = min_distance;
             }
 //            std::cout << " fg[n_obs]: " << fg[n_obs] << " | __LINE__:" << __LINE__ << std::endl;
         }
@@ -246,7 +246,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
     float distance = Pose2d::distance(obs_relative_pose_, Pose2d(0, 0, 0));
     bool find_obs = distance < 10;
     if (find_obs) printf(" find obs ..............................\n");
-    size_t n_constraints = 5 * N + find_obs * N;
+    size_t n_constraints = 5 * N + find_obs * N/2;
 
     Dvector constraints_lowerbound(n_constraints);
     Dvector constraints_upperbound(n_constraints);
@@ -267,7 +267,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
 
     // 与障碍物保持距离的约束
     if (find_obs) {
-        for (int i = nobs; i < nobs + N; ++i) {
+        for (int i = nobs; i < nobs + N/2; ++i) {
             constraints_lowerbound[i] = 1.0;
             constraints_upperbound[i] = 1e19;
         }

MPC/navigation.cpp

@@ -99,14 +99,14 @@ bool Navigation::Init(const NavParameter::Navigation_parameter &parameter) {
         if (brotherEmqx_ == nullptr) {
             brotherEmqx_ = new Terminator_emqx(brotherEmqx.nodeid());
 
-            if (brotherEmqx_->Connect(brotherEmqx.ip(), brotherEmqx.port()) == true) {
-                brotherEmqx_->AddCallback(brotherEmqx.subbrotherstatutopic(), Navigation::BrotherAgvStatuCallback,
-                                          this);
-            } else {
-                printf(" brother emqx connected failed\n");
-                // return false;
-            }
-
+//            if (brotherEmqx_->Connect(brotherEmqx.ip(), brotherEmqx.port()) == true) {
+//                brotherEmqx_->AddCallback(brotherEmqx.subbrotherstatutopic(), Navigation::BrotherAgvStatuCallback,
+//                                          this);
+//            } else {
+//                printf(" brother emqx connected failed\n");
+//                // return false;
+//            }
+//
         }
     }
     inited_ = true;
@@ -200,7 +200,7 @@ void Navigation::publish_statu(NavMessage::NavStatu &statu) {
             pose->set_theta(pt.theta());
         }
     }
-    //std::cout<<"nav statu:"<<statu.DebugString()<<std::endl;
+//    std::cout<<"nav statu:"<<statu.DebugString()<<std::endl;
 
     MqttMsg msg;
     msg.fromProtoMessage(statu);
@@ -609,8 +609,14 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
         rotated.mutable_theta() = space.theta();
         rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
     }
-    double x = space.x() - wheelbase / 2 * cos(rotated.theta());
-    double y = space.y() - wheelbase / 2 * sin(rotated.theta());
+
+    double x = space.x();
+    double y = space.y();
+    if (move_mode_ == eSingle) {
+        x -= wheelbase / 2 * cos(rotated.theta());
+        y -= wheelbase / 2 * sin(rotated.theta());
+        printf("确定车位点：eSingle\n");
+    }
     target.set_x(x);
     target.set_y(y);
     target.set_theta(rotated.theta());
@@ -769,13 +775,16 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
     statu[4] = angular_velocity;
 
     NavParameter::MPC_parameter mpc_parameter = parameter_.x_mpc_parameter();
-    double obs_w = 2;//0.85;
-    double obs_h = 3;//1.45;
+    double obs_w = 0.95;//0.85;
+    double obs_h = 1.55;//1.45;
 
     MpcError ret = failed;
     Pose2d brother = timedBrotherPose_.Get();
     Pose2d brother_in_self = Pose2d::relativePose(brother, pose);
 //    std::cout<<" brother_in_self: "<<brother_in_self<<std::endl;
+    if (move_mode_ == eDouble) {
+        brother_in_self = Pose2d(pose.x()+100,pose.y()+100,0);
+    }
     RotateMPC MPC(brother_in_self, obs_w, obs_h, limit_wmg.min, limit_wmg.max);
     RotateMPC::MPC_parameter parameter = {mpc_parameter.shortest_radius(), mpc_parameter.dt(),
                                           mpc_parameter.acc_velocity(), mpc_parameter.acc_angular()};
@@ -784,6 +793,10 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
         printf(" Rotation_mpc solve no solution set v/w = 0\n");
         out.clear();
         out.push_back(0);
+    } else
+    {
+        double min_angular_velocity = 3.0/180*M_PI;
+        if (out[0] < min_angular_velocity) out[0] = min_angular_velocity;
     }
     return ret == success || ret == no_solution;
 }
@@ -817,8 +830,8 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
     Trajectory optimize_trajectory;
     Trajectory selected_trajectory;
     NavParameter::MPC_parameter mpc_parameter = parameter_.x_mpc_parameter();
-    double obs_w = 0.85;
-    double obs_h = 1.45;
+    double obs_w = 0.95;//0.85;
+    double obs_h = 1.55;//1.45;
 
     if (directY == true) {
         //将车身朝向旋转90°，车身朝向在(-pi,pi]
@@ -826,8 +839,8 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
         if (statu[2] > M_PI)
             statu[2] -= 2 * M_PI;
         mpc_parameter = parameter_.y_mpc_parameter();
-        obs_w = 1.45;
-        obs_h = 0.85;
+        obs_w = 0.95;//0.85;
+        obs_h = 1.55;//1.45;
     }
 
     Pose2d brother = timedBrotherPose_.Get();
@@ -979,7 +992,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
     }
     printf(" exec along autoDirect:%d\n", autoDirect);
     while (cancel_ == false) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+        std::this_thread::sleep_for(std::chrono::milliseconds(30));
         if (pause_ == true) {
             //发送暂停消息
             continue;
@@ -1024,7 +1037,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
                 out[0] = -limit_v.min;
         }
         //下发速度
-        printf("  nav  input :%f   out:%f\n",timedV_.Get(),out[0]);
+        //printf("  nav  input :%f   out:%f\n",timedV_.Get(),out[0]);
         if (directY == false)
             SendMoveCmd(move_mode_, eVertical, out[0], out[1]);
         else

MPC/navigation_main.cpp

@@ -153,13 +153,20 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
     target.set_id(space.id());
     //后车先到,当前车进入2点,保持与后车一致的朝向
     if (brother.in_space() && brother.space_id() == space.id()) {
+        printf("RotateBeforeEnterSpace | 后车先到, __LINE__ = %d\n", __LINE__);
         rotated.mutable_theta() = brother.odom().theta();
     } else { //当前车先到,正向
+        printf("RotateBeforeEnterSpace | 该车先到, __LINE__ = %d\n", __LINE__);
         rotated.mutable_theta() = space.theta();
     }
     std::cout << "===============================> RotateBeforeEnterSpace ,target:" << rotated << std::endl;
-    double x = space.x() + wheelbase / 2.0 * cos(rotated.theta());
-    double y = space.y() + wheelbase / 2.0 * sin(rotated.theta());
+    double x = space.x();
+    double y = space.y();
+    if (move_mode_ == eSingle) {
+        x += wheelbase / 2 * cos(rotated.theta());
+        y += wheelbase / 2 * sin(rotated.theta());
+        printf("确定车位点：eSingle\n");
+    }
     target.set_x(x);
     target.set_y(y);
     target.set_theta(rotated.theta());

MPC/rotate_mpc.cpp

@@ -7,7 +7,7 @@
 #include <cppad/cppad.hpp>
 #include <cppad/ipopt/solve.hpp>
 
-size_t mpc_rotate_steps = 25;                  //优化考虑后面多少步
+size_t mpc_rotate_steps = 20;                  //优化考虑后面多少步
 #define N mpc_rotate_steps
 
 class FG_eval_rotate {
@@ -123,6 +123,8 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
     double max_angular_velocity = 6.0 / 180 * M_PI;
     double min_angular_velocity = 0.1 / 180 * M_PI;
     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;
 
     // 优化
     typedef CPPAD_TESTVECTOR(double) Dvector;
@@ -175,10 +177,10 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
         }
 
     // fg_eval for IPOPT solver
-    size_t n_fg_status = 1 + 3*find_obs;
+    size_t n_fg_status = 1 + 3 * find_obs;
     Eigen::VectorXd fg_status(n_fg_status);
     fg_status[0] = angular_velocity;
-    if (find_obs){
+    if (find_obs) {
         fg_status[1] = obs_relative_pose_.x();
         fg_status[2] = obs_relative_pose_.y();
         fg_status[3] = obs_relative_pose_.theta();
@@ -188,8 +190,12 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 
     Pose2d targetAngularInAGV = current_to_target;
     /// 调整到适合的目标角度
-    if (targetAngularInAGV.theta() > max_angular_velocity * N * dt)
-        targetAngularInAGV.set_theta(max_angular_velocity * N * dt);
+    double acc_time = (max_angular_velocity - angular_velocity) / max_angular_acceleration_velocity;
+    double max_angular_once = max_angular_velocity * N * dt - acc_time * (max_angular_velocity - angular_velocity);
+    if (targetAngularInAGV.theta() > max_angular_once ) {
+        targetAngularInAGV.set_theta(max_angular_once);
+    }
+
 
     Eigen::VectorXd condition(4);
     condition << dt, targetAngularInAGV.theta(), obs_w_, obs_h_;