Occupancy Grid - beginner

[activex]

Hi

If I want to make occupancy grid, what are the steps I have to do. Prefer C# or psuedocode algorithm

Thanks

[activex]

Hi

So I initialize a matrix of 17x17.
I am initializing my self at [8,0] -> to represent myself at 0,0.
I use a laser range finder to find the surrounding objects.
And then I map it over to the matrix cells.
The picture attached is an example of the 180 degrees front view which all blocked by walls 8 meters ahead, 8 meter to the left, and 8 meter to the right. The 1 represent wall. The 0 means empty. Note, the map should be 16x16 meter, not 8x8 meter.

My problem is,
If i make another pose, how can i update the new info onto the existing map? or, how to enable the map to expand to radially.

[Mr. Ninja]

From my understanding (which is, to say the least, very small) of two-dimensional arrays, what you could do is set a variable to each of the axis (x for the x-axis and vice versa) and add or subtract to the variable to locate your position. Every time it was moved, you would remap it to the occupancy grid. (I would think)

[activex]

does someone know where to find a source code or nice tutorial on making occupancy grid

thanks

[jwatte]

I see two problems here. One is that you seem to center the map on (0, instead of (8, . The other is that you're using a fixed-size array, but then want to expand it.

I'd recommend implementing your grid as a "sparse array." When you detect an obstacle, insert that grid cell as "blocked" in the array. The array starts out empty. A sparse array is easiest to implement as a hash table where the key is struct { public int x; public int y; }. The good news with this is that it can be as large as you want; just keep adding blocked cell coordinates to it. To check whether a cell is blocked or not when searching, just check whether the cell in question is in the hash table.

Also, it seems as if you insert "blocked" when the range finder returns the maximum value. It's better to treat this as "unknown" and only insert blocking information when you have positive confirmation that a cell is blocked (a reading shows that it's significantly closer than the max range, for more than one cycle, say.) Also, you want to keep an estimate of likelihood that something is blocked; add 1 to the "blockedness value" of a cell rather than just using a boolean, and don't mark the cell as fully blocked until you get N positive confirmations that it's blocked. Over time, slowly decay the blockedness value, so that things that can move, will eventually go away from the map. Also, you may be able to remove (or decrement) cells from the table when you get a reading that shows that that cell is now non-blocked.

Finally, an equally hard problem is to correlate the sensor reading to real world. What exact direction is the sensor pointing in? What exact direction is the bot pointing in? What exact position does the bot have in world space? How does this change as the robot moves around? Using good IMU and GPS sensors as well as movement wheel/motor sensors is pretty much required.

[activex]

jwatte

At this point, I have a fixed 2D array to represent the grid. I have pose estimation using wheel encoder and MEM with EKF so the pose estimation I got now is reasonable. Also, I have successfully map the laser data from the real world coordinate over to the 2D Array map. This means, I have the robot position and heading, and able to map the laser measurements into the 2D Array map at the moment.

The problem I am having right now is the calculation of probability of occupant part I though I have a good understanding of how Bayes theorem works but when it comes to this problem, I am not sure if the theorem can apply straight forwardly.

So, for a Bayes theorem, I know that

P(H|D) = P(H)*P(D|H)/P(D)

where H is hypothesis and D is the data.

So, I am not sure what is my P(H) and my P(D) and my P(D|H).

Initially, all cells will have an occupancy probability of 0.5.

Help!!

[jwatte]

Each time you take a reading, you update the value for all the tiles that the measurement spans.
The data for tiles between the robot and the detected blocker is "0" and the data for the tile that the block is detected into is "1."
So, run measurement, update tiles, repeat forever.

[activex]

No, this is not how I want it to be. I want this grid to be dynamic. That is, I have to update the map every time a new sensor data is retrieved (only update the cells that within range of the sensors.)

Do you know what I mean?

[jwatte]

That's exactly what I described.

[activex]

Clearly what you mention doesn't make sense. If every time you you update the map cell to hold 1 whenever the laser measurement return object, even if its a dynamic objects, that cell will always holds 1 and would never return (or converge) to 0 since you are not updating (or looping) the same cells.

[activex]

For example, this is what i am having right now, but I believe these initial value could be chosen better.


P(occ) = 0.5 -> this is initial occupancy percentage for all cells
P(data|occ) = 0.1 -> this is the laser data does not tell there is an object (means empty)
P(notOcc) = 1 - 0.5 = 0.5 -> this is occupancy percentage of a cell not occupied
P(data|notOcc) = 0.5 -> this is probaility of a cell is not occupied at anypoint (now I still think this value could be better chosen because right now I have a very slow convergent rage)

Example.

If the laser scanner is telling that there is a object, so this means
P(data|Occ) = 0.9

therefore

P(Occ|data) = [P(Occ) * P(data|Occ)] / [(P(Occ)*P(data|Occ) + P(notOCC)*P(data|notOcc)]
                   = (0.5 * 0.9) / [(0.5 * 0.9) + (0.5 * 0.5)]
                   = 0.64

This 0.64 means there is a 64% chance that this cell is occupied. Now, if you repeat this step on the same cells, you can get it to converge. But again, the rate of converge is kinda slow!

[jkerns]

Clearly what you mention doesn't make sense.

Then you probably want to go back and re-read carefully.

You can either clear the whole stored array of "blocked" cells before you update, you can build in a "clear any point not written this cycle" into your sub-function, or you can use a "forgetting factor" where you decrement the values of each cell over time as  JWatte suggests.

[activex]

I am not sure if I miss understood jwatte previous post but what I got from him was

and don't mark the cell as fully blocked until you get N positive confirmations that it's blocked

. This is rather not what I wanted to do (although it can do the job) but this will be problematic when come to storing into array and take average to see if occupied or not... What I was thinking of is apply Bayes theorem directly, so sorry to jwatte if I missed explaining that.

@jkerns
 

You can either clear the whole stored array of "blocked" cells before you update

.... don't you think this would waste a lot of time?

can you please explain what you mean by this

you can build in a "clear any point not written this cycle" into your sub-function, or you can use a "forgetting factor" where you decrement the values of each cell over time as  JWatte suggests.

... I am not really understand that part.

[jwatte]

First, with the Markov update that you suggest, the update of a cell would be incremental, not forcing it to 1.
And, if that cell would become unblocked, then further sweeps across that cell would pass through that cell, and start marking the cell as unoccupied.
This is why I explicitly suggested you update *all cells* that a sweep goes through (in a line) with the first N-1 cells being updated as "unoccupied" and the last cell updated as "occupied."
"Update" probably means re-evaluate the Markov estimation, rather than just slamming the value to 1/0.

[activex]

Yep, I want to do an incremental update, not just forcing it to be 1 or 0

[jwatte]

So, what part of this is unclear?

Each time you take a reading, you update the value for all the tiles that the measurement spans.
The data for tiles between the robot and the detected blocker is "0" and the data for the tile that the block is detected into is "1."
So, run measurement, update tiles, repeat forever.

Specifically, by "data," I mean the input data to the Bayes estimator you quote. (I think I called it Markov before; sorry.)

[activex]

I think at this point, I have a working occupancy grid using Bayes theorem to updates the occupancy percentage. I also have a A* algorithm for path planning successfully implemented!

However, my problem now is, each cell of my occupancy grid is a size of 10x10 cm and while my robot is of size 1x1 meter. So, with the A* algorithm, I can plan out a path (has dynamic planning capability) but, each cell on the grid is smaller than my robot actual size; therefore, in simulation it looks like it works fine, but in reality the robot will hit the wall/object at some time for sure:(

I don't want to make the grid cell size too big or too small because it will wast the grid's space or increase computational time, respectively. My map  quite big, compared to robot size, too.

Do you guys know of any idea to overcome this? I am pretty sure there is way of work around this, because I seen Thrun's using A* and make the Google's car drive like a dream!

Any help is greatly appreciated!

[jwatte]

There's so many things wrong with your assumptions it's almost scary.

First, if the robot is 1x1 meter, using a smaller grid than about 25x25 cm is a total waste of RAM, and a 50x50 cm cell size is probably more reasonable.

Second, you can use a hash grid rather than a dense array, just like I suggested above.

Third, the self-driving cars use high-capacity computers similar to those used for servers in data centers; RAM for the area you can reasonably cover with scanning is not a problem there.

Fourth, more modern approaches to SLAM create and track estimates of "blockers" and "objects" rather than using an "occupancy grid." "objects" are discovered and estimated using sensor fusion, rather than single discrete measurements. Those "object" are typically put into a spatial index such as a kD tree or loose occtree or whatever.

So, if what you want to do is navigate large areas at high speeds, I suggest you re-think your approach in both hardware, sensors, and software.

[activex]

First and foremost, I am not navigating at high speed; typically maximum is 10 km/h which is less than 3 m/s.
However, I think your idea is valid, which I might in fact change my program in the future.

Anyway,

First, if the robot is 1x1 meter, using a smaller grid than about 25x25 cm is a total waste of RAM, and a 50x50 cm cell size is probably more reasonable.

If I were to choose this grid size it still smaller than my robot size; and that my A* will give a path for the robot to follow. In which the path will be a list of cells (single cell). If the robot is to set following this path, it will hit the wall, too. How can I work around this?

Second, you can use a hash grid rather than a dense array, just like I suggested above.

I'll take a look into this, soon.

Third, the self-driving cars use high-capacity computers similar to those used for servers in data centers; RAM for the area you can reasonably cover with scanning is not a problem there.

RAM is not a problem here.

I am interested in this part

Fourth, more modern approaches to SLAM create and track estimates of "blockers" and "objects" rather than using an "occupancy grid." "objects" are discovered and estimated using sensor fusion, rather than single discrete measurements.

Can you elaborate more on this?



BTW: jwatte

You said,

First, with the Markov update that you suggest, the update of a cell would be incremental, not forcing it to 1.
And, if that cell would become unblocked, then further sweeps across that cell would pass through that cell, and start marking the cell as unoccupied.
This is why I explicitly suggested you update *all cells* that a sweep goes through (in a line) with the first N-1 cells being updated as "unoccupied" and the last cell updated as "occupied."
"Update" probably means re-evaluate the Markov estimation, rather than just slamming the value to 1/0.

which I didn't really understand what you meant when I first saw it and I forgot to comeback to it. But, now that I looked carefully, this logic is exactly what I am doing. The N-1 cells is updated as lower chance of occupy. Sory, hehe.

[activex]

I am thinking of before generating the path, I should add in a safety margin around each obstacle with a value of say, robot's width/2, and then generating the path from there. What you guys think?

[jwatte]

the path will be a list of cells

If the size of a cell is 50x50 cm, then the admissibility criteria for a cell in the path should be that every neighboring cell is not blocked. This will effectively achieve making a "safety margin" in the way that you describe, except it will do it when searching, rather than when storing the data. Storing the safety margin in the grid is less optimal because it doesn't allow you to run different kinds of searches on the same grid, and it also makes it harder to tell whether a "blocked" cell is blocked because of safety margin, or because of an actual reading.

In general, path planning is a well solved problem assuming you have the map; you should go read some textbooks on the subject. Note that space for the robot to turn may also be needed -- part of the "search space" would be to include turning/re-orientation sequences.

Finally, to track objects on an object basis rather than a grid basis, go look at research papers in the subject. That's pretty much state of the art. I imagine CiteSeerX or Google Scholar would have a good index. citeseerx.ist.psu.edu

[activex]

Storing the safety margin in the grid is less optimal because it doesn't allow you to run different kinds of searches on the same grid

If I make a Clone() of this Map and dealt with the safety margin in the cloned map, what you think?

[jwatte]

That might work, but it's still an inelegant way of doing it IMO. Compensating for limitations in the admissibility test is the more elegant and "right" way to do it in my experience.

[beautifulsmall]

The problem of the chair legs !
a probability map
Generated over several days to determine the probability of an object location.

The data must be in such a format that it can be used to create paths of varying likley sucess's.  A wonderful problem . the filter time can be self adjusting to the object frequency, Im talikng myself into kalman on each leg.

I think the chair legs is a nice model as it has multiple fixed similar objects which a small domestic robot would need to manouver between. THey are fixed to each other but random in the room.

I m also unsure if the data needs to be aligned to real world map, dont use wheel odometry. Use a high rate distance ranging method to measure distance and acceleration.
Im now trying to build a 1Khz sample laser tof for under $150.

With the sensors you have you should be able to move at half the max sensor range distance in the sample time if you go from a standing start and your accel is the same as your brake. No accel over 1G for wheel based standard vehicles.