一种基于环境特征匹配的四向车无码定位导航方法与流程

本发明属于智能物流仓储领域，具体涉及一种基于环境特征匹配的四向车无码定位导航方法。

背景技术：

1、随着智能仓储与现代物流行业的高速发展，四向穿梭车（以下简称“四向车”）作为一种能够在立体货架的纵横轨道上任意穿行的高密度存储核心搬运设备，得到了极其广泛的应用。

2、在目前的仓储应用现状中，四向车绝大多数依赖于有码导航方案，即通过读取预先张贴在货架轨道交叉点上的二维码或反光板获取绝对坐标，辅助以惯性测量单元（imu）或编码器进行航位推算。然而，在真实的仓储作业环境下，二维码极易因车辆碾压、灰尘覆盖、金属摩擦或轨道油污而发生磨损甚至脱落。一旦发生丢码，四向车的定位系统将瞬间瘫痪，极易导致脱轨坠落等严重安全事故。此外，海量二维码的初始测绘贴片与后期维护成本极高，严重制约了仓储系统的柔性化部署。

3、为了解决有码导航带来的痛点，本领域技术人员开始尝试引入基于车载激光雷达或视觉传感器的无码化自然特征导航技术（如slam，同步定位与建图技术）。现有技术中，通常利用传感器扫描周边的货架轮廓建立全局的度量地图（metricmap，如栅格地图或点云地图），再通过当前扫描的局部点云与全局点云进行实时匹配（如采用icp迭代最近点算法）来实现定位。

4、然而，在针对四向车+高密度立体货架这一特定应用场景时，现有的无码导航匹配技术暴露出以下细微且难以克服的底层技术缺陷：

5、第一，极易陷入由感知混淆引起的局部最优陷阱，导致严重的误定位。真实的高密度立体货架是由大量标准化的立柱、牛腿和横梁拼接而成的高度重复、极度对称的物理网格结构。当四向车在货架深处行驶时，其传感器在第2排巷道和第10排巷道扫描到的局部点云几何特征几乎完全一致；传统的度量地图匹配算法在这种长廊道或重复网格结构中，极易发生特征误匹配，导致算法误认当前坐标，进而引发严重的导航偏移。

6、第二，局部实时度量数据与全局仓储调度系统之间存在严重的跨域语义鸿沟。真实的仓储调度系统（wms/wcs）为了实现多车群控和路径规划，下发给车辆的通常是全局拓扑地图（仅包含节点和边缘连接关系），而不是庞大的全局点云文件；现有的slam算法输出的是含有三维空间精确距离坐标的局部度量数据；直接利用含有庞大散乱点云的局部度量地图，去匹配高度抽象的全局拓扑地图，在底层数据结构上存在维度不匹配的问题。如果强行在四向车的嵌入式控制器中同时维护全局拓扑和全局点云两套地图，不仅会极大地消耗计算资源导致匹配延迟，而且由于感知混，依然无法保证节点映射的准确性。

7、综上所述，如何打破度量空间与拓扑空间的数据壁垒，在算力受限的车载硬件平台上，从高度同质化的仓储环境特征中提取出抗混淆的拓扑关系，实现局部数据与全局拓扑地图的低延迟、零误判的精准匹配，是当前四向车无码化导航领域亟待解决的核心底层技术问题。

技术实现思路

1、针对现有技术的不足，本发明的目的在于提供一种基于环境特征匹配的四向车无码定位导航方法，解决了现有技术中的问题。

2、本发明的目的可以通过以下技术方案实现：

3、一种基于环境特征匹配的四向车无码定位导航方法，包括以下步骤：

4、s1，获取车载激光雷达扫描的三维点云数据，并同步读取惯性测量单元输出的重力方向向量；计算所述三维点云数据中各数据点的局部表面曲率和法向量，基于预设的曲率阈值以及所述法向量与重力方向向量的夹角约束阈值执行过滤，提取保留刚性几何面数据点，生成结构化点云集合；

5、s2，基于所述结构化点云集合拟合生成空间立柱模型和空间牛腿模型；通过联立所述空间立柱模型与所述空间牛腿模型的几何方程，求解出对应的空间交点，并在所述空间交点周围建立物理边界框提取点云簇，计算该点云簇的坐标平均值，得到三维空间几何质心坐标向量并将其映射为单一的局部拓扑节点，生成局部拓扑节点序列；

6、s3，利用预设容量的时空滑动窗口截取所述局部拓扑节点序列，构建带有物理距离权重的局部拓扑子图；计算所述局部拓扑子图的加权邻接矩阵和度矩阵，进而生成归一化拉普拉斯矩阵；对所述归一化拉普拉斯矩阵进行特征值分解，提取按升序排列的特征值序列，构建局部谱图特征向量；

