UWB定位算法

前言

最近公司在研发室内定位的产品，作为后端工程师自然也开始了UWB定位算法的研究。

协议

解析

根据协议内容，我们可以简单编写工具类，快速解析

private static byte[] getSHA1Digest(String data) throws IOException {
        byte[] bytes = null;
        try {
            MessageDigest md = MessageDigest.getInstance("SHA-1");
            bytes = md.digest(data.getBytes("UTF-8"));
        } catch (GeneralSecurityException gse) {
            throw new IOException(gse);
        }
        return bytes;
    }


    /**
     * 二进制转十六进制字符串
     *
     * @param bytes
     * @return
     */
    private static String byte2hex(byte[] bytes) {
        StringBuilder sign = new StringBuilder();
        for (int i = 0; i < bytes.length; i++) {
            String hex = Integer.toHexString(bytes[i] & 0xFF);
            if (hex.length() == 1) {
                sign.append("0");
            }
            sign.append(hex.toUpperCase());
        }
        return sign.toString();
    }

    /**
     * 将16进制转换为二进制
     *
     * @param hexStr
     * @return
     */
    public static byte[] parseHexStr2Byte(String hexStr) {
        if (hexStr.length() < 1) {
            return null;
        }
        byte[] result = new byte[hexStr.length() / 2];
        for (int i = 0; i < hexStr.length() / 2; i++) {
            int high = Integer.parseInt(hexStr.substring(i * 2, i * 2 + 1), 16);
            int low = Integer.parseInt(hexStr.substring(i * 2 + 1, i * 2 + 2),
                    16);
            System.out.println("字节位：" + (byte) (high * 16 + low));
            result[i] = (byte) (high * 16 + low);
        }
        System.out.println(new String(result));
        return result;
    }

    /**
     * 十六进制字符串转二进制字符串
     *
     * @param hexStr 十六进制字符串
     * @return 二进制字符串
     */
    public static String hexToBin(String hexStr) {
        String hexToBinStr = "";
        try {
            hexToBinStr = Long.toBinaryString(Long.valueOf(hexStr, 16));
        } catch (NumberFormatException e) {
            e.printStackTrace();
        }
        return hexToBinStr;
    }

    /**
     * 十六进制字符串转十进制
     *
     * @param hexStr 十六进制字符串
     * @return 二进制字符串
     */
    public static int hexToTen(String hexStr) {
        BigInteger lngNum = new BigInteger(hexStr, 16);
        return lngNum.intValue();
    }


    public static void main(String[] args) {
        String payload = "mc0f00000663000005a300000512000004cbffffffffffffffffffffffffffffffff095fc100146fb7a0:022be";
        String hexStr = payload.substring(2, 4);
        String mask = hexToBin(hexStr);
        System.out.println("消息头，固定为 mc:" + mask);

        String range = payload.substring(4, 68);
        System.out.println("基站A0-A8：" + range);
        System.out.println(range.length());

        // 1:通过渠道获取imei
        // 2:通过imei获取该设备绑定的室内地图，通过地图找到地图x,y,z轴坐标

        List<BaseStation> stationList = new ArrayList<>();
        for (int i = 0; i < mask.length(); i++) {
            String result = mask.substring(mask.length() - 1 - i, mask.length() - i);
            if (result.equals("1")) {
                Double rawDistance = Double.valueOf(hexToTen(range.substring(i * 8, (i + 1) * 8))) / 1000;
                System.out.println("标签到A" + i + "的距离:" + rawDistance);
                BaseStation baseStation = new BaseStation();
                baseStation.setImei("imei");
                baseStation.setRawDistance(rawDistance);
                stationList.add(baseStation);
            }
        }

        //  baseStationService.calculate(stationList);
    }

UWB定位算法编写

BaseStation.java

package com.bbzn.device.client.dataobject;

import lombok.Getter;
import lombok.NoArgsConstructor;
import lombok.Setter;
import lombok.ToString;

