移动机器人未知环境探险（C语言）
这个项目是本人的高级语言（C语言）程序设计这门课程的实验项目，项目要求的所有功能（在下面的项目描述中）基本都已实现，上传一下供大家交流学习（编译环境为Visual Studio Code(MinGW-w64编译器)）。
项目描述：
考虑一个简单的移动机器人，它能够在平面上沿着可以行走的路径移动，并能够记录自身的运行轨迹。假定在每一个地点，机器人移动的方向只能是前、后、左、右、右前、右后、左前和左后八个方向，而且也只能感知到其周围局部八个位置的信息(是可行路径、不可达区域还是目标点)。编写程序，由文件读取一张100x100 个格点的平面地图('.‘表示不可行走的地域，’+‘表示机器人可行走的路径，’*‘表示机器人要寻找的探险目标点，’#'表示特殊重要的、必须到达的目标点)，让移动机器人按照感知和移动规则在该地图所表示的区域内探险。
要求：1) 机器人能找到所有的目标点和路径然后返航；2) 根据探险的经历绘制出该区域的目标-路线图(注意，不能直接把所给出的地图画出来就行了，因为机器人的目标是探险，它事先并不知道全局的目标和路径分布，只能通过移动和局部感知信息并记录下来的手段来重建地图)，完成我们派遣机器人对该领域探险的目的。注意，地图中路线是假想路径,可能导致闭合或不可到达。3) 尝试使用最省能量的方法再次遍历所有目标点。
实现思路及代码
项目实现采用模块化编程，分为主模块 main.c 文件，子模块 rt.h 、 rt.c 文件。
首先考虑的是地图文件的读写以及函数的调用，这里使用二维数组来实现地图文件的读写，文件格式为TXT（地图可参考下面给出）。
文件读写操作如下：
主模块：（即main.c文件）：
#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include"rt.h"
int main() {
char store[MaxLNum][MaxCNum] = {0}; //建立存储区并初始化
FILE *fp = fopen("map.txt","r");
int Clen; /*行字符个数*/
int Llen = 0;
int i,j;
if(fp==NULL) {
printf("Can't find the file!");
exit(0);
}
for(i=0;i<MaxLNum;i++) {
if(!feof(fp)) {
fgets(store[i],MaxLNum,fp);
Llen++;
}
else break;
}
Llen = Llen;
Clen = strlen(store[0])-1;
fclose(fp);
char expor[MaxLNum][MaxCNum];
for(i=0;i<Llen;i++){
for(j=0;j<Clen;j++) {
expor[i][j] = ' ';
}
} //建立存储区并初始化
Nicolas(store,expor,Llen,Clen); //调用函数使机器人开始探险
FILE *fpw = fopen("record_data.txt","w+");
for(i=0;i<Llen;i++) {
for(j=0;j<Clen;j++) {
fprintf(fpw,"%c",expor[i][j]);
printf("%c",expor[i][j]);
}
fprintf(fpw,"\n");
printf("\n");
}
fclose(fpw);
int goal_num = 0,s_num = 0;
for(int k=0;k<MaxSize;k++)
if(goal[k][0]&&goal[k][1]) goal_num++;
for(int k=0;k<MaxSize;k++)
if(s_vital[k][0]&&s_vital[k][1]) s_num++;
printf("The number of goal points and special points are %d,%d.\n",goal_num,s_num);
for(int i=0;i<goal_num;i++) g_nodes[i] = goal[i][0]*Clen+goal[i][1];
for(int i=0;i<s_num;i++) s_nodes[i] = s_vital[i][0]*Clen+s_vital[i][1];
dijkstra(0,g_nodes,Llen,Clen);
for(int i=0;i<goal_num;i++){
print_path(pre,0,goal[i][0]*Clen+goal[i][1],Clen);
printf("\n");
}
return 0;
}
功能实现（子模块）
这里我采用的是BFS（广度优先搜索）来使机器人行走并完成探险，绘制地图。其中保存机器人已经行走的路径所用的数据结构为队列（以链表表示），并基于此来实现广度优先搜索。
最短路径算法采用的是著名的迪杰斯特拉（dijkstra）算法，计算出最短路径以便于机器人以最省能量的方法对地图上的目标点进行遍历。
代码如下：
头文件（rt.h）：
#ifndef FUNC_H_INCLUDED
#define FUNC_H_INCLUDED
#define MaxLNum 110
#define MaxCNum 110
#define MaxSize 10100
#define inf 10000
extern int arcs[MaxSize][MaxSize];
extern int s_nodes[MaxSize];
extern int g_nodes[MaxSize];
extern int dist[MaxSize];
extern int visited[MaxSize];
extern int pre[MaxSize];
extern int s_path[MaxSize][MaxSize];
extern int goal[MaxSize][2];
extern int s_vital[MaxSize][2];
//定义机器人(结构体)。
struct Robot{
int Pos[2]; //当前位置
char CTYPE; //当前的字符类型
struct ArEle{
char CType;
int flag;
}Around[8]; //周围结点的字符类型及其标记（从North开始，沿顺时针排列）
};
typedef struct QNode* Queue;
typedef struct Robot* PtrRt;
typedef struct Node* PtrToNode;
struct Node{ //队列中的结点
PtrRt Rt;
PtrToNode Next;
};
struct QNode {
PtrToNode Front, Rear; // 队列的头、尾指针
};
Queue CreateQueue();
Queue AddQ( Queue Q, PtrRt Rt );
int IsEmpty( Queue Q );
PtrRt DeleteQ( Queue Q );
int** around(int pos[2]);
int Judge(char c);
void Record(PtrRt Rt,Queue Q,char expor[][MaxCNum]);
PtrRt CreateRt(int x,int y,char store[][MaxCNum],int Llen,int Clen);
void save_path(PtrRt Rt_1,PtrRt Rt_2,int Clen);
PtrRt move(PtrRt Rt,int pos[2],char store[][MaxCNum],int Llen,int Clen);
void BFS(PtrRt Rt,Queue Q,char store[][MaxCNum],char expor[][MaxCNum],int Llen,int Clen);
void print_path(int path[],int u, int v,int Clen);
void dijkstra(int begin,int nodes[],int Llen,int Clen);
void Nicolas(char store[][MaxCNum],char expor[][MaxCNum],int Llen,int Clen);
#endif // FUNC_H_INCLUDED
具体实现（rt.c）：
#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include"rt.h"
int goal[MaxSize][2];
int s_vital[MaxSize][2];//建立重要点地存储区
int arcs[MaxSize][MaxSize];//建立邻接矩阵以保存路径
int dist[MaxSize];
int visited[MaxSize] = {0};
int pre[MaxSize];//存放路径
int s_path[MaxSize][MaxSize];//建立最短路径存储区(坐标转化为单个数字(下表为i*5+j))
int s_nodes[MaxSize];
int g_nodes[MaxSize];
Queue CreateQueue() {
Queue Q;
Q = (Queue)malloc(sizeof(struct QNode));
Q->Front = NULL;
Q->Rear = NULL;
return Q;
}
int IsEmpty(Queue Q){
return (Q->Front == NULL);
}
Queue AddQ(Queue Q,PtrRt Rt) {
PtrToNode Cell;
Cell = (PtrToNode)malloc(sizeof(struct Node));
Cell->Rt = Rt;
Cell->Next = NULL;
if(IsEmpty(Q)){
Q->Front = Cell;
Q->Rear = Cell;
}
else{
Q->Rear->Next = Cell;
Q->Rear = Cell;
}
return Q;
}
PtrRt DeleteQ(Queue Q){
PtrToNode Cell;
PtrRt FrontElem;
if (IsEmpty(Q)) {
printf("The queue is enpty!");
return 0;
}
else {
Cell = Q->Front;
if (Q->Front == Q->Rear) // 若队列只有一个元素
Q->Front = Q->Rear = NULL; // 删除后队列置为空
else
Q->Front = Q->Front->Next;
FrontElem = Cell->Rt;
free(Cell); // 释放被删除结点空间
return FrontElem;
}
}
//获取某一位置周围的元素的坐标
int** around(int pos[2]) {
int **a;
a = (int**)malloc(8*sizeof(int));
for(int i=0;i<8;i++) {
a[i] = (int*)malloc(2*sizeof(int));
}
a[2][0] = a[6][0] = pos[0];
a[0][1] = a[4][1] = pos[1];
a[0][0] = a[1][0] = a[7][0] = pos[0]-1;
a[1][1] = a[2][1] = a[3][1] = pos[1]+1;
a[3][0] = a[4][0] = a[5][0] = pos[0]+1;
a[5][1] = a[6][1] = a[7][1] = pos[1]-1;
return a;
}
//进行字符的判断.
int Judge(char c) {
if(c=='+') return 3;
else if(c=='#') return 6;
else if(c=='*') return 9;
else return 0;
}
//进行数据的存储与记录
void Record(PtrRt Rt,Queue Q,char expor[][MaxCNum]) {
expor[Rt->Pos[0]][Rt->Pos[1]] = Rt->CTYPE;
}
//创建一个机器人
PtrRt CreateRt(int x,int y,char store[][MaxCNum],int Llen,int Clen) {
PtrRt Rt;
Rt = (PtrRt)malloc(sizeof(struct Robot));
int pos[2] = {x,y};
int** a = around(pos);
Rt->Pos[0] = x;
Rt->Pos[1] = y;
Rt->CTYPE = store[x][y];
for(int i=0;i<8;i++) {
if((a[i][0]>-1)&&(a[i][0]<Llen)&&(a[i][1]>-1)&&(a[i][1]<Clen)) {
Rt->Around[i].CType = store[a[i][0]][a[i][1]];
Rt->Around[i].flag = Judge(Rt->Around[i].CType);
}
else{
Rt->Around[i].CType = '0';
Rt->Around[i].flag = 0;
}
}
return Rt;
}
//保存行走路径(邻接矩阵)
void save_path(PtrRt Rt_1,PtrRt Rt_2,int Clen) {
arcs[(Rt_1->Pos[0]*Clen+Rt_1->Pos[1])][Rt_2->Pos[0]*Clen+Rt_2->Pos[1]] = 1;
arcs[(Rt_2->Pos[0]*Clen+Rt_2->Pos[1])][Rt_1->Pos[0]*Clen+Rt_1->Pos[1]] = 1;
}
//进行机器人行走
PtrRt move(PtrRt Rt,int pos[2],char store[][MaxCNum],int Llen,int Clen) {
PtrRt temp;
temp = (PtrRt)malloc(sizeof(struct Robot));
int i;
temp->Pos[0] = pos[0];
temp->Pos[1] = pos[1];
temp->CTYPE = store[pos[0]][pos[1]];
int **a;
a = around(temp->Pos);
for(i=0;i<8;i++) {
if((a[i][0]>-1)&&(a[i][0]<Llen)&&(a[i][1]>-1)&&(a[i][1]<Clen)) {
temp->Around[i].CType = store[a[i][0]][a[i][1]];
temp->Around[i].flag = Judge(temp->Around[i].CType);
}
else{
temp->Around[i].CType = '0';
temp->Around[i].flag = 0;
}
}
save_path(Rt,temp,Clen);
return temp;
}
//使用广度优先搜索(BFS)进行地图遍历。
void BFS(PtrRt Rt,Queue Q,char store[][MaxCNum],char expor[][MaxCNum],int Llen,int Clen) {
int flag_B[MaxLNum][MaxCNum];
for(int i=0;i<MaxLNum;i++) {
for(int j=0;j<MaxCNum;j++) {
flag_B[i][j] = 0;
}
}
Q = AddQ(Q,Rt);
int** a;
int count_s = 0;
int count_g = 0;
while(!IsEmpty(Q)) {
Rt = DeleteQ(Q);
Record(Rt,Q,expor);
if(Judge(Rt->CTYPE)==6){
s_vital[count_s][0] = Rt->Pos[0];
s_vital[count_s][1] = Rt->Pos[1];
count_s++;
}
if(Judge(Rt->CTYPE)==9){
goal[count_g][0] = Rt->Pos[0];
goal[count_g][1] = Rt->Pos[1];
count_g++;
}
a = around(Rt->Pos);
for(int i=0;i<8;i++) {
if((a[i][0]>-1)&&(a[i][0]<Llen)&&(a[i][1]>-1)&&(a[i][1]<Clen)&&(Rt->Around[i].flag)&&(!flag_B[a[i][0]][a[i][1]])) {
flag_B[a[i][0]][a[i][1]] = 1;
Q = AddQ(Q,move(Rt,a[i],store,Llen,Clen));
}
}
}
}
//查找并输出从u到v的路径
void print_path(int path[],int u, int v,int Clen) {
int k;
k = path[v];
if(k == -1) {
printf("no path!");
return;
}
printf("(%d,%d)",path[v]/Clen,path[v]%Clen);
while(k != u) {
printf(" <- ");
printf("(%d,%d)",path[k]/Clen,path[k]%Clen);
k = path[k];
}
}
//Dijkstra(迪杰斯特拉)算法
void dijkstra(int begin,int nodes[],int Llen,int Clen) {
int num;
num = Llen*Clen;
int temp;
int min;
pre[begin] = 0;
for(int i=0;i<(Llen*Clen);i++) {//初始化dist、visited、pre数组
visited[i] = 0;
if(i==begin) {
dist[i] = 0;
}
else if (arcs[begin][i]==1) {
dist[i] = 1;
pre[i] = begin;
}
else {
dist[i] = inf;
pre[i] = -1;
}
}
visited[begin] = 1;
for(int i=0;i<num;i++) {
temp = begin;
min = inf;
for(int j=0;j<num;j++) {//找到目前尚未访问过的最近的点，并从该点出发向后查找.
if(!visited[j]&&(dist[j]!=0)&&(dist[j]<min)) {
min = dist[j];
temp = j;
}
}
if(temp==begin) continue;
visited[temp] = 1;
for(int k=0; k<num; k++)
{
if(visited[k] == 0 && arcs[temp][k]==1) {
if(dist[temp] + arcs[temp][k] < dist[k]){
dist[k] = dist[temp] + arcs[temp][k];
pre[k] = temp;
}
}
}
}
}
/*完成行走并记录的综合操作
采取BFS进行遍历.*/
void Nicolas(char store[][MaxCNum],char expor[][MaxCNum],int Llen,int Clen) {
PtrRt Rt;
Queue Q;
Rt = CreateRt(0,0,store,Llen,Clen);
Q = CreateQueue();
for(int i=0;i<MaxSize;i++) { //初始化邻接矩阵
for(int j=0;j<MaxSize;j++) {
arcs[i][j] = inf;
}
}
BFS(Rt,Q,store,expor,Llen,Clen);
}
很多人要地图文件，这里我附上自己绘制的地图，保存为map.txt，仅供测试。
+..........................+++++++++++++++++#+++++++++++++..........+.........+.....................
.++++.....................++..............................++++++++++...........+....................
....+++..................+..++....................................++++++++++++++....................
......+.................+.....+..................................+..............+...................
.....++................+.......+.........++++++++++++++++........+...............+..................
....+.................+.........+...+++++................+.......+................+.................
.+++..................+..........+++......................+.....+.................+.................
+.....................+..........+.........................+...+..................+.................
+.....................+...........+.........................+.+....................+++++++..........
.+...............++++++..........+..........................++............................+.........
..+++++++++++++++................+........................++.+.............................+........
................................+.......................++....+.............................+.......
................................+.....................++.......+.............................++++...
................................+..................+++..........+++.............................+...
...............................+................+++................+++..........................+...
..............................+...............+++...................++..........................+...
.............................+................+.....................+++++........................+..
..................++++*++++++.................+..........................+..........................
................++............................+...........................+.........................
...............+..............................+............................+........................
..............+..............................+..............................+.......................
..............+..............................+..............................+.......................
.............+...............................+.............................+........................
.............+...............................+............................+.........................
..............+......................+++++++++++++*+++++++++..............+.........................
..............+................++++++........+..............++++++.......+..........................
...............+++++++++++*++++.............+.....................++....+...........................
............................................+.......................+++++...........................
...........................................+...........................++...........................
...........................................+..........................+..++.........................
..........................................+...........................+....++.......................
..........................................+..........................+.......++.....................
........................................++...........................+.........++...................
......................................++............................+............++.................
....................................++..............................+..............++...............
.................................+++...............................+.................+++............
............................+++++..................................+....................++..........
..........................++.......................................+......................+.........
....................#+++++........................................+.................................
..........................++........................................................................
............................+++++...................................................................
.................................+..................................................................
.................................+..................................................................
.................................++.................................................................
............++++++++++++++++++*++...................................................................
.........+++........................................................................................
.......++...........................................................................................
......++.................................................++++++++++++...............................
......+...........................................+++++++............+++++..........................
......++........................................++........................++++......................
........++++++++++++++++++++++++++++++++++++++++++++++........................+.....................
............................................++........+++++++++................++...................
...........................................+...................++++..............+..................
..........................................+........................+++............+.................
.........................................+............................+...........+.................
........................................+..............................++..........+................
........................................+................................+..........+...............
........................................+.................................+..........++.............
........................................++................................+............+............
..........................................*+.............................+..............+...........
............................................++.........................++................+..........
..............................................+++.................+++++...................+.........
.................................................+++++++++++++++++.........................+........
............................................................................................+.......
.............................................................................................+......
..............................................................................................+.....
...............................................................................................+....
................................................................................................+...
.................................................................................................+..
.................................................................................................+..
.................................................................................................+..
.................................................................................................+..
..................................................................................................+.
.........................................+......................................................+++.
.........................................+...................................................+++....
........................................+.................................................+++.......
.......................................+...............................................+++..........
......................................+............................................++++.............
....................................++........................................+++++.................
...........++.....................++...................................+++++++......................
.............+++++..............++...........................++++++++++.............................
..................++++++++...+++...............++++++++++++++.......................................
..........................+#+++++++++++++++++++.....................................................
......+++...............++++........................................................................
.........++..........*++............................................................................
...........+....+++++...............................................................................
...........+++++....................................................................................
...++++++++.....+++++...............................................................................
....................++++............................................................................
.......................+++++........................................................................
............................++++++++................................................................
....................................++++++++........................................................
............................................++++++++++++............................................
............................................+..........++++++++++++++++++++#+++.....................
............................................+.................................++++++++++*+++........
...........................................+........................................................
...........................................+........................................................
...........................................+........................................................
...........................................+........................................................
...........................................+........................................................
写在最后
代码和思路就是以上这些，注释不算太多，但大致思路、核心算法已经叙述清楚。对于作业，还是建议大家自己完成，本代码仅供参考，希望能对大家有所帮助。
本文章内容仅供交流学习，不作他用。
206
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
