海康威視復賽題 ---- 碰撞避免方案（1）
阿新 • • 發佈：2017-07-08
更新 bsp 題目 比較 toc .html 可能 pro 徹底
題目詳情：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）