devbox
你好赵伟
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

海康威视复赛题 ---- 碰撞避免方案（1）

作者：你好赵伟 | 2024-05-04 23:10:41

赞

踩

输入一个用户名、密码,判断密码强度。 分四类: 1. 强度0:密码长度小于8|用户名与

题目详情：http://www.cnblogs.com/wlzy/p/7096182.html

复赛题要求机器人之间不允许发生碰撞和相遇，拿到题目后，大体有以下几个解题思路：

方案一：基于侧边停车的碰撞避免算法

　　侧边停车是属于一种局部路径规划，只要保证每辆车时时刻刻拥有独立的侧停车位，那么即使发生相遇冲突，也可以及时侧停避让，算法大体描述如下：

定义机器人 占据点属性：    机器人可获取独立侧停车位的最小路径点集。
          预测点属性：    从占据点下个点到路径终点的点集。

开始：
        1.初始化地图map
        2.计算每个机器人的占据点，然后添加到map中相应的占据点队列中
        3.计算每个机器人的预测点和机器人到每个预测点的距离值 ，并添加到map相应的预测点队列中。
        4.检测占据点是否冲突。
        5.遍历每个机器人，检测第一个预测点是否冲突
            若无冲突，继续前进
            有冲突，检测冲突类型：如果是相交冲突，只需简单令其中一个暂停。如果是相遇冲突，根据优先级选择一辆车侧停避让。
        6.死锁检测和处理
结束

 

　

　　该算法只需遍历每个机器人的占据点和第一个预测点，执行速度很快，机器人通行效率较高，但是存在以下问题：当机器人较多时，动态移动的机器人可能随时占用其他机器人的侧停车位，使得机器人无法满足调度的基本条件，从而容易导致死锁。

 

　　可以通过加长预测点来减少死锁发生的概率，或者添加一些死锁处理算法，但是这只能是治标不治本，总会有遗漏的情况。

 

方案二：基于相遇检测的碰撞避免算法

　　为了避免调度陷入死锁状态，提出了一种相遇避免算法，只要时刻检测每个机器人的预测点是否会相遇，若发生相遇则调度使其中部分暂停。

　　

  1. 定义机器人占据点属性：    机器人当前位置点。
  2.          预测点属性：    从占据点下个点到路径终点的点集。
  3.  
  4. 开始：
  5.         1.初始化地图map
  6.         2.计算每个机器人的占据点，然后添加到map中相应的占据点队列中
  7.         3.计算每个机器人的预测点和机器人到每个预测点的距离值 ，并添加到map相应的预测点队列中。
  8.         4.检测占据点是否冲突。
  9.         5.遍历每个机器人：
  10.             6.检测第一个预测点是否有冲突，若无则继续走，如有则记录该点中所有机器人到RobotList中。
  11.             7.遍历剩余的预测点：若RobotList中机器人的占据在预测点中被找到，说明该机器人未来路径会发生相遇，立即停止运行。
  12.                              若没有机器人机器人占据点，则比较所有冲突机器人到那段路径的距离，距离最小的取得改路径使用权。
  13.     
  14. 结束。

　　　　算法2已经成功实现了，执行速度还算快，结果稳定无错误。由于添加了相遇检测，彻底避免了机器人在移动过程中相遇的情况，因此只需处理十字路口相交的冲突。

　　　　但是算法2采用的相遇检测机制注定了效率低下，会因为避免相遇而等待大量时间。不过有个优化的解决方案：在机器人执行任务前规划一条较优的路径，这样就能大量减少相遇冲突的等待时间。

 核心代码：

