Navigation with VFH Plus algorithm