7、s4，将所述局部谱图特征向量与全局谱图特征库进行相似度比对，锁定相似度距离最小的全局目标子图；通过空间字典序重排算法对局部拓扑节点与所述全局目标子图中的目标节点建立一对一顶点映射关系，并基于所述映射关系中对应节点的三维空间绝对坐标构建空间交叉协方差矩阵；引入手性校对角矩阵对所述空间交叉协方差矩阵进行奇异值分解，解算出绝对位姿数据并注入执行电机。

8、进一步地，s1中执行过滤的步骤包括：

9、通过预先标定的外参旋转矩阵将惯性测量单元坐标系下的所述重力方向向量投影至车载激光雷达坐标系中，生成绝对参考基准向量；

10、利用向量点乘公式，计算各数据点的所述法向量与所述绝对参考基准向量之间的空间包含夹角；

11、当任一数据点的所述局部表面曲率大于预设的动态散乱曲率阈值，或者，当任一数据点的所述空间包含夹角不满足与所述绝对参考基准向量平行的角度容差阈值、且不满足与所述绝对参考基准向量垂直的角度容差阈值时，将其判定为非结构化干扰点并剔除。

12、进一步地，在基于所述结构化点云集合拟合生成空间立柱模型和空间牛腿模型之前，还包括：

13、将所述结构化点云集合中满足垂直约束条件的数据点归类划入立柱候选点云子集；

14、将所述结构化点云集合中满足平行约束条件的数据点归类划入牛腿候选点云子集。

15、进一步地，拟合生成空间立柱模型和空间牛腿模型的步骤包括：

16、采用随机采样一致性算法，从所述立柱候选点云子集中拟合出所述空间立柱模型对应的圆柱轴线参数方程；

17、采用随机采样一致性算法，从所述牛腿候选点云子集中拟合出所述空间牛腿模型对应的空间平面模型方程；

18、求解空间交点的步骤包括：

19、联立所述圆柱轴线参数方程与所述空间平面模型方程，求解获取所述空间交点的绝对坐标向量。

20、进一步地，在所述空间交点周围建立物理边界框提取点云簇的步骤包括：

21、以所述空间交点的绝对坐标向量为三维中心点，配置尺寸为预设长宽高的轴对齐刚性物理边界框；

22、对所述结构化点云集合进行空间裁剪，将落入所述刚性物理边界框内的所有数据点提取为有限物理特征点云簇。

23、进一步地，生成归一化拉普拉斯矩阵的步骤包括：

24、计算时空滑动窗口内任意两个不同节点间的相对空间欧氏距离；

25、当所述相对空间欧氏距离小于或等于空间连通阈值时，判定两节点间存在物理轨道连接边，在加权邻接矩阵对应元素处赋予呈高斯指数衰减的物理距离权重值；

26、当两节点间不存在物理轨道连接边或为同一节点时，在加权邻接矩阵对应元素处赋值为零；

27、根据所述加权邻接矩阵的行求和构建对角线度矩阵，并利用单位矩阵、对角线度矩阵及加权邻接矩阵联立计算得出所述归一化拉普拉斯矩阵。

28、进一步地，通过空间字典序重排算法建立一对一顶点映射关系的步骤包括：

29、将所述时空滑动窗口内的多个局部拓扑节点携带的三维空间质心坐标，依次按照x轴绝对数值升序、y轴绝对数值升序、z轴绝对数值升序的多级物理空间优先级进行排序，生成有序局部点集；

30、将所述全局目标子图中的全局拓扑节点绝对坐标，按照相同的x轴、y轴、z轴绝对数值升序进行多级排序，生成有序全局点集；

31、基于多级排序后的相同索引下标，建立有序局部点集与有序全局点集的一对一刚性对齐映射关系。

32、进一步地，解算绝对位姿数据的步骤包括：

33、计算所述有序局部点集与有序全局点集的三维空间几何中心列向量，分别进行去中心化平移并通过向量外乘求和重构所述空间交叉协方差矩阵；

34、对所述空间交叉协方差矩阵进行奇异值分解获取第一正交矩阵和第二正交矩阵，利用所述第一正交矩阵与第二正交矩阵的乘积行列式标量值构建手性校对角矩阵；

35、依次连乘所述第二正交矩阵、所述手性校对角矩阵与所述第一正交矩阵的转置矩阵，计算输出无镜像畸变的刚体变换旋转矩阵；

36、根据所述刚体变换旋转矩阵与几何中心列向量计算绝对平移向量，进而生成所述绝对位姿数据。

37、一种基于环境特征匹配的四向车无码定位导航系统，执行上述的方法，包括：

38、结构化点云生成模块，用于获取车载激光雷达扫描的三维点云数据，并同步读取惯性测量单元输出的重力方向向量；计算所述三维点云数据中各数据点的局部表面曲率和法向量，基于预设的曲率阈值以及所述法向量与重力方向向量的夹角约束阈值执行过滤，提取保留刚性几何面数据点，生成结构化点云集合；

