Development of a model predictive controller for motion planning in a dynamic urban search and rescue environment