A星算法简单机器人积木动作序列问题

来自人工智能的实验题实现（block,简单积木机器人问题，给出若干积木状态以及目标状态，为机器人给出动作序列）

（关于A星算法网上有一个经典的寻路例子的中文翻译，有兴趣可以先原英文翻译和中文翻译

现有积木若干，积木可以放在桌子上，也可以放在另一块积木上面。

状态表示如下：

clear（x）：积木x上面没有任何积木。
on（x,y）：积木x在积木y上面。
ontable（x）：积木x在桌子上。
操作表示如下：

move（x,y）：把积木x放到积木y上面。前提是积木x和积木y上面都没有其他积木。
moveToTable（x）：把积木x放到桌子上，前提是积木x上面无其他积木，且积木x不在桌子上（即x在其他积木上面）。
举例如下：

初始状态序列：clear（a）, on（a,b）, on（b,c）, ontable（c）, clear（d）, ontable（d），
目标状态序列：clear（a）, on（a,c）, ontable（c）, clear（b）, on（d,b）, ontable（b），
操作方案序列：moveToTable（a）, moveToTable（b）, move（a,c）, move（d,b）。
请使用A*算法来规划此问题。INPUT为初始状态序列和目标状态序列，OUTPUT为操作方案序列。

对于A星算法，其实就是比我们一般的BFS，DFS通过评价函数来约束，使得我们的搜索更快的接近最优解

