aco2.c

蚁群算法在路径规划中的应用:在启发式因子的设计上考虑了路径的方向性,在算法中加入了蚁群学习机制

/*********************角度计算函数 主启发式因子********************************/
float NodeAngle(float currx,float curry,float nextrx,float nextry,float destrx,float destry){
     float nexttocurrx,nexttocurry,desttocurrx,desttocurry;
     double cosangle,cos_top,cos_bottom;
     nexttocurrx=nextrx-currx;
     nexttocurry=nextry-curry;
     desttocurrx=destrx-currx;
     desttocurry=destry-curry;
     cos_top=nexttocurrx*desttocurrx+nexttocurry*desttocurry; 
     cos_bottom=sqrt(nexttocurrx*nexttocurrx+nexttocurry*nexttocurry)*sqrt(desttocurrx*desttocurrx+desttocurry*desttocurry);
     cosangle=cos_top/cos_bottom;
     return cosangle;    
}
/********************信息素全局更新函数****************************************/
void UpdateGlobalTPheromone(){
	VNODES *fvnode,*nvnode;
	RNode *rnode;
	TRNODE *trnode;
	fvnode=bestPath;
	if(bestPath!=NULL){
	  nvnode=bestPath->nextVnode;
	  for(;nvnode!=NULL;fvnode=nvnode,nvnode=nvnode->nextVnode){
		  for(trnode=(rNodes+(fvnode->vnodeid))->firstTRNode;trnode!=NULL;trnode=trnode->nextTRNode){
			  if(trnode->RNodeid==nvnode->vnodeid){
			  	  trnode->TPheromone=(trnode->TPheromone)+Q/(bestDistance+0.01);
				      if(trnode->TPheromone>MaxPh)
					      trnode->TPheromone=MaxPh;
			  }
			   
		  }
	  }
	}
}


void  UpdateGlobalTPheromone2(){
	UpdateP2PTPheromone(destnodeid);
}
void UpdateP2PTPheromone(int nodeid){
	RNode *rnode;
	TRNODE *fvnode;
	rnode=rNodes+nodeid;
	if(rnode->prevnodeid!=sourcenodeid){
         UpdateP2PTPheromone(rnode->prevnodeid);
	}
//	printf("up:%d ",nodeid);
	
    rnode=rNodes+(rnode->prevnodeid);
	for(fvnode=rnode->firstTRNode;fvnode!=NULL;fvnode=fvnode->nextTRNode){
		if(fvnode->RNodeid==nodeid){
			 fvnode->TPheromone=(fvnode->TPheromone)+Q/(bestDistance+0.01);
				      if(fvnode->TPheromone>MaxPh)
					      fvnode->TPheromone=MaxPh;
					 // printf("%f ",fvnode->TPheromone);
		}
	}
	
}

		




/********************信息素路径更新函数****************************************/
void UpdateSigalTPheromone(VNODES *path){
	VNODES *fvnode,*nvnode;
	RNode *rnode;
	TRNODE *trnode;
	fvnode=path;
	if(path!=NULL)
	     nvnode=path->nextVnode;
	else
		return;
	for(;fvnode!=NULL&&nvnode!=NULL;fvnode=nvnode,nvnode=nvnode->nextVnode){
		for(trnode=(rNodes+(fvnode->vnodeid))->firstTRNode;trnode!=NULL;trnode=trnode->nextTRNode){
			if(trnode->RNodeid==nvnode->vnodeid){
				trnode->TPheromone=(1-ad_rho)*(trnode->TPheromone);
				if(trnode->TPheromone<MinPh)
					trnode->TPheromone=MinPh;
			}
			   
		}
	}
}
/*******************死亡路径信息素挥发函数*********************************/
void DeathPathPHVaporate(VNODES *path){
    VNODES *fvnode,*nvnode;
	RNode *rnode;
	TRNODE *trnode;
	fvnode=path;
	if(path!=NULL)
	     nvnode=path->nextVnode;
	else
		return;
	for(;fvnode!=NULL&&nvnode!=NULL;fvnode=nvnode,nvnode=nvnode->nextVnode){
		for(trnode=(rNodes+(fvnode->vnodeid))->firstTRNode;trnode!=NULL;trnode=trnode->nextTRNode){
			if(trnode->RNodeid==nvnode->vnodeid){
				trnode->TPheromone=(1-Drho)*(trnode->TPheromone);
			//	printf("tp:%f",trnode->TPheromone);
				if(trnode->TPheromone<MinPh)
					trnode->TPheromone=MinPh;
			}
			   
		}
	}
}	

