RoboCup智能机器人足球教程（四）
实现坐标定位
坐标定位是指球员能够根据发送过来的信息，通过运算解析出自己所在位置的坐标，如果球员能够看见球的话，也可以根据球的极坐标转换出全局直角坐标，因此坐标定位实际上就是坐标转换，坐标转换对于本球员逻辑代码至关重要。
关于全局坐标，全局坐标向下为y轴正方向，向敌方球门方向为x轴正方向，因此两支球队的全局坐标系并不相同。
由信息发送来的坐标信息为极坐标，第一个参数为球员所看到的物体与球员之间的距离，第二个参数为球员所看到的物体与球员的角度，注意顺时针方向是正方向。
下面说明球员定位的算法：
球员的位置状态有两部分组成，即坐标位置和朝向，这里先说朝向，首先，球员在初始化的时候，默认是朝x轴正方向，因此，设置一个全局变量angle，初始化为0，当球员按照自己发出的命令进行转身时，修改angle，这样就能够知道自己的朝向角度了。
然而并没有那么简单，在球员运动的过程中，由于惯性的存在，转身更为困难。一般地，球员的实际转身角度由下式计算：
Actual_angle = moment/(1.0 + inertia_moment * player_speed)
inertia_moment是一个参数，缺省值为5.0。由上式可知，当一个球员以最大速度（1.05）跑动时，他最大可以转过的角度会略小于30，因此我们在改变angle的时候，必须使用这个公式，来修正改变值。
即便修正后，仍然也只是大致的估计角度，因为球员在转向的过程中，会有大约10%的误差，也就是说，如果你命令转60度，根据上式修正为50度，最终结果是在40度到60度之间的某一个角度，时间越长，误差就会越大，最终angle的误差就会到达难以忍受的地步，因此，还需要定位点定位技术，来实时修正其角度。
确定定位点，之前提到过，球场白线上的每一个空心白色小圆圈都是一个定位点，当球员能够看到这些定位点的时候，服务器就会发送来这些点距离球员的信息和角度的信息，如图所示：
球员在看到定位点后，就可以根据发送过来的定位点距球员的距离和角度，十分精确地推断出球员的球场坐标和朝向，从这方面来说，球员定位就是从以球员为中心的极坐标系转化为以球场中央为中心的直角坐标系转化的过程。
我们以(flag p r b)为例，此时服务器发来的信息为((flag p r b) 42.9 -12)，表明现在球员距离这个定位点有42.9个距离远，在球员的-12度的方向上（逆时针为负）。
实际上 ，光有一个定位点是不能够获得球员的朝向的，必须用两个定位点才行，也就是说，球员只有在看到至少两个定位点，才能根据信息求出朝向，另外，还要根据点的不同组合，分别编程，这貌似是一个不可能完成的事情。
因此，我们只挑选有限的定位点，但是也要确保球员在大多数情况下都能看到我们选定点的至少两个点，我选择的点如下：
我们不一定要让球员时时刻刻看到两个点然后计算朝向角度，上文提到过，根据修正的angle值就可以大致估算出球员的朝向角度，等过一段时间后，球员看到选中定位点的其中两个点，然后进行修正，这样angle又会精准了，根据实际的经验，选这八个点足够保证球员在angle值误差变很大之前及时修正精度。
下面讲一讲如何通过两个定位点计算出球员的朝向。
假设现在球员能够看到定位点1和定位点2，因此能求出球员距离定位点的距离长度a和长度b，其与球员正方向（蓝线方向）的角度m和角度n，因此可求出两个定位点与球员之间的总角度O=m+n（注意是角度的绝对值），同时也知道定位点1与定位点2之间的距离（为一个半场），因此可以根据余弦定理求出角度Q：
b2 = a2 + c2 - 2 * a * c * COS(Q)
又因为图中黑线代表正方向，与边界线垂直，因此我们就可以得到黑线与蓝线之间的夹角P = 90 - Q，即球员的朝向角度。
代码如下，解释见注释，注意我只用了边界上的4个点，并没有用8个点。
void resetAngle(char *msg){char *p1, *p2;double r1, r2, d1, d2;if((p1 = strstr(msg, "(flag r t)")) != 0){ // 左边修正std::sscanf(p1, "(flag r t) %lf %lf", &d1, &r1);if((p2 = strstr(msg, "(flag r b)"))){std::sscanf(p2, "(flag r b) %lf %lf", &d2, &r2);double angle_t = (pow(d1, 2)+pow(68, 2)-pow(d2, 2))/(2*d1*68); // 余弦算出角度angle_t = acos(angle_t); // 反cos化angle_t = angle_t*180/pi; // 进行角度化angle_t = 180 - abs(r1) - angle_t; // 算出另一个角if(angle_t > 90) angle_t = 180 - angle_t; // 转换成锐角angle_t = 90 - angle_t; // 得到值if(abs(r1) < abs(r2)) angel = angle_t * (-1); // 进行方向赋值else angel = angle_t;return;}return;}if((p1 = strstr(msg, "(flag l t)")) != 0){ // 右边修正std::sscanf(p1, "(flag l t) %lf %lf", &d1, &r1);if((p2 = strstr(msg, "(flag l b)"))){std::sscanf(p2, "(flag l b) %lf %lf", &d2, &r2);double angle_t = (pow(d1, 2)+pow(68, 2)-pow(d2, 2))/(2*d1*68); // 余弦算出角度angle_t = acos(angle_t); // 反cos化angle_t = angle_t*180/pi; // 进行角度化angle_t = 180 - abs(r1) - angle_t; // 算出另一个角if(angle_t > 90) angle_t = 180 - angle_t; // 转换成锐角angle_t = 90 - angle_t; // 得到值if(abs(r1) < abs(r2)) angel = angle_t - 180; // 进行方向赋值else angel = 180 - angle_t;return;}return;}}为什么球员的朝向这么重要呢？因为我们需要这个角度来计算球员的坐标，有了这个角度可以很轻易地求出球员坐标，我们只需要一个定位点信息，就可以求出球员坐标：
由图易得，根据定位点的长度角度和球员的朝向，可以计算出总角度R = P + n，然后根据正弦函数，余弦函数计算出定位点距离球员的坐标 x = a * SIN(R)，y = a * COS(R)，这里注意几个问题:
1. 此时的坐标是以球员为原点，定位点的坐标，我们还需要通过坐标转换，转换成以球场中央为原点，球员的坐标，因为比较好做，就不详细解释了，可以看下面的代码理解；
2. 整个RoboCup的坐标系定义y轴为向下为正，x轴为向对方球门为正。
相关代码如下，解释见注释：
location Getlocation(char *msg){location l;double dis = 0;double dir = 0;char *p;if ((p = strstr(msg, "(flag r t)")) != 0)//可以看到球场右上角{l.y = dis * sin((dir + angel) / 180 * pi) + 34;l.x = 52.5 - dis * cos((dir + angel) / 180 * pi);l.y = (-1)*l.y;}else if ((p = strstr(msg, "(flag r b)")) != 0)//可以看到球场右下角{l.y = dis * sin((dir + angel) / 180 * pi) + (-34);l.x = 52.5 - dis * cos((dir + angel) / 180 * pi);l.y = (-1)*l.y;}else if ((p = strstr(msg, "(flag l t)")) != 0)//可以看到球场左上角{l.y = dis * sin((dir + angel) / 180 * pi) + 34;l.x = -52.5 - dis * cos((dir + angel) / 180 * pi);l.y = (-1)*l.y;}else if ((p = strstr(msg, "(flag l b)")) != 0)//可以看到球场左下角{l.y = dis * sin((dir + angel) / 180 * pi) + (-34);l.x = -52.5 - dis * cos((dir + angel) / 180 * pi);l.y = (-1)*l.y;}return l;}知道了球员的坐标，那么球的坐标也就非常好求了，因为自始至终，球员就一直面对着球，只要知道球相对于球员的极坐标，就可以根据球员朝向，球与球员之间的距离计算出相对于球员的球的坐标，进而计算出相对于球场的球的坐标，代码如下：
location GetBalllocation(char *msg, double ball_distance){location l = Getlocation(msg);double A_x = ball_distance * cos((angel/180) * pi);double A_y = ball_distance * sin((angel/180) * pi);double ball_x = l.x + A_x;double ball_y = l.y + A_y;l.x = ball_x;l.y = ball_y;return l;}至此，我们就完成了坐标定位功能。
RoboCup智能机器人足球教程（三）
RoboCup智能机器人足球教程（五）
RoboCup智能机器人足球教程（四）相关推荐
- RoboCup智能机器人足球教程（三）
RoboCup智能机器人足球教程(三) 实现守门员代码 守门员逻辑实现比较容易,但是最好开始的时候画好流程图,理顺逻辑,守门员一直盯着球,当球距离守门员够近的时候,守门员前去扑球,扑到球后朝对面射出, ...
- RoboCup智能机器人足球教程（五）
一个简易的足球战略实现 我之所以编写这套教程呢,是因为我在小学期学这个智能机器人足球的课程,一共才两三个星期,注定编写不了什么高级的AI,实际上,跑起来都费劲,但即便如此,我们老师在最终要 ...
- RoboCup智能机器人足球教程（二）
RoboCup智能机器人足球教程(二) 运行方式 RoboCup2D仿真平台通过一个服务端,若干客户端联系而成,同时通过监视器进行画面播放.当启动服务端后,客户端通过改写程序内部的client.cpp ...
- 史上最详细的Android Studio系列教程四--Gradle基础
史上最详细的Android Studio系列教程四--Gradle基础 转载于:https://www.cnblogs.com/zhujiabin/p/5125917.html
- excel计算机不准确,Excel小教程四十一：关于Excel计算不准确，我们应该这样解决！...
原标题:Excel小教程四十一:关于Excel计算不准确,我们应该这样解决! 我们有时候在用excel进行计算的时候,会遇到Excel计算不准确的的时候! 别怕,先听小雅为您一一道来! 5.1-5.2 ...
- 开发教程(四) MIP组件平台使用说明
组件审核平台用于上传 MIP 组件.经过自动校验之后,提交审核,通过审核的组件会定时推送到线上,供网站使用. 平台地址:https://www.mipengine.org/platform/ 1. 使 ...
- docker 打包镜像_Spring Boot2 系列教程(四十一)部署 Spring Boot 到远程 Docker 容器
不知道各位小伙伴在生产环境都是怎么部署 Spring Boot 的,打成 jar 直接一键运行?打成 war 扔到 Tomcat 容器中运行?不过据松哥了解,容器化部署应该是目前的主流方案. 不同于传 ...
- MVC5+EF6 入门完整教程四
MVC5+EF6 入门完整教程四 原文:MVC5+EF6 入门完整教程四 上篇文章主要讲了如何配置EF, 我们回顾下主要过程: 创建Data Model à 创建Database Context à创 ...
- MongoDB 教程四: 高级更改操作
视频地址:MongoDB 教程四: 高级更改操作 db.collection.update() 说明 db.collection.update(query, update, options) 修改一个 ...
- Docker教程(四) Docker镜像构建
Docker教程(四) Docker镜像构建 本文链接:https://blog.csdn.net/yuan_xw/article/details/77744272 Docker教程(四) Docke ...
最新文章
- devc++64位不兼容_DNF玩家遭强制脱坑，只因64位更新后无法上游戏，如何解决？...
- 实现BFS之“营救”
- python图片二进制流转换成图片_python将图片二进制数据转换成Django file对象
- linux 安装tcl命令,TCL/TK Linux下安装 | 勤奋的小青蛙
- 手机如何访问电脑局域网文件共享服务器,数据共享 手机怎么访问电脑文件？多个设备之间数据共享...
- 纯python好找工作吗_Python现在好找工作么？
- js判断手机端和pc端
- 如何优化及安全设置Linux系统
- Matlab读取显示图像顺序
- oracle同义词很慢,通过问题长知识----ORACLE同义词
- sublime中文乱码问题
- Python 智能银行卡识别系统的实现 （2）—系统的实现
- 暂存更改 切换分支 git
- (转载)FPGA工程师必上的论坛
- packetdrill 深入理解内核网络协议栈的工具集
- 《万里长江图》告诉我们：金沙江是长江的正源
- 【PHP】进一法取整、四舍五入取整、忽略小数等的取整数方法大全
- Apache Doris 单节点（可多节点）Docker集群制作教程
- Android PackageManager 基本使用
- html--div+css样式布局(div+span)