pioneer3AT/DX沿墙壁行走并记录数据
来源:互联网 发布:linux 重启网络服务 编辑:程序博客网 时间:2024/04/28 16:32
//来源于网络
—————————————————————————————
改程序主函数部分,在仿真和实体机运行时,需要修改一下连接对象的实例化。
使用了0 1 2 3 4 15号声纳,基本依靠机器人左前方的声纳数据数值,来决定小车朝向,再设置左右轮车速。
txt文件在源文件目录。
————————————————————————————
机器人沿墙行进及简单地图绘制
→Wall Following Using Sonars
Local To Global Frame Mapping
→机器人通过一侧的声纳数据确定机器人的前进状态(↗或者↘)调整机器人两轮的速度,通过前面的声纳数据确定机器人是否需要调整方向,从而完成沿墙行进的过程。
1)由于用到声纳数目较多,需要注意不同声纳的优先级
2)机器人局部坐标向全局坐标的转换,经极坐标直角坐标变换,坐标旋转,坐标平移得
Xg=(r*cos(θ)+X0)*cos(θr)-(r*sin(θ)+Y0)*sin(θr)+X0
Yg=(r*cos(θ)+X0)*sin(θr)+(r*sin(θ)+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 robotArRobot *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 readingsdouble 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;//用于得到对应声纳得到的距离,rangedouble 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;
- pioneer3AT/DX沿墙壁行走并记录数据
- DX记录
- DX学习记录-顶点结构
- 【行走】
- 行走
- 行走
- 行走
- 行走
- DX
- 墙壁涂色
- nginx接受post数据,并在日志里记录数据
- dx控件获取数据相关代码
- Android-传感器-实现记录人行走的轨迹
- Cocos2D 地图制作,并让小人沿着路行走
- 通过Lua代码创建一个Cube,并实现控制行走
- 如何完全卸载MySQL并恢复数据记录
- 记录一次安装配置mongodb并做数据预分片
- Oracle查询重复数据并删除,只保留一条记录
- 使用Markdown来编写博客
- Lucene6入门教程(七)多目录多线程索引/检索
- uva 10491 概率
- android中多线进程开发
- 有关JS构造函数
- pioneer3AT/DX沿墙壁行走并记录数据
- 学习PHP所有知识点
- shell脚本 冒泡法对数字排序
- 数码管移动显示
- AI人工智能 机器人 无人驾驶 可穿戴设备 自动化识别技术是未来趋势 抓住
- memcached缓存服务器简介及其应用
- Http状态码
- Struts2-result
- Android自定义小红点消息提示