It seems to me to be a problem that PSO can't solve. Path planning is indeed non-trivial. You should not use a heuristic - you're not going to get close to the real solution, I think. And "path planning" is a rather large field. I would assume that you can actually find a path planner for free, if you just know to arrange the search string.

Here is a wiki page: https://en.wikipedia.org/wiki/Motion_planning