巷道里的人员定位，导航及规划路径上的设备扫描，欧几里得的应用与实践

今天要分享的是如何在纵横交错且高低不平的巷道中做人员定位及导航，如何显示规划路线路径上的设备及点位，附上一张效果图

我们在过去交付过的项目中发现，井下巷道中的人员定位主要依靠人员定位基站，且甲方提供的数据中只包含定位基站的编号及人员距离该定位基站的正负值，

下面是一部分客户提供数据的样例

{
		"mine_code": "141121001699",
		"staff_card_code": "14112100169901083",
		"person_name": "测试",
		"well_sign": "1",
		"enter_well_time": "2023-12-22 07:45:39",
		"out_of_well_time": null,
		"area_code": "1411210016999999",
		"enter_current_area_time": "2023-12-22 08:19:22",
		"station_code": "1411210016999999000051",
		"enter_current_station_time": "2023-12-22 08:19:22",
		"shift_rule": "三八制",
		"distance_from_station": "75",
		"staff_status": "正常",
		"is_leader": "0",
		"is_special": "0",
}

station_code就是人员定位基站的编号，distance_from_station是该人距离基站的距离（正代表前方，负代表后方），这样的数据是无法满足我们在巷道中准确的把人创建出来的，因为高度差以及并非直线的情况下，人和marker会偏离巷道或悬浮在空中，在经过与引擎组的谢帅同学沟通过以后，我们目前在项目中使用的方案经过测试和验证之后效果比较理想，接下来我们来看看具体步骤；

第一：巷道要做导航片

这一步是项目前期需要跟建模同学沟通的，略过

第二：使用three导航的寻路将导航片切割成线段

既然定位数据是基站的前后距离，那么基站就是有覆盖范围的，按照基站的覆盖范围（window.config.CUTTINGRANGE自定义覆盖范围半径），将路网切割成线段，存储起来，这个过程比较慢，建议放到loading期间，window.navIns是自己封装的寻路类

const cuttingRoadNetwork = async (data) => {
    try {
        data.forEach(item => {
            const parent = window.app.query('.Thing').query(`[userData/UniqueCode=${item.UniqueCode}]`)[0];
            const start = parent.selfToWorld([window.config.CUTTINGRANGE, -1.5, 0]);
            const end = parent.selfToWorld([-window.config.CUTTINGRANGE, -1.5, 0]);
            const theFirstHalf = window.navIns.getPathTwoPointsBetween(parent.position, start);
            const theSecondHalf = window.navIns.getPathTwoPointsBetween(parent.position, end);
            window.config.ROAD_INFO.set(item.UniqueCode, {
                theFirstHalf,
                theSecondHalf
            });
        });

        return Promise.resolve();
    } catch (error) {
        // 如果发生错误，拒绝 Promise
        return Promise.reject(error);
    }
};

带着基站编号及该基站前半段后半段覆盖范围的所有路线点位（拿的是路网点位，当然所有的点都是贴在路面上的）

第三步：拿到人员定位数据，将其创建在巷道中的对应位置
首先按照切割时定位基站的覆盖范围半径计算距离基站的百分比（为了在切割的线段上找到相对精确的位置）；

然后：

1. 判断所处基站前半段or后半段，计算线段的总长度。
2. 计算人所在位置的百分比的长度。
3. 遍历线段，累加长度，当累加的长度超过人所在位置时，停止遍历并获取该点的坐标。

