iOS蓝牙定位算法

      室内定位的解决方案有很多种，但是由于信标（Beacons）的信号是不断变化的，所以想要获取准确的定位信息减小误差，最有效的方式就是尽可能多的铺设信标，除此之外就是要有行之有效的算法。室内定位最常用的就是指纹法和三边测距法：指纹法就是在信标覆盖范围内获取多个信标的信号强度进行匹配，三边测距法则是通过三个信号点（Beacons）和分别对应的距离，形成三个圆并相交于一点。

      本篇主要讲述三边测距法的进一步优化，因为三边测距法在实际情况中并没有那么理想化，有可能会出现两圆不相交、圆包含圆、只有两个信号点或者多个信号点排成一列的情况（过道里），这都是一些比较常见的场景。所以我们需要一个能同时解决上面这些问题的计算方法--分步定位。

     分步定位

      在iOS开发中，使用CLLocationManager的startRangingBeaconsSatisfyingContraint:方法监听Beacons，并通过代理回调中获得Beacons列表。取出rssi信号值最强的三个点，取accuracy值作为圆半径（需要减去高度差），用major、minor值从后台返回的数据中取出对应的坐标点数据即为三个圆的圆心。

       

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

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

           ；
           
      采用分步定位法测量一个移动节点的位置，只需要3个参考节点。该定位法还避免了采用三边测量法可能无解的情况，使得该方法的适应性更强。

     相关代码

      1. 信标（坐标）点准备：

       （1）信标使用自建坐标系（以米为单位）

       //注意：x1, y1, r1, x2, y2, r2, x3, y3都是以米为单位
        let pointA = sidePointCalculation(with: x1, y1, r1, x2, y2, r2, x3, y3)
        let pointB = sidePointCalculation(with: x2, y2, r2, x3, y3, r3, x1, y1)
        let pointC = sidePointCalculation(with: x1, y1, r1, x3, y3, r3, x2, y2)

        let Mx = Double((pointA.x + pointB.x + pointC.x) / 3)
        let My = Double((pointA.y + pointB.y + pointC.y) / 3)

         （2）使用经纬度坐标系

                如果信标的坐标使用的是经纬度坐标系需要将经纬度坐标系转换成墨卡托坐标系（墨卡托坐标是将经纬度转换成以米为单位），计算出点坐标后再将墨卡托坐标转换成经纬度坐标系。

        //注意：x1, y1, r1, x2, y2, r2, x3, y3都是以米为单位
        let pointA = sidePointCalculation(with: lat2Meters(location1.latitude), lon2Meters(location1.longitude), r1, lat2Meters(location2.latitude), lon2Meters(location2.longitude), r2, lat2Meters(location3.latitude), lon2Meters(location3.longitude))
        let pointB = sidePointCalculation(with: x2..., y2..., r2..., x3..., y3..., r3..., x1..., y1...)
        let pointC = sidePointCalculation(with: x1..., y1..., r1..., x3..., y3..., r3..., x2..., y2...)

        let Mx = Double((pointA.x + pointB.x + pointC.x) / 3)
        let My = Double((pointA.y + pointB.y + pointC.y) / 3)
        
        let lat = meters2Lat(Mx)
        let lon = meters2Lon(My)

extension MapController {
    //////添加坐标转换相应的方法
    /**
     * X米转经纬度
     */
    func meters2Lon(_ mx: Double) -> Double {
        let lon = mx * (180.0/20037508.342789244)//2*Math.PI*6378137/2.0=20037508.342789244
        return lon
    }

    /**
     * Y米转经纬度
     */
    func meters2Lat(_ my: Double) -> Double {
        var lat = my * (180.0/20037508.342789244)
        lat = 180.0 / .pi * (2 * atan(exp(lat * (.pi/180.0))) - .pi/2.0)
        return lat
    }
    
    /**
     * X经纬度转米
     */
    func lon2Meters(_ lon: Double) -> Double {
        let mx = lon * (20037508.342789244/180.0)
        return mx
    }

