Трилатерация с ограничениями? - PullRequest
5 голосов
/ 27 мая 2011

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

Итак:
Ввод: Список вершин (v_1, v_2, ... v_n), вершина v_* (роботы)
Вывод: Координаты неизвестной вершины v_* (объект)

Координаты каждой вершины v_1 - v_n хорошо известны (предоставляется путем вызова getX() и getY() в вершине), и можно получить приблизительный диапазон до v_* путем вызова; getApproximateDistance(v_*), функция getApproximateDistance() возвращает переменные двух переменных, то есть; minDistance и maxDistance. - Фактическое расстояние лежит между ними.

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

Примечание: триангуляция - путь вместо этого?
Примечание. Возможно, мне хотелось бы узнать, как это сделать, компромиссы между производительностью и точностью.

Пример данных:

[Vertex . `getX()` . `getY()` . `minDistance` . `maxDistance`]
[`v_1`  .  2       .  2       .  0.5          .  1  ]
[`v_2`  .  1       .  2       .  0.3          .  1  ]
[`v_3`  .  1.5     .  1       .  0.3          .  0.5]

Картинка для отображения данных: http://img52.imageshack.us/img52/6414/unavngivetcb.png

Очевидно, что аппроксимация для v_1 может быть лучше, чем [0.5; 1], поскольку фигура, которую создают приведенные выше данные, представляет собой небольшой разрез кольца (ограниченный v_3), однако как бы я рассчитал это, и, возможно, найти приблизительное значение на этой фигуре (эта фигура может быть вогнутой)?

Это лучше подходит для MathOverflow?

Ответы [ 3 ]

1 голос
/ 30 мая 2011

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

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

Небольшая игрушка-питон, которую я написал, может генерировать 400x400 пиксельное изображение области пересечения примерно за 0,5 секунды (это тот тип вычислений, который увеличился бы в 100 раз, если бы был выполнен с C).

enter image description here

# x, y, r0, r1
data = [(2.0, 2.0, 0.5, 1.0),
        (1.0, 2.0, 0.3, 1.0),
        (1.5, 1.0, 0.3, 0.5)]

x0 = max(x - r1 for x, y, r0, r1 in data)
y0 = max(y - r1 for x, y, r0, r1 in data)
x1 = min(x + r1 for x, y, r0, r1 in data)
y1 = min(y + r1 for x, y, r0, r1 in data)

def hit(x, y):
    for cx, cy, r0, r1 in data:
        if not (r0**2 <= ((x - cx)**2 + (y - cy)**2) <= r1**2):
            return False
    return True

res = 400
step = 16
white = chr(255)
grey = chr(192)
black = chr(0)
img = [black] * (res * res)

# Low-res pass
cells = {}
for i in xrange(0, res, step):
    y = y0 + i * (y1 - y0) / res
    for j in xrange(0, res, step):
        x = x0 + j * (x1 - x0) / res
        if hit(x, y):
            for h in xrange(-step*2, step*3, step):
                for v in xrange(-step*2, step*3, step):
                    cells[(i+v, j+h)] = True

# High-res pass
for i in xrange(0, res, step):
    for j in xrange(0, res, step):
        if cells.get((i, j), False):
            img[i * res + j] = grey
            img[(i + step - 1) * res + j] = grey
            img[(i + step - 1) * res + (j + step - 1)] = grey
            img[i * res + (j + step - 1)] = grey
            for v in xrange(step):
                y = y0 + (i + v) * (y1 - y0) / res
                for h in xrange(step):
                    x = x0 + (j + h) * (x1 - x0) / res
                    if hit(x, y):
                        img[(i + v)*res + (j + h)] = white

open("result.pgm", "wb").write(("P5\n%i %i 255\n" % (res, res)) +
                               "".join(img))

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

Например, для Python / Qt код для выполнения этого вычисления просто:

img = QImage(res, res, QImage.Format_RGB32)
dc = QPainter(img)
dc.fillRect(0, 0, res, res, QBrush(QColor(255, 255, 255)))
dc.setPen(Qt.NoPen)
dc.setBrush(QBrush(QColor(0, 0, 0)))
for x, y, r0, r1 in data:
    xa1 = (x - r1 - x0) * res / (x1 - x0)
    xb1 = (x + r1 - x0) * res / (x1 - x0)
    ya1 = (y - r1 - y0) * res / (y1 - y0)
    yb1 = (y + r1 - y0) * res / (y1 - y0)
    xa0 = (x - r0 - x0) * res / (x1 - x0)
    xb0 = (x + r0 - x0) * res / (x1 - x0)
    ya0 = (y - r0 - y0) * res / (y1 - y0)
    yb0 = (y + r0 - y0) * res / (y1 - y0)
    p = QPainterPath()
    p.addEllipse(QRectF(xa0, ya0, xb0-xa0, yb0-ya0))
    p.addEllipse(QRectF(xa1, ya1, xb1-xa1, yb1-ya1))
    p.addRect(QRectF(0, 0, res, res))
    dc.drawPath(p)