void UpdateLocalTPTPheromone(){
	int i;
	TRNODE *p;
	for(i=0;i<maxnodes;i++){
		for(p=(rNodes+i)->firstTRNode;p!=NULL;p=p->nextTRNode){
			p->TPheromone*=(1-rho);
			if(p->TPheromone<MinPh)
				p->TPheromone=MinPh;
           // printf("%f->",p->TPheromone);//test
		}
	//	printf("\n");//test
	}
}

void PrintOutTest(){
    int i;
	TRNODE *p;
	for(i=0;i<maxnodes;i++){
		for(p=(rNodes+i)->firstTRNode;p!=NULL;p=p->nextTRNode){
            printf("%f->",p->TPheromone);//test
		}
		printf("\n");//test
	}
}
/**********************构造解空间函数******************************************/
void ConstructSolution(){
    int i=0;
    int flag=0,j=0;//test
    /*for(;i<num_ants;i++)
       Empty_Ant_Memory(i);   */         /*****消除蚂蚁记忆并重新初始化************/
    while(1){
       flag=0;
       for(i=0;i<num_ants;i++){          
           if((ants+i)->terminate!=0){   			   /********存在无路可走的蚂蚁***********/
			 /* if((ants+i)->terminate==2&&deadants>20) {   				  
			      EmptyOneantMemory(i);// printf("no way to turn"); //test
				  //flag=0;//test
				 // printf("%d\n",(ants+i)->nodes_visited->vnodeid);
				 // break;//test
			   }*/
			   if((ants+i)->terminate==2&&firstrun)
					  DeathPathPHVaporate((ants+i)->nodes_visited);
			  continue;
           }
           LocalSearch_Move(i);//test
          /* printf("%d\n",(ants+i)->terminate);*/
           flag=1;
       }
	   //j++;//test
       if(flag==0)
           break;                     /******如果所有蚂蚁终止了搜索则退出此次解空间构造****/
    }     
}
/**************************蚂蚁移动函数****************************************/
void LocalSearch_Move(int oneant){     
     long neighbor[num_maxturns];  
	 //int visitedflag[num_maxturns]={NONE};
     float posscalpar[num_maxturns][6];          /***********可行路径参数传递数组**************/
     int i=0,j;
     RNode *rnode;
     TRNODE *trnode;
	 PTRNODE *ptrnode;
     ant_struct *ant;
     VNODES *vnode;
     int flag=1,xflag=1;//test j xflag为弧访问判断标志
     ant=ants+oneant;
     //printf("\nant%dcurnode:%d,ter:%d",oneant,ant->curNode->vnodeid,ant->terminate);//test
     vnode=ant->curNode;
	 if(vnode==NULL){
		 return;
	 }
     rnode=rNodes+(vnode->vnodeid);
     posscalpar[0][0]=rnode->RX;
     posscalpar[0][1]=rnode->RY;
     trnode=rnode->firstTRNode;
     for(;trnode!=NULL;trnode=trnode->nextTRNode){
		   flag=1;	
		  // if(1){j++;j--;}
		   if(ant->visitedflag[trnode->RNodeid]==1)
		  	   flag=0;
           /*
		   
           for(vnode=ant->nodes_visited;vnode!=NULL;vnode=vnode->nextVnode){
				if(vnode->vnodeid==trnode->RNodeid){
					flag=0;
					break;
				}
			}       /*蚂蚁禁忌表 */
            

		   // printf("\nant%d->turn:%d\n",oneant,trnode->RNodeid);//test
		  /*  for(vnode=ant->nodes_visited;vnode!=ant->curNode&&vnode!=NULL;vnode=vnode->nextVnode){		//已访问限制判断	  
			 // printf("ant%d->visited:%d\n",oneant,vnode->vnodeid);//test
              if(ant->curNode->vnodeid==vnode->vnodeid){
                  xflag=0;
				  //printf("\nzhiyanjl\n");
			      break;
              }
            }
			if(xflag)
			    flag=1;
			else{				
					vnode=vnode->nextVnode;
					if(vnode!=NULL){ 
						if(vnode->vnodeid==trnode->RNodeid)
                          flag=0;
                        else
					      flag=1;
					}
            }*/
			if(flag)
			   for(ptrnode=(rNodes+(ant->curNode->vnodeid))->firstPtrnode;ptrnode!=NULL;ptrnode=ptrnode->nextPtrnode){//转弯限制判断
				  if((ant->turNode==ptrnode->frnodeid)&&(ptrnode->nrnodeid==trnode->RNodeid)){
					 flag=0;
				     break;
				  }
			   }
            if(flag){
            // printf("****ant:%d->tovisit:",oneant);//test
              neighbor[i]=trnode->RNodeid;
			//  printf("%d",trnode->RNodeid); ///test
              posscalpar[i][2]=trnode->RX;
              posscalpar[i][3]=trnode->RY;
              posscalpar[i][4]=trnode->TWeight;
              posscalpar[i][5]=trnode->TPheromone;              
              i++;			
			} 
     }  /************禁止访问已经访问过的节点 禁止转弯考虑中***************/  
     if(i==0) {
         ant->terminate=2;      /********表示蚂蚁无路可走 终止此蚂蚁搜索***********/
		 //printf("\nwhat's wrong\n");
		 deadants++;
         return;
     }
    /* ant->terminate=1;
     return;*/
     flag=i;	
     CalculateTotal(&posscalpar[0][0],flag,ant->localstep);                 /***********************计算出各路段概率并存放在全局数组中此处有待考虑*****************/
     //LocalSearch();
	 if(totalchange>NestTurn)
	        ant->localstep=1;
     if(ran01(&seed)<q_0)
         SearchNeiBest(&neighbor[0],&posscalpar[0][0],totalpossibility,flag,oneant); /*******加快收敛 直接选取概率最大的那个********/ 
     else 
         SearchNeiMost(&neighbor[0],&posscalpar[0][0],totalpossibility,flag,oneant); /*****以概率随机选取*******/   
	
	 /*if(j>10){
	   ant->terminate=1;
       return;
	 }*///test
}

