Robot Area Mapping Python

Question

Recently I have been working on a robotics project in Java. The robot is instructed to drive around the room logging data about the area around it. The robot has 16 sonar sensors all the way around the robot. The Java code I have written is irrelevant to my question but all it does is drive the robot around and writes data out a CSV file.

The sensors are positioned as follows:

Sonar layout

The file looks like this: file output here

Each row in the CSV file contains the following information: xpos, ypos, yaw(in radians), 16 sonar readouts, timestamp

When the robot starts it is in the top right corner facing right. The robot sees the world as a 20x20 grid (-10 to 10).

I am using python to process this information and I am trying to map the environment around the robot. As the CSV file tells me the robots x,y location in the world and the robots yaw I want to then use python Image library to plot the robots position on the image and then add plot a red dot around the robot at the specified distance from the sonar sensors.

Currently I have this code to generate the robots path:

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))

Giving the following output: enter image description here

I'm at a lost cause trying to get the sonar points to map. I was taking the approach of working out where the sensors distance is and then rotate the coordinates around the robots yaw.

This has not worked at all and has given me some rather strange results: enter image description here

Any help will be appreciated! Thanks!

For clarity is the following true? Taking the robot in the image to be facing north, then a reading of 5 from sensor 0 indicates an object 5 units west of the robot and when the robot is facing east then a reading of 5 from sensor 0 indicates an object 5 units north of the robot. The robot is about 1/10 of a unit square. — jing3142, Apr 04 '14 at 15:54
Yes, that's correct. The grid is 20Mx20M and the readings from the sonar are also in meters. Thanks — , Apr 04 '14 at 16:04
Do you know the angle of each sensor in relation to the zero-yaw orientation of the robot? (And possible the offset of each sensor from the center of the robot, or wherever the robot position is measured from.) — beaker, Apr 04 '14 at 16:43
I'm not 100% sure on all of the angle of the sensors. [0] shows what I found from a google search. The robot I am using is a Pioneer P3-DX. [0] http://img.springerimages.com/Images/Springer/JOU=12065/VOL=2010.3/ISU=3-4/ART=39/MediaObjects/WATER_12065_2010_39_Fig3_HTML.jpg — , Apr 04 '14 at 16:59
look here http://stackoverflow.com/q/21489288/2521214 for some hints it is a bit similar question — Spektre, Apr 05 '14 at 10:48
what is the sonar configuration (single transceiver multiple receiver ? or multiple pairs ? or single receiver multiple transceivers ?) also do you use sound direction sweeping or just direct direction transmit ? in both cases make sure that interference is not invalidating your measurements ... — Spektre, Apr 05 '14 at 10:53
They are single transceiver sonar and as for direction, I think they are are direct direction. In the case that I have here (the data csv file) is from the simulator where all of the data is reasonably accurate. I also get the readings a couple of times to remove anomalies before writing to the csv file. Thanks for the link :) — , Apr 05 '14 at 11:08

jing3142 · Accepted Answer · 2014-04-05T13:22:26.013

Assumptions

The area is 20m by 20m with origin at centre of area and x positive towards the right and y positive upwards. x,y gives position of ‘centre’ of robot in meters. (this makes calculations regarding sensors more straight forward.) Each sensor is 5cm from ‘centre’ of robot. Angle, A, of sensor, S, is as given in the image below

enter image description here

Yaw 0 is when robot is facing as shown below.

enter image description here

Yaw is given in anti-clockwise direction in radians.

NOTE:-The follow calculations are based on real world coordinates adjustments for screen coordinates for drawing on screen would still need to be made.

With robot at (x,y) and yaw T

Calculation of coordinates of object sensed by sensor S, where angle of S is A in degrees, A*PI/180 in radians and reading for S is r metres

enter image description here

(x+(r+0.05)cos(API/180 +T),y+(r+0.05)sin(API/180 +T))

Using these calculations gives the following scatter graph (which I produced in Excel - sorry but Python is not my language).

If this scattergraph is close to what you expect then above calculation is on right lines. robot path in blue, readings in red

enter image description here

Thank you very much!! Looks good, I will generate the python code and see how it looks. Thanks again :) — , Apr 05 '14 at 13:47

Robot Area Mapping Python

1 Answers1