关于java: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;
}