/// <summary> /// 为单架无人机规划航迹 /// </summary> /// <param name="iTaskIndex">无人机/任务索引</param> /// <returns></returns> protected virtual MPath BuildPathForSingleUAV(int iTaskIndex) { //实例化航路 MPath mPath = new MPath(); mPath.Index = AlgoInput.UAVTask[iTaskIndex].Index; mPath.Waypoints = new List <MWaypoint>(); int iWaypointIndex = 0; //定义树 List <RRTNode> mRRTTree = new List <RRTNode>(); //定义航路 List <RRTNode> mRRTPath = new List <RRTNode>(); //定义树节点 RRTNode mRRTNode = null; //定义树中间节点 RRTNode mRRTRandNode = null; RRTNode mRRTNearNode = null; RRTNode mRRTNewNode = null; //计数器 int nCount = 1; //对每一个阶段规划航路 for (int iStageIndex = 0; iStageIndex < AlgoInput.UAVTask[iTaskIndex].Stages.Count; ++iStageIndex) { //实例化 mRRTTree = new List <RRTNode>(); //加入开始点为第一个点 mRRTNode = new RRTNode(0, AlgoInput.UAVTask[iTaskIndex].Stages[iStageIndex].StartState.Location, AlgoInput.UAVTask[iTaskIndex].Stages[iStageIndex].StartState.Direction, null); mRRTTree.Add(mRRTNode); //初始化计数器 nCount = 1; //是否到达目标 - 初始未到达 bool isReachTarget = false; //循环构建新点 - 到达目标时自动退出 while (!isReachTarget) { //-------------------------------随机生成节点--------------------------------// if (NodeSelectionType.Classical == m_RRTParameter.SelectionType) { //方案1:随机生成节点 - 标准RRT采用的方案 mRRTRandNode = RandomConfiguration(iTaskIndex, iStageIndex, mRRTTree); } else //if (NodeSelectionType.Optimzed == m_RRTParameter.SelectionType) { //方案2:按照一定概率随机生成节点或者采用目标点 (改进方案) mRRTRandNode = RandomConfigurationOrTargetConfiguration(iTaskIndex, iStageIndex, mRRTTree); } //-----------------------------------end-------------------------------------// //----------------------------搜索距离最近的节点-----------------------------// mRRTNearNode = NearestNeighbor(mRRTTree, mRRTRandNode); //-----------------------------------end-------------------------------------// //--------------------------------生成新节点---------------------------------// if (PlanningStepType.Constant == m_RRTParameter.StepType) { mRRTNewNode = NewConfiguration(iTaskIndex, iStageIndex, mRRTNearNode, mRRTRandNode); } else //if (PlanningStepType.Random == m_RRTParameter.m_StepType) { mRRTNewNode = NewConfigurationWithRandomStep(iTaskIndex, iStageIndex, mRRTNearNode, mRRTRandNode); } //-----------------------------------end-------------------------------------// //计数器累加 nCount = nCount + 1; //-----------------------如果到达允许的搜索上限,则退出-----------------------// if (nCount >= m_RRTParameter.MaxNodeNumber) { Console.WriteLine(""); break; } //-----------------------------------end-------------------------------------// //----------------威胁规避(禁止在威胁区域内选择/生成新点/连线)---------------// if (!IsSafePoint(mRRTNewNode.NodeLocation)) { continue; } if (!IsSafeLine(mRRTNewNode.NodeLocation, mRRTNearNode.NodeLocation)) { continue; } //-----------------------------------end-------------------------------------// //------------------------------扩展树 - 核心--------------------------------// isReachTarget = ExpandTree(iTaskIndex, iStageIndex, ref mRRTTree, mRRTNearNode, mRRTNewNode); //-----------------------------------end-------------------------------------// //------------判断新结点到目标连线是否安全, 若安全则直接连接目标点-----------// if (TargetReachMode.Direct == m_RRTParameter.ReachMode) { isReachTarget = isReachTarget || DirectlyToTarget(iTaskIndex, iStageIndex, mRRTNewNode, ref mRRTTree); } //-----------------------------------end-------------------------------------// } //构建当前阶段航路 mRRTPath = BuildPath(mRRTTree); //为可视化输出保存 Visualization.MPathForVisualizaition = mRRTTree; if (!PathPlaningDataVisualization.PathPlaningDataVisualization.AlgoDataDict.ContainsKey(mPath.Index.ToString() + "-" + iStageIndex.ToString())) { //为ResultShowForm分析窗口保存数据 //3.30.2018 刘洋添加 PathPlaningDataVisualization.PathPlaningDataVisualization.AlgoDataDict.Add(mPath.Index.ToString() + "-" + iStageIndex.ToString(), Visualization.MyTreeNodeConverter(Visualization.MPathForVisualizaition)); } else { PathPlaningDataVisualization.PathPlaningDataVisualization.AlgoDataDict[mPath.Index.ToString() + "-" + iStageIndex.ToString()] = Visualization.MyTreeNodeConverter(Visualization.MPathForVisualizaition); } //添加当前阶段航路到总航路(起始航路点为当前阶段, 目标航路点为下一个阶段, 注意总航路的最后一个航路点属于最后一个阶段) for (int k = 0; k < mRRTPath.Count - 1; ++k) { mPath.Waypoints.Add(new MWaypoint(iWaypointIndex, RRTNode.ConvertNodeToUAVState(mRRTPath[k]), iStageIndex)); iWaypointIndex = iWaypointIndex + 1; } } //增加最后的目标点 mPath.Waypoints.Add(new MWaypoint(iWaypointIndex, RRTNode.ConvertNodeToUAVState(mRRTPath[mRRTPath.Count - 1]), AlgoInput.UAVTask[iTaskIndex].Stages.Count - 1)); //返回路径 return(mPath); }
/// <summary> /// 从指定航路点开始为单架无人机规划航迹 /// </summary> /// <param name="iTaskIndex">无人机/任务索引</param> /// <param name="mWaypoint">指定的起始航路点</param> /// <param name="mPath">当前路径</param> /// <returns></returns> protected MPath BuildPathForSingleUAV(int iTaskIndex, MWaypoint mWaypoint, MPath mPath) { //移除起始航路点之后(含)的所有航路点 if (mPath.Waypoints != null) { mPath.Waypoints.RemoveRange(mWaypoint.Index, mPath.Waypoints.Count - mWaypoint.Index); //从数据库移除 } //定义树 List <RRTNode> mRRTTree = null; //定义航路 List <RRTNode> mRRTPath = null; //定义树节点 RRTNode mRRTNode = null; //定义树中间节点 RRTNode mRRTRandNode = null; RRTNode mRRTNearNode = null; RRTNode mRRTNewNode = null; //以当前航路点为当前阶段的起始状态点(以后的阶段均不变), 构建新的阶段 List <MStage> mNewStages = new List <MStage>(); MStage mTempStage = null; mTempStage = new MStage(); mTempStage.StageIndex = mWaypoint.StageIndex; mTempStage.StartState = new MKeyState(mWaypoint.State); mTempStage.TargetState = AlgoInput.UAVTask[iTaskIndex].Stages[mWaypoint.StageIndex].TargetState; mNewStages.Add(mTempStage); for (int i = mWaypoint.StageIndex + 1; i < AlgoInput.UAVTask[iTaskIndex].Stages.Count; ++i) { mTempStage = new MStage(); mTempStage = AlgoInput.UAVTask[iTaskIndex].Stages[i]; mNewStages.Add(mTempStage); } //当前航路点编号 int iWaypointIndex = mWaypoint.Index; //计数器 int nCount = 1; //对每一个阶段规划航路 for (int iStageIndex = mNewStages[0].StageIndex; iStageIndex < mNewStages[0].StageIndex + mNewStages.Count; ++iStageIndex) { //实例化 mRRTTree = new List <RRTNode>(); //加入开始点为第一个点 mRRTNode = new RRTNode(0, mNewStages[iStageIndex - mNewStages[0].StageIndex].StartState.Location, mNewStages[iStageIndex - mNewStages[0].StageIndex].StartState.Direction, null); mRRTTree.Add(mRRTNode); //初始化计数器 nCount = 1; //是否到达目标 - 初始未到达 bool isReachTarget = false; //循环构建新点 - 到达目标时自动退出 while (!isReachTarget) { //-------------------------------随机生成节点--------------------------------// if (NodeSelectionType.Classical == m_RRTParameter.SelectionType) { //方案1:随机生成节点 - 标准RRT采用的方案 mRRTRandNode = RandomConfiguration(iTaskIndex, iStageIndex, mRRTTree); } else //if (NodeSelectionType.Optimzed == m_RRTParameter.SelectionType) { //方案2:按照一定概率随机生成节点或者采用目标点 (改进方案) mRRTRandNode = RandomConfigurationOrTargetConfiguration(iTaskIndex, iStageIndex, mRRTTree); } //-----------------------------------end-------------------------------------// //----------------------------搜索距离最近的节点-----------------------------// mRRTNearNode = NearestNeighbor(mRRTTree, mRRTRandNode); //-----------------------------------end-------------------------------------// //--------------------------------生成新节点---------------------------------// if (PlanningStepType.Constant == m_RRTParameter.StepType) { mRRTNewNode = NewConfiguration(iTaskIndex, iStageIndex, mRRTNearNode, mRRTRandNode); } else //if (PlanningStepType.Random == m_RRTParameter.m_StepType) { mRRTNewNode = NewConfigurationWithRandomStep(iTaskIndex, iStageIndex, mRRTNearNode, mRRTRandNode); } //-----------------------------------end-------------------------------------// //计数器累加 nCount = nCount + 1; //-----------------------如果到达允许的搜索上限,则退出-----------------------// if (nCount >= m_RRTParameter.MaxNodeNumber) { break; } //-----------------------------------end-------------------------------------// //----------------威胁规避(禁止在威胁区域内选择/生成新点/连线)---------------// if (!IsSafePoint(mRRTNewNode.NodeLocation)) { continue; } if (!IsSafeLine(mRRTNewNode.NodeLocation, mRRTNearNode.NodeLocation)) { continue; } //-----------------------------------end-------------------------------------// //------------------------------扩展树 - 核心--------------------------------// isReachTarget = ExpandTree(iTaskIndex, iStageIndex, ref mRRTTree, mRRTNearNode, mRRTNewNode); //-----------------------------------end-------------------------------------// //------------判断新结点到目标连线是否安全, 若安全则直接连接目标点-----------// if (TargetReachMode.Direct == m_RRTParameter.ReachMode) { isReachTarget = isReachTarget || DirectlyToTarget(iTaskIndex, iStageIndex, mRRTNewNode, ref mRRTTree); } //-----------------------------------end-------------------------------------// } //构建当前阶段航路 mRRTPath = BuildPath(mRRTTree); //添加当前阶段航路到总航路 for (int k = 0; k < mRRTPath.Count - 1; ++k) { mPath.Waypoints.Add(new MWaypoint(iWaypointIndex, RRTNode.ConvertNodeToUAVState(mRRTPath[k]), iStageIndex)); iWaypointIndex = iWaypointIndex + 1; } } //增加最后的目标点 mPath.Waypoints.Add(new MWaypoint(iWaypointIndex, RRTNode.ConvertNodeToUAVState(mRRTPath[mRRTPath.Count - 1]), AlgoInput.UAVTask[iTaskIndex].Stages.Count - 1)); //返回路径 return(mPath); }