Refactoring for Efficiency and Safety

At Berkshire Grey, this function runs on the control loop of a warehouse picking robot, deciding whether the arm can safely move to a new target position given nearby obstacles reported by the vision system. It runs at the control loop's fixed frequency, and it has started missing its deadline as the obstacle count on a busy conveyor has grown.

bool CanMoveTo(const Position& target, std::vector<Obstacle> obstacles, double armRadius) {
    for (int i = 0; i < obstacles.size(); i++) {
        double dx = target.x - obstacles[i].x;
        double dy = target.y - obstacles[i].y;
        double distance = sqrt(dx * dx + dy * dy);
        if (distance < armRadius + obstacles[i].radius) {
            return false;
        }
    }

    std::vector<Obstacle> nearby;
    for (int i = 0; i < obstacles.size(); i++) {
        double dx = target.x - obstacles[i].x;
        double dy = target.y - obstacles[i].y;
        if (sqrt(dx * dx + dy * dy) < 2.0) {
            nearby.push_back(obstacles[i]);
        }
    }

    if (nearby.size() > 3) {
        return false;
    }

    return true;
}

This runs on every control-loop tick against a live obstacle list that can have dozens of entries when the conveyor is busy. Explain what you would change about this function to make it meet its deadline reliably, without weakening the safety check, and why.