39、拓扑节点降维映射模块，用于基于所述结构化点云集合拟合生成空间立柱模型和空间牛腿模型；通过联立所述空间立柱模型与所述空间牛腿模型的几何方程，求解出对应的空间交点，并在所述空间交点周围建立物理边界框提取点云簇，计算该点云簇的坐标平均值，得到三维空间几何质心坐标向量并将其映射为单一的局部拓扑节点，生成局部拓扑节点序列；

40、局部谱图特征解析模块，用于利用预设容量的时空滑动窗口截取所述局部拓扑节点序列，构建带有物理距离权重的局部拓扑子图；计算所述局部拓扑子图的加权邻接矩阵和度矩阵，进而生成归一化拉普拉斯矩阵；对所述归一化拉普拉斯矩阵进行特征值分解，提取按升序排列的特征值序列，构建局部谱图特征向量；

41、绝对位姿匹配与解算模块，用于将所述局部谱图特征向量与全局谱图特征库进行相似度比对，锁定相似度距离最小的全局目标子图；通过空间字典序重排算法对局部拓扑节点与所述全局目标子图中的目标节点建立一对一顶点映射关系，并基于所述映射关系中对应节点的三维空间绝对坐标构建空间交叉协方差矩阵；引入手性校对角矩阵对所述空间交叉协方差矩阵进行奇异值分解，解算出绝对位姿数据并注入底层执行电机。

42、一种计算机存储介质，存储有可读程序，当程序运行时，所述程序能够指示计算设备执行如上述的一种基于环境特征匹配的四向车无码定位导航方法。

43、本发明的有益效果：

44、1、本发明通过步骤s1在底层利用主成分分析（pca）计算每个数据点的局部表面曲率，并引入四向车内置imu的重力方向向量作为绝对参考基准，计算法向量与重力基准的包含夹角，进而通过曲率阈值和重力正交几何约束执行双阈值过滤；在数据输入端便将表面粗糙、不规则的动态物体判定为高信息熵噪点并加以剔除，只保留与重力方向严格平行（立柱）或垂直（牛腿）的刚性几何面数据点。这避免了因货物溢出或临时障碍物遮挡导致的局部点云畸变，为后续的特征提取提供了高置信度的纯净结构化点云，有效缓解了定位系统因动态局部遮挡产生跳变的问题。

45、2、本发明在步骤s2中，利用ransac算法从垂直和水平点云子集中分别拟合出三维空间圆柱模型和空间平面模型，通过解析几何联立方程求解轴线与平面的交点，并在该交点周围建立物理边界框，最终计算该有限区域内点云的空间几何质心，将其映射为单一的局部拓扑节点；实现了从三维度量空间向一维拓扑空间的跨域转换。它将原本需要处理的几十万个散乱数据点，压缩成了由极少数质心坐标构成的定长节点序列，使得传感器采集的实时数据在结构和维度上与调度系统的全局拓扑地图实现了对齐；这不仅打破了度量与拓扑地图之间的语义鸿沟，还显著降低了车载控制器的存储与矩阵计算开销，满足了四向车端侧设备的实时性要求。

46、3、本发明在步骤s3中引入时空滑动窗口，截取固定数量w的连续拓扑节点构建局部拓扑子图，计算该子图的加权邻接矩阵、度矩阵以及归一化拉普拉斯矩阵l，并对其进行特征值分解，提取按升序排列的实数特征值序列，构建出固定维度的局部谱图特征向量；拉普拉斯矩阵的特征值序列（频谱）对坐标系的平移和旋转具有代数不变性；这意味着，无论四向车由于轨道形变或车体颠簸产生何种角度的姿态漂移，只要它所处区域的货架拓扑连接关系是固定的，底层算出的谱图特征向量就具备唯一性。将对点云外观的比对转化为对内在拓扑结构代数特征的比对，为局部的物理结构生成了数学层面的唯一身份标识，有效改善了四向车在高度同质化场景下因感知混淆而导致的误定位与错轨现象。

47、4、本发明在步骤s4中，首先通过空间字典序重排算法对局部和全局点集进行排序，建立一对一的顶点映射；接着在进行奇异值分解（svd）解算时，引入了kabsch算法的手性校对角矩阵，对交叉协方差矩阵的行列式符号进行约束；空间字典序重排避开了耗时的迭代匹配过程，通过一次排序即可确定点对点的对应关系。更重要的是，手性校对角矩阵的引入，解决了在货架垂直面高度共面或噪声干扰下，svd分解极易输出镜像反射矩阵的数学多义性缺陷，确保了解算出的旋转矩阵r和平移向量t严格符合物理世界的刚体变换规律。这使得四向车即使在高速行驶中发生轮轨滑移，也能在毫秒级内通过解析闭式解找回真实的全局绝对位姿，为驱动电机的闭环控制提供了稳定、可靠的反馈控制量。