pathplan.cpp

在vc6.0的环境下实现机器人路径规划

	m_midnum=0;
	for(i=0;i<m_posnum;i++)
	{ 
		if(m_pos[i].height<u.height&&m_pos[i].height<m_Kvh)
		{
			m_Midpos[m_midnum]=m_pos[i];
			m_midnum++;
		}
	}
}

//测试一个栅格是否在一条路径中
int Pathplan::testPosInPath(position u,path l)
{
	int i;
	for(i=l.pathlength-1;i>=0;i--)
	//for(i=0;i<l.pathlength;i++)
	{
		if(l.pos[i].x==u.x&&l.pos[i].y==u.y)
		{
			return 1;
		}
	}
    return 0;
}

//得到分流后的各路径
void  Pathplan::getMidPath(path p, position pos[MAX])
{
	int i;
	for(i=0;i<m_midnum;i++)
	{
		m_midpath[i]=p;
		m_midpath[i].pos[p.pathlength]=pos[i];
		m_midpath[i].pathlength=p.pathlength+1;
	}
}

//得到汇流后的路径
void Pathplan::getIntoPath(path p, path q, position s)
{
	int i,j,pm;
	m_intopath.pathlength=0;
	for(i=0;i<p.pathlength;i++)
	{
		if(p.pos[i].x==s.x&&p.pos[i].y==s.y)
		{
			pm=i;
			break;
		}
	}
	if(pm<=q.pathlength-1)
	{
		m_intopath=p;
	}
	else
	{
		for(i=0;i<q.pathlength;i++)
		{
			m_intopath.pos[i]=q.pos[i];
			m_intopath.pathlength++;
		}
		for(j=pm+1;j<p.pathlength;j++)
		{
			m_intopath.pos[m_intopath.pathlength]=p.pos[j];
			m_intopath.pathlength++;
		}
	}
}

//把第n条路径从路径表中删除
void Pathplan::removePath(int  n, path  s[MAX],int &num)
{
	int i;
	for(i=n;i<num;i++)
	{
		s[i-1]=s[i];
	}
	num--;
}

//进行消环操作
void Pathplan::removeCircle(path &p)
{
	int i,j,k;
	for(i=0;i<p.pathlength;i++)
	{
		for(j=i+1;j<p.pathlength;j++)
		{
			if(p.pos[i].x==p.pos[j].x&&p.pos[i].y==p.pos[j].y)
			{
				for(k=0;k<p.pathlength-j-1;k++)
				{
					p.pos[i+k]=p.pos[j+k];
				}
				p.pathlength=p.pathlength-j+i;
			}
			//
		}
	}
}

//得到最短路径
void Pathplan::getShortest(path s[MAX],int num)
{
	int i;
	m_resultpath=s[0];
	for(i=0;i<num;i++)
	{
		if(s[i].pathlength<m_resultpath.pathlength)
		{
			m_resultpath=s[i];
		}
	}
}

//设置机器人下一步要到达的位置
void Pathplan::setNextStep(path  p)
{
	m_nextstep=p.pos[1];
}

