合肥工业大学机器人足球仿真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 math


class Point:
    """
    坐标点类，用于封装坐标点，并且提供两点之间的坐标和两点之间的绝对角度
    """
    def __init__(self, x=0, y=0):
        self.__x = x
        self.__y = y
        pass

    def __str__(self):
        return "("+str(self.__x)+","+str(self.__y)+")"

    def get_x(self):
        return self.__x

    def get_y(self):
        return self.__y

    def get_distance(self, point):
        result = ((self.__x - point.get_x())**2+(self.__y - point.get_y())**2)**0.5
        return result

    def 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.0
        dx = x2 - x1
        dy = y2 - y1
        if x2 == x1:
            angle = math.pi / 2.0
            if y2 == y1:
                angle = 0.0
            elif y2 < y1:
                angle = 3.0 * math.pi / 2.0
        elif 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.pi

    pass


# 所有标志点组成一个字典，方便后续使用
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()
        pass

    def __str__(self):
        return "我在场上的绝对位置为：" + str(self.point)

    def get_position(self, s):
        print("输入："+s)
        point = decode_point(s)
        self.point = point[0]
        return point

    pass


class Circle:
    def __init__(self,point, r):
        """
        园的一般式(x-x0)^2+(y-y0)^2=r^2
        :param point:圆心坐标
        :param r:
        """
        self.point = point
        self.__r = float(r)
        pass

    def __str__(self):
        return "圆心"+str(self.point)+"半径"+str(self.__r)      # 返回基本信息

    def get_center(self):
        return self.point

    def get_r(self):
        return self.__r

    def 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("两圆重合！")
            pass
        elif center_dis > r_1 + r_2 or center_dis < max(r_1, r_2) - min(r_1, r_2):      # 当距离小于两圆半径差或者大于半径和时不存在交点
            # print("无交点！")
            pass
        else:                           # 有交点的情况
            # 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 - CE
                cros_y_1, cros_y_2 = y1 + AE, y1 + AE
            elif 1e-6 > y1 - y2 > 1e-6:                                             # 近似看成相等，此时两标志物水平
                cros_x_1, cros_x_2 = x1 + AE, x1 + AE
                cros_y_1, cros_y_2 = y1 + CE, y1 - CE
            else:                                                                   # 此时两标志点所在直线倾斜
                k1 = (y1 - y2) / (x1 - x2)                                          # 圆心连线斜率
                k2 = -1 / k1                                                        # 圆交点与圆心和垂线交点的斜率
                cos = (1 / (k2**2 + 1))**0.5                                        # 将斜率转化为cos
                sin = k2 * cos                                                      # 将斜率转化为sin
                cros_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)
                pass

        return cros_point_1, cros_point_2                                               # 返回所求的两个交点
    pass



def 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啦，而且这个代码还可以处理输入为四个点的数据哦，求出来多个我的位置，但是为了方便（懒得继续搞），我只取了第一个为我的位置，大家可以找例子试试（求出来的位置会有一点误差是很正常的哦，老师说这个本来就不是很准确），我这里就不给大家演示了。

还是那句话，学以致用才是真正的学到了。