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

Posted ZYVV

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了海康威视复赛题 ---- 碰撞避免方案相关的知识,希望对你有一定的参考价值。

题目详情: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\\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;
        }

    }
}

  

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

以上是关于海康威视复赛题 ---- 碰撞避免方案的主要内容,如果未能解决你的问题,请参考以下文章

海康威视复赛题

海康威视RTSP摄像头转RTMP推流前端方案(浏览器兼容及权限验证)

海康威视错误代码说明

海康威视数字400万像素摄像头带宽是多少

海康威视相机 RTSP 传输延迟解决方案

海康威视相机 RTSP 传输延迟解决方案