, а вычисление для изображения с разрешением 800x800 занимает около 8 мс (и я не уверен, что оно аппаратно ускорено).

Если только барицентр пересечения должен быть вычислен, тогда выделение памяти вообще не производится. Например, подход "грубой силы" - это всего лишь несколько строк C

typedef struct TReading {
    double x, y, r0, r1;
} Reading;

int hit(double xx, double yy,
        Reading *readings, int num_readings)
{
    while (num_readings--)
    {
        double dx = xx - readings->x;
        double dy = yy - readings->y;
        double d2 = dx*dx + dy*dy;
        if (d2 < readings->r0 * readings->r0) return 0;
        if (d2 > readings->r1 * readings->r1) return 0;
        readings++;
    }
    return 1;
}

int computeLocation(Reading *readings, int num_readings,
                    int resolution,
                    double *result_x, double *result_y)
{
    // Compute bounding box of interesting zone
    double x0 = -1E20, y0 = -1E20, x1 = 1E20, y1 = 1E20;
    for (int i=0; i<num_readings; i++)
    {
        if (readings[i].x - readings[i].r1 > x0)
          x0 = readings[i].x - readings[i].r1;
        if (readings[i].y - readings[i].r1 > y0)
          y0 = readings[i].y - readings[i].r1;
        if (readings[i].x + readings[i].r1 < x1)
          x1 = readings[i].x + readings[i].r1;
        if (readings[i].y + readings[i].r1 < y1)
          y1 = readings[i].y + readings[i].r1;
    }

    // Scan processing
    double ax = 0, ay = 0;
    int total = 0;
    for (int i=0; i<=resolution; i++)
    {
        double yy = y0 + i * (y1 - y0) / resolution;
        for (int j=0; j<=resolution; j++)
        {
            double xx = x0 + j * (x1 - x0) / resolution;
            if (hit(xx, yy, readings, num_readings))
            {
                ax += xx; ay += yy; total += 1;
            }
        }
    }
    if (total)
    {
        *result_x = ax / total;
        *result_y = ay / total;
    }
    return total;
}

И на моем ПК можно вычислить барицентр с resolution = 100 за 0,08 мс (х = 1,50000, y = 1,383250) или с resolution = 400 с 1,3 мс (х = 1,500000, y = 1,383308). Конечно, двухступенчатое ускорение может быть реализовано даже для версии только с барицентром.

1 голос
/ 28 мая 2011

Я бы переключился с "макс / мин" на попытку минимизировать функцию ошибки.Это подводит вас к проблеме, обсуждаемой на . Нахождение точки, которая наилучшим образом соответствует пересечению n сфер , которая более удобна, чем пересечение серии сложных форм(А что, если датчик одного робота испортится, и он даст невозможное значение? Этот вариант все равно обычно дает разумный ответ.)

0 голосов
/ 01 июня 2011

Не уверен насчет вашего случая, но в типичном приложении для робототехники вы будете периодически считывать датчики и обрабатывать данные.Если это так, вы пытаетесь оценить местоположение на основе шумных данных, и это распространенная проблема.В качестве простого (менее строгого) метода вы можете взять существующую позицию и скорректировать ее в направлении или от каждой известной точки.Возьмите измеренное расстояние до цели минус текущее расстояние до цели, умножьте эту дельту (ошибку) на какое-то значение от 0 до 1 и переместите свою оценочную позицию так сильно к цели.Повторите для каждой цели.Затем повторяйте каждый раз, когда вы получаете новый набор измерений.Множитель будет иметь эффект, подобный фильтру нижних частот, меньшие значения дадут вам более стабильную оценку положения с более медленным откликом на движение.Для расстояния используйте среднее значение мин и макс.Если вы можете установить более жесткие границы диапазона для одной цели, вы можете увеличить множитель ближе к 1 для этой цели.

Это, конечно, грубая оценка позиции.Математические парни могут быть более строгими, но и более сложными.Решение определенно не имеет ничего общего с пересекающимися областями и работой с геометрическими фигурами.

...