其核心思想就是通过定义评价函数f=g+h(其中g其实可以当作是到达当前状态已经走过的步数或者说是消耗代价，h则是我们定义的一个由当前状态到达目标状态的估值函数，一般该值小于真实值，所以f其实就是整个过程的估值）

其次就是我们要维护的两个表：openlist和closelist

openlist其实就是我们从开始状态搜索后续状态是衍生的后继状态加入到其中，然后每次从中取出评价函数值最小的状态进行后续搜索，我比较倾向于用存有状态的优先队列来作为我们的openlist,因为我们可以定义优先队列按评价函数值大小来排序，这样一来每次取得的状态点其实就是评价函数值最小的点。至于closelist为了后面查重需要所以可以用vector或者list

直接附上我的代码，一口气写下来的，没有考虑空间时间的问题，考虑空间问题其实可以用一维数组存的。

比如clear(a),on(a,d),on(d,b),ontable(b),ontable(c)就可以用

[ 3,-1,-1,1]

[a][b][c][d]

按a,b,c,d索引为0，1，2，3来设计，a下面是d，指向d的索引3，b下面没有，指向-1，c下面也没有，指向-1，d下面是b，指向b的索引1

我这里就用二维数组存，a下面有d,则mat[0][3]=1;d下面有b，则mat[3][1]=1;b在桌面上，则mat[1][1]=1;c在桌面上，则mat[2][2]=1;其他为0

所以如果一个木块a不在桌面上并且其上是clear的，则mat[0~n][0]=0

#include <iostream>
#include <cmath>
#include <string>
#include <queue>
#include <list>
#include <cstring>
using namespace std;
const int num =20; //预定义下规模
int mat_begin[num][num]; //初始状态矩阵
int mat_end[num][num];  //目标状态矩阵
int index1,index2; //状态的语句数，如on(a,b),ontable(b),ontable(c) 为3个
int row = -1;//可能积木个数没有num那么多，可以对矩阵操作时候节省时间 ，row=col

struct operator_string
{
string str_operator; //moveToTable 或者 move
string str_A; //第一个操作对象
string str_B; //第二个操作对象
};

struct status
{
int g; //==搜索了第几步
int h;//估值，与目标的估计距离
int mat[num][num];//每一个状态下的矩阵
list<operator_string> array_operator;//用来存储操作序列
//可移动的对象
string not_ontable; //不在桌面上并且上面没有其他积木的木块
string ontable; //在桌面上的木块并且上面没有其他积木的木块
string sum;
friend bool operator< (status n1, status n2){
return n1.g+n1.h > n2.g+n2.h;   //">"为从小到大排列 f=g+h
}
};

list<status> li;
string deal_space(string s)  //预处理一下客户输入的字符串，因为可能出现多余空格的问题
{
string temp = s;
int size = temp.size();
for( int i = 0; i < size; i ++ ){
if( temp[i] ==' '){
for ( int j = i; j < size; j ++ ) temp[j] = temp[j+1];
i--;
size--;
}
if( temp[i] == ',' && temp[i-1] == ')'){        //将‘）’后面的逗号处理掉
for ( int j = i; j < size; j ++ ) temp[j] = temp[j+1];
i--;
size--;
}
}
return temp;
}

void show_mat(int mat[num][num])//显示矩阵
{
for(int i=0;i<row;i++){
for(int j=0;j<row;j++){
cout<<mat[i][j]<<" ";
}
cout<<endl;
}
}

bool same_mat(int a[num][num],int b[num][num])//矩阵是否相同
{
int ans = 0;
for(int i=0;i<row;i++){
for(int j=0;j<row;j++){
if(a[i][j]!=b[i][j]){
ans = 1;
break;
}
}
}
if(ans == 0) return true;
else
return false;
}

void copy_mat(int a[num][num],int b[num][num])//拷贝矩阵
{
for( int i = 0 ; i < num ; i++ )
for ( int j = 0; j < num; j ++ ) a[i][j]=b[i][j];
}

void string_to_mat(string s,int & index,int & row,int  mat[num][num])//将字符串转为状态矩阵
{
int size = s.size();
int flag1=0,flag2=0;//flag1对应着左括号，flag2对应逗号
string temp1="",temp2="",temp3="";  //temp代表on/ontable/clear, temp2代表第一个操作对象，temp3代表第二个对象
for( int i = 0; i < size; i++ ){
if( flag1 == 0 && s[i] != '(' ) temp1 += s[i];
else if( flag1 == 0 && s[i] == '(' ) flag1 = 1;
else if( flag2 == 0 && s[i] != ',' && s[i] != ')' ) temp2 += s[i];
else if( s[i] == ',' ) flag2 = 1;
else if( flag2 == 1 && s[i] != ')' ) temp3 += s[i];
else if( s[i] == ')' ){     //一旦遇到有括号，就开始操作
flag1 = 0,flag2 = 0;
if( temp1 == "on" ){
row = max(row,temp2[0]-'a');
row = max(row,temp3[0]-'a');//row 就是为了找总共有a、b、c、d、e...多少个
mat[ temp2[0] - 'a' ][ temp3[0] - 'a' ] = 1;//设为1
}
else if( temp1 == "ontable" ){
row = max(row,temp2[0]-'a');
mat[ temp2[0] -'a' ][ temp2[0] - 'a' ] = 1;//设为1
}
else{
row = max(row,temp2[0]-'a');//clear，不用操作
}
temp1 = "",temp2 = "",temp3 = "";
index++;
}
else
continue;
}
row++;
}

int count_h(int a[num][num],int b[num][num]) //计算h的估值 方法是从底部网上找相同，相同加一
{
int visit[num][num];
memset(visit,0,sizeof(visit));
int ans  = 0;
for(int i = 0; i < row; i++ ){
if(a[i][i]==b[i][i]&&a[i][i]==1&&visit[i][i]==0){
visit[i][i]=1;
ans++;
int k = i;
int flag=1;
while(flag){//如果flag=0,证明该木块往上已经找不到木块了，终止循环
flag=0;
int temp;
for(int j=0;j<row;j++){
if(a[j][k]==b[j][k]&&a[j][k]==1&&visit[j][k]==0){//该列
flag=1;
visit[j][k]=1;
ans++;
temp = j;
}
}
k=temp;
}
}
}
return row - ans;
}
void strategy(status & current)//计算可以做的下一步操作，将对象加到not_ontable 和ontable两个字符串中，以便后面取得下一步操作策略
{
for( int j = 0 ; j < row ; j ++ ){
int ans1=0,ans2=0;
for( int i = 0 ; i < row ; i ++ ){
if( current.mat[i][j] == 1 ){
if( i != j) ans2++;
ans1++;
}
}
if( ans1 == 0 ) current.not_ontable += ( j + 'a');
else
if( ans2 == 0) current.ontable += ( j + 'a');
}
current.sum = current.not_ontable + current.ontable;
}

void copy_and_change(status & to,status from,int index)//从上个状态中继承可以继承的数据
{
to.g=from.g+1;
copy_mat(to.mat,from.mat);
list<operator_string>::iterator iter;
for(iter = from.array_operator.begin();iter!=from.array_operator.end();iter++){
operator_string temp;
temp.str_operator=(*iter).str_operator;
temp.str_A=(*iter).str_A;
temp.str_B=(*iter).str_B;
to.array_operator.push_back(temp);
}
for (int j = 0 ; j < num ; j++ )  { //将对应的行置0
if( from.mat[index][j] == 1 ){
to.mat[index][j] = 0;
}
}
}

void add_move_operator(status & to,int index1,int index2)
{
operator_string temp_string;
temp_string.str_operator="move";
temp_string.str_A+=( index1 + 'a' );
temp_string.str_B+=( index2 + 'a' );
to.array_operator.push_back(temp_string);
to.mat[index1][index2] = 1;
}
status not_ontable_to_table( status current, int index )//将不在桌面的移到桌面
{
status temp;
int char_index = current.not_ontable[index] - 'a';
copy_and_change(temp,current,char_index);
temp.mat[char_index][char_index] = 1;
operator_string temp_string;
temp_string.str_operator="moveToTable";
temp_string.str_A+=( char_index + 'a' );
temp.array_operator.push_back(temp_string);
temp.h = count_h(temp.mat,mat_end);
strategy(temp);

return temp;
}
status move_between(status current,int index1,int index2)
{
status temp;
int char_index1 = current.sum[index1] - 'a';
int char_index2 = current.sum[index2] - 'a';
copy_and_change(temp,current,char_index1);
add_move_operator(temp,char_index1,char_index2);
temp.h = count_h(temp.mat,mat_end);
strategy(temp);
return temp;
}

bool have_same(status a)//在li链表中是否相同的状态矩阵，有的话就是搜索过的
{
list<status>::iterator iter;
int ans = 0;
for(iter = li.begin();iter!=li.end();iter++){
if(same_mat((*iter).mat,a.mat)){
ans=1;break;
}
}
if(ans == 1) return true;
else
return false;
}

int main()
{
string s1,s2;
cout<<"请输入初始串:"<<endl;
getline(cin,s1);
cout<<"请输入目标串:"<<endl;
getline(cin,s2);
cout<<"=============================================block begin==============================================="<<endl;
string begin = deal_space(s1);
string end = deal_space(s2);
string_to_mat(begin,index1,row,mat_begin);
row=0;
string_to_mat(end,index2,row,mat_end);
cout<<"原始状态矩阵："<<endl;
show_mat(mat_begin);
cout<<"目标状态矩阵"<<endl;
show_mat(mat_end);
priority_queue <status> que;    //优先队列存储结点
status start;
start.g=0;
start.h=count_h(mat_begin,mat_end);
copy_mat(start.mat,mat_begin);
strategy(start);
que.push(start);
li.push_back(start);
status p;
status pp; //用来记录最后的结果
int ans = 0;
int count=0;//记录访问过多少个走步
while( que.size() )
{
if(ans == 1)
break;
p=que.top();
que.pop();
int size1 = p.not_ontable.size();
for(int i=0;i<size1;i++)//从不再桌面的移到桌面上
{
count++;
status temp;
temp=not_ontable_to_table(p,i);
if ( same_mat(temp.mat,mat_end) ) {
pp=temp;
ans=1;
break;
}
if(!have_same(temp)){
li.push_back(temp);
que.push(temp);
}
}
int size3=p.sum.size();//不再桌面的和在桌面上的总序列
for(int i=0;i<size3;i ++ )//之间相互移动
{
if(ans == 1)
break;
for(int j = 0;j<size3;j++){
count++;
if(i!=j){
status temp;
temp=move_between(p,i,j);
if ( same_mat(temp.mat,mat_end) ) {
pp=temp;
ans=1;
break;
}
if(!have_same(temp)){
li.push_back(temp);
que.push(temp);
}
}
}
}
}
cout<<"count="<<count<<endl;
cout<<"求得的结果步数 = "<<pp.g<<endl;
list<operator_string>::iterator iter;
for(iter = pp.array_operator.begin();iter!=pp.array_operator.end();iter++)
{
if((*iter).str_B!="")
cout<<(*iter).str_operator<<"("<<(*iter).str_A<<","<<(*iter).str_B<<")  ";
else
cout<<(*iter).str_operator<<"("<<(*iter).str_A<<")  ";
}
return 0;
}

附上相关的测试样例：

/*样例输出下：
===========================================================================================================================3
clear(a),on(a,b),ontable(b),ontable(c)
clear(b),on(b,c),ontable(c),ontable(a)
原始状态矩阵：
0 1 0
0 1 0
0 0 1
目标状态矩阵
1 0 0
0 0 1
0 0 1
count=11
求得的结果步数 = 2
moveToTable(a)  move(b,c)
===========================================================================================================================4
clear(a),on(a,b),on(b,c),ontable(c),clear(d),ontable(d)
clear(a),on(a,c),ontable(c),clear(d),on(d,b),ontable(b)
原始状态矩阵：
0 1 0 0
0 0 1 0
0 0 1 0
0 0 0 1
目标状态矩阵
0 0 1 0
0 1 0 0
0 0 1 0
0 1 0 0
count=54
求得的结果步数 = 4
moveToTable(a)  moveToTable(b)  move(d,b)  move(a,c)
===========================================================================================================================5
clear(a),on(a,b),on(b,c),ontable(c),clear(d),on(d,e),ontable(e)
clear(a),on(a,d),on(d,b),ontable(b),clear(e),on(e,c),ontable(c)
原始状态矩阵：
0 1 0 0 0
0 0 1 0 0
0 0 1 0 0
0 0 0 0 1
0 0 0 0 1
目标状态矩阵
0 0 0 1 0
0 1 0 0 0
0 0 1 0 0
0 1 0 0 0
0 0 1 0 0
count=91
求得的结果步数 = 5
moveToTable(a)  moveToTable(b)  move(d,b)  move(a,d)  move(e,c)
===========================================================================================================================6
clear(a),on(a,b),on(b,c),clear(d),on(d,e),on(e,f),ontable(c),ontable(f)
clear(f),on(f,a),on(a,c),clear(d),on(d,b),on(b,e),ontable(c),ontable(e)
原始状态矩阵：
0 1 0 0 0 0
0 0 1 0 0 0
0 0 1 0 0 0
0 0 0 0 1 0
0 0 0 0 0 1
0 0 0 0 0 1
目标状态矩阵
0 0 1 0 0 0
0 0 0 0 1 0
0 0 1 0 0 0
0 1 0 0 0 0
0 0 0 0 1 0
1 0 0 0 0 0
count=382
求得的结果步数 = 7
moveToTable(d)  moveToTable(e)  move(a,f)  move(b,e)  move(a,c)  move(d,b)  move(f,a)

===========================================================================================================================7
clear(a),on(a,b),on(b,c),clear(d),on(d,e),on(e,f),on(c,g),ontable(f),ontable(g)
clear(f),on(f,a),on(a,c),clear(d),ontable(g),on(d,b),on(b,e),ontable(c),ontable(e)
原始状态矩阵：
0 1 0 0 0 0 0
0 0 1 0 0 0 0
0 0 0 0 0 0 1
0 0 0 0 1 0 0
0 0 0 0 0 1 0
0 0 0 0 0 1 0
0 0 0 0 0 0 1
目标状态矩阵
0 0 1 0 0 0 0
0 0 0 0 1 0 0
0 0 1 0 0 0 0
0 1 0 0 0 0 0
0 0 0 0 1 0 0
1 0 0 0 0 0 0
0 0 0 0 0 0 1
count=597
求得的结果步数 = 8
moveToTable(d)  moveToTable(e)  move(a,f)  move(b,e)  moveToTable(c)  move(a,c)  move(f,a)  move(d,b)

===========================================================================================================================8
clear(a),on(a,b),on(b,h),on(h,e),ontable(e),clear(g),on(g,f),ontable(f),clear(c),on(c,d),ontable(d)
clear(d),on(d,e),on(e,g),ontable(g),clear(c),on(c,h),on(h,a),ontable(a),clear(f),on(f,b),ontable(b)
原始状态矩阵：
0 1 0 0 0 0 0 0
0 0 0 0 0 0 0 1
0 0 0 1 0 0 0 0
0 0 0 1 0 0 0 0
0 0 0 0 1 0 0 0
0 0 0 0 0 1 0 0
0 0 0 0 0 1 0 0
0 0 0 0 1 0 0 0
目标状态矩阵
1 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0
0 0 0 0 0 0 0 1
0 0 0 0 1 0 0 0
0 0 0 0 0 0 1 0
0 1 0 0 0 0 0 0
0 0 0 0 0 0 1 0
1 0 0 0 0 0 0 0
count=420
求得的结果步数 = 8
moveToTable(a)  moveToTable(b)  moveToTable(g)  move(h,a)  move(f,b)  move(e,g)  move(c,h)  move(d,e)

===========================================================================================================================9
clear(c),on(c,d),on(d,f),on(f,g),ontable(g),clear(e),on(e,h),on(h,a),ontable(a),clear(i),on(i,b),ontable(b)
clear(h),on(h,a),on(a,g),on(g,c),ontable(c),clear(b),on(b,d),ontable(d),clear(f),on(f,e),on(e,i),ontable(i)
原始状态矩阵：
1 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0
0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 1 0 0
1 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0
目标状态矩阵
0 0 0 0 0 0 1 0 0
0 0 0 1 0 0 0 0 0
0 0 1 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 0 1
0 0 0 0 1 0 0 0 0
0 0 1 0 0 0 0 0 0
1 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1
count=1964
求得的结果步数 = 10
moveToTable(c)  moveToTable(d)  moveToTable(i)  move(e,i)  move(f,e)  move(g,c)  move(h,d)  move(a,g)  move(h,a)  move(b,d)

===========================================================================================================================10
clear(c),on(c,d),on(d,f),on(f,g),ontable(g),clear(e),on(e,h),on(h,a),ontable(a),clear(i),on(i,b),ontable(b),ontable(j)
clear(a),on(a,h),on(h,j),on(j,g),on(g,c),ontable(c),clear(b),on(b,d),ontable(d),clear(f),on(f,e),on(e,i),ontable(i)
原始状态矩阵：
1 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0
0 0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 1 0 0 0
1 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 1
目标状态矩阵
0 0 0 0 0 0 0 1 0 0
0 0 0 1 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0
0 0 0 0 1 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 1
0 0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 1 0 0 0
count=651
求得的结果步数 = 10
moveToTable(c)  moveToTable(d)  moveToTable(i)  move(e,i)  move(b,d)  move(f,e)  move(g,c)  move(j,g)  move(h,j)  move(a,h)
===========================================================================================================================10
clear(c),on(c,d),on(d,f),on(f,g),ontable(g),clear(e),on(e,h),on(h,a),ontable(a),clear(j),on(j,i),on(i,b),ontable(b)
clear(a),on(a,h),on(h,j),on(j,g),on(g,c),ontable(c),clear(b),on(b,d),ontable(d),clear(f),on(f,e),on(e,i),ontable(i)
原始状态矩阵：
1 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0
0 0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 1 0 0 0
1 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0
目标状态矩阵
0 0 0 0 0 0 0 1 0 0
0 0 0 1 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0
0 0 0 0 1 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 1
0 0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 1 0 0 0
count=1429
求得的结果步数 = 11
moveToTable(c)  moveToTable(d)  moveToTable(j)  moveToTable(i)  move(e,i)  move(f,e)  move(b,d)  move(g,c)  move(j,g)  move(h,j)  move(a,h)
===========================================================================================================================11
clear(k),on(k,c),on(c,d),on(d,f),on(f,g),ontable(g),clear(e),on(e,h),on(h,a),ontable(a),clear(j),on(j,i),on(i,b),ontable(b)
clear(a),on(a,h),on(h,k),on(k,j),on(j,g),on(g,c),ontable(c),clear(d),on(d,b),ontable(b),clear(f),on(f,e),on(e,i),ontable(i)
原始状态矩阵：
1 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0 0
0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 1 0 0 0 0
1 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0
0 0 1 0 0 0 0 0 0 0 0
目标状态矩阵
0 0 0 0 0 0 0 1 0 0 0
0 1 0 0 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0
0 0 0 0 1 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 1
0 0 0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 0 0 0 1 0
count=3355
求得的结果步数 = 12
moveToTable(k)  moveToTable(c)  move(j,k)  moveToTable(i)  move(d,b)  move(e,i)  move(f,e)  move(g,c)  move(j,g)  move(k,j)  move(h,k)  move(a,h)
*/