Есть избегание попадания ключевых точек в запрещенную окружность (на
основе штрафных функций). Надо добавить обработку пересечения окружности
траекторией движения.