1.旋转MPC提速

2.优化判断无法到达目标点时，调整到邻近点
3.驶离出入口旋转180添加日志
4.MPC失败时下发获取的速度

MPC/loaded_mpc.cpp

@@ -221,7 +221,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
 
     //printf("min_v:%f   max_v:%f\n",min_velocity_,max_velocity_);
     double max_dlt = max_wmg;//5*M_PI/180.0;
-    double acc_sigmoid= (max_velocity_+1.3)/(1+exp(-4.*(fabs(line_velocity)-0.7)))+0.3;
+    double acc_sigmoid= (max_velocity_+1.15)/(1+exp(-4.*(fabs(line_velocity)-0.7)))+0.45;
     double max_acc_line_velocity = mpc_param.acc_velocity*acc_sigmoid;
     double max_acc_dlt = mpc_param.acc_angular;
 

MPC/navigation.cpp

@@ -666,6 +666,21 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
         ////////////////////////////////////////////////////////////
         for (int i = 0; i < action.pathnodes_size(); ++i) {
             NavMessage::PathNode node = action.pathnodes(i);
+
+            if (i == 0) {
+                bool x_enable = PossibleToTarget(node, false, 1);
+                bool y_enable = PossibleToTarget(node, true, 1);
+                if (x_enable == false && y_enable == false) {
+                    stLimit l_w = {2 * M_PI / 180.0, 20 * M_PI / 180.0};
+                    int directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
+                    bool adjustRet = AdjustReferToTarget(Pose2d(node.x(), node.y(), 0),
+                                                         l_w, directions);
+                    if (adjustRet == false) {
+                        return false;
+                    }
+                }
+            }
+
             if (i < action.pathnodes_size() - 1) {
                 NavMessage::PathNode next = action.pathnodes(i + 1);
                 Pose2d vec(next.x() - node.x(), next.y() - node.y(), 0);
@@ -677,7 +692,7 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
                 Pose2d vec(node.x() - befor.x(), node.y() - befor.y(), 0);
                 node.set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
                 node.set_l(0.02);
-                node.set_w(0.25);
+                node.set_w(0.2);
             }
             int directions = Direction::eForward;
             if (anyDirect)
@@ -711,13 +726,13 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
             }*/
 
             //提升机构提前动作
-            if (GetRemainActionsCount() > 1){
+            if (GetRemainActionsCount() > 1) {
                 NavMessage::NewAction last_action = GetLastAction();
-                if (IsUperSpace(last_action.spacenode())){
-                    if ((i+1) == action.pathnodes_size()){
+                if (IsUperSpace(last_action.spacenode())) {
+                    if ((i + 1) == action.pathnodes_size()) {
                         clampLifterAction |= eByteLifterUp;
                     }
-                } else{
+                } else {
                     clampLifterAction |= eByteLifterDown;
                 }
             }
@@ -796,7 +811,18 @@ Navigation::MpcResult Navigation::RotateAfterOutExport() {
     double diff1 = fabs(M_PI / 2.0 - init.theta());
     double diff2 = fabs(-M_PI / 2.0 - init.theta());
     double target = diff1 > diff2 ? M_PI / 2.0 : -M_PI / 2.0;
-    stLimit limit_rotate = {3 * M_PI / 180.0, 15 * M_PI / 180.0};
+    stLimit limit_rotate = {parameter_.r_mpc_parameter().min(), parameter_.r_mpc_parameter().max()};
+
+    //文件log
+    std::ofstream ofs;  //创建流对象
+    std::string log_file_dir = "../data/";
+    time_t t = time(0);
+    char tmp[32] = {NULL};
+    strftime(tmp, sizeof(tmp), "%Y_%m_%d_%H_%M_%S_Rotate.txt", localtime(&t));
+    std::string log_file_name = tmp;
+    ofs.open(log_file_dir + log_file_name, std::ios::app);  //打开的地址和方式
+    auto beg_time = std::chrono::steady_clock::now();
+
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(80));
         Pose2d current = timedPose_.Get();
@@ -810,13 +836,31 @@ Navigation::MpcResult Navigation::RotateAfterOutExport() {
         }, "Rotation_mpc_once");
         if (ret == false) {
             Stop();
+            ofs.close();
             return eMpcFailed;
         }
 
+        std::string log_inf_line = "";
+        auto cur_time = std::chrono::steady_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(cur_time - beg_time);
+        double time =
+                double(duration.count()) * std::chrono::microseconds::period::num /
+                std::chrono::microseconds::period::den;
+        log_inf_line += std::to_string(time);
+
+        log_inf_line += " ";
+        log_inf_line += std::to_string(timedA_.Get());
+        log_inf_line += " ";
+        log_inf_line += std::to_string(out[0]);
+
+        ofs << log_inf_line << std::endl;  //一行日志的内容
+
+
         //下发速度
         if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
+            ofs.close();
             return eMpcSuccess;
         } else {
             const int down_count = 3;
@@ -829,6 +873,7 @@ Navigation::MpcResult Navigation::RotateAfterOutExport() {
         }
         continue;
     }
+    ofs.close();
     return eMpcFailed;
 }
 