const calculationPosition = (key, value) => {
    let lineCoordinates = null;
    if (value > 0) {
        lineCoordinates = window.config.ROAD_INFO.get(key).theFirstHalf
    } else {
        lineCoordinates = window.config.ROAD_INFO.get(key).theSecondHalf
    }
    //换算在其方向上的百分比
    const proccess = Math.abs(value) / 200;
    // 获取线段点坐标
    const midpoint = getMidpointCoordinates(lineCoordinates, proccess);
    return midpoint;
};
const getMidpointCoordinates = (coordinates, proccess) => {
    const totalLength = calculateLineLength(coordinates);
    const targetLength = totalLength * proccess;

    let currentLength = 0;
    let midpoint;

    for (let i = 1; i < coordinates.length; i++) {
        const prevPoint = coordinates[i - 1];
        const currentPoint = coordinates[i];

        const segmentLength = Math.sqrt(
            Math.pow(currentPoint[0] - prevPoint[0], 2) +
            Math.pow(currentPoint[1] - prevPoint[1], 2) +
            Math.pow(currentPoint[2] - prevPoint[2], 2)
        );

        // 如果当前累加的长度超过，则获取该点的坐标
        if (currentLength + segmentLength >= targetLength) {
            const remainingLength = targetLength - currentLength;
            const ratio = remainingLength / segmentLength;

            midpoint = [
                prevPoint[0] + ratio * (currentPoint[0] - prevPoint[0]),
                prevPoint[1] + ratio * (currentPoint[1] - prevPoint[1]),
                prevPoint[2] + ratio * (currentPoint[2] - prevPoint[2])
            ];

            break;
        }

        currentLength += segmentLength;
    }

    return midpoint;
};

这里使用了简单的欧几里得距离来估算线段的长度，如果需要更高精度，可以尝试一下其他算法

通过上面的计算，我们就得到了该人员在距离基站X米的巷道中的位置然后创建对应的人模型和marker，这种方式的就算速度快，开销小，目前1000人，一秒500条数据很轻松。

第四步：规划导航路径

开源算法的能力，这里不赘述，需要注意的一点是，我们在项目中遇到的需求是点击人员，规划路径到最近的避难点，我们选择的方式是在人员定位基站的孪生体信息中绑定该基站最近的避难点编号的方式规划的路径，寻路和空间点位置计算都无法准确的解决这个问题，这个需要后面再探讨

第五步：展示导航路径上的孪生体及marker

一般的思路是扫描，但是巷道中的路径是一条直线，用这条线扫描半径范围内的对象肯定不合适，而且我们的巷道不是一个平面，相当于我们需要实现一个球状的扫描才能满足需求

步骤一：提前存储所有要通过扫描来展示的设备

步骤二：使用欧几里得距离（Euclidean distance）来计算两个空间坐标点之间的距离。

我们在做导航路径的时候得到导航路径的所有点位数组B，所有设备的坐标为数组A，其中，(x1​,y1​,z1​) 是A组中的一个点的坐标，(x2​,y2​,z2​) 是B组中的一个点的坐标。通过计算A组中的点到B组中点的距离，并筛选出距离在10米以内的点（范围可以自定义）

//根据欧几里得计算满足条件的坐标
const calculateDistance = (pointA, pointB) => {
    const dx = pointB[0] - pointA.position[0];
    const dy = pointB[1] - pointA.position[1];
    const dz = pointB[2] - pointA.position[2];

    return Math.sqrt(dx * dx + dy * dy + dz * dz);
}
const filterPointsWithinDistance = (pointsA, pointsB, maxDistance) => {
    const processedCodes = new Set();  // 用于记录已处理过的 code 值
    const result = [];

    for (let i = 0; i < pointsA.length; i++) {
        for (let j = 0; j < pointsB.length; j++) {
            const distance = calculateDistance(pointsA[i], pointsB[j]);

            if (distance <= maxDistance && !processedCodes.has(pointsA[i].code)) {
                processedCodes.add(pointsA[i].code);  // 记录已处理过的 code 值

                result.push({
                    pointA: pointsA[i],
                    // pointB: pointsB[j],
                    // distance
                });

                break;  // 找到满足条件的点对后跳出内层循环，继续下一个 pointA
            }
        }
    }

    return result;
}

拿到返回结果就是在该路线上且距离该路线10米以内的所有孪生对象信息，再将其展示，就OK啦！

在这里要注意的是，因为会将数组A中的每一个对象位置都和B中的点计算一次，但实际上我们只需要满足一次就行，所以需要特别注意，不然消耗性能且后期处理比较复杂。

这里只是一个简单的思路分享，如果有问题，可以找我讨论，如果有哪里不对可以给我指点出来，谢谢，哈哈哈哈