三边测量定位算法C语言实现

原创

于 2021-12-22 11:30:03 发布 · 5k 阅读

39 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #c语言 #开发语言 #定位

三边定位算法

三边定位算法简介

三个位置已知点（锚节点，圆心）以及其到另外一个未知点（待定位点）的距离即半径（不准确），求位置节点坐标的过程；

实际上（例如用rssi算距离）三个点到未知点的距离半径很可能不交与一点；怎么办呢？
定位过程示意图

参考文章 https://www.jianshu.com/p/94475aacc95a

这里引用下方法的逻辑：

两圆不相交时，按比例取中点（和）。当两圆相交时，就是拆分成几个三角形，通过一系列三级函数计算出未知的两个交点。最后将三点连成三角形，此三角形的重心（即点M）就是最终定位点，步骤如下：

通过勾股定律用a、b长度计算出线段AB长度（即点A到点B距离），使用 ra + rb 与AB对比即可得知两圆的对应情况，一共有三种情况：两圆相离ra + rb < AB、两圆相切ra + rb == AB、两圆相交ra + rb > AB。
两圆相离：按照两圆半径的比例在线段AZ上求点，即；因为“两圆相切ra + rb == AB”在实际程序中出现的几率太小，所以直接使用“两圆相离”相同的求法。
两圆相交：求出相交点C的坐标 {Cx, Cy}，可通过得出Q1，通过得出Q2，最后计算出点C的坐标：

同理可求出点D的坐标。得到C、D两交点后取距离圆心Z点近的交点作为最后三个参考点中的一点。
将最后求得的三个参考点连接成一个三角形，该三角形的重心即为最后的定位点M：

作者：Hank_Zhong
链接：https://www.jianshu.com/p/94475aacc95a
来源：简书

发现的问题

但是这篇文章对应的如下代码好像不太对，，
具体就是Eo=0.5*EF那里（第11行），这应该是一种工程近似，在两圆相交的很少的情况之下可以这样近似，可实际情况复杂多变，两个圆相交很多的情况很常见；
总要判断相对位置正负号很麻烦；

-(CGPoint)sidePointCalculationWith:(double)x1 :(double)y1 :(double)r1 
                                  :(double)x2 :(double)y2 :(double)r2 
                                  :(double)x3 :(double)y3{
   
   
    //勾股定理  sqrt(X)是X开根号  pow（X,n）是X的n次方
    //取beacon1圆心A 与 beacon2圆心B的距离
    double AB = sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2));
    double rAB = (r1 + r2);
    if (rAB > AB && (r1 < AB && r2 < AB)) {
   
   
        //两圆有相交点,两圆相交点为C、D。两圆与AB的相交点为E、F。o是EF的中点。
        double EF = rAB - AB;
        double Eo = EF * 0.5;
        double AE = r1 - EF;
        double Ao = AE + Eo;
        double AQ1 = acos((x2 - x1) / AB);
        double AQ2 = ac