JS手写三点质心定位算法 - 《算法》

1. 算法分析
2. 完整代码实现：
3. 仿真三点定位效果

1. 算法分析

1.1 三点定位的介绍

假设有三个基站信标： A、B、C, 其根据rssi信号的强度不同，形成不同半径的圆形范围。

而上图是一种非常理想的情况，即终端测得的信号非常稳定和准确，ABC三个终端采集的信号强度形成的圆交于一点。那么这个点就是我们当前用户终端的准确位置。

然鹅，。。。现实的信号强度受环境影响，以及信号转化成距离的精确度影响。往往会出现👇的几种情况：

1.2 三点定位的条件判断

那么我们需要根据多个信号形成的状态圆，分别进行考虑计算：
根据上面的几张图，我们可知两个圆会出现相交，相切，相离，内含等情况。

1.2.1 计算相交的情况

设点A坐标为(x1, y1), B坐标为（x2, y2）.通常A、B点可视为部署信标的坐标。r1,r2为信号圆的半径。通常为当前终端采集到的AB信标的信号强度。

由此我们可以逐步推导E点公式
获取欧几里得距离 AB^2 = (x2 - x1)^2 + (y2 - y1)^2
同时可得公式

AE^2 = r1^2 - CE^2
EB^2 = r2^2 - CE^2
AB^2 = (AE + EB) ^2

连立可解得公式： AE = (AB^2 + r1^2 - r2^2) / 2 * AB
同时解得：CE = sqrt(r1^2 - AE^2)

这样我们就求得了E点坐标，接下来就可以通过夹角很容易算出C，D点的坐标

Ex = x1 + (x2 - x1) * AE / AB
Ey = y1 + (y2 - y1) * AE / AB

接下来：

先计算出夹角：

angle = asin((y2 - y1) / AB)

然后计算出两个圆心的左右排列顺序

direct = x1 - x2 > 0 ? -1 : 1

计算出C点和D点坐标

Cx = Ex + direct _sin(angle) _CE

Cy = Ey - cos(angle) * CE

Dx = Ex - direct _sin(angle) _CE

Dy = Ey - cos(angle) * CE

这时候我们该选哪个点作为质心计算的三角点呢？我们还需要引入第三个信标作为参考

我们只需要根据CD 两点到第三点信标坐标的曼哈顿距离来进行判断即可：

RestPointerC = abs(Cx - x3) + abs(Cy - y3)
RestPointerD = abs(Dx - x3) + abs(Dy - y3)

然后RestPointerC，RestPointerD的距离哪个小，选取作为质心加权的参考点。

1.2.2 计算相切的情况

这种情况就相对简单

Ex = x1 + (x2 - x1) * AE / AB
Ey = y1 + (y2 - y1) * AE / AB

AE即为A圆的半径，同时选取Ex,Ey点作为参考点

1.2.3 计算相离的情况

根据二者信号强度的比例来计算对应的参考点O
distX = Bx - Ax
disY = By - Ay
Ox = Ax + (AB + (r1 - r2) _ 0.5) / AB distX
_Oy = Ay + (AB + (r1 - r2) _ 0.5) / AB distY_

1.2.4 计算内含的情况

同样根据强度比例获取对应的参考点O
distX = Bx - Ax
disY = By - Ay
Ox = Ax + (r1 + r2 + AB) 0.5 distX / AB
Oy = Ay + (r1 + r2 + AB) 0.5 distY / AB

理解了上述存在的几种信号分布情况，我们可以两两计算出对应的三点坐标：

1.3 三点定位的质心加权

我们知道三角形的三个点对应的坐标分别为x1、y1, x2、y2, x3、y3

通过三角质心的公式可以推导出：

x = (x1 + x2 + x3) / 3
y = (y1 + y2 + y3) / 3

但是这里的质心坐标的权重是一样的，我们还需要对其进行加权优化.
我们依据Rssi的信号强度（圆半径）来进行加权，优化的公式为：

x = (x1 / (r1 + r2) + x2 / (r1 + r3) + x3 / (r2 + r3)) / (1 / (r1 + r2) + 1/ (r1 + r3) + 1 / (r2 + r3))

从上述公式可以看出，我们将下面的除数3根据信号强度的加权因子重新进行了加权计算。

质心加权算法的优化

两个已知节点的半径和的倒数这个因子的选取，例如r1和r2成为决定未知节点位置的关键。然而r1和r2中数值较大的一个会在这个因子中起到更大的作用，使得较小的数值对应的节点所发挥的作用弱化，造成定位误差。

因此参考相关资料，通过改善参考因子的决定权，采取增加幂值的做法，进一步权衡加权因子，防止偏大的信号修正过度。

