三边定位法有哪些限制？

Question

我需要帮助解决一个问题，这个问题出现在我的小型机器人实验中。基本思路是每个小机器人都能够近似计算到物体的距离，但我得到的近似值太粗略了，我希望能够计算出更准确的数值。

输入：顶点列表(v1,v2,...vn)，一个顶点v*（机器人） 输出：未知顶点v*（物体）的坐标

每个顶点v1到vn的坐标都是已知的（通过调用顶点上的getX()和getY()方法提供），并且可以通过调用getApproximateDistance(v*)方法获得到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上讨论？

- Skeen

---

快去math.stackexchange.com吧 - 他们会非常熟悉这个问题！ - Matty

Answer 1

我会选择一个简单的离散方法。环形区域的隐式公式是微不足道的，如果环形区域的数量很多，则可以使用基于扫描线的方法相对高效地计算它们的交集。

为了在快速计算中获得高精度，一种选择可能是使用多分辨率方法（即首先从低分辨率开始，然后仅重新计算靠近有效点的高分辨率样本）。

我写的一个小型Python程序可以在约0.5秒内生成一个400x400像素的交集区域图像（如果使用C进行计算，则可以获得100倍的加速）。

# 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))

另一个有趣的选择可能是使用GPU（图形处理器），如果可用的话。从白色图片开始，用黑色绘制每个环状物的外部，最终留下的交集区域将是白色的。

例如，使用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分辨率的图像，计算部分大约需要8ms（我不确定它是否是硬件加速的）。

如果仅需要计算交点的重心，则根本不需要进行内存分配。例如，“暴力法”只需要几行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毫秒内计算重心（x=1.50000，y=1.383250），或者使用resolution = 400在1.3毫秒内计算（x=1.500000，y=1.383308）。当然，即使是仅针对重心版本，也可以实现双倍速度提升。

- 6502

---

虽然将图片作为输出很好，但我该如何修改它以输出坐标呢？- 如io分析所述 - Skeen

我的机器人不幸地没有使用GPU硬件运行 ;) - Skeen

可能平均值（重心）是一个合理的选择（虽然理论上重心可能会落在不可接受的区域之外，但考虑到具体应用，我认为这种情况不太可能发生）。否则，一个好的选择可能是“最内部点”（即距离排除区域最远的内部点）；然而，计算这个点有点麻烦（在我看来，最好的方法是使用欧几里得变换并选择绝对最大值——仍然是结果边界框面积的线性，但算法并不简单）。 - 6502

我不太确定这是否是我要找的，可能只是因为我似乎无法弄清楚如何应用你所说的内容，或者可能只是因为我有点害怕必须分配一个巨大的字符数组。 - Skeen

我已经在C语言中添加了一个质心计算实现，它完全不需要内存分配（简单的暴力算法）。对于计算“最内部点”，我认为可行的解决方案需要分配一个矩阵，以便能够快速计算欧几里得距离变换。 - 6502

我玩了一下Python，接下来我会试试C语言，初看似乎很完美，但我会试用一下，如果没问题的话，你就会得到认可和奖励 ;) - Skeen

Answer 2

我会从“最大/最小值”转向尝试最小化一个误差函数。这将让我们解决寻找最佳拟合n个球的交点中讨论的问题，这比相交一系列复杂形状更容易处理。（如果其中一个机器人的传感器出了问题并给出了不可能的值怎么办？即使存在这种变异，通常也能得到合理的答案。）

- btilly

---

机器人传感器读数被检查，任何无法使用的值都不会包含在三边测量中。 - Skeen

Answer 3

我不确定您的情况，但在典型的机器人应用中，您将定期读取传感器并处理数据。如果是这种情况，您正在尝试基于嘈杂的数据估计位置，这是一个常见问题。作为一种简单（不太严格）的方法，您可以采取现有位置并将其向或远离每个已知点进行调整。将目标测量距离减去当前目标距离，将该增量（差错）乘以介于0和1之间的某个值，然后将您的估计位置移动到目标那么远。为每个目标重复此操作。然后在获得新的一组测量值时重复。乘数将具有类似低通滤波器的效果，较小的值将使您获得更稳定的位置估计，并对运动反应较慢。对于距离，请使用最小值和最大值的平均值。如果您可以对范围收紧较好地限制到一个目标，则可以将乘数更接近于1。

当然，这是一个简陋的位置估算器。数学专家可能会更严谨，但也更复杂。解决方案绝对与相交区域和处理几何形状无关。

- phkahler