当前位置：文档库 › C++AGV 路径规划与运行仿真程序

C++AGV 路径规划与运行仿真程序

程序代码

#include

#define MAX 100

#define maxD 99999

class CFixedMap

{

struct Coordinate

{

int x;

int y;

int num;

};//记录点的结构体

struct combp

{

int x1,y1;

int x2,y2;

};//连通点坐标

struct Combname

{

int num1;

int num2;

};//连通点名

public:

int mVexNum; // 顶点数

int mEdgNum; // 边数

double mMatrix[MAX][MAX]; // 邻接矩阵

Combname comb[MAX]; //连通点名

combp cp[MAX]; //连通点坐标

Coordinate mVexs[MAX]; // 顶点集合

int getx(int n1);

int gety(int n2);

void Input();//以文件方式输入地图信息

void ShowMap();//easyx显示地图

};

int CFixedMap::getx(int n1)

{

int x=0;

for(int i=0;i

if(n1==mVexs[i].num)

x=mVexs[i].x;

return x;

}

int CFixedMap::gety(int n2)

{

int y=0;

for(int i=0;i

if(n2==mVexs[i].num)

y=mVexs[i].y;

return y;

}

void CFixedMap::Input()

{

int panju;

char fname[10]={"map2.txt"};

cout<<"是否载入默认地图？（1:是，2:否）";

cin>>panju;

if(panju==2)

{

cout<<"请输入地图信息文件名:";

cin>>fname;

}

ifstream file(fname);

char use1[100];

char use2[100];

char use3[100];

char use4[100]; //use数组用来储存提示信息。

file>>use1;

file>>mVexNum; //端点个数

file>>use2;

file>>mEdgNum; //边数

file>>use3;

for(int i=0;i

{

file>>mVexs[i].num>>mVexs[i].x>>mVexs[i].y;

}

file>>use4;

for(i=0;i

{

file>>comb[i].num1>>comb[i].num2;

}

file.close();

}

//easyx显示地图

void CFixedMap::ShowMap()

{

initgraph(800,600,SHOWCONSOLE| NOCLOSE); //初始化绘图环境setbkcolor(WHITE); //设置背景色

cleardevice(); //用背景色清空屏幕

setfillcolor(BLACK); //设置填充色

//画顶点

for(int i=0;i

{

fillcircle(mVexs[i].x,mVexs[i].y,5);

settextcolor(BLACK);

char s[MAX];

sprintf(s, "%d", i+1);

outtextxy(mVexs[i].x-10,mVexs[i].y-20,s);

}

//给实际连通的顶点连线

setlinecolor(RGB(0,0,0));

setlinestyle(PS_SOLID,6,NULL,0);

for(i=0;i

{

cp[i].x1=getx(comb[i].num1);

cp[i].y1=gety(comb[i].num1);

cp[i].x2=getx(comb[i].num2);

cp[i].y2=gety(comb[i].num2);

line(cp[i].x1,cp[i].y1,cp[i].x2,cp[i].y2);

}

class CPathPlan:public CFixedMap

{

public:

int path[MAX]; //存放最后一个点

double dist[MAX]; //存放路径距离int v0; //起始点

int route[40]; //路径

int rnum; //路径顶点数

Coordinate rpoints[MAX];//路径点集

void MaptoGraph();

void Dijstra();

void Putpath();

};

void CPathPlan::MaptoGraph()

{

rnum=0;

int a,b,x,y;

for(int i=0;i

{

for(int j=0;j

{

if(i==j)

{

mMatrix[i][j]=0;

}

else

{

mMatrix[i][j]=maxD;

}

for(i=0;i

{

if (comb[i].num1)

{

a=comb[i].num1-1;

b=comb[i].num2-1;

x=mVexs[a].x-mVexs[b].x;

y=mVexs[a].y-mVexs[b].y;

mMatrix[a][b]=sqrt(x*x+y*y);

mMatrix[b][a]=sqrt(x*x+y*y);

}

void CPathPlan::Dijstra()

{

cout<<"输入起点:";

cin>>v0;

v0--;

int s[MAX];

int v;

int i;

int j;

int w;

double min;

for(v=0;v

{

s[v]=0; //0表示未求出最短路径

dist[v]=mMatrix[v0][v]; //开始时假定为最短路径

if(dist[v]

path[v]=v0; //直达情况

else path[v]=-1; //无直达路径

}

dist[v0]=0; //初始时v0属于s集，v0到v0路径最短

s[v0]=1;

for(i=1;i

{

min=maxD;

for(w=0;w

if(s[w]==0 && dist[w]

{

v=w; //经点w中转

min=dist[w];

}

s[v]=1; //将v并入S,由v0到达v点最短距离为min,假设由v0到v再由v 到其余各点，更新当前最后一个点及距离

for(j=0;j

if(s[j]==0 && (min+mMatrix[v][j]

{

dist[j]=min+mMatrix[v][j];

path[j]=v; //点的序号

}

void CPathPlan::Putpath()

{

int opp[20];

int i=0;

cout<<"输入终点:";

cin>>z;

z--;

int next;

ofstream file2("fops.txt");

if(dist[z]

{

opp[i++]=mVexs[z].num; //终点

next=path[z]; //第一个点

while(next!=v0)

{

opp[i++]=mVexs[next].num;

next=path[next]; //下一个点

}

opp[i]=mVexs[v0].num;

file2<<"路径长度为:"<

else if(z!=v0) //无路径

file2<

for(int j=0;j

route[j]=opp[i-j];

file2<<"路径为:";

for(j=0;j

file2<";

rnum=i+1;

file2<

file2.close();

for(i=0;i

{

rpoints[i].x=getx(route[i]);

rpoints[i].y=gety(route[i]);

rpoints[i].num=route[i];

}

class CAGV

{

int speed; //AGV速度

int width; //AGV宽度

int length; //AGV长度

public:

int getl();

int getw();

int gets();

};

CAGV::CAGV()

{

int panju;

cout<<"是否自动设置AGV信息?（1:是,2:否）";

cin>>panju;

speed=8;

width=6;

length=6;

if(panju==2)

{

cout<<"输入AGV的长:";

cin>>length;

cout<<"输入AGV的宽:";

cin>>width;

cout<<"输入AGV的速度(1-10):";

cin>>speed;

}

int CAGV::getl()

{

return length;

}

int CAGV::getw()

{

return width;

}

int CAGV::gets()

{

return speed;

}

class CAGVShow:public CPathPlan,public CAGV

{

public:

#define k (abs(y2-y1))/(abs(x1-x2))

int a,b,c,d;

int x1,x2,y1,y2;

int l,w,s;

void show();

};

void CAGVShow::show()

{

l=getl();

w=getw();

s=11-gets();

for(int o=0;o

{

if(o+1==rnum)break;

x1=rpoints[o].x;

x2=rpoints[o+1].x;

y1=rpoints[o].y;

y2=rpoints[o+1].y;

if(y1!=y2&&x1!=x2)

{

if(x2>x1&&y2>y1){

for(j=1;j

{

a=x1+j-l;

b=y1+j*k+w;

c=x1+j+l;

d=y1+j*k-w;

setfillcolor(RGB(0,0,0));

rectangle(a,b,c,d);

clearrectangle(a-1,b-k,c,d-k);

setlinecolor(RED);

line(x1+j,y1+j*k,x2,y2);

setlinecolor(GREEN);

line(x1,y1,x1+j,y1+j*k);

if(j%2==0)

{

setfillcolor(RGB(255,255,0));

circle(x2,y2,5);

}

Sleep(s);

}

if(x2

for(j=1;j

{

a=x1-j-l;

b=y1-j*k+w;

c=x1-j+l;

d=y1-j*k-w;

setfillcolor(RGB(0,0,0));

rectangle(a,b,c,d);

clearrectangle(a-1,b-k,c,d-k);

setlinecolor(RED);

line(x1-j,y1-j*k,x2,y2);

setlinecolor(GREEN);

line(x1,y1,x1-j,y1-j*k);

if(j%2==0)

{

setfillcolor(RGB(255,255,0));

circle(x2,y2,5);

}

Sleep(s);

}

if(x2y1)

{

for(j=1;j

{

a=x1-j-l;

b=y1+j*k+w;

c=x1-j+l;

d=y1+j*k-w;

setfillcolor(RGB(0,0,0));

rectangle(a,b,c,d);

clearrectangle(a-1,b-k,c,d-k);

setlinecolor(RED);

line(x1-j,y1+j*k,x2,y2);

setlinecolor(GREEN);

line(x1,y1,x1-j,y1+j*k);

if(j%2==0)

{

setfillcolor(RGB(255,255,0));

circle(x2,y2,5);

}

Sleep(s);

}

if(x2>x1&&y2

{

for(j=1;j

{

a=x1+j-l;

b=y1-j*k+w;

c=x1+j+l;

d=y1-j*k-w;

setfillcolor(RGB(0,0,0));

rectangle(a,b,c,d);

clearrectangle(a-1,b-k,c,d-k);

setlinecolor(RED);

line(x1+j,y1-j*k,x2,y2);

setlinecolor(GREEN);

line(x1,y1,x1+j,y1-j*k);

if(j%2==0)

{

setfillcolor(RGB(255,255,0));

circle(x2,y2,5);

}

Sleep(s);

}

else if(y1==y2)

{

if(x2>x1)

{

for(j=1;j<(x2-x1);j++)

{

setfillcolor(RGB(0,0,0));

rectangle(x1+j-l,y1+w,x1+j+l,y1-w);

clearrectangle(x1+j-l-1,y1+w,x1+j+l-1,y1-w);

setlinecolor(RED);

line(x1+j,y1,x2,y2);

setlinecolor(GREEN);

line(x1,y1,x1+j,y1);

if(j%2==0)

{

setfillcolor(RGB(255,0,255));

circle(x2,y2,5);

}

Sleep(s);

}

if(x2

{

for(j=1;j<(x1-x2);j++)

{

setfillcolor(RGB(0,0,0));

rectangle(x1-j-l,y1+w,x1-j+l,y1-w);

clearrectangle(x1-j-l-1,y1+w,x1-j+l-1,y1-w);

setlinecolor(RED);

line(x1-j,y1,x2,y2);

setlinecolor(GREEN);

line(x1,y1,x1-j,y1);

if(j%2==0)

{

setfillcolor(RGB(255,0,255));

circle(x2,y2,5);

}

Sleep(s);

}

else if(x1==x2)

{

if(y2>y1)

{

for(j=1;j<(y2-y1);j++)

{

setfillcolor(BLUE);

rectangle(x1-l,y1+j+w,x1+l,y1+j-w);

setfillcolor(WHITE);

rectangle(x1,y1+j+w-1,x1+1,y1+j-1-w);

clearrectangle(x1-l-1,y1+(j-1)+w,x1+l-1,y1+(j-1)-w);

setlinecolor(RED);

line(x1,y1+j,x2,y2);

setlinecolor(GREEN);

line(x1,y1,x1,y1+j);

if(j%2==0)

{

setfillcolor(RGB(255,255,0));

circle(x2,y2,5);

}

Sleep(s);

}

if(y2

{

for(j=1;j<(y1-y2);j++)

{

setfillcolor(BLUE);

rectangle(x1-l,y1-j+w,x1+l,y1-j-w);

setfillcolor(WHITE);

rectangle(x1,y1-j+w-1,x1+1,y1-j-1-w);

clearrectangle(x1-l-1,y1-(j-1)+w,x1+l-1,y1-(j-1)-w);

setlinecolor(RED);

line(x1,y1-j,x2,y2);

setlinecolor(GREEN);

line(x1,y1,x1,y1-j);

if(j%2==0)

{

setfillcolor(RGB(255,255,0));

circle(x2,y2,5);

}

Sleep(s);

}

cout<<"成功显示!!\n";

getch(); // 按任意键继续

closegraph(); // 关闭图形界面

}

void main()

{

CAGVShow a;

int panju;

a.Input();

cout<<"多次演示？（1:是，2:否）";

cin>>panju;

if(panju==1)

{

for(int i=0;i<20;i++)

{

a.ShowMap();

a.MaptoGraph();

a.Dijstra();

a.Putpath();

a.show();

panju=0;

cout<<"是否结束？(1:结束)";

cin>>panju;

if(panju==1)

break;

}

else

{

a.ShowMap();

a.MaptoGraph();

a.Dijstra();

a.Putpath();

a.show();

}

多机器人路径规划研究方法

多机器人路径规划研究方法张亚鸣雷小宇杨胜跃樊晓平瞿志华贾占朝摘要：在查阅大量文献的基础上对多机器人路径规划的主要研究内容和研究现状进行了分析和总结，讨论了多机器人路径规划方法的评判标准，并阐述了研究遇到的瓶颈问题，展望了多机器人路径规划方法的发展趋势。关键词：多机器人；路径规划；强化学习；评判准则 Abstract:This paper analyzed and concluded the main method and current research of the path planning research for multi robot.Then discussed the criterion of path planning research for multi robot based large of literature.Meanwhile,it expounded the bottleneck of the path planning research for multi robot，forecasted the future development of multi robot path planning. Key words:multi robot；path planning;reinforcement learning;evaluating criteria 近年来，分布式人工智能(DAI)成为人工智能研究的一个重要分支。DAI研究大致可以分为DPS（distributed problem solving）和MAS（multi agent system）两个方面。一些从事机器人学的研究人员受多智能体系统研究的启发，将智能体概念应用于多机器人系统的研究中，将单个机器人视做一个能独立执行特定任务的智能体，并把这种多机器人系统称为多智能体机器人系统（MARS）。因此，本文中多机器人系统等同于多智能体机器人系统。目前，多机器人系统已经成为学术界研究的热点,而路径规划研究又是其核心部分。机器人路径规划问题可以建模为一个带约束的优化问题，其包括地理环境信息建模、路径规划、定位和避障等任务，它是移动机器人导航与控制的基础。单个移动机器人路径规划研究一直是机器人研究的重点，且已经有许多成果[1~3]，例如在静态环境中常见的有连接图法、可视图法、切线图法、Voronoi图法、自由空间法、栅格法、拓扑法、链接图法、Dempster Shafer 证据理论建图等；动态环境中常见的有粒子群算法、免疫算法、遗传算法、神经网络、蚁群算法、模拟退火算法、人工势场法等。然而，多机器人路径规划

路径规划论文：路径识别与路径规划方法的研究

路径规划论文：路径识别与路径规划方法的研究【中文摘要】随着科学技术的发展,各种各样的机器人陆续出现,机器人越来越受到人们的重视,而中国大学生机器人大赛暨Robocup 公开赛更是吸引了很多爱好者。机器人游中国可看成迷你的旅游,这跟目前假期短,如何能够合理安排,多参观几个景点的问题相吻合,故也大大引起大家的兴趣。本文以游中国机器人为研究对象,研究路径规划和路径识别的方法。基本思想是:首先根据比赛要求,建立大赛的基本界面平台,对图像进行数据存储,为遍历做准备。然后根据经典的遍历方法,通过改进来实现景点的遍历,比较得到一条较理想的旅游路线。另一方面机器人在前进时要根据看到的路况来控制速度,因此对道路情况进行提取,并通过一系列方法去由环境等引起除噪声及冗余信息,得到较好的道路目标。然后对前方路况的几种情况进行提取分析,利用角点检测的方法来确定分支数,使机器人能够更好的选择路线。具体实现过程如下:1.地图的存储。首先建立大赛涉及的基本界面平台,分析比较了几种常用的图存储方法的优劣。2.遍历算法。首先介绍了人工智能中常用的几种遍历算法,并对各个算法的复杂性和适用情况进行分析比较。根据该项目中的需要加以改进,得到不同的遍历路线。再... 【英文摘要】With the development of science and technology, a variety of robots showed up, that’s the reason that more and more people pay attention to the robot. However, Robot

路径规划算法

[选取日期] NUAA未知环境的动态路径规划 [键入文档副标题] | 刘绍翰

度量距离灰度化四连通和8连通。第一章、静态搜索与A*算法很多时候，我们需要在一个图中寻找一条从源点到目标节点的最短路径，我们称之为路径规划。搜索算法主要分为，盲目搜索和启发式搜索，它们的一个作用是能够从解空间中需找一条从源点到目标节点的最短路径。启发式搜索是在搜索的过程中，参考一定的指标函数来决定搜索的策略。迪杰斯特拉算法，类似于广度优先遍历，利用源点到当前节点的代价值作为指标，其一定可以获得从原点到目标节点的最短路，但是其访问的节点数很多。而最好优先搜索，采用离标节点的距离作为搜索的代价参考值，贪心选择最小的扩展节点，也可以获得最短路径，而且其搜索的节点数目大大减少。图1 迪杰斯特拉算法图2 最好优先搜索算法当地图中包含障碍物时，迪杰斯特拉算法，仍然可以获得最短路径的路径，最好优先搜索的节点尽管少，但是其不能获得最优解。图3 迪杰斯特拉算法图4 最好优先搜索算法而A*算法，参考了从原点到当前节点的代价值和当前节点到目标节点启发值，综合了迪杰斯特拉算法和做好优先搜索算法优点，在有障碍物和无障碍物的地图上，可以像迪杰斯特拉算法一样求得最短路径同时，同时能够像最好优先搜索一样减少搜索范围，减少搜索节点的数目。

图5 无障碍物时A*路径规划图6 有障碍物时A*路径规划算法经典的迪杰斯特拉算法可以求得最短的路径，而启发式搜索A* 算法，不但可以求得最短路，而且可以使得搜索的范围大大减少，上述算法是传统的静态路径规划算法，其规划的前提条件是已经知地图的结构。A*算法属于离线事先规划，在规划完毕之后，可以沿着最优路径移动，不是在线规划，不能一边规划一边移动。 A*算法的基本理论 A*算法又叫做启发式搜索算法，具有悠久的历史，其启发函数f=g+h 。其中g 表示从原点到当前节点已经付出的代价，好表示从当前节点到目标节点的启发值。 1) A*算法必须满足h(x)<=h*(x)，其中h*(x)是实际的启发值，h*(x)在实际中通常是无法事先得知的，但是这个条件是很容易满足，只要满足该条件，一定能够获得最优解。 2) 如果最短路径长度为C*, 则在算法结束前，open 表中至少有一个节点n, 满足 f(n) <= C*. 这个性质可以这样理解，因为最短路径存在，我们不妨设它为: source->a->b->c->...->n->.....->goal. 且在当前时刻，路径中在节点n 前的节点都在closed 表中，即已经扩展了，而节点n 自己在open 表中（注意：算法结束前任意时刻都有这样的节点n 存在）。则由于该条路劲是最短路径，我们可以知道此时在open 表中的n 的 g(n)值已经是准确值, 即最小值了。而 f(n) = g(n) + h(n) = g*(n) + h(n) <= g*(n) + h*(n) = C* . (最后一个式子取等号是由于n 在最短路径上) 有了这个性质，我们就知道，当A*算法扩展到目标节点时，必有f(goal) = g(goal) <= C* (即 = C*)。否则, 如果f(goal) > C*,由于目标节点是被扩展节点, 则open 表中其他任意其他节点t, 都有f(t) >= f(goal) > C*, 和性质1 矛盾。 3) 扩展新节点时很容易出现重复节点的问题，从上面的伪代码可以看出，如果新扩展节点已经存在于closed 表中，且f 值比表中节点的f 值还要小的话，则除了更新该节点f 值，还需要重新扩展该节点，这简直就是把人从棺材里拖出来。但是如果h 函数满足相容性，这一步就可以省掉了。所谓相容性就是指对任意节点s1,都满足： h(s1) <= h(s2) + c(s1,s2) (其中c(s1,s2)是指从s1转移到s2的代价)有这个性质我们在不等号两边加上g(s1), 则有 g(s1) + h(s1) <= h(s2) + g(s1) + c(s1,s2)。如果我们此时扩展s1, 而s2又是能被s1扩展的节点，则由这个式子我们得到 f(s1) <= f'(s2). (若s2之前就已经被扩展出了，则当前的f(s2)可能比f'(s2)小) 这个式子的意义在于由当前节点进行扩展这个方案下得到的节点的f 值总比当前扩展节点的f 值大（子节点总比父节点