三边测距定位算法详解

“定位”这个概念相信大家都不陌生，可以说定位技术的快速发展极大地方便了我们的日常生活。例如，手机的定位导航系统帮助那些方向感差的人更加自信地外出；航天飞行器可以自由地在宇宙中穿行，但仍然受制于人类的掌控能力；无人驾驶技术也离不开定位系统的支持……

接下来将向大家介绍一种简单的定位算法——三边测距定位算法。

【简介】

简单的说，实现定位你只需要做好以下两点：

测量值 从某种意义上说，几乎所有你能测量的数据都取决于“位置”，并且可以利用测得的数据进行定位。最理想的情况是测量那些对“位置”非常敏感的数据。例如，测量温度可能会知道你身在哪个大陆，当然这样的结果并不准确，但是如果测量的是你相对于某些点的距离或者角度，那么对你的位置的定位可能会更精确一些。

参考点 描述一个确定的位置，正确的方法就是描述它相对于某些参考点的位置。参考点可以是“你的家”、“北极星”或“天空中的一些卫星”。对我们来说，我们将使用锚。利用3个锚点就可以描述一个二维的坐标系统，在这个系统中我们可以找到我们的位置。对于三维定位系统，我们则需要4个锚点。

下面是一些例子:

 

注意：一些GPS接收器也使用多普勒频移定位

【三边测距法】

最常用的定位方法是使用基本的几何图形来估计位置。通过测量与锚点的距离，就可以确定你的位置。如果我们只知道自己与锚点的距离，那么我们的位置肯定会在以锚点P为圆心以测得距离d为半径的圆上。如果我们用3个锚进行距离测量，我们会发现我们的位置在三个圆的交点上，如图1所示。这种方法被称为三边测距法（如果使用的锚点数量超过3个，则称为多边测距法）。

这种方法的困难在于测量中总会有一些噪音，测量并不完美。因此，圆不会在一个点相交。为了解决这个问题，我们试着找出最接近所有圆的点。

图1 三边测量

注：你可以选择跳过该算法的描述，直接参阅下一篇文章：超宽带是如何工作的。

【一个基本的算法】

在本节中，我们将介绍一种简单的算法，它可以从一系列的范围测量中计算出位置。这个基本算法不是最优的，但是当范围测量足够精确时，它就会很好地工作。

我们将解释2D定位的算法。位置P由坐标x和y给出。第i个锚点pi的位置坐标为(xi,yi)，如果我们有N个锚，那么i的取值为从1到N，这些锚点的坐标是已知的。

现在，位置P与第i个锚点之间的距离由di表示，di由下面的公式给出：

对等式两边取平方：

上面方程的问题在于含有非线性项x^2和y^2。我们可以通过从di^2中减去dN^2来消除这些非线性项，得到N-1个方程，其中第i个方程式为：

现在我们有了一些关于坐标x和y的线性方程，这很好，因为线性方程很容易求解。我们把它写成矩阵的形式：

其中

我们现在可以解这个方程组了。

如果我们恰好有3个锚点：N=3，我们就会得到两个方程来求解两个未知数，通过求解下面的方程，我们可以找到P的位置：

如果我们有超过3个锚点：N>3，我们得到的方程的数量要多于未知数的数量，此时A的逆矩阵是不存在的。为了解决这个问题，我们可以利用伪逆算子来计算这个位置。这就产生了下面的方程式：

请注意，上面的公式将尽可能地将坐标x和y与所有不同的方程相匹配。因此，当使用更多的锚时，定位的准确性也会随之提高。

上面描述的算法被称为线性最小二乘算法。“线性”是因为我们把方程进行了线性化（通过平方）和“最小二乘”是因为矩阵的（伪）逆矩阵会使所有方程的平方误差最小化。

【拓展】

上面描述的算法是一种非常简单和低复杂度的算法。如果你想了解更多关于先进定位技术的知识，建议你寻找以下主题：非线性最小二乘、卡尔曼滤波、粒子滤波、置信传播……

以上就是良许教程网为各位朋友分享的Linu系统相关内容。想要了解更多Linux相关知识记得关注公众号“良许Linux”，或扫描下方二维码进行关注，更多干货等着你 ！