具体算法如下：

    // 优化的三点质心加权
    const x1 = (retPoint1.x * (1 / r1 + 1 / Math.pow(r2, r1 / r2))
      + retPoint2.x * (1 / Math.pow(r2, r1 / r2) + 1 / Math.pow(r3, r1 / r3))
      + retPoint3.x * (1 / r1 + 1 / Math.pow(r3, r1 / r3))) / (
        2 * (1 / r1 + 1 / Math.pow(r2, r1 / r2) + 1 / Math.pow(r3, r1 / r3))
      );
    const y1 = (retPoint1.y * (1 / r1 + 1 / Math.pow(r2, r1 / r2))
      + retPoint2.y * (1 / Math.pow(r2, r1 / r2) + 1 / Math.pow(r3, r1 / r3))
      + retPoint3.y * (1 / r1 + 1 / Math.pow(r3, r1 / r3))) / (
        2 * (1 / r1 + 1 / Math.pow(r2, r1 / r2) + 1 / Math.pow(r3, r1 / r3))
      );

即通过幂值进行加权，使得在系数中原本起主要作用的信号较强的一方所起作用稍微弱化，以防止过度修正。

参考文献：基于RSSI测距的改进加权质心定位算法

2. 完整代码实现：

/*
* @Author: kunnisser
* @Date: 2020-12-03 10:17:56
* @LastEditors: kunnisser
* @LastEditTime: 2021-01-21 16:00:43
* @FilePath: /kunigame/kuni/src/state/postion/index.ts
* @Description: ---- 三点加权质心定位算法 ----
*/
import KnScene from "ts@/kuni/lib/gameobjects/kn_scene";
import Game from "ts@/kuni/lib/core";
class Triangulation extends KnScene {
  public game: Game;
  public shootType: number;
  public tween: any;
  constructor(game: Game, key: string) {
    super(game, key);
    this.game = game;
    this.resouces = {
    }
  }
  boot() {
  }
  create() {
    this.tween = this.game.add.tween();
    this.addBackground();
    let r1 = 100 + Math.random() * 100;
    let r2 = 30 + Math.random() * 100;
    let r3 = 200 + Math.random() * 100;
    let point1 = {
      x: 600,
      y: 400,
    }
    let point2 = {
      x: 600,
      y: 450,
    }
    let point3 = {
      x: 300,
      y: 300
    }
    let c1 = this.game.add.graphics().generateLineCircle(0xd10311, [point1.x, point1.y, r1]);
    let c2 = this.game.add.graphics().generateLineCircle(0xe8a91a, [point2.x, point2.y, r2]);
    let c3 = this.game.add.graphics().generateLineCircle(0x1a73e8, [point3.x, point3.y, r3]);
    this.addChild(c1);
    this.addChild(c2);
    this.addChild(c3);
    let retPoint1 = this.computedPointer(r1, r2, point1, point2, point3);
    let retPoint2 = this.computedPointer(r1, r3, point1, point3, point2);
    let retPoint3 = this.computedPointer(r2, r3, point2, point3, point1);
    // 质心加权
    let x = (retPoint1.x / (r1 + r2) + retPoint2.x / (r1 + r3) + retPoint3.x / (r2 + r3)) / (
      1 / (r1 + r2) + 1 / (r1 + r3) + 1 / (r2 + r3)
    );
    let y = (retPoint1.y / (r1 + r2) + retPoint2.y / (r1 + r3) + retPoint3.y / (r2 + r3)) / (
      1 / (r1 + r2) + 1 / (r1 + r3) + 1 / (r2 + r3)
    );
    // 绘制定位点
    let pos = this.game.add.graphics().generateCircle(0x1ae846, [x, y, 8]);
    this.addChild(pos);
    console.log('定位坐标 x: ' + x, 'y: ' + y);
    // 优化的三点质心加权
    let x1 = (retPoint1.x * (1 / r1 + 1 / Math.pow(r2, r1 / r2))
              + retPoint2.x * (1 / Math.pow(r2, r1 / r2) + 1 / Math.pow(r3, r1 / r3))
              + retPoint3.x * (1 / r1 + 1 / Math.pow(r3, r1 / r3))) / (
      2 * (1 / r1 + 1 / Math.pow(r2, r1 / r2) + 1 / Math.pow(r3, r1 / r3))
    );
    let y1 = (retPoint1.y * (1 / r1 + 1 / Math.pow(r2, r1 / r2))
              + retPoint2.y * (1 / Math.pow(r2, r1 / r2) + 1 / Math.pow(r3, r1 / r3))
              + retPoint3.y * (1 / r1 + 1 / Math.pow(r3, r1 / r3))) / (
      2 * (1 / r1 + 1 / Math.pow(r2, r1 / r2) + 1 / Math.pow(r3, r1 / r3))
    );
    let pos1 = this.game.add.graphics().generateCircle(0xb51ae8, [x1, y1, 8]);
    this.addChild(pos1);
    console.log('定位坐标2 x: ' + x1, 'y: ' + y1);
  }
  computedPointer(r1, r2, p1, p2, rest) {
    let AB: number = Math.sqrt(Math.pow(p1.x - p2.x, 2) + Math.pow(p1.y - p2.y, 2));
    console.log(AB);
    if (AB < r1 + r2 && Math.abs(r1 - r2) < AB) {
      console.log('相交');
      let AE: number = (Math.pow(r2, 2) - Math.pow(r1, 2) - Math.pow(AB, 2)) / (-2 * AB);
      console.log(AE);
      let CE: number = Math.sqrt(Math.pow(r1, 2) - Math.pow(AE, 2));
      console.log(CE);
      let flag = p1.x - p2.x > 0 ? -1 : 1;
      let pointE = {
        x: p1.x + ((p2.x - p1.x) * AE) / AB,
        y: p1.y + ((p2.y - p1.y) * AE) / AB
      };
      console.log(pointE);
      let angle = Math.asin((p2.y - p1.y) / AB);
      console.log(angle);
      let pointC = {
        x: pointE.x + flag * Math.sin(angle) * CE,
        y: pointE.y - Math.cos(angle) * CE
      };
      console.log(pointC);
      let pointD = {
        x: pointE.x - flag * Math.sin(angle) * CE,
        y: pointE.y + Math.cos(angle) * CE
      };
      let restPointC = Math.abs(pointC.x - rest.x) + Math.abs(pointC.y - rest.y);
      let restPointD = Math.abs(pointD.x - rest.x) + Math.abs(pointD.y - rest.y);
      if (restPointC > restPointD) {
        let pd = this.game.add.graphics().generateCircle(0xffffff, [pointD.x, pointD.y, 6]);
        this.addChild(pd);
        return pointD;
      } else {
        // 绘制相交点
        let pc = this.game.add.graphics().generateCircle(0xffffff, [pointC.x, pointC.y, 6]);
        this.addChild(pc);
        return pointC;
      }
    } else if (AB == r1 + r2) {
      console.log('相切');
      let AE: number = (Math.pow(r2, 2) - Math.pow(r1, 2) - Math.pow(AB, 2)) / (-2 * AB);
      let pointE = {
        x: p1.x + ((p2.x - p1.x) * AE) / AB,
        y: p1.y + ((p2.y - p1.y) * AE) / AB
      };
      console.log(pointE);
      // 绘制相交点
      let pe = this.game.add.graphics().generateCircle(0xffffff, [pointE.x, pointE.y, 6]);
      this.addChild(pe);
      return pointE;
    } else if (AB > r1 + r2) {
      console.log('相离');
      let disX = p2.x - p1.x;
      let disY = p2.y - p1.y;
      let pointE = {
        x: p1.x + (AB + r1 - r2) * 0.5 / AB * disX,
        y: p1.y + (AB + r1 - r2) * 0.5 / AB * disY
      };
      console.log(pointE);
      // 绘制相交点
      let pe = this.game.add.graphics().generateCircle(0xffffff, [pointE.x, pointE.y, 6]);
      this.addChild(pe);
      return pointE;
    } else {
      console.log('内含');
      let disX = p2.x - p1.x;
      let disY = p2.y - p1.y;
      let pointE = {
        x: p1.x + (r1 + r2 + AB) * 0.5 * disX / AB,
        y: p1.y + (r1 + r2 + AB) * 0.5 * disY / AB
      };
      console.log(pointE);
      // 绘制相交点
      let pe = this.game.add.graphics().generateCircle(0xffffff, [pointE.x, pointE.y, 6]);
      this.addChild(pe);
      return pointE;
    }
  }
  addBackground() {
    const bg = this.game.add.image('wsjBg', this);
    bg.width = this.game.config.width;
    bg.height = this.game.config.height;
  }
  reset() {
    if (this.children.length > 1) {
      // 清除group子对象
      this.removeChildren(1, this.children.length);
    }
  }
}
export default Triangulation;

3. 仿真三点定位效果

白色为计算出的三角参考点，绿色为普通加权的结果坐标，紫色为优化后加权的坐标：

结尾：三点定位的深入点在于加权因子的不断优化，实际上距离完整成熟的技术方案落地，还需要考虑进一步优化算法，同时降低计算成本等多方面因素，本文仅供研究参考。