Как рассчитать оптимальную точку перехвата линейно движущегося объекта на плоскости?

Рейтинг: 5Ответов: 5Опубликовано: 05.03.2023

Пусть у нас есть две точки:

  • бегунок (runner) с вектором скорости определённым триадой runnerTargetX, runnerTargetY, runnerVelocity
  • перехватчик (chaser) с модулем скорости chaserVelocity

Стало быть сигнатура искомой функции будет примерно такой:

function calculateInterceptionPoint(
  runnerX, runnerY, runnerTargetX, runnerTargetY, runnerVelocity,
  chaserX, chaserY, chaserVelocity
) {
  // ...
  return [chaserTargetX, chaserTargetY]; // оптимальная точка перехвата
}

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

Ответы

▲ 9

Судя по аргументам, случай у вас двумерный.

Пусть цель имеет координаты (rx,ry) и скорость (вектор) vr. Преследователь имеет координаты (cx,cy), а его скорость по модулю - vc. Перейдем в систему координат, связанную с целью. В ней он покоится, а преследователь движется со скоростью -vr. К ней нужно добавить скорость самого преследователя, в кока что неизвестном направлении. Очевидно, что оптимальный перехват — движение по прямой, соединяющей цель и преследователя, так что из всех возможных направлений vc нас интересует то, которое в сумме с -vr, даст направление на цель.

Таких направлений может быть два (см. рис.), понятно, что надо выбрать то, которое даст больший модуль относительной скорости.

Так мы находим направление скорости преследователя о исходной системе отсчета. Все дальнейшие расчеты не сложнее теоремы Пифагора и тригонометрических функций.

Если же цель находится вне раствора возможных значений углов (на рис. — например, в точке (rx', ry') — то никакими ухищрениями догнать цель не получится. Сами сообразите, когда это условие не выполняется, и преследование всегда успешно :))

введите сюда описание изображения

Словом, обычная школьная задача примерно 8 класса советской десятилетки. Как сейчас помню, там был автобус, едущий по дороге, и бегущий к нему человек, и спрашивалось, успеет ли он на автобус или нет, и если да - то куда ему бежать, в каком направлении...

Можно и чисто алгебраически — задача сводится к квадратному уравнению относительно времени. Нет решений — перехват невозможен, есть — выбираем меньшее положительное значение и получаем точку перехвата...

введите сюда описание изображения

▲ 7

Решение покажу на Пайтоне.

Идея тривиальна. Нужно перейти в систему отсчёта, связанную с бегунком, и повернуть оси так, чтобы точка старта перехватчика находилась на оси Ox: введите сюда описание изображения

Решатель:

# Решатель задачи догона
# r0 - начальное положение бегунка
# rV - вектор скорости бегунка
# cVAbs - модуль скорости перехватчика
# Возвращает вектор скорости перехватчика и время погони
# Если задача не решается, то выбрасывает исключение
def solve(r0:Vector, rV:Vector, cVAbs:float) -> tuple(Vector,float):
    if abs(r0) == 0.0:
        raise ValueError("Уже догнал")
    if cVAbs < 0.0:
        raise ValueError("Отрицательная скорость", cVAbs)
    # Перейти в систему отсчёта бегунка
    # Поворот системы координат
    sin = r0.y/abs(r0)
    cos = r0.x/abs(r0)
    c0 = -r0.rot_sin_cos(sin, cos)
    v0 = -rV.rot_sin_cos(sin, cos)

    # Догонный курс вдоль оси Ох'
    v_y = -v0.y
    v_x_sq = cVAbs*cVAbs - v_y*v_y
    if v_x_sq < 0:
        # При заданном модуле скорости перехватчик никогда не догонит бегунка
        raise ValueError("Нет решения")
    v_x = math.sqrt(v_x_sq)
    # Выбор направления - v_x и c_0 должны быть разных знаков
    if c0.x > 0:
        v_x = -v_x
    # Скорость сближения
    u = v0.x + v_x
    t = abs(c0.x/u)
    v_rotated = Vector(v_x, v_y)
    # Поворот системы координат обратно
    v = v_rotated.rot_sin_cos(-sin, cos)
    return v,t

Решатель возвращает вектор скорости v и время перехвата t. Так как перехватчик стартует из начала координат, то точка перехвата будет v*t.

Проверка

r0 = Vector(1,1)
rV = Vector(1,0)
cV = 1.5
v, t = solve(r0, rV, cV)
print("Начальная точка: ", r0, ", начальная скорость: ", rV)
print("Результат: скорость ", v,abs(v),", время преследования, секунды: ", t)
print("Положение бегунка", r0+rV*t)
print("Положение преследователя", v*t)

r0 = Vector(1,1)
rV = Vector(-1,0)
cV = 1.5
v, t = solve(r0, rV, cV)
print("Начальная точка: ", r0, ", начальная скорость: ", rV)
print("Результат: скорость ", v,abs(v),", время преследования, секунды: ", t)
print("Положение бегунка", r0+rV*t)
print("Положение преследователя", v*t)

Результаты:

Начальная точка:  Vector(1.000000,1.000000) , начальная скорость:  Vector(1.000000,0.000000)
Результат: скорость  Vector(1.435414,0.435414) 1.4999999999999998 , время преследования, секунды:  2.296662954709576
Положение бегунка Vector(3.296663,1.000000)
Положение преследователя Vector(3.296663,1.000000)
Начальная точка:  Vector(1.000000,1.000000) , начальная скорость:  Vector(-1.000000,0.000000)
Результат: скорость  Vector(0.435414,1.435414) 1.4999999999999998 , время преследования, секунды:  0.6966629547095765
Положение бегунка Vector(0.303337,1.000000)
Положение преследователя Vector(0.303337,1.000000)

