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

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

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

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

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

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

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

　

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

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

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

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

　　

定义机器人占据点属性：    机器人当前位置点。
         预测点属性：    从占据点下个点到路径终点的点集。

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

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

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

 核心代码：

1.冲突地图生成函数

//生成冲突地图
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;
        }
    }
}

　　

 1.冲突检测和调度处理

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
";
#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;
        }

    }
}

　　

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