/*****************选取概率最大的路径***************************/
void SearchNeiBest(long neighbor[],float posspar[][6],float totalpossibility[],int num,int oneant){
    int i=0,max=0;
	VNODES *p;
	RNode *q;
	ant_struct *ant;
	float maxpossibility=0;
	for(i=0;i<num;i++){
		if(maxpossibility<totalpossibility[i]){
			max=i;
			maxpossibility=totalpossibility[i];
		}
	}
//	printf("the best route to choose is route%d\n",neighbor[max]);//test
    ant=ants+oneant;
	p=malloc(sizeof(VNODES));
	p->vnodeid=neighbor[max];
	p->nextVnode=NULL;
	ant->turNode=ant->curNode->vnodeid;
	ant->curNode->nextVnode=p;
	ant->curNode=p;
	ant->antcurrentpathdistance+=posspar[max][4];
	ant->visitedflag[neighbor[max]]=1;
	if(neighbor[max]==destnodeid){
		ant->terminate=1;
	}
	q=rNodes+neighbor[max];
//	testteaching();
//	AntTeaching(oneant,neighbor[max]);
    //AntLearning(oneant,neighbor[max]);
	/*学习函数待进一步测试 */
	if(ant->antcurrentpathdistance>q->bestnodepathdistance&&q->bestnodepathdistance!=0)
		AntLearning(oneant,neighbor[max]);
	else
		AntTeaching(oneant,neighbor[max]);	
}