1.冲突地图生成函数

  1. //生成冲突地图
  2. void Dispatch::GenConflict(TreeMsg &msg)
  3. {
  4.     msg.Map_map.clear(); //清空冲突地图
  5.     //busy机器人冲突点设置
  6.     if(msg.RobotBusy_map.empty() == false)
  7.     {
  8.  
  9.         map<ZY_UINT32,RobotInfo>::iterator rit;
  10.         for(rit = msg.RobotBusy_map.begin();rit != msg.RobotBusy_map.end();rit++)
  11.         {
  12.             RobotInfo robot_info = rit->second;
  13.  
  14.             //获取占据点 只有一个
  15.             map<PointItem,PointInfo>::iterator fit_o1 =  msg.Map_map.find(robot_info.Current_Pos.Pos);
  16.             if(fit_o1 != msg.Map_map.end())   //在map中存在,添加占据点
  17.             {
  18.                 PointInfo point_info = fit_o1->second;
  19.                 point_info.OccupyList.insert(RobotInfoP(robot_info,0));
  20.                 msg.Map_map[robot_info.Current_Pos.Pos] = point_info;
  21.             }
  22.             else
  23.             {
  24.                 PointInfo point_info;
  25.                 point_info.OccupyList.insert(RobotInfoP(robot_info,0));
  26.                 msg.Map_map.insert(map<PointItem,PointInfo>::value_type(robot_info.Current_Pos.Pos,point_info));
  27.             }
  28.  
  29.             //获取预测点
  30.             if(robot_info.Relative_Pos < robot_info.Route_Size)   //为走到终点
  31.             {
  32.                 for(int i=(robot_info.Relative_Pos +1);i <= robot_info.Route_Size;i++)
  33.                 {
  34.                     PointItem point_item = robot_info.Route->at(i);
  35.                     map<PointItem,PointInfo>::iterator fit_p =  msg.Map_map.find(point_item);
  36.                     if(fit_p != msg.Map_map.end())
  37.                     {
  38.                         PointInfo point_info = fit_p->second;
  39.                         point_info.PredictList.insert(RobotInfoP(robot_info,i - robot_info.Relative_Pos));
  40.                         msg.Map_map[point_item] = point_info;
  41.                     }
  42.                     else
  43.                     {
  44.                         PointInfo point_info;
  45.                         point_info.PredictList.insert(RobotInfoP(robot_info,i - robot_info.Relative_Pos));
  46.                         msg.Map_map.insert(map<PointItem,PointInfo>::value_type(point_item,point_info));
  47.                     }
  48.                 }
  49.             }
  50.  
  51.             //
  52.             robot_info.LastOccupyP = robot_info.Route->at(robot_info.Relative_Pos);
  53.             if(robot_info.Relative_Pos < robot_info.Route_Size)
  54.                 robot_info.FirstPredictP = robot_info.Route->at(robot_info.Relative_Pos+1);
  55.             else
  56.                 robot_info.FirstPredictP = robot_info.Route->at(robot_info.Relative_Pos);
  57.             msg.RobotBusy_map[rit->first] = robot_info;
  58.         }
  59.     }
  60. }

　　

 1.冲突检测和调度处理

  1. void Dispatch::ConflictProcess(TreeMsg &msg)
  2. {
  3.     //将机器人状态全部置为未更新
  4.     {
  5.         map<ZY_UINT32,RobotInfo>::iterator rit;
  6.         for(rit = msg.RobotBusy_map.begin();rit != msg.RobotBusy_map.end();rit++)
  7.         {
  8.             RobotInfo robot_info = rit->second;
  9.             robot_info.IsUpdated = false;
  10.             robot_info.AddTail = false;
  11.             msg.RobotBusy_map[rit->first] = robot_info;
  12.         }
  13.         map<RobotKey,RobotInfo>::iterator rit2;
  14.         for(rit2 = msg.RobotIdle_map.begin();rit2 != msg.RobotIdle_map.end();rit2++)
  15.         {
  16.             RobotInfo robot_info = rit2->second;
  17.             robot_info.IsUpdated = false;
  18.             robot_info.AddTail = false;
  19.             msg.RobotIdle_map[rit2->first] = robot_info;
  20.         }
  21.     }
  22.     if(msg.RobotBusy_map.empty() == false)
  23.     {
  24.         set<PointItem> IO_CollisionPoint;             //出入口出的冲突点
  25.         IO_CollisionPoint.clear();
  26.         while(1)
  27.         {
  28.             bool IsChanged = false;
  29.             map<ZY_UINT32,RobotInfo>::iterator ritg;
  30.             GenConflict(msg);            //生冲突处理地图
  31.             for(ritg = msg.RobotBusy_map.begin();ritg != msg.RobotBusy_map.end();ritg++)
  32.             {
  33.                 RobotInfo robot_info = ritg->second;
  34.  
  35.                 //检测是否更新过
  36.                 if(robot_info.IsUpdated == true)
  37.                     continue;
  38.  
  39.                 //占据点与占据点冲突检测 忽略出入口冲突
  40.                 if((msg.Map_map.at(robot_info.LastOccupyP).OccupyList.size() > 1)&&(robot_info.LastOccupyP != Map_In)\
  41.                         &&(robot_info.LastOccupyP != Map_Out))
  42.                 {
  43. #ifdef ZYVV_DEBUG
  44.                     cout<<"OccupyP Error\r\n";
  45. #endif
  46.                     continue;
  47.                 }
  48.  
  49.                 //若预测点为出入口 则忽略
  50.                 if((robot_info.FirstPredictP == Map_In)||(robot_info.FirstPredictP == Map_Out))
  51.                 {
  52.                     robot_info.Relative_Pos++;
  53.                     robot_info.Current_Pos.Pos = robot_info.Route->at(robot_info.Relative_Pos);
  54.                     robot_info.DistToE--;
  55.                     robot_info.IsUpdated = true;
  56.                     msg.RobotBusy_map[ritg->first] = robot_info;
  57.                     IsChanged = true;
  58.                     GenConflict(msg);
  59.                     //添加到出入口冲突点
  60.                     IO_CollisionPoint.insert(robot_info.LastOccupyP);
  61.                     continue;
  62.                 }
  63.  
  64.                 //预测点和占据点冲突
  65.                 if(msg.Map_map.at(robot_info.FirstPredictP).OccupyList.empty() == false)
  66.                 {
  67.                     //等待
  68.                     continue;
  69.                 }
  70.  
  71.                 {
  72.                 //剔除同向预测点
  73.                 set<RobotInfoP>::iterator rit1;
  74.                 set<RobotInfoP> predict_listp = msg.Map_map.at(robot_info.FirstPredictP).PredictList;
  75.                 set<RobotInfoP> predict_listp2 = msg.Map_map.at(robot_info.LastOccupyP).PredictList;
  76.                 for(rit1 = predict_listp.begin();rit1 != predict_listp.end();)
  77.                 {
  78.                     RobotInfoP robot_infop1 = *rit1;
  79.                     set<RobotInfoP>::iterator rit2;
  80.                     rit2 = predict_listp2.find(robot_infop1);
  81.                     if((rit2 != predict_listp2.end()))  //若找到并且方向相同
  82.                     {
  83.                         RobotInfoP robot_infop2 = *rit2;
  84.                         if(robot_infop1.pos > robot_infop2.pos)
  85.                             rit1 = predict_listp.erase(rit1);
  86.                         else
  87.                             rit1++;
  88.                     }
  89.                     else
  90.                         rit1++;
  91.                 }
  92.                 msg.Map_map[robot_info.FirstPredictP].PredictList = predict_listp;
  93.                 }
  94.  
  95.                 //无预测点冲突 继续前进
  96.                 if(msg.Map_map.at(robot_info.FirstPredictP).PredictList.size() == 1)
  97.                 {
  98.                     //若在出入口，需要避免相遇
  99.  
  100.                     if((robot_info.LastOccupyP != Map_In)&&(robot_info.LastOccupyP != Map_Out))
  101.                     {
  102.                         robot_info.Relative_Pos++;
  103.                         robot_info.Current_Pos.Pos = robot_info.Route->at(robot_info.Relative_Pos);
  104.                         robot_info.DistToE--;
  105.                         robot_info.IsUpdated = true;
  106.                         msg.RobotBusy_map[ritg->first] = robot_info;
  107.                         IsChanged = true;
  108.                         GenConflict(msg);
  109.                         continue;
  110.                     }
  111.                     else
  112.                     {
  113.                         set<PointItem>::iterator fit;
  114.                         fit = IO_CollisionPoint.find(robot_info.FirstPredictP);
  115.                         if(fit == IO_CollisionPoint.end())   //未找到，表示不冲突
  116.                         {
  117.                             robot_info.Relative_Pos++;
  118.                             robot_info.Current_Pos.Pos = robot_info.Route->at(robot_info.Relative_Pos);
  119.                             robot_info.DistToE--;
  120.                             robot_info.IsUpdated = true;
  121.                             msg.RobotBusy_map[ritg->first] = robot_info;
  122.                             IsChanged = true;
  123.                             GenConflict(msg);
  124.                             continue;
  125.                         }
  126.                         else
  127.                         {
  128.                             continue;
  129.                         }
  130.                     }
  131.  
  132.                 }
  133.  
  134.                 //预测点冲突
  135.                 if(msg.Map_map.at(robot_info.FirstPredictP).PredictList.size() > 1)
  136.                 {
  137.                     set<RobotInfoP>::iterator rit;
  138.                     bool IsWin = true;
  139.                     set<RobotInfoP> predict_listp = msg.Map_map.at(robot_info.FirstPredictP).PredictList;
  140.  
  141.                     //检测相同距离预测点冲突
  142.                     for(rit = predict_listp.begin();rit != predict_listp.end();rit++)
  143.                     {
  144.                         RobotInfoP robot_infop1 = *rit;
  145.                         if((robot_infop1.pos == 1)&&(robot_infop1.robot_info.Robot_Serial != robot_info.Robot_Serial))
  146.                         {
  147.                             if(robot_infop1.robot_info.Robot_Priority > robot_info.Robot_Priority)
  148.                             {
  149.                                 IsWin == false;
  150.                                 break;
  151.                             }
  152.                         }
  153.                     }
  154.  
  155.                     if((IsWin == true)&&((robot_info.Relative_Pos + 1) < robot_info.Route_Size))
  156.                     {
  157.                         //检测相遇冲突
  158.                         for(int i = (robot_info.Relative_Pos + 2); i <= robot_info.Route_Size;i++)
  159.                         {
  160.                             //忽略出入口冲突
  161.                             if((robot_info.Route->at(i) == Map_In)||(robot_info.Route->at(i) == Map_Out))
  162.                                 continue;
  163.                             set<RobotInfoP>::iterator rit3;
  164.                             set<RobotInfoP> predict_listp_t = msg.Map_map.at(robot_info.Route->at(i)).PredictList;
  165.                             set<RobotInfoP> occupy_listp_t = msg.Map_map.at(robot_info.Route->at(i)).OccupyList;
  166.                             for(rit3 = predict_listp.begin();rit3 != predict_listp.end();)
  167.                             {
  168.                                 set<RobotInfoP>::iterator t_it;
  169.                                 t_it = occupy_listp_t.find(*rit3);
  170.                                 if(t_it != occupy_listp_t.end())   //预测点被占据 则暂停
  171.                                 {
  172.                                     IsWin = false;
  173.                                     break;
  174.                                 }
  175.                                 else
  176.                                 {
  177.                                     t_it = predict_listp_t.find(*rit3);
  178.                                     if(t_it == predict_listp_t.end())    //冲突预测点中间断开，则移除
  179.                                     {
  180.                                         //rit3 = predict_listp.erase(rit3);
  181.                                         rit3++;
  182.                                     }
  183.                                     else
  184.                                         rit3++;
  185.                                 }
  186.                             }
  187.                             if(IsWin == false)
  188.                                 break;
  189.                         }
  190.                     }
  191.  
  192.                     //赢得预测点，更新机器人运行状态
  193.                     if(IsWin == true)
  194.                     {
  195.                         if((robot_info.LastOccupyP != Map_In)&&(robot_info.LastOccupyP != Map_Out))
  196.                         {
  197.                             robot_info.Relative_Pos++;
  198.                             robot_info.Current_Pos.Pos = robot_info.Route->at(robot_info.Relative_Pos);
  199.                             robot_info.DistToE--;
  200.                             robot_info.IsUpdated = true;
  201.                             msg.RobotBusy_map[ritg->first] = robot_info;
  202.                             IsChanged = true;
  203.                             GenConflict(msg);
  204.                             continue;
  205.                         }
  206.                         else
  207.                         {
  208.                             set<PointItem>::iterator fit;
  209.                             fit = IO_CollisionPoint.find(robot_info.FirstPredictP);
  210.                             if(fit == IO_CollisionPoint.end())   //未找到，表示不冲突
  211.                             {
  212.                                 robot_info.Relative_Pos++;
  213.                                 robot_info.Current_Pos.Pos = robot_info.Route->at(robot_info.Relative_Pos);
  214.                                 robot_info.DistToE--;
  215.                                 robot_info.IsUpdated = true;
  216.                                 msg.RobotBusy_map[ritg->first] = robot_info;
  217.                                 IsChanged = true;
  218.                                 GenConflict(msg);
  219.                                 continue;
  220.                             }
  221.                             else
  222.                             {
  223.                                 continue;
  224.                             }
  225.                         }
  226.                     }
  227.                     else
  228.                     {
  229.                         continue;
  230.                     }
  231.                 }
  232.             }
  233.             if(IsChanged == false)
  234.                     break;
  235.         }
  236.  
  237.     }
  238. }

　　

完整源码：https://github.com/ZyvvL

转载于:https://www.cnblogs.com/wlzy/p/7096387.html

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/你好赵伟/article/detail/536527
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号