You should take a look at the research [0] people are doing using the little dog [1]. They are creating some very interesting planning algorithms for unstructured terrain like that.

0 - http://cs.stanford.edu/groups/littledog/ 1 - http://www.bostondynamics.com/robot_littledog.html