Motion planning algorithm for cleaning robot