Нетрудно видеть, что в момент перехвата координаты бегунка и перехватчика совпадают.

Решатель использует класс Vector, который можно складывать, умножать на число, вычислять модуль и поворачивать.

# Двумерный вектор
class Vector:
    def __init__(self, x:float, y:float) -> None:
        self.x = x
        self.y = y
    # Сложение с вектором 
    def __add__(self, other:any) -> Vector:
        if isinstance(other, Vector):
            return Vector(self.x+other.x, self.y+other.y)
        raise ValueError("Not a vector", other)
    # Умножение на число справа, до кучи скалярное произведение (не понадобилось)
    def __mul__(self, other) -> Vector:
        if isinstance(other, float) or isinstance(other, int):
            return Vector(self.x*other, self.y*other)
        if isinstance(other, Vector):
            return self.x*other.x + self.y*other.y
        raise ValueError("Not a number", other)
    # Деление на число
    def __div__(self, other) -> Vector:
        if isinstance(other, float) or isinstance(other, int):
            return Vector(self.x/other, self.y/other)
        raise ValueError("Not a number", other)
    # Умножение на число слева
    def __rmul__(self, other) -> Vector:
        if isinstance(other, float) or isinstance(other, int):
            return Vector(self.x*other, self.y*other)
        raise ValueError("Not a number", other)
    # Унарный минус
    def __neg__(self):
        return Vector(-self.x, -self.y)
    # Модуль
    def __abs__(self):
        return math.sqrt(self.x*self.x + self.y*self.y)
    # Поворот
    def rot_sin_cos(self, sin : float, cos: float) -> Vector:
        return Vector(self.x*cos + self.y*sin, self.y*cos - self.x*sin)
    # ToString
    def __repr__(self) -> str:
        return "Vector(%f,%f)"%(self.x, self.y)

▲ 5

UPD
Если честно, только сейчас заметил что @Harry уже привел то же самое квадратное уравнение, но выводит он его через тригонометрическое тождество, так что, пожалуй оставлю решение для разнообразия.

Чтобы не возиться с переводами туда-сюда

r = вектор начальной позиции бегунка
c = вектор начальной позиции перехватчика
v = вектор скорости бегунка
u = вектор скорости перехватчика (неизвестен)
|u| = модуль вектора скорости перехватчика (известен)

Спустя время t, перехватчик будет в одной из точек, принадлежащих окружности с радиусом |u|t и центром c

(x - xc)2 + (y - yc)2 = (|u|t)2

В это же время бегунок будет в точке

r + vt

Подставляем координаты этой точки в первое уравнение вместо x и y

(xr + xvt - xc)2 + (yr + yvt - yc)2 = (|u|t)2

Решаем его как обычное квадратное уравнение с неизвестной t (все остальные коэффициенты известны)

Получаем до двух вариантов времени столкновения (бесконечное количество вариантов, если точки начала и модули скорости совпадают), для оптимального решения берем минимальное из неотрицательных значений.

Зная время столкновения, находим точку столкновения по второй формуле, а из нее, если нужно, и вектор скорости перехватчика.

u = v + (r - c) / t

import numpy as np

def solve(r_x, r_y, c_x, c_y, v_x, v_y, u_length):
    if (r_x, r_y) == (c_x, c_y):
        return r_x, r_y
    else:
        a = v_x ** 2 + v_y ** 2 - u_length ** 2
        b = 2 * ((r_x - c_x) * v_x + (r_y - c_y) * v_y)
        c = (r_x - c_x) ** 2 + (r_y - c_y) ** 2

        roots = np.roots([a, b, c])
        try:
            t = roots[~np.iscomplex(roots) & (roots > 0)].min()
        except ValueError:
            raise ValueError('нет решения') from None

        return r_x + v_x * t, r_y + v_y * t

Небольшая демонстрациядемонстрация

▲ 3

Реализуя идею Harry на С++, можно написать такой код:

bool intercept(double xt,  double yt,  // target coordinates
               double vtx, double vty, // target velocity
               double xi,  double yi,  // interceptor coordinates
               double v,               // interceptor velocity
               double&x,   double&y) { // interception point
    double a = vtx*vtx + vty*vty - v*v;
    double b = 2*((xt-xi)*vtx + (yt-yi)*vty);
    double c = (xt-xi)*(xt-xi) + (yt-yi)*(yt-yi);

    double D = b*b-4*a*c;
    if (D < 0) return false;

    double t;

    if (a == 0) t = -c/b;
    else
    {
        D = sqrt(D);
        double t1 = (-b+D)/(2*a);
        double t2 = (-b-D)/(2*a);

        if (t1 >= 0 && t2 >= 0) t = t1 < t2 ? t1 : t2;
        else if (t1 < 0) t = t2; else t = t1;
    }
    if (t < 0) return false;

    x = xt+vtx*t;
    y = yt+vty*t;

    return true;
    }

Если функция возвращает false, перехват невозможен. Если true - координаты точки перехвата будут в передаваемых по ссылке аргументах x и y.

▲ 1

Дело на плоскости. Добавим третье измерение - время. Область достижимых положений для преследователя - круговой конус (половинка конуса (t ≥ 0)). Траектория бегунка - отрезок в пространстве. Центральная (центр - стартовая позиция преследователя) проекция на плоскость t = const (> 0) переведёт конус в круг. Та же проекция переведёт отрезок в луч (не отрезок, так как у отрезка в пространстве начало лежит на плоскости t = 0). Если луч и круг пересекаются, преследователь может встретиться с бегунком.