@@ -935,7 +980,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
 
 
     double angular_pericision = 0.5;
-    if(GetSequenceType() == eSequencePick && TargetIsExport(space)){
+    if (GetSequenceType() == eSequencePick && TargetIsExport(space)) {
         angular_pericision = 5;
     }
     while (cancel_ == false) {
@@ -1031,6 +1076,22 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
     stLimit limit_w = {2 * M_PI / 180.0, 20 * M_PI / 180.0};
     //入库,起点为streetNode
     if (action.type() == 1) {
+        if (TargetIsExport(action.spacenode()) && GetSequenceType() == eSequencePick) {
+
+        } else {
+            Pose2d targetInCurrent = Pose2d::relativePose(Pose2d(action.streetnode().x(),
+                                                                 action.streetnode().y(), 0), timedPose_.Get());
+            if (fabs(targetInCurrent.x()) < 0.5 && fabs(targetInCurrent.y()) < 0.5) {
+                int directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
+                bool adjustRet = AdjustReferToTarget(Pose2d(action.streetnode().x(), action.streetnode().y(), 0),
+                                                     limit_w, directions);
+                if (adjustRet == false) {
+                    return false;
+                }
+            }
+        }
+
+
         Pose2d vec(action.spacenode().x() - action.streetnode().x(),
                    action.spacenode().y() - action.streetnode().y(),
                    0);
@@ -1038,7 +1099,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         action.mutable_streetnode()->set_w(0.2);
         action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()) + M_PI / 2.0);
 
-        if (GetSequenceType() == eSequencePick && TargetIsExport(action.spacenode())){
+        if (GetSequenceType() == eSequencePick && TargetIsExport(action.spacenode())) {
             action.mutable_streetnode()->set_l(0.5);
             action.mutable_streetnode()->set_w(0.5);
         }
@@ -1056,6 +1117,19 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         if (move_mode_ == eSingle)
             clampLifterAction |= eByteClampHalfOpen;
 
+
+        bool x_enable = PossibleToTarget(action.streetnode(), false, 1);
+        bool y_enable = PossibleToTarget(action.streetnode(), true, 1);
+        if (x_enable == false && y_enable == false) {
+            stLimit l_w = {2 * M_PI / 180.0, 20 * M_PI / 180.0};
+            int directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
+            bool adjustRet = AdjustReferToTarget(Pose2d(action.streetnode().x(), action.streetnode().y(), 0),
+                                                 l_w, directions);
+            if (adjustRet == false) {
+                return false;
+            }
+        }
+
         if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, true, clampLifterAction) == false) {
             printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
             return false;
@@ -1174,35 +1248,32 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         if (move_mode_ == eSingle)
             clampLifterAction |= eByteClampFullyOpen;  //////// 后续再改为全打开位
         else {
-            if (GetSequenceType()== eSequencePark ){
-                if (TargetIsEntrance(action.spacenode())){
+            if (GetSequenceType() == eSequencePark) {
+                if (TargetIsEntrance(action.spacenode())) {
                     clampLifterAction |= eByteClampFullyOpen;
-                }else{
+                } else {
                     clampLifterAction |= eByteCLampClose;
                 }
 
-            } else{
-                if (TargetIsEntrance(action.spacenode())){
+            } else {
+                if (TargetIsEntrance(action.spacenode())) {
                     clampLifterAction |= eByteCLampClose;
-                }else{
+                } else {
                     clampLifterAction |= eByteClampFullyOpen;
                 }
             }
-
         }
 
 
-
         printf(" clampLifter_statu2 :%d\n", clampLifterAction);
         if (!TargetIsExport(action.spacenode()) && !TargetIsEntrance(action.spacenode()))
             limit_w = {0.01, 0.05}; //进库纠偏角速度限制
         else {
             // 整车进 出入口速度调整为与巡线一致
             if (move_mode_ == eDouble) {
-                if (GetSequenceType() == eSequencePark){
+                if (GetSequenceType() == eSequencePark) {
 
-                }
-                else if (GetSequenceType() == eSequencePick){
+                } else if (GetSequenceType() == eSequencePick) {
                     limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
                     limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
                 }
@@ -1217,10 +1288,10 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
 
     } else if (action.type() == 2) {
         //出库,起点为spaceNode
-        if (GetSequenceType() == eSequencePick && TargetIsExport(action.spacenode())){
+        if (GetSequenceType() == eSequencePick && TargetIsExport(action.spacenode())) {
             Pose2d current_AGV = timedPose_.Get();
             double new_street_x = (action.streetnode().y() - current_AGV.y()) * cos(current_AGV.theta())
-                    / sin(current_AGV.theta()) + current_AGV.x();
+                                  / sin(current_AGV.theta()) + current_AGV.x();
 
             action.mutable_spacenode()->set_x(current_AGV.x());
             action.mutable_spacenode()->set_y(current_AGV.y());
@@ -1236,7 +1307,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         Pose2d diff = Pose2d::abs(Pose2d::relativePose(Pose2d(current.x(), current.y(), 0), space));
         // 整车从出入口出库速度调整为与巡线一致
         if (move_mode_ == eDouble && (TargetIsExport(action.spacenode()) || TargetIsExport(action.spacenode()))) {
-            if (GetSequenceType() == eSequencePark){
+            if (GetSequenceType() == eSequencePark) {
                 limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
                 limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
             }
@@ -1259,7 +1330,7 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         isInSpace_ = false;
 
         // 存车流程离开入口，掉头
-        printf("SequenceType: %d\n",GetSequenceType());
+        printf("SequenceType: %d\n", GetSequenceType());
         if (GetSequenceType() == eSequencePark) {
             if (move_mode_ == eDouble && TargetIsExport(action.spacenode()) && GetSpaceId(action.spacenode()) != 1102) {
                 Pose2d current_pose = timedPose_.Get();
@@ -1304,7 +1375,7 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
     statu[3] = line_velocity;
     statu[4] = angular_velocity;
 
-    NavParameter::MPC_parameter mpc_parameter = parameter_.x_mpc_parameter();
+    NavParameter::R_mpc_parameter mpc_parameter = parameter_.r_mpc_parameter();
     double obs_w = 0.95;//0.85;
     double obs_h = 1.55;//1.45;
 
@@ -1316,8 +1387,8 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
         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()};
+    RotateMPC::MPC_parameter parameter = {0, mpc_parameter.dt(),
+                                          0, mpc_parameter.acc_angular()};
     ret = MPC.solve(current_to_target, statu, parameter, out);
     if (ret == no_solution) {
         printf(" Rotation_mpc solve no solution set v/w = 0\n");
@@ -1342,7 +1413,7 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
     return ret == success || ret == no_solution;
 }
 
-bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, int& mpc_result, bool directY) {
+bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, int &mpc_result, bool directY) {
     if (PoseTimeout() == true) {
         printf(" MPC once Error:Pose is timeout \n");
         return false;
@@ -1408,27 +1479,49 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
     mpc_result = ret;
 
     //std::cout<<" traj size %d  %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
-    if (ret == no_solution) {
-        printf(" mpc solve no solution set v/w = 0\n");
-        out.clear();
-        out.push_back(0);
-        out.push_back(0);
-        out.push_back(0);
-        out.push_back(0);
-        out.push_back(0);
-        out.push_back(0);
-    }
 
-    if (ret == unknown) {
-        printf(" mpc solve unknown set v/w = current\n");
-        double current_v = timedV_.Get(), current_w = timedA_.Get();
+    double current_v = timedV_.Get(), current_w = timedA_.Get();
+    if (ret != success) {
         out.clear();
-        out.push_back(current_v);
-        out.push_back(current_w);
-        out.push_back(current_v);
-        out.push_back(current_w);
-        out.push_back(current_v);
-        out.push_back(current_w);
+        int v_bool_flag;
+        if (current_v > 0) {
+            v_bool_flag = 1;
+        } else if (current_v < 0) {
+            v_bool_flag = -1;
+        } else {
+            v_bool_flag = 0;
+        }
+        double new_v = current_v - v_bool_flag * mpc_parameter.acc_velocity()/2 * mpc_parameter.dt();
+        if ((current_v * new_v) < 0){
+            new_v = 0;
+        }
+
+        int w_bool_flag;
+        if (current_w > 0) {
+            w_bool_flag = 1;
+        } else if (current_w < 0) {
+            w_bool_flag = -1;
+        } else {
+            w_bool_flag = 0;
+        }
+        double new_w = current_w - w_bool_flag * mpc_parameter.acc_angular() * mpc_parameter.dt();
+        if ((current_w * new_w) < 0){
+            new_w = 0;
+        }
+
+        out.push_back(new_v);
+        out.push_back(new_w);
+        out.push_back(new_v);
+        out.push_back(new_w);
+        out.push_back(new_v);
+        out.push_back(new_w);
+        if (ret == unknown) {
+            printf(" mpc solve unknown set v/w = current-acc*dt. cur_v:%f, down_v:%f\n",current_v, new_v);
+        } else if (ret == no_solution) {
+            printf(" mpc solve no solution set v/w = 0\n");
+        } else {
+            printf(" mpc solve failed set v/w = 0\n");
+        }
     }
     //
 //    diff_yaw.push_back(0);
@@ -1436,7 +1529,7 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
 
     predict_traj_.reset(optimize_trajectory, 1);
     selected_traj_.reset(selected_trajectory, 1);
-    return ret == success || ret == no_solution;
+    return ret == success;
 }
 
 bool Navigation::IsArrived(NavMessage::PathNode targetNode) {
@@ -1930,7 +2023,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
         }, "mpc_once");
 
         if (ret == false) {
-            if (mpc_result == unknown){
+            if (mpc_result == unknown) {
                 retry_count++;
                 if (retry_count > retry_count_limit) {
                     printf("MpcToTarget failed,ret:%d\n", ret);
@@ -2072,7 +2165,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
     return eMpcFailed;
 }
 
-bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY) {
+bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY, double wheel_yaw) {
     if (timedPose_.timeout()) {
         return false;
     }
@@ -2097,7 +2190,7 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
 //    }
     double W = 2.5;
     double L = 1.565;
-    double minYaw = 5 * M_PI / 180.0;
+    double minYaw = wheel_yaw * M_PI / 180.0;
     if (move_mode_ == eDouble) {
         L = 1.565 + 2.78;
     }
@@ -2107,6 +2200,13 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
         W = L;
         L = t;
     }
+
+    /*Pose2d targetInCurrent=Pose2d::relativePose(Pose2d(tx,ty,theta),current);
+    if(fabs(targetInCurrent.x())>0.1 && atan(fabs(targetInCurrent.y()/targetInCurrent.x()))>30.0*M_PI/180.0 ){
+        std::cout<<"impossible to target, target in pose:"<<targetInCurrent<<", atan >20"<<std::endl;
+        return false;
+    }*/
+
     double minR = W / 2 + L / tan(minYaw);
     //printf("l:%f,w:%f\n",l,w);
     std::vector<Pose2d> poses = Pose2d::generate_rectangle_vertexs(Pose2d(tx, ty, 0), l * 2, w * 2);
@@ -2159,7 +2259,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
     }
     //add 先检查当前点与起点的距离
     global_navCmd_ = cmd;
-    printf("SequenceType: %d\n",GetSequenceType());
+    printf("SequenceType: %d\n", GetSequenceType());
 
     for (int i = 0; i < cmd.newactions_size(); ++i)
         newUnfinished_cations_.push(cmd.newactions(i));
@@ -2215,7 +2315,8 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
                 break;
             }
         } else if (act.type() == 7) {
-            if (this->clamp_fully_open() == false) {
+            ret = clamp_fully_open();
+            if (ret == false) {
                 printf("打开夹持 failed...\n");
                 break;
             }
@@ -2521,11 +2622,10 @@ NavMessage::NewAction Navigation::GetLastAction() {
 }
 
 int Navigation::GetSequenceType() {
-    if (global_navCmd_.has_sequence()){
+    if (global_navCmd_.has_sequence()) {
         return global_navCmd_.sequence();
 
-    }
-    else{
+    } else {
         return eSequenceDefault;
     }
 }

MPC/navigation.h

@@ -158,7 +158,7 @@ protected:
 
     bool IsArrived(NavMessage::PathNode targetNode);
 
-    bool PossibleToTarget(NavMessage::PathNode targetNode, bool directY);
+    bool PossibleToTarget(NavMessage::PathNode targetNode, bool directY,double wheel_yaw=5.0);
 
     /*
      * 移动到目标点，不能到达则旋转

MPC/rotate_mpc.cpp

@@ -124,9 +124,10 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 
     /// 定义最大最小角度,角速度,角加速度(不区分方向)
     double max_angular = M_PI;
-    double max_angular_velocity = max_velocity_ / (1 + exp(-10 * fabs(current_to_target.theta()) - 0.1));
+//    double max_angular_velocity = max_velocity_ / (1 + exp(-10 * fabs(current_to_target.theta()) - 0.1));
+    double max_angular_velocity = max_velocity_ / (1 + exp(-4 * (fabs(current_to_target.theta()) - 0.57*max_velocity_)));
     double min_angular_velocity = min_velocity_ ;
-    double max_angular_acceleration_velocity = mpc_param.acc_angular;
+    double max_angular_acceleration_velocity = mpc_param.acc_angular/ (1 + exp(-7 * (fabs(current_to_target.theta()) + 0.1*mpc_param.acc_angular)));
     //double max_angular_acceleration_velocity=0.44;///(1+exp(-36.*(angular_velocity-0.15)))+0.085;
     /// 调整输入
     if (angular_velocity > max_angular_velocity) angular_velocity = max_angular_velocity;

parameter.proto

@@ -45,6 +45,14 @@ message Accuracy{
   float w=2;
 }
 
+message R_mpc_parameter
+{
+  float dt = 1;
+  float acc_angular = 2;
+  float min = 3;
+  float max = 4;
+}
+
 message Navigation_parameter
 {
   bool main_agv=1;   //是否是两车整体
@@ -58,5 +66,8 @@ message Navigation_parameter
   SpeedLimit NodeVelocityLimit=8; //马路点MPC速度限制
   SpeedLimit NodeAngularLimit=9;  //马路点原地旋转速度限制
   string rpc_ipport=10;
+  R_mpc_parameter r_mpc_parameter = 11; //整车旋转参数
 }
 
+
+