合肥工业大学机器人足球仿真robcup作业三（python实现）附代码有注释
第三次作业
题目：已知2个点的信息,定位自己的绝对坐标。
设图中C(0,0),P1(-52.5,-32), P2(-52.5, 32), P3(52.5,32), P4(52.5,-32), P5(0,-32), P6(0,32), P7(-30,-7), P8(-30, 7), P9(30,7), P10(30, -7),G1(-52.5,0),G2(52.5,0)
随机得到附近2点距离自己的信息(r,theta), r表示目标点距离自己的距离，theta表示以自己中心的极角.（顺时针（0,180），逆时针（0，-180）。计算自己的位置。
输入示例：(P8 22 0) (P7 27.7 30)
求出机器人在场上的绝对位置。
px=-8.2，py=10.14
如果是输入为
(P8 22 0) (P7 10.4 30)
(P8 14 -30) (P7 14 30)
则计算结果是什么？
注意： 角度是目标相对于身体的角度，server发给agent的相对角度的值是视野估算的，是很大误差的，所以用来确定自己位置的大概范围，不能用来参与计算。
首先我们根据题目可以分析，角度是不能用来计算的，所以只能使用距离来计算，但是我们只知道两个目标点和分别到这两个目标点的距离怎么才能求出自己的位置呢？
相信有聪明的同学就知道了，以目标点为圆心做圆，两个圆的交点处即为我所在的位置。
但是这又迎来一个新的问题，两个圆有交点除了相切一个交点和重合无数个交点以外，都是两个交点，这样我们又如何才能知道自己的位置是在哪个交点处呢？
这就需要用到我们之前不用多的角度了，角度是以我面朝方向为基准的相对角度，所以跟球场上的绝对角度不一样，所以我们需要经过一点处理才能使用这个角度。
用第一个点的角度减去第二个点的角度，这样我们就能得到两个点在我视野内的角度差（注意这个是有正负的，正表示面朝第二个点时顺时针才能转到第一个点，负则相反）。
然后我们要根据已知两点的坐标求两点相对于我可能在的两个交点的绝对角度，然后两个绝对角度相减就能得到两个相对角度，两个相对角度与前面题目已知求出来的相对角度对比，由于有误差，所以我们不能用等于来判断，而是在几度的误差内就判定为相等。
这是绝对坐标的坐标系图
代码我跟以前一样在关键地方写了注释，所以大家观看应该不是问题
import re
import mathclass Point:"""坐标点类，用于封装坐标点，并且提供两点之间的坐标和两点之间的绝对角度"""def __init__(self, x=0, y=0):self.__x = xself.__y = ypassdef __str__(self):return "("+str(self.__x)+","+str(self.__y)+")"def get_x(self):return self.__xdef get_y(self):return self.__ydef get_distance(self, point):result = ((self.__x - point.get_x())**2+(self.__y - point.get_y())**2)**0.5return resultdef azimuthangle(self,point):"""求出对象与点point的绝对角度，水平向右为0顺时针为正，角度范围0-360:param point::return:"""x1, y1, x2, y2 = self.__x, self.__y, point.get_x(), point.get_y()angle = 0.0dx = x2 - x1dy = y2 - y1if x2 == x1:angle = math.pi / 2.0if y2 == y1:angle = 0.0elif y2 < y1:angle = 3.0 * math.pi / 2.0elif x2 > x1 and y2 > y1:angle = math.atan(dx / dy)elif x2 > x1 and y2 < y1:angle = math.pi / 2 + math.atan(-dy / dx)elif x2 < x1 and y2 < y1:angle = math.pi + math.atan(dx / dy)elif x2 < x1 and y2 > y1:angle = 3.0 * math.pi / 2.0 + math.atan(dy / -dx)return angle * 180 / math.pipass# 所有标志点组成一个字典，方便后续使用
flag = {
'c' : Point(0,0),
'P1' : Point(-52.5,-32),
'P2' : Point(-52.5, 32),
'P3' : Point(52.5,32),
'P4' : Point(52.5,-32),
'P5' : Point(0,-32),
'P6' : Point(0,32),
'P7' : Point(-30,-7),
'P8' : Point(-30, 7),
'P9' : Point(30,7),
'P10' : Point(30, -7),
'G1' : Point(-52.5,0),
'G2' : Point(52.5,0),
}class Robot:"""简单封装一个机器人类，用于输出结果"""def __init__(self):self.point = Point()passdef __str__(self):return "我在场上的绝对位置为：" + str(self.point)def get_position(self, s):print("输入："+s)point = decode_point(s)self.point = point[0]return pointpassclass Circle:def __init__(self,point, r):"""园的一般式(x-x0)^2+(y-y0)^2=r^2:param point:圆心坐标:param r:"""self.point = pointself.__r = float(r)passdef __str__(self):return "圆心"+str(self.point)+"半径"+str(self.__r) # 返回基本信息def get_center(self):return self.pointdef get_r(self):return self.__rdef get_point(self, cir, angle):"""用于求取两个圆之间的交点:param cir: 要求交点的圆:param angle: 后续取舍交点要用的角度:return: 满足条件的点"""center_1, center_2 = self.point, cir.get_center() # 获取两个圆的圆心坐标点cros_point_1, cros_point_2 = Point(-100, -100), Point(-100, -100) # 初始化两个要返回的坐标点r_1, r_2 = self.__r, cir.get_r() # 获取两个圆的半径x1, y1, x2, y2 = center_1.get_x(), center_1.get_y(), center_2.get_x(), center_2.get_y() # 将坐标拆开方便使用center_dis = center_1.get_distance(center_2) # 获取两圆心之间的距离# print("距离：",center_dis, center_1,center_2, r_1-r_2)if center_dis < 1e-6: # 当两圆心的距离小于1e-6时可近似看成重合# print("两圆重合！")passelif center_dis > r_1 + r_2 or center_dis < max(r_1, r_2) - min(r_1, r_2): # 当距离小于两圆半径差或者大于半径和时不存在交点# print("无交点！")passelse: # 有交点的情况# print("有交点")AE = (center_dis ** 2 + r_1 ** 2 - r_2 ** 2) / (2 * center_dis) # 圆1到圆心连线与垂线交点的距离x0 = x2 + (x1 - x2) * AE / center_dis # 该交点的横坐标y0 = y2 + (y1 - y2) * AE / center_dis # 该交点的纵坐标CE = (r_1 ** 2 - AE ** 2) ** 0.5 # 两圆交点到该交点的距离if 1e-6 > x1 - x2 > -1e-6: # 近似看成相等，此时两标志物垂直cros_x_1, cros_x_2 = x1 + CE, x1 - CEcros_y_1, cros_y_2 = y1 + AE, y1 + AEelif 1e-6 > y1 - y2 > 1e-6: # 近似看成相等，此时两标志物水平cros_x_1, cros_x_2 = x1 + AE, x1 + AEcros_y_1, cros_y_2 = y1 + CE, y1 - CEelse: # 此时两标志点所在直线倾斜k1 = (y1 - y2) / (x1 - x2) # 圆心连线斜率k2 = -1 / k1 # 圆交点与圆心和垂线交点的斜率cos = (1 / (k2**2 + 1))**0.5 # 将斜率转化为cossin = k2 * cos # 将斜率转化为sincros_x_1, cros_x_2 = x0 + CE*cos, x0 - CE*cos # 求出两圆的交点x坐标cros_y_1, cros_y_2 = y0 + CE*sin, y0 - CE*sin # 求出交点y坐标pass# print(cros_point_1, cros_point_2)if 32 > cros_y_1 > -32 and 52.5 > cros_x_1 > -52.5: # 判断交点在不在场内cros_point_1 = Point(cros_x_1, cros_y_1) # 打包交点# 求出交点从第一个标志点到第二个标志点转过的角度，正表示逆时针转动，负表示顺时针转动ang = cros_point_1.azimuthangle(cir.get_center()) - cros_point_1.azimuthangle(self.point)a = ang - angle # 求出程序所得的角度与题目所给的角度的差值# print(a, ang,angle,cros_point_1)if not 2 > a > -2: # 角度差值在2度以内就算重合cros_point_1 = Point(100,100) # 不满足条件就弃用该点if 32 > cros_y_2 > -32 and 52.5 > cros_x_2 > -52.5: # 同理cros_point_2 = Point(cros_x_2, cros_y_2)ang = cros_point_2.azimuthangle(cir.get_center()) - cros_point_2.azimuthangle(self.point)a = ang - angle# print(a, ang,angle, cros_point_2)if not 2 > a > -2:cros_point_2 = Point(100,100)passreturn cros_point_1, cros_point_2 # 返回所求的两个交点passdef decode_point(msg):points = list() # 用来存放求出的所有交点tmp = re.findall(r'[(](.*?)[)]',msg) # 正则匹配括号内的内容cirs = list() # 用于存放构造的所有圆angle = list() # 用于存放机器人到每个标志点的相对角度for item in tmp:info = item.split() # info为解析后的标志点的信息，依次为标志点名字，标志点的角度（弃用），标志点与我的距离angle.append(float(info[2])) # 角度信息保存cirs.append(Circle(flag[info[0]], info[1])) # 以标志点为中心，与我的距离为半径做圆for c1 in cirs: # 遍历圆for c2 in cirs: # 再遍历圆if c1 is not c2: # 如果不是同一个圆，就计算交点ang = angle[cirs.index(c1)] - angle[cirs.index(c2)] # 计算第一个标志点到第二个标志点的角度point = c1.get_point(c2, ang) # 得到的交点for p in point:if 32 > p.get_y() > -32 and 52.5 > p.get_x() > -52.5: # 如果在圆内，就添加到点中points.append(p) # 不出意外只有一个点，如果有多个标志位可能会有一点小误差，所以需要把所有满足条件的点都添加进去# for p in points:# print(p)return points # 返回所有满足条件的点if __name__ == '__main__':robot = Robot() # 创建一个机器人对象用来调用函数和输出结果。s_1 = '(P8 22 0) (P7 27.7 30)' # 测试用例s_2 = '(P8 22 0) (P7 10.4 30)'s_3 = '(P8 14 -30) (P7 14 30)'robot.get_position(s_1) # 直接调用封装好的函数获取坐标print(robot) # 输出绝对坐标没啦，这一次不是临时写的，没有bug啦，而且这个代码还可以处理输入为四个点的数据哦，求出来多个我的位置，但是为了方便（懒得继续搞），我只取了第一个为我的位置，大家可以找例子试试（求出来的位置会有一点误差是很正常的哦，老师说这个本来就不是很准确），我这里就不给大家演示了。
还是那句话，学以致用才是真正的学到了。
合肥工业大学机器人足球仿真robcup作业三（python实现）附代码有注释相关推荐
- 合肥工业大学机器人足球仿真robcup作业二（python实现）附代码有注释
用面向对象的思维设计相关类,从而实现直线与直线.直线与圆.直线与矩形的交点. 要求各给出每个案例的至少一个示例的程序. 这是第二次机器人足球的作业代码,写的比较潦草,但是用的方法还是通俗易懂的,基本都 ...
- 机器人足球仿真第一次作业
机器人足球仿真第一次作业 机器人足球仿真是一门与RoboCup有关的一门课程,讲到了有关球员的决策,球队的开发等知识. 这是老师布置的第一次作业,基本任务就是解析字符串,其功能相当于uva中Prase ...
- 合肥工业大学机器人技术作业一
合肥工业大学机器人技术作业一 题目: 在机器人足球比赛中,server和球员client之间通过发送字符串来进行信息交互,其中server要把某球员的听觉和视觉信息发送给该球员,信息的格式如下所示: ...
- 机器人足球仿真中的三角进攻算法研究
摘 要:机器人足球仿真系统提供了实时对抗环境下研究多智能体协作问题的一个良好平台.本文基于仿真环境下的理论基础及模型,通过逐场次逐帧地测试与分析研究,设计出用于控制机器人协作队形的三角进攻算法,并对基 ...
- 计算机仿真作业三,计算机仿真技术作业三.doc
计算机仿真技术作业三.doc 计算机仿真技术作业三 题目:三相桥式全控整流电路仿真 姓名: 班级: 学号: 计算机仿真技术作业三 题目:三相桥式全控整流电路仿真 利用simpowersystems建立 ...
- 合肥工业大学机器人技术五十六题
合肥工业大学机器人技术五十六题 题目要求 //拿球后行为,利用已有 Worldmodel(21) (1)在 playOn 模式下,拿到球以后朝前方快速带球. (2)在 PlayOn 模式下,拿到球以后 ...
- python五子棋游戏大作业_python-大作业之五子棋游戏(附代码)
<python-大作业之五子棋游戏(附代码)>由会员分享,可在线阅读,更多相关<python-大作业之五子棋游戏(附代码)(6页珍藏版)>请在金锄头文库上搜索. 1.Pytho ...
- 合肥工业大学机器人技术期末_合肥工业大学 机器人技术 作业和实验
简介 你好! 在这篇文章中,我将免费共享合肥工业大学<机器人技术>作业和实验环节的代码和个人报告,以供交流学习.为了方便更多的同学搜索到这篇博客,我会在下面贴出一部分实验题目. 共享资源包 ...
- 合工大php期末试卷_完美起航-合肥工业大学机器人技术作业和实验
简介 你好! 在这篇文章中,我将免费共享合肥工业大学<机器人技术>作业和实验环节的代码和个人报告,以供交流学习.为了方便更多的同学搜索到这篇博客,我会在下面贴出一部分实验题目. 共享资源包 ...
最新文章
- java核心api_java核心API
- jquery网页刷新后控件失效_jquery动态增减控件如何才能不刷新页面
- safari使用canvas引入域外的图片
- Cocos2d手机游戏引擎介绍
- 图的长宽_新车｜官方预告图发布，外观大变样，三菱新欧蓝德明年2月发布
- Silverlight 里获取摄像头视频
- matlab自带回归拟合数据,matlab数据拟合与线性回归
- 在 Java 中将 Word 转换为 PDF
- 2022届浙江工业大学考研计算机技术专硕上岸经验 初试复试经验
- 通过python刷android步数,使用 Python 修改微信/QQ/支付宝运动步数
- 视觉感知在数据可视化中的作用
- 铁匠smith_铁匠镇的皱纹地图
- 万向节死锁的简易理解
- 数据结构 | 3.树与二叉树
- 四、Amlogic A311D 音频回采信号LOOPBACK指南
- 第十一章 SQL谓词 %INLIST
- android new file 产生重复文件 ~2,Android Gradle在打包httpmime期间生成重复文件
- 安全防护工具之：Clair
- Eolink上传文件和其他参数设置
- 佛光大藏经与firebird数据库