基于RRT及其改进型的路径规划算法

本设计学习并分析了基本RRT路径规划的原理，并通过参考资料及自己分析，提出了一种改进的RRT路径规划算法。为了验证改进型RRT算法的正确性以及合理性。最后在VS2010开发环境下用C 编写了两种RRT算法的程序代码及演示界面。通过一定量的实验得到了大量数据。经过数据分析，验证了改进型RRT是正确的，并且在不破坏基本RRT算法的随机性的前提下，有效的将随机性和目的性结合起来，提高了RRT算法的效率和路径的质量。

【核心代码】

void OPT_randTree_build()
{
 
flag3=false;
sizeoftree.clear();
   TreePath.clear();
   TreePath1.clear();
   randomTree.clear();

   pos tempnew=mystart;  //tempnew存储树上最新的节点
   //bool flag=true;
   long cnt=0;
   Treestr temp1;
   temp1.x=tempnew.x;
   temp1.y=tempnew.y;
   temp1.prtpos=-1;
   randomTree.push_back(temp1);
   
   
   double optdis=30;
   

   while(getDis(tempnew,myend)>minPath)
   {
  cnt ;

       pos randomPos=Random_Configuration();
 if(getDis(randomPos,myend)<optdis )
 {
 randomPos=myend;
 }
  double mindistance=getDis(randomPos,mystart);
 
   bool flag1=true;
   Treestr neartrpos;
   Treestr newtrpos;
   pos nearpos;
   pos newpos;
   int i=0;
pos tempnear;
 
  for(i=0;i<randomTree.size();i )
  {


 tempnear.x=randomTree[i].x;
          tempnear.y=randomTree[i].y;

 
 double tempdis=getDis(randomPos,tempnear);
 
 if(tempdis<=mindistance)
  {
  
mindistance =tempdis;
neartrpos.x=tempnear.x;
neartrpos.y=tempnear.y;
neartrpos.prtpos=i;
nearpos.x=tempnear.x;
nearpos.y=tempnear.y;
 
 }
  }



 newpos=New_Configuration( randomPos,nearpos);


int j=0;
for(j=0;j<data.size();j )
{
if(getDis(newpos,data[j])<minDis)
{
flag1=false;
 
break;
}
}
 
if(flag1)
{
if(cnt>10000)break;
newtrpos.x=newpos.x;
    newtrpos.y=newpos.y;
newtrpos.prtpos=neartrpos.prtpos;
randomTree.push_back(newtrpos);
tempnew=newpos;
}

   }
  if(cnt<=10000)
  {
 
 int k=randomTree.size();
 sx=k;
 flag3=true;
 pos temp;
 long q=k-1;
 int t=0;
 for( int evyend=0;evyend<k;evyend )
 {
 bool flag2=false;
 for(int i=1;i<k;i )
 {   
 int cmp=randomTree[i].prtpos;
 if(cmp==evyend)
 {
    flag2=true;break;
 }
 }
 if(flag2==false)
sizeoftree.push_back(evyend);
 }
 
 TreePath.push_back(myend);
 while(randomTree[q].prtpos!=-1)
 {
 
 temp.x=randomTree[q].x;
 temp.y=randomTree[q].y;
 TreePath.push_back(temp);
 t=q;
 q=randomTree[q].prtpos;
 randomTree[t].prtpos=-1;
  }
TreePath.push_back(mystart);
sy=TreePath.size();




     for(int icount=0;icount<sizeoftree.size();icount )
{
pos temp2;
int endpos=sizeoftree[icount];
int t=0;
int beforepos=endpos;
while(randomTree[beforepos].prtpos!=-1)
   {
 
 temp2.x=randomTree[beforepos].x;
 temp2.y=randomTree[beforepos].y;
 TreePath1.push_back(temp2);
 t=beforepos;
 beforepos=randomTree[beforepos].prtpos;
 randomTree[t].prtpos=-1;
   }
 temp2.x=randomTree[beforepos].x;
 temp2.y=randomTree[beforepos].y;
 TreePath1.push_back(temp2);
 temp2.x=0;
 temp2.y=0;
 TreePath1.push_back(temp2);
 }
   TreePath1.push_back(mystart);
 
  }

}