机器人区域映射Python

Question

最近我一直在研究Java中的机器人项目。机器人被指示在房间周围驾驶记录有关其周围区域的数据。机器人在机器人周围有16个声纳传感器。我编写的Java代码与我的问题无关，但它所做的只是驱动机器人并将数据写出CSV文件。

传感器定位如下：

该文件如下所示： file output here

CSV文件中的每一行都包含以下信息： xpos，ypos，yaw（以弧度表示），16个声纳读数，时间戳

当机器人启动时，它位于右上角。机器人将世界视为20x20网格（-10到10）。

我正在使用python来处理这些信息，我正在尝试映射机器人周围的环境。当CSV文件告诉我世界上的机器人x，y位置和机器人偏航时，我想使用python图像库绘制图像上的机器人位置，然后在距指定距离的机器人周围添加一个红点。声纳传感器。

目前我有这段代码来生成机器人路径：

with open('data.csv') as fp:
    for line in fp:
        tempLine = line.split(',')
        x = (float(tempLine[0])+10.0)*100
        y = (float(tempLine[1])+10.0)*100

        idraw.rectangle([(x,2000-y), (x+10,2000-(y+10))], fill=(0,0,0)) 

提供以下输出：

我试图让声纳点映射失败。我正在研究传感器距离的位置，然后旋转机器人偏航周围的坐标。

这根本没用，给了我一些相当奇怪的结果：

任何帮助将不胜感激！谢谢！

Answer 1

假设

该区域为20m×20m，原点位于区域中心，x向右为正，y为正向上。 x，y给出机器人的“中心”位置，以米为单位。 （这使得关于传感器的计算更直接。） 每个传感器距离机器人“中心”5厘米。 传感器的角度A，如下图所示

偏航0是机器人面向如下所示。

以弧度为单位以逆时针方向给出偏航。

注意： - 以下计算基于真实世界坐标调整，仍然需要在屏幕上绘制屏幕坐标。

机器人在（x，y）和偏航T

传感器S检测到的物体坐标的计算，其中S的角度为A度，A * PI / 180弧度，S读数为r米

（x +（r + 0.05）cos（A PI / 180 + T），y +（r + 0.05）sin（A PI / 180 + T））

使用这些计算得到以下散点图（我在Excel中生成 - 抱歉，但Python不是我的语言）。

如果此散点图接近您的预期，则上面的计算在右边。 蓝色的机器人路径，红色的读数