I guess your question boils down to: "How do I find the shortest path to my destination?"
Personally I always use something called A* search. It's relatively fast at finding a good path to the destination.
It's hard to explain in a simple way without knowing the details of your project.
Also
you should move to your next way point directly. If you first move horizontally and then vertically, you may end up "stuck behind a wall".