import java.io.Serializable;
import java.util.Date;

@Getter
@Setter
@ToString
@NoArgsConstructor
public class BaseStation implements Serializable {
    private Long id;

    /**
     * 室内定位X轴
     */
    private Integer xAxis;

    /**
     * 室内定位Y轴
     */
    private Integer yAxis;

    /**
     * 室内定位Z轴
     */
    private Integer zAxis;

    /**
     * 设备imei号
     */
    private String imei;

    /**
     * 状态 0 默认 1 禁用 2 正常
     */
    private Integer status;

    /**
     * 创建时间
     */
    private Date createTime;

    /**
     * 更新时间
     */
    private Date updateTime;

    /**
     * 获取的距离
     */
    private Double rawDistance;

    private static final long serialVersionUID = 1L;
}

计算定位坐标：

/**
     * 计算定位坐标
     *
     * @param bases 接收到的一组基站对象列表(此处列表中的基站应当是id各异的)
     * @return 返回定位坐标
     */
    @Override
    public Location calculate(List<BaseStation> bases) {

        Location location = new Location();
        int baseNum = bases.size();

        /*距离数组*/
        double[] distanceArray = new double[baseNum];

        String[] ids = new String[baseNum];

        int j = 0;


        /*获得基站id*/
        for (BaseStation baseHeight : bases) {
            ids[j] = baseHeight.getId().toString();
            distanceArray[j] = UwbMathUtils.getDistance(baseHeight.getZAxis(),baseHeight.getRawDistance());
            j++;
        }

        int disArrayLength = distanceArray.length;

        double[][] a = new double[baseNum - 1][2];

        double[][] b = new double[baseNum - 1][1];

        /*数组a初始化*/
        for (int i = 0; i < 2; i++) {
            a[i][0] = 2 * (bases.get(i).getXAxis() - bases.get(baseNum-1).getXAxis());
            a[i][1] = 2 * (bases.get(i).getYAxis() - bases.get(baseNum-1).getYAxis());
        }

        /*数组b初始化*/
        for (int i = 0; i < 2; i++) {
            b[i][0] = Math.pow(bases.get(i).getXAxis(), 2)
                    - Math.pow(bases.get(baseNum-1).getXAxis(), 2)
                    + Math.pow(bases.get(i).getYAxis(), 2)
                    - Math.pow(bases.get(baseNum-1).getYAxis(), 2)
                    + Math.pow(distanceArray[disArrayLength - 1], 2)
                    - Math.pow(distanceArray[i], 2);
        }

        /*将数组封装成矩阵*/
        Matrix b1 = new Matrix(b);
        Matrix a1 = new Matrix(a);

        /*求矩阵的转置*/
        Matrix a2 = a1.transpose();

        /*求矩阵a1与矩阵a1转置矩阵a2的乘积*/
        Matrix tmpMatrix1 = a2.times(a1);
        Matrix reTmpMatrix1 = tmpMatrix1.inverse();
        Matrix tmpMatrix2 = reTmpMatrix1.times(a2);

        /*中间结果乘以最后的b1矩阵*/
        Matrix resultMatrix = tmpMatrix2.times(b1);
        double[][] resultArray = resultMatrix.getArray();

        location.setX(resultArray[0][0]);
        location.setY(resultArray[1][0]);

        /*设置定位结果的时间戳*/
        Timestamp ts = new Timestamp(System.currentTimeMillis());
//        location.setTimeStamp(ts);
        return location;
    }

Location.java

@Getter
@Setter
@ToString
@NoArgsConstructor
public class Location implements Serializable {

    private static final long serialVersionUID = 1L;

    /*x轴坐标*/
//    private Double xAxis;
    /*x轴坐标*/
    private Double x;

    /*y轴坐标*/
//    private Double yAxis;
    /*y轴坐标*/
    private Double y;

    /*时间戳*/
//    private Timestamp timeStamp;

    private String imei;
}