Real-time optimal motion planning for autonomous underwater vehicles