void testteaching(){
	int i=0,j=100;
	for(i=0;i<100;i++)
		for(j=0;j<100;j++)
			;
}
/********************以概率选取路径**************************/
void SearchNeiMost(long neighbor[],float posspar[][6],float totalpossibility[],int num,int oneant){
	int i=0,poss=0;
	VNODES *p;
	RNode *q;
	ant_struct *ant;
	float rand=ran01(&seed);
	float allpossibility=0;
	for(i=0;i<num;i++){
		allpossibility+=totalpossibility[i];
		if(allpossibility>=rand){
            poss=i;
            break;
		}
	}
	if(i==num)
		poss=num-1;
//	printf("the most possible route to choose is route%d\n",neighbor[poss]);//test
    ant=ants+oneant;
	p=malloc(sizeof(VNODES));
	p->vnodeid=neighbor[poss];
	p->nextVnode=NULL;
	ant->turNode=ant->curNode->vnodeid;
	ant->curNode->nextVnode=p;
	ant->curNode=p;
	ant->antcurrentpathdistance+=posspar[poss][4];
	ant->visitedflag[neighbor[poss]]=1;
//	printf("%f  ",ant->antcurrentpathdistance);    //test
	if(neighbor[poss]==destnodeid){
		ant->terminate=1;
		return;
	}
	q=rNodes+neighbor[poss];
//	printf(" number:%d ",neighbor[poss]);
	//AntLearning(oneant,neighbor[poss]);
	//AntTeaching(oneant,neighbor[poss]);
	/* 学习函数待进一步测试 */
	if((ant->antcurrentpathdistance>q->bestnodepathdistance)&&q->bestnodepathdistance!=0)
		AntLearning(oneant,neighbor[poss]);
	else
		AntTeaching(oneant,neighbor[poss]);	 
}

/************************蚂蚁向路径上的最佳蚂蚁学习***************/
void AntLearning(int oneant,int nodeid){
	VNODES *vnode,*nodevnode,*head,*q;
	RNode *rnode;
	ant_struct *ant;
	rnode=rNodes+nodeid;
	ant=ants+oneant;
    ant->antcurrentpathdistance=rnode->bestnodepathdistance;
	ant->lastnodeid=nodeid;
}
void AntTeaching(int oneant,int nodeid){
	 RNode *rnode;
	 ant_struct *ant;
	 if(nodeid==sourcenodeid)
		 return;
	 rnode=rNodes+nodeid;
	 ant=ants+oneant;
	 rnode->bestnodepathdistance=ant->antcurrentpathdistance;
	 rnode->prevnodeid=ant->lastnodeid;
	 ant->lastnodeid=nodeid;
}


/************************蚂蚁向路径上的最佳蚂蚁学习***************/
void AntLearning2(int oneant,int nodeid){
	VNODES *vnode,*nodevnode,*head,*q;
	RNode *rnode;
	ant_struct *ant;
	ant=ants+oneant;
	head=NULL;
	rnode=rNodes+nodeid;
	EmptyOneantMemory(oneant);      /********清除该蚂蚁的记忆**********/	
	nodevnode=rnode->bestnodepath;	
	if(nodevnode==NULL)
		return;
	vnode=malloc(sizeof(VNODES));
	vnode->vnodeid=nodevnode->vnodeid;
	vnode->nextVnode=NULL;
	q=vnode;
	head=vnode;
    nodevnode=nodevnode->nextVnode;
	while(nodevnode!=NULL){
         vnode=malloc(sizeof(VNODES));
		 vnode->vnodeid=nodevnode->vnodeid;
		 vnode->nextVnode=NULL;
		 q->nextVnode=vnode;
		 q=vnode;
		 nodevnode=nodevnode->nextVnode;
	}
	ant->nodes_visited=head;
	ant->antcurrentpathdistance=rnode->bestnodepathdistance;
	ant->curNode=q;
}
void AntTeaching2(int oneant,int nodeid){
	VNODES *vnode,*nodevnode,*head,*q;
	float xx;
	RNode *rnode;
	ant_struct *ant;
	ant=ants+oneant;
	head=NULL;
	rnode=rNodes+nodeid;	
	nodevnode=rnode->bestnodepath;	
	for(;nodevnode!=NULL;){
		q=nodevnode;
		nodevnode=nodevnode->nextVnode;
		free(q);                   /****清除路节点记忆库*****/
	}
	rnode->bestnodepath=NULL;
	vnode=ant->nodes_visited;
	if(vnode==NULL)
		return;
	nodevnode=malloc(sizeof(VNODES));
    nodevnode->vnodeid=vnode->vnodeid;