    /**
     * Y经纬度转米
     */
    func lat2Meters(_ lat: Double) -> Double {
        var my = log(tan((90 + lat) * (.pi/360.0)))/(.pi/180.0)
        my = my * (20037508.342789244/180.0)
        return my
    }
}

     2. ：计算点坐标

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

            let BF = r2 - EF
            let Bo = BF + Eo
            //let BQ1 = acos(fabs(x1 - x2) / AB);
            let BQ2 = acos(Bo / r2)
            
            //原点{0,0}在左上角的情况下
            let Cx = x1 + (r1 * cos(AQ1 + AQ2))
            var Cy = 0.0
            var Dx = x2 - (r2 * cos(AQ1 + BQ2))
            var Dy = 0.0
            if x1 < x2 {
                Dx = x2 - (r2 * cos(AQ1 + BQ2))
                if y1 < y2 {
                    Cy = y1 + (r1 * sin(AQ1 + AQ2))
                    Dy = y2 - (r2 * sin(AQ1 + BQ2))
                } else {
                    Cy = y1 - (r1 * sin(AQ1 + AQ2))
                    Dy = y2 + (r2 * sin(AQ1 + BQ2))
                }
            } else {
                Cy = y1 + (r1 * sin(AQ1 + AQ2))
                if y1 < y2 {
                    Dy = y2 - (r2 * sin(AQ1 + BQ2))
                } else {
                    Dy = y2 + (r2 * sin(AQ1 + BQ2))
                }
            }

            let Cc = sqrt(pow(Cx - x3, 2) + pow(Cy - y3, 2))
            let Dc = sqrt(pow(Dx - x3, 2) + pow(Dy - y3, 2))

            return Cc < Dc ? CGPoint(x: CGFloat(Cx), y: CGFloat(Cy)) : CGPoint(x: CGFloat(Dx), y: CGFloat(Dy))
        } else {
            //两圆无相交点
            return midpointCalculation(with: x1, y1, r1, x2, y2, r2)
        }
    }
    
    //两圆无相交点
    func midpointCalculation(with x1: Double, _ y1: Double, _ r1: Double, _ x2: Double, _ y2: Double, _ r2: Double) -> CGPoint {
        let a = y1 - y2//竖边
        let b = x1 - x2//横边
        let rr = r1 + r2
        let s = r1 / rr

        let x = Double(abs(Float(x1 - (b * s))))
        let y = Double(abs(Float(y1 - (a * s))))

        return CGPoint(x: CGFloat(x), y: CGFloat(y))
    }
}

        拓展⚠️：如果没有使用CLLocationManager的startRangingBeaconsSatisfyingContraint:而是通过import CoreBluetooth获取的蓝牙信号需要计算距离

      （1）如果使用CoreBluetooth获取的蓝牙信号，需要遵守代理协议CBCentralManagerDelegate，通过centralManagerDidUpdateState(_ central: CBCentralManager)方法扫描信标

func centralManagerDidUpdateState(_ central: CBCentralManager) {
        //确保本中心设备支持蓝牙低能耗（BLE）并开启时才能继续操作
        switch central.state{
        case .unknown:
            print("未知")
        case .resetting:
            print("蓝牙重置中")
        case .unsupported:
            print("本机不支持BLE")
        case .unauthorized:
            print("未授权")
        case .poweredOff:
            print("蓝牙未开启")
        case .poweredOn:
            //扫描正在广播的外设--每当发现外设时都会调用didDiscover peripheral方法
            //withServices:[xx]--只扫描正在广播xx服务的外设，若nil则扫描所有外设（费电，不推荐）let serviceUUIDS = [CBUUID(string: "FFE0"),CBUUID(string: "FEE7")]//[CBUUID(string: "")]
            central.scanForPeripherals(withServices: serviceUUIDS, options: [CBCentralManagerScanOptionAllowDuplicatesKey:true])
        @unknown default:
            print("来自未来的错误")
        }
    }
    
    /// 发现外设：获取RSSI信号和信标信息
    /// - Parameters:
    ///   - central: 提供此更新的中央管理器
    ///   - peripheral: 外设
    ///   - advertisementData: 包含任何广告和扫描响应数据的字典
    ///   - RSSI: 当前RSSI，以dBm为单位
    func centralManager(_ central: CBCentralManager, didDiscover peripheral: CBPeripheral, advertisementData: [String : Any], rssi RSSI: NSNumber) {
        
        #if DEBUG
        print("发现设备：peripheral = (peripheral),advertisementData = (advertisementData),RSSI = (RSSI)")
        #endif
    }

      （2）根据RSSI计算距离

    /*
    计算公式: d = 10^((abs(RSSI) - A) / (10 * n))
    d - 计算所得距离
    RSSI - 接收信号强度（负值）
    A - 发射端和接收端相隔1米时的信号强度
    n - 环境衰减因子
    */
    func calcDistByRSSI(_ rssi: Int) -> Double {
        //TODO:需要多次测试确定A和n的值
        let dis = pow(10.0, (Double((abs(rssi)) - 65)/(10 * 0.8)))
        
        return dis
    }