//来源于网络
—————————————————————————————
改程序主函数部分，在仿真和实体机运行时，需要修改一下连接对象的实例化。
使用了0 1 2 3 4 15号声纳，基本依靠机器人左前方的声纳数据数值，来决定小车朝向，再设置左右轮车速。
txt文件在源文件目录。
————————————————————————————
机器人沿墙行进及简单地图绘制
→Wall Following Using Sonars
Local To Global Frame Mapping
→机器人通过一侧的声纳数据确定机器人的前进状态（↗或者↘）调整机器人两轮的速度，通过前面的声纳数据确定机器人是否需要调整方向，从而完成沿墙行进的过程。
1）由于用到声纳数目较多，需要注意不同声纳的优先级
2）机器人局部坐标向全局坐标的转换，经极坐标直角坐标变换，坐标旋转，坐标平移得
Xg=(r*cos(θ)+X0)cos(θr)-(rsin(θ)+Y0)sin(θr)+X0
Yg=(rcos(θ)+X0)sin(θr)+(rsin(θ)+Y0)*cos(θr)+Y0
3）可以考虑使用不同的声纳绘制地图，但由于声纳干涉衍射的影响，可以考虑去掉杂散噪声后绘制地图。从结果中可以看出楼道的大体情况，如门的位置，拐角所在等等。
利用声纳检测到机器人距离障碍物的距离来调节机器人左右轮的速度控制机器人沿墙走。
当机器人与墙体平行且左侧靠近墙体时（与墙体的距离可事先由操作者任意确定），可以根据0#和15#两个声纳探测到的机器人与墙的距离（分别用d0和d15来表示），来设定机器人的左右轮速：当d0 〉d15（或d15小于某一定值）时，意味着机器人的头部发生远离墙的偏转，为保持机器人与墙体的平行，则需在程序中设定右轮速度大于左轮速度，即robot->setvel(200,300);同样当d0 〈 d15（或d0小于某一定值）时，需右轮速度小于左轮速度，程序中用robot->setvel(300,200);来设定。
主要利用六个声纳15，0，1，2，3，4号声纳，我们设定它沿一边墙走。设声纳探测到的距离为d，d0、d15主要是负责机器人走直线，其他的就负责机器人遇到的特殊情况，比如说进入一个拐角会怎么样，这就需要我们设一定的预值，以防撞墙。
我们绘出的是机器人在全局坐标系下墙的位置如下，这中间涉及到一个机器人坐标系与全局坐标系的转化问题。
————————————————————————————
# include "Aria.h"
# include <cstdlib>
# include <iostream>
# include <iomanip>
# include <fstream>
# include <ctime>
# include <math.h>
# include <cmath>
using namespace std;
// the robot
ArRobot *robot;
ArSonarDevice sonar; // sonar, must be added to the robot
/*********************************************************************************
Files for logging position and sensor data
*********************************************************************************/
ofstream positionData("positionvals.txt");
ofstream sonarVals("sonarvals.txt");
ofstream globalFrame("globalFrame.txt");
//sonar sensor offset positions with respect to robot local coordinate frame
//这部分数据见aria/param/p3dx.p或者针对你的机器人，表示声纳在机器人相对其坐标原点的位置。
double robotSonarX[8] = {69,114,148,166,166,148,114,69};
double robotSonarY[8] = {136,119,78,27,-27,-78,-119,-136};
double robotSonarTh[8] = {90.0, 50.0, 30.0, 10.0, -10.0, -30.0, -50.0, -90.0};
double sonarData[5][16]; // last 5 sets of sonar readings
double posData[5]; // odometry readings，robot.getX();
long frameCount = -1; //how many iterations of the sensor grabbing functions have been run
/*********************************************************************************
Function Definitions
*********************************************************************************/
void grabNewPositionData(void);
void logPositionData(void);
void grabNewSonarData(void);
void logRawSonarData(void);
void MapToGlobalFrame(void);
double d0,d1,d3,d2,d4,d15;//用于得到对应声纳得到的距离，range
double th,r,PI=3.1415926;
double minimum=0; //用于比较0 1 2 3 4声纳最小值，为了判断车辆与墙壁朝向往哪边偏
//主函数，
void update(void)
{
// Your Code Here
grabNewSonarData();
grabNewPositionData();
logPositionData();
logRawSonarData();
MapToGlobalFrame();
d0=sonarData[0][0];
d1=sonarData[0][1];
d2=sonarData[0][2];
d3=sonarData[0][3];
d4=sonarData[0][4];
d15=sonarData[0][15];
int n;int j=0;
th=atan((sonarData[0][0]-sonarData[1][0])/1);
r=(180*th)/PI;
minimum=((d1<d2)?d1:d2)<d3?((d1<d2)?d1:d2):d3;
double d[8];
//double sonarData[0][];
double min=sonarData[0][0];
/*for( int i=0;i<16;i++)
{d[i]=sonarData[0][i];
if(sonarData[0][i]<min)
{min=sonarData[0][i];
j++;
}
}
if(sonarData[0][3]<300&&sonarData[0][4]<250)
{ robot->setVel2(0,0);
}
if(j==0||j==7||j==15)
{robot->setVel2(300,200);}
else robot->setVel2(300,300);*/
if(d0<d15)
{
robot->setVel2(300,200);
}
else if(d0>d15)
{
robot->setVel2(200,300);
}
if((d0>600)&&(d1>900)&&(d2>800)&&(d3>700))
{
robot->setVel2(100,300);
}
else
{
if(d3<900)
{
robot->setVel2(300,50);
}
if(d2<800)
{
robot->setVel2(300,80);
}
if(d1<700)
{
robot->setVel2(300,100);
}
}
}
/**************************************
Log the current Raw Sonar Range data
**************************************/
void logRawSonarData(){
if(frameCount==0){
sonarVals << "No.\t";
}
sonarVals << frameCount << "\t" ;
for (int i=0; i<16; i++) {
sonarVals<< sonarData[0][i] <<"\t";
//cout<< sonarData[0][i] <<"\t";
}
sonarVals<< endl;
//cout<< endl;
}
/************************************************************************************
Saves a snapshot of the sonar readings at the start of each cycle to
ensure both efficiency and consistancy. For use in the rest of a processing cycle.
************************************************************************************/
void grabNewSonarData()
{
int i;
//get the current range readings for each of the 8 sonar sensors
for (i=0; i<16; i++)
sonarData[0][i] = robot->getSonarRange(i);
}
/**********************************************************************************
Saves the current global frame odometry readings for use by other parts of the program
***********************************************************************************/
void grabNewPositionData(){
frameCount++;
posData[0] = robot->getX();
posData[1] = robot->getY();
posData[2] = robot->getTh();
posData[3] = robot->getLeftVel();
posData[4] = robot->getRightVel();
}
/**************************************
Log the current Odometry values
**************************************/
void logPositionData(){
if(frameCount==0){
positionData << "X\tY\tTh\tlVel\trVel" << endl;
}
positionData << posData[0] << "\t" <<
posData[1] << "\t" <<
posData[2] << "\t" <<
posData[3] << "\t" <<
posData[4] << endl;
}
void MapToGlobalFrame()
{
double r,u,v,th;
double Xr,Yr,Thr;
double Xo,Yo,Xg,Yg;
for(int i=0; i<1; i++)
{
r = sonarData[0][i];
u = robotSonarX[i];
v = robotSonarY[i];
th = (robotSonarTh[i]*PI)/180;
Xr = posData[0];
Yr = posData[1];
Thr = (posData[2]*PI)/180;
double u2 =u*cos(Thr)-v*sin(Thr);
double v2 =u*sin(Thr)+v*cos(Thr);
double a=r*cos(th+Thr);
double b=r*sin(th+Thr);
Xg = a+u2+Xr;
Yg = b+v2+Yr;
/*
Xo = u + r*cos(th);
Yo = v + r*sin(th);
Xg = (Xo*cos(Thr) - Yo*sin(Thr)) + Xr;
Yg = (Xo*sin(Thr) + Yo*cos(Thr)) + Yr;
*/
cout << r << "\tx:\t" << Xg << "y:\t" <<Yg << endl;
globalFrame << Xg << "\t" <<Yg << endl;
}
}
int main(int argc, char **argv)
{
int ret;
std::string str;
// the connection for Remote Host and Simulator
//ArTcpConnection con;
// the connection through Serial Link to the robot
ArSerialConnection con;
ArGlobalFunctor updateCB(&update);
robot = new ArRobot;
// mandatory init
Aria::init();
// open the connection, if this fails exit
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the device connection on the robot
robot->setDeviceConnection(&con);
// try to connect, if we fail exit
if (!robot->blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// add the sonar to the robot
robot->addRangeDevice(&sonar);
// user task
robot->addUserTask("update", 50, &updateCB);
// turn on the motors, turn off amigobot sounds
robot->comInt(ArCommands::SONAR, 1);
//robot->comInt(ArCommands::ENABLE, 1);
robot->comInt(ArCommands::SOUNDTOG, 0);
// start the robot running, true so that if we lose connection the run stops
robot->run(true);
robot->lock();
robot->disconnect();
robot->unlock();
// now exit
Aria::shutdown();
return 0;
}
障碍物相对于机器人坐标的位置坐标表示为：
X0= U + r* cos(th)
Y0= v + r* sin(th)
X0 Y0 表示障碍物在机器人坐标系下位置
Xr Yr 表示机器人里程计读数；
障碍物转换为在在全局坐标系下位置为（结合里程计）：
Xg = X0 * cos(Thr) - Y0 * sin(Thr) + Xr;
Yg = X0 * sin(Thr) - Y0 * cos(Thr) + Yr;