第 9 章 机器人的行为控制
10.3 基于 AS-UII 的智能体的设计
基于 AS-UII 的智能体的设计，其主要解决如下问题
1． 机器人之间的相互通讯 2． 群体机器人动作协调 3． 群体机器人智能化信息处理
能力风暴机器人作为一款优秀的开放的软硬件教育平台，通过扩展无线通讯模块，能较 好的解决群体机器人之间相互信息交换和数据处理的问题，配以其他传感器模块和执行机构 能完成各种群体机器人系统的研究。
作为群体机器人简单应用之一，本系统通过在能力风暴机器人上扩展一块无线通讯模块 和一块数字指南针，并借助能力风暴自带的红外传感器和碰撞检测传感器，能较好模仿部队 列队前进、周围环境检测等。配合地面灰度传感器后也是研究群体机器人进行足球比赛或其 他项目的理想平台。
群体机器人系统组成
群体机器人系统包括硬件和软件两个部分。
硬件由三台能力风暴机器人组成，其中一个为主机器人，另外两个为从机器人，能力风 暴机器人装有各种传感器和 CPU，能进行一定程度的自律判断。主机器人接收上位机发送的 控制指令和数据，根据控制指令做出相应的动作，同时控制从机器人的行为，在没有上位机 系统参与的情况下，主机器人能根据软件的设定完成既定的动作方案，或组织从机器人一起 对周围环境进行探测。
主从机器人或主机器人与上位机之间的通讯通过扩展的无线通讯模块完成，机器人的方 位通过数字指南针模块完成。
软件是群体机器人系统的重要组成部分，群体机器人系统软件根据机器人角色不同分为 主机器人和从机器人控制软件，该软件主要演示利用无线通讯模块实现机器人之间的通讯、
数字指南针的使用技术。主机器人控制软件为 Master.jc，从机器人控制软件为 Slave.jc。
软件流程图如下
开始
完成预定队列 初始化
选择任务
预定序列？
Y
随机漫游 N
结束 接收到退出信
号吗 N
开始
发送开始命令
发送任务号
发送执行命令
读取队列数据
队列数据执行 完毕吗？
调整方位角
执行
退出 发送结束命令 N
开始
随机运行时间 到？
结束 发送结束命令 Y
碰撞避障
红外避障
发送速度和方位数 据 随机运行模块
完成预定队列模块 主程序
图10.1 主机器人软件流程图
主机器人软件流程图
开始
设置从机器人ID
启动执行线程
接收命令
处理控制命令
主程序
接收到退出命
令吗 Y 退出
N
开始
接收到退出命
令吗 Y 退出
接收到预定队 列命令吗
N
执行预定队列 Y 处理控制命令
接收到速度调 N 整命令吗
调整速度
接收到方位调 N 整命令吗
Y
执行其它控制指令 N
Y
执行线程模块
图10.2 从机器人软件流程图
本系统只是能力风暴机器人在多机器人协作方面的一个简单应用，只是对群体机器人进行原
int TASK_BEGIN = 0xF0;
int TASK_EXE = 0xE0;
int TASK_STOP = 0xD0;
int TASK_GETINFO = 0xC0;
int RS_IDLE = 0;/*任务执行完毕，处于空闲状态*/
int RS_SETPARAMS = 1;/*处于参数设置状态*/
int RS_BEGIN = 2;/*收到任务开始命令*/
int RS_EXE = 3;/*正在执行任务*/
int RS_STOP =4;/*任务数据接收完毕*/
int ROBOTID_DEFAULT = 0;/*Master Robot Id*/
int ROBOT_ALL = 0xFF;/*广播方式*/
int ROBOT_ODD = 0xFE;/*奇数 Robot*/
int ROBOT_EVEN = 0xFD;/*偶数 Robot*/
float fTimeWait = 8.0;/*操作等待时间*/
float fTimeMagAdjust = 8.0;/*磁场校验时间*/
float fFriction = 0.008;/*调整方向时系数，需根据场地调整*/
float fTimeFollowSpan=30.0;/*随机跟随任务持续时间*/
int ORIENTION_DIFF = 3;/*方位偏差范围*/
...
int iSongFlag = 0;
int TRANS = 30;/*平移速度*/
int ROT = 5;/*转速*/
float fTransTime = 1.5;/*平移时间*/
float fRotTime = 0.2;/*转向时间*/
int NAK = 0;/*信号没确认*/
int ACK = 1;/*信号确认*/
int ACK_WAIT = 3;/*等待信号确认时间*/
int SLAVE_MAXNUM = 9;/*从机器人最大数目*/
int iRobotId;
int DRILL_MAXNUM = 2;
int DRILLFIX_NUM = 2;/*固定队列数*/
/*机器人代号/角度/速度/时间
RobotId: 255-广播方式 Oriention: 0-360C，
Speed: -100 - +100，速度为 254 时表示机器人停止，为 255 时表示任务结束
int Drill1[] = {255,166,30,150, 255,254,255,3, };
255,346,30,150, 255,254,255,3, 255,255,255,255
};
/*全局变量*/
int iRobotStatus;
int iTaskId = 0;/*当前任务 ID*/
void main() { int a = 0;
int i=0;
int iTaskId = 1;
iRobotId = ROBOTID_DEFAULT;
printf("I'm Master,Id=%d!\n",ROBOTID_DEFAULT);
wait(0.5);
hold_serial();
while(1) {
printf("TaskId = %d,Select new TaskId!\n",iTaskId);
wait(0.5);
iTaskId = GetExeTask();
Execute(iTaskId);
for(i = 0;i<3;i++)
float fabs(float fValue) { float fTemp;
if(fValue > 0.000001) fTemp = fValue;
else if(fValue <-0.000001) fTemp = -fValue;
else
fTemp = 0.0;
return fTemp;
} /*得到方位*/
int GetOriention()
{ int Oriention = peek(0x4000);
return Oriention*2;
} /*调整自己方位*/
void AdjustSelfOriention(int oriention) { int iONow,iDiff;
int iOTemp;
while(1)
{ iONow = GetOriention();
iDiff = abs(iONow-oriention);
if(iDiff >180)
iDiff { = 360-iDiff;
printf("d=%d,o=%d=>%d\n",iDiff,iONow,oriention); }
if(iDiff < ORIENTION_DIFF) { stop();
break;
}
if(oriention >= 0 && oriention <=180)
if(iONow { > oriention && iONow < oriention+180)
{ if(iONow < oriention && iONow > oriention-180) iDirection { = 0;/*CW*/
} else
iDirection { = 1;/*CCW*/
} }
if(iDirection==0)
drive(0,10+iDiff/10); {
void AdjustAllOriention(int data) { /*将方位变为奇数*/
if(data%2 == 0) data += 1;
SendData(data);
return;
} /*调整机器人速度*/
void AdjustAllV(int data) { /*将速度变为偶数*/
if(data%2 == 1) data += 1;
if(data%16 == 0)/*0xf0,0xe0...0x10 有特别含义*/
data += 2;
SendData(data);
return;
}
void MoveRobot(int trans,int rot,float t) { drive(trans,rot);
wait(t);
/* stop();*/
return;
} void straight(int iTrans,float t) { float fTimeNow;
float fTimeBegin = seconds();
int iVLeft=iTrans,iVRight=iTrans;
int ileft_clks,iright_clks;
rotation_clear(1);
else if(iright_clks-ileft_clks>10) {
iVLeft += 1;
}
motor(1,iVLeft);
motor(2,iVRight);
printf("LV=%d,RV=%d,L=%dR=%d\n",iVLeft,iVRight,ileft_clks,iright_clks);
wait(0.1);
{ printf("TaskId=%d\n",iId);
wait(1.0);
} printf("Select End,TaskId=%d!\n",iId);
wait(2.0);
return iId;
}
/*向从机器人发送命令*/
void SendCmd(int Cmd[]) { int i=0;
int iNum = _array_size(Cmd);
for(i = 0;i<iNum;i++)
{ serial_putchar(Cmd[i]);
printf("Num=%d Cmd[%d] = %d\n",iNum,i,Cmd[i]);
wait(0.2);
} return;
}
/*发送任务开始命令*/
void SendBeginCmd() { int Cmd[3];
Cmd[0] = TASK_BEGIN;
Cmd[1] = TASK_BEGIN;
Cmd[2] = TASK_BEGIN;
SendCmd(Cmd);
wait(0.3);
return;
} /*发送任务序号*/
void SendTaskCmd(int TaskId) { int Cmd[3];
Cmd[0] = TaskId;
Cmd[1] = TaskId;
Cmd[2] = TaskId;
SendCmd(Cmd);
wait(0.3);
return;
} /*发送速度或方位数据*/
void SendData(int data) { int Cmd[3];
Cmd[0] = data;
Cmd[1] = data;
Cmd[2] = data;
SendCmd(Cmd);
wait(0.3);
return;
} /*发送任务结束命令*/
void SendStopCmd() { int Cmd[3];
Cmd[0] = TASK_STOP;
Cmd[1] = TASK_STOP;
Cmd[2] = TASK_STOP;
SendCmd(Cmd);
wait(0.3);
return;
} /*发送执行命令*/
void SendExeCmd() { int Cmd[3];
Cmd[0] = TASK_EXE;
Cmd[1] = TASK_EXE;
Cmd[2] = TASK_EXE;
SendCmd(Cmd);
wait(0.3);
return;
}
void Execute(int iTaskId)
{ if(iTaskId > DRILL_MAXNUM || iTaskId <=0) return;
{ SendBeginCmd();
SendTaskCmd(iTaskId*16);/*将任务号变换为 0x10-0xA0*/
SendExeCmd();
if(iTaskId == 1)
printf("Run { = %d\n",iTaskId);
wait(1.5);
iRobotStatus = RS_EXE;
if( iSongFlag==0) start_process(song1());
Run(Drill0);
void Run(int data[]) { int i = 0;
float fTimeBegin ; float fTimeNow;
int iAvailData;/*该组运动数据是否有效*/
int iRobot;/*运动的机器人序号*/
int iOriention,iV;/*运动角度和速度*/
float fTimeSpan;/*运动时间*/
int iSize = _array_size(data);
int iGroup = iSize/4;
iRobotStatus = RS_EXE;
for(i = 0;i<iGroup;i++) { iRobot = data[i*4];
iOriention = data[i*4+1];
iV = data[i*4+2];
fTimeSpan = ((float)(data[i*4+3]))/(float)10;
iAvailData = ((iRobot == iRobotId)||(iRobot == ROBOT_ALL)
||((iRobotId%2 == 0)&& (iRobot == ROBOT_EVEN) ) ||((iRobotId%2 == 1)&&(iRobot == ROBOT_ODD)));
/*printf("G=%d A=%d O=%d V=%d T=%f\n",i,iAvailData,iOriention,iV,fTimeSpan);*/
if(iAvailData == 1)
} } else
AdjustSelfOriention(iOriention); { fTimeBegin = seconds();
AdjustSelfOriention(iOriention);
printf("Orient=%d\n",GetOriention());
/* straight(iV,fTimeSpan);*/
iRobotStatus = RS_IDLE;
return;
}
void FollowRun()
{ float fTimeBegin,fTimeNow;
int iO;/*方位*/
fTimeBegin = seconds();
fTimeNow = fTimeBegin;
printf("FollowRun!\n");
iRobotStatus = RS_EXE;
while(fTimeNow-fTimeBegin < fTimeFollowSpan) { RandRun();
fTimeNow = seconds();
} stop();
iO = GetOriention();
AdjustAllOriention(iO);
wait(1.0);/*从机器人调整方位时间*/
SendStopCmd();
iRobotStatus = RS_IDLE;
return;
}
void RandRun() { int iO;/*方位*/
int iTrans,iRot;
int iIr,iBump;
iIr = ir_detector();
iBump = bumper();
AdjustAllOriention(iO);
MoveRobot(0,-ROT,fRotTime); { iO = GetOriention();
AdjustAllOriention(iO);
wait(1.0);/*从机器人调整方位时间*/
MoveRobot(TRANS,0,fTransTime);
AdjustAllV(TRANS);
iTrans = 0;
AdjustAllV(TRANS);
iTrans = 0;
iRot = ROT;
} else {
MoveRobot(TRANS,0,fTransTime);
AdjustAllV(TRANS);
wait(1.0);
iSongFlag=0;
return;
int TASK_BEGIN = 0xF0;
int TASK_EXE = 0xE0;
int TASK_STOP = 0xD0;
int TASK_GETINFO = 0xC0;
int TRANS = 30;/*平移速度*/
int ROT = 5;/*转速*/
int ROBOT_ALL = 0xFF;/*广播方式*/
int ROBOT_ODD = 0xFE;/*奇数 Robot*/
int ROBOT_EVEN = 0xFD;/*偶数 Robot*/
int RS_IDLE = 0;/*任务执行完毕，处于空闲状态*/
int RS_SETPARAMS = 1;/*处于参数设置状态*/
int RS_BEGIN = 2;/*收到任务开始命令*/
int RS_EXE = 3;/*正在执行任务*/
int RS_STOP =4;/*任务数据接收完毕*/
int RS_STANDBY = 5;/*等待执行命令*/
float fTimeWait = 2.0;/*操作等待时间*/
float fTimeMagAdjust = 3.0;/*磁场校验时间*/
float fFriction = 0.009;/*调整方向时系数*/
int ORIENTION_DIFF = 3;/*方位偏差范围*/
int NAK = 0;/*信号没确认*/
int ACK = 1;/*信号确认*/
int ACK_WAIT = 3;/*等待信号确认时间*/
int ALM_RAY = 3;/*执行任务遇到红外信号*/
int ALM_BUMP = 4;/*执行任务遇到障碍*/
int SLAVE_MAXID = 10;/*从机器人数目*/
int ROBOTID_DEFAULT = 1;/*缺省机器人 ID*/
int iRobotId;
int iRobotStatus ;
int iOriention,iV;/*随机运动时方位和角度*/
int DRILL_MAXNUM = 2;/*最大队列数*/
int DRILLFIX_NUM = 2;/*固定队列数*/
/*机器人代号/角度/速度/时间 RobotId: 255-广播方式 Oriention: 0-360C，
Speed: -100 - +100，速度为 254 时表示机器人停止，为 255 时表示任务结束
int Drill1[] = {255,166,30,150, 255,254,255,3, };
255,346,30,150, 255,254,255,3, 255,255,255,255
/*自选动作序号*/ };
/*int TASK_ID_FOLLOW = 10;*/
int iTaskId = 0;/*当前任务 ID*/
void main() {
int a = 0;
printf("I'm Slave,Id=%d!\n",ROBOTID_DEFAULT);
wait(0.5);
SetRobotId();
iRobotStatus = RS_IDLE;/*开始时处于空闲状态*/
hold_serial();
start_process(Execute());
Process_Rev();
share_serial();
} float fabs(float fValue) { float fTemp;
if(fValue > 0.000001) fTemp = fValue;
else if(fValue <-0.000001) fTemp = -fValue;
else
fTemp = 0.0;
return fTemp;
}
void SetRobotId() { int iSetButton = 0;
float fTimeBegin = seconds();
float fTimeNow;
iRobotId = ROBOTID_DEFAULT;
printf("RobotId=%d, Push runbutton to set!\n",ROBOTID_DEFAULT);
wait(0.5);
{ fTimeNow = seconds();
if((fTimeNow-fTimeBegin) > fTimeWait) {
printf("Set Id End! Id=%d\n",iRobotId);
wait(1.5);
return;
} int GetOriention() { int Oriention = 0;
Oriention = peek(0x4000);
return Oriention*2;
}
void Process_Rev() { int a;
int i = 0;
int iId;
printf("Rev...\n");
wait(0.3);
while(1)
{ a=serial_getchar();
printf("Rev=%d\n",a);
beep();
if(iId<= DRILLFIX_NUM)
void Run(int data[]) { int i = 0;
float fTimeBegin ; float fTimeNow;
int iAvailData;/*该组运动数据是否有效*/
int iRobot;/*运动的机器人序号*/
int iOriention,iV;/*运动角度和速度*/
float fTimeSpan;/*运动时间*/
int iSize = _array_size(data);
int iGroup = iSize/4;
iRobotStatus = RS_EXE;
for(i = 0;i<iGroup && (iRobotStatus == RS_EXE);i++) { iRobot = data[i*4];
iOriention = data[i*4+1];
iV = data[i*4+2];
fTimeSpan = (float)(data[i*4+3]/10);
iAvailData = ((iRobot == iRobotId)||(iRobot == ROBOT_ALL)
||((iRobotId%2 == 0)&& (iRobot == ROBOT_EVEN) ) ||((iRobotId%2 == 1)&&(iRobot == ROBOT_ODD)));
printf("G=%d A=%d O=%d V=%d T=%f\n",i,iAvailData,iOriention,iV,fTimeSpan);
wait(0.5);
AdjustOriention(iOriention);
fTimeBegin = seconds();
while(1)
fTimeNow = seconds();
if(fTimeNow-fTimeBegin > fTimeSpan) break;
AdjustOriention(iOriention);
printf("Orient=%d\n",GetOriention());
drive(iV,0);
iRobotStatus = RS_IDLE;
return;
}
void AdjustOriention(int oriention) { int iONow,iDiff;
if(oriention >= 0 && oriention <=180)
if(iONow { > oriention && iONow < oriention+180)
{ if(iONow < oriention && iONow > oriention-180) iDirection { = 0;/*CW*/
} else
iDirection { = 1;/*CCW*/
} }
if(iDirection==0)
drive(0,10+iDiff/10); {
/* stop();*/
} } return;
} void Execute() { while(1) { wait(0.5);
if(iRobotStatus == RS_EXE) if(iTaskId { == 1)
wait(1.0); {
Run(Drill0);
else } if(iTaskId == 2)
wait(1.0); {
Run(Drill1);
} else stop();
} else { stop();
wait(0.1); } }
return;
}