首页 > 代码库 > 海康威视复赛题 ---- 碰撞避免方案（1）
海康威视复赛题 ---- 碰撞避免方案（1）
题目详情：http://www.cnblogs.com/wlzy/p/7096182.html
复赛题要求机器人之间不允许发生碰撞和相遇，拿到题目后，大体有以下几个解题思路：
1.基于侧边停车的碰撞避免算法
侧边停车是属于一种局部路径规划，只要保证每辆车时时刻刻拥有独立的侧停车位，那么即使发生相遇冲突，也可以及时侧停避让，算法大体描述如下：
定义机器人占据点属性： 机器人可获取侧停车位的最小路径点集。
预测点属性： 从占据点下个点到路径终点的点集。
开始：
1.初始化地图map
2.计算每个机器人的占据点，然后添加到map中相应的占据点队列中
3.计算每个机器人的预测点和机器人到每个预测点的距离值 ，并添加到map相应的预测点队列中。
4.检测占据点是否冲突。
5.遍历每个机器人，检测第一个预测点是否冲突
若无冲突，继续前进
有冲突，检测冲突类型：如果是相交冲突，只需简单令其中一个暂停。如果是相遇冲突，根据优先级选择一辆车侧停避让。
6.死锁检测和处理
结束。
该算法只需遍历每个机器人的占据点和第一个预测点，执行速度很快，机器人通行效率较高，但是存在以下问题：当机器人较多时，动态移动的机器人可能随时占用其他机器人的侧停车位，使得机器人无法满足调度的基本条件，从而容易导致死锁。
可以通过加长预测点来减少死锁发生的概率，或者添加一些死锁处理算法，但是这只能是治标不治本，总会有遗漏的情况。
2.基于相遇检测的碰撞避免算法
为了避免调度陷入死锁状态，提出了一种相遇避免算法，只要时刻检测每个机器人的预测点是否会相遇，若发生相遇则调度使其中部分暂停。
定义机器人占据点属性： 机器人当前位置点。
预测点属性： 从占据点下个点到路径终点的点集。
开始：
1.初始化地图map
2.计算每个机器人的占据点，然后添加到map中相应的占据点队列中
3.计算每个机器人的预测点和机器人到每个预测点的距离值 ，并添加到map相应的预测点队列中。
4.检测占据点是否冲突。
5.遍历每个机器人：
6.检测第一个预测点是否有冲突，若无则继续走，如有则记录该点中所有有冲突的机器人RobotList。
7.遍历剩余的预测点，若RobotList中机器人的占据在预测点中被找到，说明该机器人未来路径会发生相遇，立即停止运行。
若没有机器人机器人占据点，则比较所有冲突机器人到那段路径的距离，距离最小的取得改路径使用权。
结束。
算法2已经成功实现了，执行速度还算快，结果稳定无错误。由于添加了相遇检测，彻底避免了机器人在移动过程中相遇的情况，因此只需处理十字路口相交的冲突。
但是算法2采用的相遇检测机制注定了效率低下，会因为避免相遇而等待大量时间。不过有个优化的解决方案：在机器人执行任务前规划一条较优的路径，这样就能大量减少相遇冲突的等待时间。
核心代码：
//生成冲突地图
void Dispatch::GenConflict(TreeMsg &msg)
{
msg.Map_map.clear(); //清空冲突地图
//busy机器人冲突点设置
if(msg.RobotBusy_map.empty() == false)
{
map<ZY_UINT32,RobotInfo>::iterator rit;
for(rit = msg.RobotBusy_map.begin();rit != msg.RobotBusy_map.end();rit++)
{
RobotInfo robot_info = rit->second;
//获取占据点 只有一个
map<PointItem,PointInfo>::iterator fit_o1 = msg.Map_map.find(robot_info.Current_Pos.Pos);
if(fit_o1 != msg.Map_map.end()) //在map中存在,添加占据点
{
PointInfo point_info = fit_o1->second;
point_info.OccupyList.insert(RobotInfoP(robot_info,0));
msg.Map_map[robot_info.Current_Pos.Pos] = point_info;
}
else
{
PointInfo point_info;
point_info.OccupyList.insert(RobotInfoP(robot_info,0));
msg.Map_map.insert(map<PointItem,PointInfo>::value_type(robot_info.Current_Pos.Pos,point_info));
}
//获取预测点
if(robot_info.Relative_Pos < robot_info.Route_Size) //为走到终点
{
for(int i=(robot_info.Relative_Pos +1);i <= robot_info.Route_Size;i++)
{
PointItem point_item = robot_info.Route->at(i);
map<PointItem,PointInfo>::iterator fit_p = msg.Map_map.find(point_item);
if(fit_p != msg.Map_map.end())
{
PointInfo point_info = fit_p->second;
point_info.PredictList.insert(RobotInfoP(robot_info,i - robot_info.Relative_Pos));
msg.Map_map[point_item] = point_info;
}
else
{
PointInfo point_info;
point_info.PredictList.insert(RobotInfoP(robot_info,i - robot_info.Relative_Pos));
msg.Map_map.insert(map<PointItem,PointInfo>::value_type(point_item,point_info));
}
}
}
//
robot_info.LastOccupyP = robot_info.Route->at(robot_info.Relative_Pos);
if(robot_info.Relative_Pos < robot_info.Route_Size)
robot_info.FirstPredictP = robot_info.Route->at(robot_info.Relative_Pos+1);
else
robot_info.FirstPredictP = robot_info.Route->at(robot_info.Relative_Pos);
msg.RobotBusy_map[rit->first] = robot_info;
}
}
}
void Dispatch::ConflictProcess(TreeMsg &msg)
{
//将机器人状态全部置为未更新
{
map<ZY_UINT32,RobotInfo>::iterator rit;
for(rit = msg.RobotBusy_map.begin();rit != msg.RobotBusy_map.end();rit++)
{
RobotInfo robot_info = rit->second;
robot_info.IsUpdated = false;
robot_info.AddTail = false;
msg.RobotBusy_map[rit->first] = robot_info;
}
map<RobotKey,RobotInfo>::iterator rit2;
for(rit2 = msg.RobotIdle_map.begin();rit2 != msg.RobotIdle_map.end();rit2++)
{
RobotInfo robot_info = rit2->second;
robot_info.IsUpdated = false;
robot_info.AddTail = false;
msg.RobotIdle_map[rit2->first] = robot_info;
}
}
if(msg.RobotBusy_map.empty() == false)
{
set<PointItem> IO_CollisionPoint; //出入口出的冲突点
IO_CollisionPoint.clear();
while(1)
{
bool IsChanged = false;
map<ZY_UINT32,RobotInfo>::iterator ritg;
GenConflict(msg); //生冲突处理地图
for(ritg = msg.RobotBusy_map.begin();ritg != msg.RobotBusy_map.end();ritg++)
{
RobotInfo robot_info = ritg->second;
//检测是否更新过
if(robot_info.IsUpdated == true)
continue;
//占据点与占据点冲突检测 忽略出入口冲突
if((msg.Map_map.at(robot_info.LastOccupyP).OccupyList.size() > 1)&&(robot_info.LastOccupyP != Map_In) &&(robot_info.LastOccupyP != Map_Out))
{
#ifdef ZYVV_DEBUG
cout<<"OccupyP Error\r\n";
#endif
continue;
}
//若预测点为出入口 则忽略
if((robot_info.FirstPredictP == Map_In)||(robot_info.FirstPredictP == Map_Out))
{
robot_info.Relative_Pos++;
robot_info.Current_Pos.Pos = robot_info.Route->at(robot_info.Relative_Pos);
robot_info.DistToE--;
robot_info.IsUpdated = true;
msg.RobotBusy_map[ritg->first] = robot_info;
IsChanged = true;
GenConflict(msg);
//添加到出入口冲突点
IO_CollisionPoint.insert(robot_info.LastOccupyP);
continue;
}
//预测点和占据点冲突
if(msg.Map_map.at(robot_info.FirstPredictP).OccupyList.empty() == false)
{
//等待
continue;
}
{
//剔除同向预测点
set<RobotInfoP>::iterator rit1;
set<RobotInfoP> predict_listp = msg.Map_map.at(robot_info.FirstPredictP).PredictList;
set<RobotInfoP> predict_listp2 = msg.Map_map.at(robot_info.LastOccupyP).PredictList;
for(rit1 = predict_listp.begin();rit1 != predict_listp.end();)
{
RobotInfoP robot_infop1 = *rit1;
set<RobotInfoP>::iterator rit2;
rit2 = predict_listp2.find(robot_infop1);
if((rit2 != predict_listp2.end())) //若找到并且方向相同
{
RobotInfoP robot_infop2 = *rit2;
if(robot_infop1.pos > robot_infop2.pos)
rit1 = predict_listp.erase(rit1);
else
rit1++;
}
else
rit1++;
}
msg.Map_map[robot_info.FirstPredictP].PredictList = predict_listp;
}
//无预测点冲突 继续前进
if(msg.Map_map.at(robot_info.FirstPredictP).PredictList.size() == 1)
{
//若在出入口，需要避免相遇
if((robot_info.LastOccupyP != Map_In)&&(robot_info.LastOccupyP != Map_Out))
{
robot_info.Relative_Pos++;
robot_info.Current_Pos.Pos = robot_info.Route->at(robot_info.Relative_Pos);
robot_info.DistToE--;
robot_info.IsUpdated = true;
msg.RobotBusy_map[ritg->first] = robot_info;
IsChanged = true;
GenConflict(msg);
continue;
}
else
{
set<PointItem>::iterator fit;
fit = IO_CollisionPoint.find(robot_info.FirstPredictP);
if(fit == IO_CollisionPoint.end()) //未找到，表示不冲突
{
robot_info.Relative_Pos++;
robot_info.Current_Pos.Pos = robot_info.Route->at(robot_info.Relative_Pos);
robot_info.DistToE--;
robot_info.IsUpdated = true;
msg.RobotBusy_map[ritg->first] = robot_info;
IsChanged = true;
GenConflict(msg);
continue;
}
else
{
continue;
}
}
}
//预测点冲突
if(msg.Map_map.at(robot_info.FirstPredictP).PredictList.size() > 1)
{
set<RobotInfoP>::iterator rit;
bool IsWin = true;
set<RobotInfoP> predict_listp = msg.Map_map.at(robot_info.FirstPredictP).PredictList;
//检测相同距离预测点冲突
for(rit = predict_listp.begin();rit != predict_listp.end();rit++)
{
RobotInfoP robot_infop1 = *rit;
if((robot_infop1.pos == 1)&&(robot_infop1.robot_info.Robot_Serial != robot_info.Robot_Serial))
{
if(robot_infop1.robot_info.Robot_Priority > robot_info.Robot_Priority)
{
IsWin == false;
break;
}
}
}
if((IsWin == true)&&((robot_info.Relative_Pos + 1) < robot_info.Route_Size))
{
//检测相遇冲突
for(int i = (robot_info.Relative_Pos + 2); i <= robot_info.Route_Size;i++)
{
//忽略出入口冲突
if((robot_info.Route->at(i) == Map_In)||(robot_info.Route->at(i) == Map_Out))
continue;
set<RobotInfoP>::iterator rit3;
set<RobotInfoP> predict_listp_t = msg.Map_map.at(robot_info.Route->at(i)).PredictList;
set<RobotInfoP> occupy_listp_t = msg.Map_map.at(robot_info.Route->at(i)).OccupyList;
for(rit3 = predict_listp.begin();rit3 != predict_listp.end();)
{
set<RobotInfoP>::iterator t_it;
t_it = occupy_listp_t.find(*rit3);
if(t_it != occupy_listp_t.end()) //预测点被占据 则暂停
{
IsWin = false;
break;
}
else
{
t_it = predict_listp_t.find(*rit3);
if(t_it == predict_listp_t.end()) //冲突预测点中间断开，则移除
{
//rit3 = predict_listp.erase(rit3);
rit3++;
}
else
rit3++;
}
}
if(IsWin == false)
break;
}
}
//赢得预测点，更新机器人运行状态
if(IsWin == true)
{
if((robot_info.LastOccupyP != Map_In)&&(robot_info.LastOccupyP != Map_Out))
{
robot_info.Relative_Pos++;
robot_info.Current_Pos.Pos = robot_info.Route->at(robot_info.Relative_Pos);
robot_info.DistToE--;
robot_info.IsUpdated = true;
msg.RobotBusy_map[ritg->first] = robot_info;
IsChanged = true;
GenConflict(msg);
continue;
}
else
{
set<PointItem>::iterator fit;
fit = IO_CollisionPoint.find(robot_info.FirstPredictP);
if(fit == IO_CollisionPoint.end()) //未找到，表示不冲突
{
robot_info.Relative_Pos++;
robot_info.Current_Pos.Pos = robot_info.Route->at(robot_info.Relative_Pos);
robot_info.DistToE--;
robot_info.IsUpdated = true;
msg.RobotBusy_map[ritg->first] = robot_info;
IsChanged = true;
GenConflict(msg);
continue;
}
else
{
continue;
}
}
}
else
{
continue;
}
}
}
if(IsChanged == false)
break;
}
}
}
海康威视复赛题 ---- 碰撞避免方案（1）
声明：以上内容来自用户投稿及互联网公开渠道收集整理发布，本网站不拥有所有权，未作人工编辑处理，也不承担相关法律责任，若内容有误或涉及侵权可进行投诉： 投诉/举报 工作人员会在5个工作日内联系你，一经查实，本站将立刻删除涉嫌侵权内容。