1.优化MPC曲线

2.降低摆正精度要求
3.取车进入口角度精度下调
4.优化并行上升下降 夹持送夹持
5.进出 根据流程入口速度下降
6.修复sequence判断bug

MPC/loaded_mpc.cpp

@@ -226,7 +226,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd
     double max_acc_dlt = mpc_param.acc_angular;
 
     double ref1=sqrt(0.5*dis/mpc_param.acc_velocity)+min_velocity_;
-    double ref2=2.5/(1.+exp(-2.5*(dis-1.1)))-0.15+min_velocity_;
+    double ref2=2.5/(1.+exp(-2.5*(dis-1.2)))-0.12+min_velocity_;
     double ref_velocity = std::min(ref1,ref2);//max_velocity_/(1+exp(-4.*(dis-0.8)))+min_velocity_;
 
     Pose2d targetPoseInAGV = Pose2d::relativePose(target, pose_agv);

MPC/navigation.cpp

@@ -767,12 +767,12 @@ Navigation::MpcResult Navigation::DoubleOutSpaceCorrectTheta() {
         }
 
         //下发速度
-        if (fabs(yawDiff) < 0.2 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(yawDiff) < 0.7 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" DoubleOutSpaceCorrectTheta refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
-            if (WaitClampLifterStatu(clampLifterAction) == false) {
-                return eMpcFailed;
-            }
+//            if (WaitClampLifterStatu(clampLifterAction) == false) {
+//                return eMpcFailed;
+//            }
             return eMpcSuccess;
         } else {
             const int down_count = 3;
@@ -933,6 +933,11 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
 //        }
 //    }
 
+
+    double angular_pericision = 0.5;
+    if(GetSequenceType() == eSequencePick && TargetIsExport(space)){
+        angular_pericision = 5;
+    }
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(30));
         Pose2d current_now = timedPose_.Get();
@@ -952,7 +957,7 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
         }
 
         //下发速度
-        if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(yawDiff) < angular_pericision * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
             return eMpcSuccess;
@@ -1032,6 +1037,12 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         action.mutable_streetnode()->set_l(0.02);
         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())){
+            action.mutable_streetnode()->set_l(0.5);
+            action.mutable_streetnode()->set_w(0.5);
+        }
+
         /*
          * 是否需要添加判断，距离较远才先运动到马路点=============================
          */
@@ -1162,8 +1173,25 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         printf(" clampLifter_statu1 :%d\n", clampLifterAction);
         if (move_mode_ == eSingle)
             clampLifterAction |= eByteClampFullyOpen;  //////// 后续再改为全打开位
-        else
-            clampLifterAction |= eByteCLampClose;
+        else {
+            if (GetSequenceType()== eSequencePark ){
+                if (TargetIsEntrance(action.spacenode())){
+                    clampLifterAction |= eByteClampFullyOpen;
+                }else{
+                    clampLifterAction |= eByteCLampClose;
+                }
+
+            } else{
+                if (TargetIsEntrance(action.spacenode())){
+                    clampLifterAction |= eByteCLampClose;
+                }else{
+                    clampLifterAction |= eByteClampFullyOpen;
+                }
+            }
+
+        }
+
+
 
         printf(" clampLifter_statu2 :%d\n", clampLifterAction);
         if (!TargetIsExport(action.spacenode()) && !TargetIsEntrance(action.spacenode()))
@@ -1171,8 +1199,13 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
         else {
             // 整车进 出入口速度调整为与巡线一致
             if (move_mode_ == eDouble) {
-                limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
-                limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
+                if (GetSequenceType() == eSequencePark){
+
+                }
+                else if (GetSequenceType() == eSequencePick){
+                    limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
+                    limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
+                }
             }
         }
         bool retMove = MoveToTarget(new_target, limit_v, limit_w, true, false, clampLifterAction);
@@ -1184,6 +1217,16 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
 
     } else if (action.type() == 2) {
         //出库,起点为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();
+
+            action.mutable_spacenode()->set_x(current_AGV.x());
+            action.mutable_spacenode()->set_y(current_AGV.y());
+
+            action.mutable_streetnode()->set_x(new_street_x);
+        }
         // 计算当前位置，在车位点到马路点的连线方向上，与车位点的相对位姿
         Pose2d vec(action.streetnode().x() - action.spacenode().x(),
                    action.streetnode().y() - action.spacenode().y(), 0);
@@ -1193,8 +1236,10 @@ 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()))) {
-            limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
-            limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
+            if (GetSequenceType() == eSequencePark){
+                limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
+                limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
+            }
         }
         if (fabs(diff.x()) < 0.15 || fabs(diff.y()) < 0.10) {
             //出库，移动到马路点，允许自由方向，不允许中途旋转
@@ -1419,7 +1464,7 @@ bool Navigation::IsArrived(NavMessage::PathNode targetNode) {
     double b = (x - tx) * sin(theta) + (y - ty) * cos(theta);
 
     if (pow(a / l, 8) + pow(b / w, 8) <= 1) {
-        if (fabs(timedV_.Get()) < 0.06 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(timedV_.Get()) < 0.1 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" Arrived target: %f  %f l:%f w:%f theta:%f\n", tx, ty, l, w, theta);
             return true;
         }
@@ -2114,6 +2159,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
     }
     //add 先检查当前点与起点的距离
     global_navCmd_ = cmd;
+    printf("SequenceType: %d\n",GetSequenceType());
 
     for (int i = 0; i < cmd.newactions_size(); ++i)
         newUnfinished_cations_.push(cmd.newactions(i));
@@ -2476,10 +2522,11 @@ NavMessage::NewAction Navigation::GetLastAction() {
 
 int Navigation::GetSequenceType() {
     if (global_navCmd_.has_sequence()){
-        return eSequenceDefault;
+        return global_navCmd_.sequence();
+
     }
     else{
-        return global_navCmd_.sequence();
+        return eSequenceDefault;
     }
 }
 

MPC/navigation_main.cpp

@@ -326,6 +326,11 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
         return eMpcSuccess;
     }
 
+    double angular_pericision = 0.5;
+    if(GetSequenceType() == eSequencePick && TargetIsExport(space)){
+        angular_pericision = 5;
+    }
+
     while (cancel_ == false) {
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
         Pose2d current_now = timedPose_.Get();
@@ -351,7 +356,7 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
         }
 
         //下发速度
-        if (fabs(yawDiff) < 0.3 * M_PI / 180.0 && fabs(timedA_.Get()) < 3 * M_PI / 180.0) {
+        if (fabs(yawDiff) < angular_pericision * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
             Stop();
             if(WaitClampLifterStatu(clampLifterAction)==false)