//根据以上的方法调用进行路径规划,参数x为目标点横坐标，y为目标点中坐标
void Pathplan::pathPlanAll(/*mapBase *pmap,dataBase *pdata,*/int aimx,int aimy)
{
	int i; 
	int j;
	int k;


	m_Kvh=59;
    m_kg=1;
    m_r=0.4;

    m_Spathnum=0; //路径表S初始化为空表
    m_Mpathnum=0; //路径表MI初始化为空表

	m_aimpos.x=aimx; //目标点横坐标 
	m_aimpos.y=aimy; //目标点纵坐标
	m_aimpos.height=m_VMap[m_aimpos.x][m_aimpos.y];


//    plan(/*pmapbase*/);//通过从图库传来的指针，设置本地地图信息

    setOposition( );//求出障碍物的位置和数目，保存在m_Oposition[]和m_onum中



    setVmap(m_kg,  m_r,  m_onum, m_Oposition);//把地图信息转换成虚拟地图信息
/*	CRobotViewApp* myapp=(CRobotViewApp*)AfxGetApp();
    CMainFrame* mainframe=(CMainFrame*)myapp->m_pMainWnd;
	GetVmap(mainframe->VirtualMap);
	Route* route=(Route*)mainframe->m_wndSplitter_Horizontal.GetPane(1,0);
	route->DrawRoute();*/


	for(i=0;i<40;i++){
		for(j=0;j<20;j++){
			if(m_VMap[i][j]<10)
				printf("  %d   ",m_VMap[i][j]);
			else if(m_VMap[i][j]!=100)
				printf(" %d   ",m_VMap[i][j]);
			else
				printf("%d   ",m_VMap[i][j]);
		}
		//printf("\n");
	}

    if(m_onum==0)
	{
		m_Kvh=59;
	}
	else
	{
		;//	m_Kvh=getKVH(m_Oposition);//得到危险临界值
	}

 //   setRobotPos(/*pdatabase*/);//得到机器人目前的位置

	if(m_aimpos.x==m_robotpos.x&&m_aimpos.y==m_robotpos.y)
	{
		;//如果目标点和机器人位置相同，则不动，否则，进行路径规划算法
	}
	else
	{
		path us;
		us.pos[0]=m_robotpos;
		us.pathlength=1;
		pathIntoPlist(m_S, us,m_Spathnum);//把机器人初始位置放到路径标m_S中

		m_Fpathnum=0;//置F为空

		int flag=1;//判断m_S中是否存在符合条件的路径的标志，为1时，表示存在，继续循环

		while(flag!=0)
		{
			flag=0;
			
			if(m_Spathnum==0)
				break; //如果m_S中没有路径存在，则结束循环，跳出看有没有可行路径
			else
			{
				m_Mpathnum=0; 
				path L=m_S[0];//取m_S的第一条路径
				removePath(1,m_S,m_Spathnum); //把第一条路径从路径表m_S中取出然后删除

				
				if(L.pos[L.pathlength-1].x==m_aimpos.x&&L.pos[L.pathlength-1].y==m_aimpos.y)
				{
					pathIntoPlist(m_F, L,m_Fpathnum);//此路径正好是一条可行路径，则把此路径放到路径表F中

					for(i=0;i<m_Spathnum;i++)//测试是否可继续循环
					{
						if(!(m_S[i].pos[m_S[i].pathlength-1].x==m_aimpos.x&&m_S[i].pos[m_S[i].pathlength-1].y==m_aimpos.y))
							flag=1;
					}
					continue;
				}
				else//此路径终点不是目标点
				{
					int exist=0;//为0时，表示虚拟高度都大于m_Kvh
					int  high=getMinHeight(L.pos[L.pathlength-1]);

					for(i=0;i<m_posnum;i++)
					{
						if(m_pos[i].height<m_Kvh)
						{
							exist=1;
							break;
						}
					}
					if(exist==0)
					{
						for(i=0;i<m_Spathnum;i++)//测试是否可继续循环
						{
							if(!(m_S[i].pos[m_S[i].pathlength-1].x==m_aimpos.x&&m_S[i].pos[m_S[i].pathlength-1].y==m_aimpos.y))
								flag=1;
						}
						continue;
					}
					else{
						int allmax=0;//为0时，表示虚拟高度都大于L末端栅格的虚拟高度
						
						for(i=0;i<m_posnum;i++)
						{
							if(m_pos[i].height<L.pos[L.pathlength-1].height)
							{
								allmax=1;
								break;
							}
						}
						if(allmax==0)
						{
							reserveWater(L.pos[L.pathlength-1], high);
						}

						setMidPos(L.pos[L.pathlength-1]);

						position temp;

						for(i=0;i<m_midnum;i++)//对midpos[]中各元素进行排序
						{
							for(j=0;j<m_midnum-i;j++)
							{
								if(m_Midpos[i].height<m_Midpos[j].height)
								{
									temp=m_Midpos[i];
									m_Midpos[i]=m_Midpos[j];
									m_Midpos[j]=temp;
								}
							}
						}

						getMidPath(L, m_Midpos); //分流操作

						int inflag=0;//标志路径是否符合直接放到M,0适合
      ////////////////////////////////////////////////////////////////////////////
						m_removeNum=0;
						for(i=0;i<m_midnum;i++)
						{
							inflag=0;
							for(j=0;j<m_Spathnum;j++)
							{
								if(testPosInPath(m_Midpos[i],m_S[j]))
								{
									removeOfItem(j);
									inflag=1;
									getIntoPath(m_S[j],m_midpath[i],m_Midpos[i]);//汇流操作


									int equalflag=0;//如果汇流结果和MI中的某条路径相同，则不把它放到MI中
									for(int g=0;g<m_Mpathnum;g++){
										if(pathEqual(m_intopath,m_MI[g])==1)
											equalflag=1;
									}
									if(equalflag==0)



										pathIntoPlist(m_MI,m_intopath,m_Mpathnum);//把汇流结果放入路径表M中

								}
							}
							if(inflag==0)
								pathIntoPlist(m_MI,m_midpath[i],m_Mpathnum);
						}


						//对m_removeItem[]中的元素进行排序，最大的在前面，以便从m_S中删除时，序号不会变
						int temp1;
						for(i=0;i<m_removeNum;i++){
							for(j=i+1;j<m_removeNum;j++){
								if(m_removeItem[i]<m_removeItem[j]){
									temp1=m_removeItem[i];
									m_removeItem[i]=m_removeItem[j];
									m_removeItem[j]=temp1;
								}
							}
						}

						for(int in=0;in<m_removeNum;in++){
							for(int ou=0;ou<m_Spathnum;ou++){
								if(m_removeItem[in]==ou)
									removePath(ou+1,m_S,m_Spathnum);
							}
						}


						if(m_Mpathnum!=0){
							for(i=0;i<m_Mpathnum;i++){
								removeCircle(m_MI[i]);
								pathIntoPlist(m_S,m_MI[i],m_Spathnum);
							}
						}
					}
				}
			}
			for(i=0;i<m_Spathnum;i++)
			{
				if(!(m_S[i].pos[m_S[i].pathlength-1].x==m_aimpos.x&&m_S[i].pos[m_S[i].pathlength-1].y==m_aimpos.y))
					flag=1;
			}
		}
		for(i=0;i<m_Spathnum;i++){
			pathIntoPlist(m_F,m_S[i],m_Fpathnum);
		}
		if(m_Fpathnum!=0)
		{
			for(i=0;i<m_Fpathnum;i++){
				removeCircle(m_F[i]);//对每条路径进行消环操作
			}
			
			getShortest(m_F, m_Fpathnum);//得到最短可行路径
			setNextStep(m_resultpath);//设置下一步要到达的位置
			
		}
		else
		{
			;//找不到可行路径
		}
	}
}
//	main(){
	//dataBase *data1;
	//mapBase *map1;
//	Pathplan npath;
//	npath.pathPlanAll(/*map1,data1,*/3,7);
//	}