基于多传感器融合与改进算法的机器人室内定位方法

Robot Indoor Positioning Method Based on Multi-Sensor Fusion and Improved Algorithm

摘要: 近年来，随着室内无人定位技术的发展，市场对无人智能系统的需求日益增长。然而，室内环境的复杂性也给定位技术带来了不小的挑战，由于空间限制，物体之间常常会互相遮挡或干扰，这不仅严重影响了传感器的识别能力，还降低了定位的准确性。为了解决这一问题，本文提出了一种多传感器融合的方法。该方法以激光雷达(LiDAR)为主传感器，同时辅以深度相机、WiFi和惯性测量单元(IMU)来增强系统的性能。针对由障碍物遮挡引起的点云匹配不准确的问题，本文设计了一种改进的点线迭代最近点(Point-to-Line Iterative Closest Point, PL-ICP)算法，该算法在遮挡场景下能够显著提高匹配的精度和速度。此外，本文还对Otsu算法进行了改进，使其能够更好地利用深度相机采集到的图像(RGB-D图像)匹配来提取额外的特征信息，从而在存在遮挡的情况下增强系统的收敛性。最后，采用扩展卡尔曼滤波器(EKF)算法来融合点云、图像以及WiFi定位数据，进一步提升了定位的准确性和鲁棒性。经过大量实验验证，本文的方法不仅提高了定位的精度和稳定性，还展现出了很好的收敛特性。这为机器人定位和导航提供了一种既经济又高效的解决方案。

Abstract: In recent years, with the development of indoor unmanned positioning technology, the market demand for unmanned intelligent systems is increasing. However, the complexity of the indoor environment also brings great challenges to the positioning technology. Due to space constraints, objects often block or interfere with each other, which not only seriously affects the recognition ability of the sensor, but also reduces the accuracy of positioning. In order to solve this problem, this paper proposes a multi-sensor fusion method. This method uses LiDAR as the main sensor, supplemented by depth camera, WiFi and inertial measurement unit (IMU) to enhance the performance of the system. Aiming at the problem of inaccurate point cloud matching caused by obstacle occlusion, this paper designs an improved Point-to-Line Iterative Closest Point (PL-ICP) algorithm, which can significantly improve the accuracy and speed of matching in occlusion scenes. In addition, this paper also improves the Otsu algorithm so that it can better use the image (RGB-D image) matching collected by the depth camera to extract additional feature information, thereby enhancing the convergence of the system in the presence of occlusion. Finally, the extended Kalman filter (EKF) algorithm is used to fuse point cloud, image and WiFi positioning data, which further improves the accuracy and robustness of positioning. After a large number of experimental verification, the method in this paper not only improves the accuracy and stability of positioning, but also shows good convergence characteristics. This provides an economical and efficient solution for robot positioning and navigation.

1. 引言

随着服务机器人、工业自动化以及智能仓储等领域的快速发展，室内自主移动机器人对高精度定位与导航的需求日益迫切。然而，室内环境的复杂性和动态性给定位技术带来了严峻挑战。一方面，室内环境中密集的障碍物布局导致频繁的遮挡问题，严重影响基于单一传感器的定位性能；另一方面，光照变化、低纹理区域以及动态干扰等因素进一步降低了传统定位方法的可靠性和鲁棒性。

当前主流的室内定位技术主要包括激光雷达SLAM、视觉SLAM、惯性导航以及无线信号定位等。激光雷达SLAM (如LOAM、LeGO-LOAM等)虽然能够提供精确的环境几何信息，但在长走廊或对称场景中容易发生匹配错误；视觉SLAM (如ORB-SLAM、VINS等)虽然成本较低，但在光照剧烈变化或纹理缺失区域性能显著下降；基于IMU的惯性导航虽然不受环境干扰，但存在严重的累积误差问题；而WiFi、蓝牙等无线信号定位虽然部署方便，但精度有限且易受多径效应影响。

针对上述问题，多传感器融合定位技术逐渐成为研究热点。通过优势互补，可以有效提升系统的整体性能。激光雷达与IMU融合可以抑制点云畸变并减少累积误差，例如刘同龑等[1]提出了一种基于惯性测量单元与激光雷达紧耦合的车辆自主泊车场景下的建图定位方法I-LOAM。视觉与IMU结合能够提高运动估计的准确性，例如程德强等[2]提出方向感知增强的单目深度估计方法，提升了复杂场景下的深度估计精度。而引入无线信号则可以为系统提供绝对位置参考，避免长期运行的漂移问题，例如胡钊政等[3]提出融合WiFi与LiDAR的定位方法，降低了部署成本，但未解决动态遮挡问题。然而，由于室内物体的排列是杂乱无章的，经常导致遮挡、重叠和其他复杂场景的情况，现有的多传感器融合方法仍面临一些关键挑战：1) 在严重遮挡场景下，点云配准算法容易失效；2) 不同传感器的数据频率和精度差异导致融合困难；3) 动态环境下的定位鲁棒性仍需提升。

本文提出了一种多传感器融合室内定位框架，通过深度融合激光雷达、IMU、深度相机和WiFi的观测数据，实现了高精度、高鲁棒性的定位。主要贡献包括：1) 提出改进的PL-ICP算法，通过引入特征点约束和自适应权重策略，显著提升了遮挡场景下的点云匹配精度；2) 设计基于多阈值分割的改进Otsu算法，有效增强了深度图像的特征提取能力；3) 构建分层融合架构，采用扩展卡尔曼滤波(EKF)实现多源数据的自适应加权融合，在保证实时性的同时提高了系统鲁棒性。

2. 相关工作

多传感器融合SLAM技术在近年来取得了显著进展，但仍存在诸多挑战。本章围绕视觉SLAM、激光雷达SLAM、WiFi定位技术以及多传感器融合方法四个方面，系统梳理现有研究，并分析其局限性，以突出本文工作的创新点。

2.1. 视觉SLAM

视觉传感器因其成本低、覆盖范围广，长期以来一直是定位与建图研究的热门选择。其原理是通过跟踪图像序列中特征点的运动，并借助特征提取、匹配和优化等技术，来确定相机姿态的时间变化，从而估计相机的运动轨迹。单目视觉SLAM是一种仅使用单个相机，同时实现相机运动估计和环境信息获取的方法。早在2007年，戴维森等[4]提出了即时定位与地图构建的概念，首次证明了单目视觉SLAM的可行性。然而，这种方法在实际应用中存在漂移问题，即随着时间推移，定位精度会逐渐下降。随后，程德强等[2]提出了一种方向感知增强的轻量级自监督单目深度估计方法，通过引入方向感知特征，显著提高了模型在复杂场景下的深度估计性能。

然而，尽管单目视觉在不断提升识别精度，但由于其深度精度的局限性，仍然存在尺度不确定性的挑战。简单来说，单目视觉难以准确判断物体的实际距离，就像我们用一只眼睛看东西时，很难准确判断物体的远近。为了解决这一问题，双目视觉SLAM应运而生。它通过两个相机从不同角度捕捉同一物体，试图消除漂移问题。例如，孔建明等[5]提出了一种完全在线的实时双目直接法视觉SLAM算法，不受词袋模型的固定词汇限制。该算法可以利用任何具有足够强度梯度的图像像素，使其在缺少特征点的区域仍具有很强的鲁棒性。但双目视觉SLAM也并非完美无缺，它在对应点匹配方面存在挑战和不确定性，这在一定程度上限制了其在机器人学领域的广泛应用。随着全景视觉技术的出现，施拉姆尔等[6]引入了事件驱动的立体视觉技术，用于全景感知。这种技术不仅提高了数据采集速度，还显著提升了在动态场景中的精度，为解决双目视觉SLAM的局限性提供了新的思路和方法。

尽管视觉传感器有诸多优点，但它们仍然容易受到光照变化和低纹理环境的影响。相比之下，深度相机通过提供深度信息，显著提升了SLAM的精度。2015年，刘骏捷等[7]展示了RGB-D传感器在几百米范围内的实时物体识别能力，甚至可以实现表面重建。2017年，Dai等[8]在此基础上进一步改进，通过稀疏特征和密集几何匹配的方法，大幅提高了识别的速度和完整性。2019年，Zou等[9]又提出了一种在RGB-D图像中识别遮挡物体的新方法，进一步拓展了深度相机的应用范围。然而，需要注意的是，深度相机对光照条件的稳定性要求较高。环境光照的变化很容易干扰其目标识别的准确性。因此，深度相机的主要应用场景仍然是光照条件稳定的室内环境。

2.2. 激光雷达SLAM

与视觉传感器相比，激光雷达(LiDAR)传感器具有独特的优势。它不受光照条件的影响，能够快速、动态地获取高精度、高密度的环境信息，非常适合在各种光照环境下工作。

2006年，Nuchter等首次引入了基于激光雷达的SLAM框架。这一创新有效缓解了运动畸变问题，但在复杂场景中，单个场景的匹配仍然容易出错。2017年，朱福利等[10]通过引入粒子滤波技术，大幅降低了SLAM系统中的局部误差。然而，这种方法在处理大规模场景时需要大量粒子来构建地图，这不可避免地延长了计算时间。2017年，Yan等[11]利用移动LiDAR扫描技术，实现了对地面点的精确分割以及在高速公路环境中对杆状目标的检测，进一步拓展了LiDAR在实际场景中的应用能力。总的来说，LiDAR传感器在SLAM领域的发展历程中，不断克服了早期的局限性，并逐步提升了其在复杂环境中的适用性和效率。

2.3. WiFi定位技术

WiFi定位技术是一种基于无线局域网(WLAN)信号的室内定位方法，广泛应用于室内环境中，如商场、医院、机场等。其核心在于利用WiFi接入点(AP)与移动设备之间的信号特征来确定设备的位置。定位技术主要分为指纹定位和测距定位两大类[12]。指纹定位通过预先采集特定区域内的信号特征(如WiFi信号强度)建立指纹库，定位时将实时采集的信号特征与指纹库进行匹配，从而确定位置。测距定位则通过计算设备与已知位置的信号源之间的距离，利用几何模型确定位置，常见的有基于RSSI (接收信号强度指示)的测距方法和基于TOF (信号飞行时间)的测距方法[13]。本文选用指纹定位方法，主要是因为其不需要对信号源的位置有精确的了解，也不受多径效应和非视距传播的显著影响，能够在复杂室内环境中提供较稳定的定位服务。此外，指纹定位利用现有的WiFi基础设施，无需额外的硬件设备，部署成本较低。

2.4. 多传感器融合SLAM研究现状

目前，学术界和工业界普遍采用将视觉传感器和LiDAR传感器融合的方法，这主要是为了弥补LiDAR技术的局限性。目前的融合方法主要有两种。第一种方法是让一个传感器主导，另一个传感器辅助进行补充调整。例如，2014年，Zhang等[14]研究了一维激光雷达与视觉和深度信息的融合，有效缓解了单目视觉SLAM的漂移问题。2019年，Gretter等[15]利用激光雷达获取场景深度，并将点云映射到视频帧上，从而高效优化了算法。还有一些学者采用基于经典方法扩展的卡尔曼滤波(EKF)。例如，Hsu等[16]将视觉特征融合到激光点云中用于定位信息，并通过基于EKF的位置校正来增强SLAM精度。第二种方法是让多个传感器同时运行SLAM。2015年，Zhang和Singh [17]提出了一种多传感器融合的里程计框架，通过将高频视觉传感器与精确的激光雷达距离并行融合，提高了运动估计精度并减少了漂移。2018年，Shin等[18]回避了多个传感器之间的对应关系，直接将LiDAR深度测量与相机图像强度相结合，用于识别环境信息。2019年，Aldrich和Wickramarathne [19]提出了一种将雷达传感器和视觉技术相结合的多模态融合框架，证明了多传感器数据融合的准确性和鲁棒性。同年，Seo和Chou [20]同时使用两种传感器的测量残差进行后端优化和地图重建。2022年，Zhou等[21]提出将深度视觉技术融入2D LiDAR-SLAM中，用于回环检测和定位。然而，多传感器数据的并行融合增加了计算复杂度和特征提取的难度。目前的多传感器融合方法大多是基于一个主要的SLAM框架，并辅以其他传感器的组合。例如，VINS-Fusion [22]以视觉–惯性里程计(VIO)为核心，辅以GPS或LiDAR进行全局优化；而LIO-SAM则以LiDAR-惯性里程计为主导，结合视觉或GPS数据进行补充修正。

此外，基于因子图的优化方法也是多传感器融合的研究热点，基于因子图的优化方法其优势在于能够灵活地集成不同传感器的约束条件，并通过全局优化实现高精度位姿估计。例如，2020年，Zou等人[23]提出了一种基于因子图的视觉–激光–惯性紧耦合框架(VILENS)，实现了多传感器数据的统一优化。2023年，何等人[24]进一步引入语义信息，通过因子图联合优化几何与语义约束，显著提升了复杂场景下的定位鲁棒性。

基于以上研究，本文提出了一种传感器融合方法，旨在解决室内环境中多传感器融合时定位误差发散的问题。该框架通过集成深度相机、WiFi、IMU和激光雷达数据来实现这一目标。在点云对齐过程中，由于遮挡等因素可能导致匹配不收敛，从而影响定位精度。为此，本研究对PL-ICP算法进行了优化，通过关注点云特征，增强其在非重叠区域中识别和匹配点云点的能力。此外，本文还改进了经典的Otsu算法，通过融入分割线索来提升图像匹配的准确性。最终，所提出的方法采用EKF方案，自适应地融合点云、WIFi以及图像信息。这种方法不仅提高了系统的鲁棒性，还有效降低了位姿跟踪的不确定性。总的来说，本文的方法为室内无人配送系统提供了一种高效且经济的解决方案。

3. 本文研究

3.1. 改进的PL-ICP

为了充分发挥各个传感器的优势，本研究提出了一种多传感器融合方法，主要包含两个核心子系统：激光雷达–惯性里程计(LIO)和视觉–惯性里程计(VIO)。在LIO子系统中，本文采用了改进的PL-ICP (点线迭代最近点)算法来进行点云配准。该算法通过迭代估计激光雷达(LiDAR)的位姿状态，并基于此状态更新地图，从而便于构建全局地图并重建环境的几何特征。与此同时，VIO子系统利用改进的Otsu算法来识别与当前帧相关的邻近关键帧。随后，将点云中的点映射到新的帧图像中，进一步优化地图的构建。最终，将这两个子系统的测量值以及WiFi定位结果融合到误差状态迭代扩展卡尔曼滤波(EKF)框架下。这种融合方式不仅实现了实时性能，还为室内导航提供了一种经济高效的解决方案。图1展示了用于无人平台定位和环境测绘的多传感器融合方法。

Figure 1. Multi-sensor fusion method for positioning and mapping

图1. 定位和建图的多传感器融合方法

PL-ICP算法(如文献[20]所述)采用点到线的匹配方法。它基于一个假设：目标帧中的每条直线都与一个点P相关联。这条直线可以通过目标帧中的两个连续点$P_{i,j}$ 和$P_{i,j+1}$ 来确定。算法的核心是通过最小化点到线的误差函数来实现匹配。

$r\left(q_{k+1},t_{k+1}\right)=\min {\displaystyle {\sum }_i{\left(n_j^T\left[R\left({\theta }_{k+1}{P}_i^w+t_{k+1}-P_i^j\right)\right]\right)}^2}$ (3.1)

其中，$r\left(q_{k+1},t_{k+1}\right)$ = 表示在第k + 1次迭代中，通过旋转$q_{k+1}$ 和平移$t_{k+1}$ 得到的误差函数值。$R\left({\theta }_{k+1}\right)$ 表示以${\theta }_{k+1}$ 为参数的旋转矩阵，用于将点从世界坐标系转换到目标坐标系。$P_i^w$ 表示第i个点在世界坐标系中的位置。$t_{k+1}$ 表示第k + 1次迭代中的平移向量。$P_i^j$ 表示第i个点在目标坐标系中的位置。

与传统的PL-IC方法(如文献[21]所述)相比，PL-ICP算法采用的点到线投影度量在处理包含遮挡的情况时，显著提高了求解精度和匹配效率。然而，这种投影残差的计算也增加了算法的复杂度，从而在一定程度上影响了点云配准的整体效率。

为了解决上述问题，本文提出了一种基于特征匹配的改进PL-ICP算法。该算法充分利用了点特征值的固有稳定性和独特性，在保留原有算法有效性的同时，有效简化了计算过程，提升了效率。改进后的PL-ICP算法如算法1所示。

算法1：改进的PL-ICP

需求：连续的激光雷达扫描输入$S_i$ ，$S_{i+1}$ 1：如果$P_i^j-P_{i+1}^j>d_t$ //当从$S_i$ 成功提取的特征与$S_{i+1}$ 的特征不一致时，采用特征匹配； 2：对于i = 1，i <= k；i++ 3：$t,\theta \leftarrow \arg {\min }_{t,\theta }{\displaystyle {\sum }_i\left({\left(R\left(\theta \right)P_i^w+t-P_i^j\right)}^2\cdot w_i\right)}$ //使用拉格朗日算子解决非线性最小化问题； 4：结束循环 5：否则//如果无法执行特征匹配，则进行点匹配； 6：对于i = 1，i <= k；i++ 7：$t,\theta \leftarrow \arg {\min }_{t,\theta }{\displaystyle {\sum }_i{\left(n_j^T\left[R\left(\theta \right)P_i^w+t-P_i^j\right]\right)}^2}$ //使用拉格朗日算子解决非线性最小化问题； 8：结束循环

其中，$P_i^w$ 是世界坐标系中要匹配的点，$q_{k+1}$ 是第k + 1次迭代的旋转四元数，$t_{k+1}$ 第k + 1次迭代的平移向量，$p_i$ 是$P_i^w$ 在关联线上的投影，$P_i^j$ 与$P_{i+1}^j$ 是与$P_i^w$ 相关联的目标坐标系中的两个连续点，$S_i$ 和$S_{i+1}$ 是连续的激光雷达扫描输入，$P_i$ 与$P_{i+1}$ 是选择的特征点集。

给定连续的激光雷达(LiDAR)扫描输入，记为$S_i$ 和$S_{i+1}$ ，可以使用Hough变换从点云中提取特征并设置阈值$d_t$ 。如果点云$S_t$ 中两点$P_i^j$ 和$P_{i+1}^j$ 之间的特征距离超过阈值，说明存在明显的特征(如点、线或形状)。相反，如果特征距离低于阈值，则表明显著特征缺失。由于特征点通常只占每个点云帧中的一小部分，我们将点分为特征点和非特征点，以便进行点云配准。从源点集$S_i$ 和目标点集$S_{i+1}$ 中分别提取特征点，构建特征点集$P_i$ 和$P_{i+1}$ 。当点$P_i^j$ 和$P_{i+1}^j$ 的特征一致时，目标函数调整为最小化误差函数，如式(3.2)所示：

$t,\theta \leftarrow \arg {\min }_{t,\theta }{\displaystyle {\sum }_i\left({\left(R\left(\theta \right)P_i^w+t-P_i^j\right)}^2\cdot w_i\right)}$ (3.2)

其中，t，θ表示要优化的平移向量t和旋转角度θ。$R\left(\theta \right)$ 表示以θ为参数的旋转矩阵，用于将源点云中的点旋转到目标点云的坐标系中。权重系数$w_i$ 是通过特征点协方差矩阵的特征值分解得到的。在ICP (迭代最近点)优化过程中，利用这些特征值和特征向量的信息来指导点云配准，从而增强了匹配的准确性和鲁棒性。对于非特征点，我们仍然采用传统的PL-ICP方法，最小化误差函数如式(3.3)所示：

$t,\theta \leftarrow \arg {\min }_{t,\theta }{\displaystyle {\sum }_i{\left(n_j^T\left[R\left(\theta \right)P_i^w+t-P_i^j\right]\right)}^2}$ (3.3)

3.2. 改进的Otsu

经典的Otsu算法是由日本学者大津展之(Nobuyuki Otsu)于1979年提出的一种自适应阈值确定方法。该算法通过遍历图像的整个灰度值范围(通常为0到255)，寻找能够最大化背景和目标区域之间类间方差的最优阈值。具体来说，Otsu算法将图像分为前景和背景两部分，通过计算类间方差来评估每个可能的阈值，最终选择使类间方差最大的阈值。然而，Otsu算法在处理有障碍物的场景时可能表现不佳，并且对噪声较为敏感。为了解决这些局限性，本文提出了一种改进的Otsu算法。该算法通过选取多个阈值进行二值化，提高了处理速度和分割质量。具体改进步骤如下：

在改进的Otsu算法中，首先将深度图像转换为灰度图像，其中灰度值的范围是从0到L − 1，L表示图像中存在的不同灰度级的总数。对于每个灰度值i，我们统计具有该特定灰度值的像素数量，记为$m_i$ 。图像中的像素总数为M。接下来，我们计算归一化灰度直方图$P_i$ ，它表示图像中具有特定灰度值的像素出现的概率。通过归一化处理，确保所有$P_i$ 值的总和等于1，从而保证了概率分布的合理性。

$P_i=\frac{m_i}{M}$ ，${\displaystyle {\sum }_{i=0}^{L-1}{P}_i}=1$ (3.4)

随后，选择了3个具体的灰度值：$k_0$ 、$k_1$ 、$k_2$ 。其中，$k_1$ 表示整幅图像的平均灰度值同时，$k_0$ 和$k_2$ 在灰度直方图中定义了两个不同的区间，每个区间由阈值k分隔。对于每个选定的阈值k，在0∼k−1区间内计算平均灰度值$g_0\left(k\right)$ ，在k∼L−1区间内计算平均灰度值$g_1\left(k\right)$ 。

$g_0\left(k\right)=\frac{1}{P_0\left(k\right)}{\displaystyle {\sum }_{i=0}^{k-1}i\cdot P_i}$ ，$g_1\left(k\right)=\frac{1}{P_1\left(k\right)}{\displaystyle {\sum }_{i=k}^{L-1}i\cdot P_i}$ (3.5)

计算每个区域的平均灰度方差为${\sigma }_0^2\left(k\right)$ 和${\sigma }_1^2\left(k\right)$

${\sigma }_0^2\left(k\right)=\frac{1}{P_0\left(k\right)}{\displaystyle {\sum }_{i=0}^{k-1}{\left(i-g_0\left(k\right)\right)}^2\cdot P_i}$ (3.6)

${\sigma }_1^2\left(k\right)=\frac{1}{P_1\left(k\right)}{\displaystyle {\sum }_{i=k}^{L-1}{\left(i-g_1\left(k\right)\right)}^2\cdot P_i}$ (3.7)

整个图像的总方差记为${\sigma }_0^2$ ，它表示所有灰度值的加权方差

${\sigma }_0^2={\displaystyle {\sum }_{i=0}^{L-1}{\left(i-k_i\right)}^2\cdot P_i}$ (3.8)

在最后一步中，通过最大化方差项的组合测度来迭代确定最佳阈值H。H对应于三个方差项中产生最高值的阈值。这一优化过程显著增强了分割图像中前景和背景区域的区分度。具体来说，H表示在三个方差项中达到最大加权方差的阈值。

$H=\arg {\max }_{0\le k\le L-1}\left[{\displaystyle {\sum }_{i=0}^2{P}_i\left(k\right)\cdot {\left({\sigma }_i\left(k\right)-{\sigma }_i\right)}^2}\right]$ (3.9)

改进后的Otsu算法如下：

算法2：改进的Otsu

需求：将灰度图分为四个区间，使用三个点$k_{i_0}$ 、$k_{i_1}$ 、$k_{i_2}$ ； 1：计算三个点$k_{i_0}$ 、$k_{i_1}$ 、$k_{i_2}$ 之间的方差，并定义为${\sigma }_t^2\left(T\right)$ ； 2：计算${\sigma }_t^2\left(T-1\right)$ ，${\sigma }_t^2\left(T\right)$ ，${\sigma }_t^2\left(T+1\right)$ ，并获取最大值${\sigma }_t^2\left(\right)$ ； 3：定义对应于${\sigma }_t^2\left(\right)$ 的点M，并将M定义为阈值 4：分割以获得二值图像F； 5：轮廓查找并计算S； 6：如果$S\le S_{\text{total}}$ 因子，则输出S或者$S_{\text{ref}}$ ，否则重新计算阈值k并执行分割以获得二值图像$S_{\text{ref}}$ ； 7：结束循环；

其中，$P_i$ 代表归一化的灰度直方图概率，$k_i$ 代表灰度值，${\sigma }_t^2$ 代表灰度值的方差，H代表三种方差中的最大值，S代表灰度值小于某阈值的像素。

首先，我们计算深度图像的灰度直方图，并确定三个参考点：$k_{i_0}$ 、$k_{i_1}$ 、$k_{i_2}$ 。其中，$k_{i_1}$ 表示整个灰度直方图的平均值，而$k_{i_0}$ 和$k_{i_2}$ 分别对应直方图上半部分和下半部分的平均值。我们通过计算这三个点之间的类内方差来评估它们的集体性能，记为最大方差${\sigma }_t\left(T\right)$ 。

${\sigma }_t\left(T\right)\frac{1}{P_0\left(T\right)}{\displaystyle {\sum }_{i=0}^{T-1}{\left(i-g_0\left(T\right)\right)}^2\cdot P_i}$ (3.10)

在连续的迭代过程中，分别计算类内方差${\sigma }_t\left(T-1\right)$ 和${\sigma }_t\left(T+1\right)$ ，并通过比较这两个值来确定最大的类内方差。这里，图像中的灰度值范围表示为$\left\{0,1,\cdots ,L\right\}$ 。识别出灰度值小于指定阈值的区域内的像素，从而构造集合S。随后，利用OpenCV库来检测和分析二值图像中的所有轮廓，并将这些轮廓的面积与预定义的阈值进行比较。如果某个轮廓的面积超过了阈值，并且与期望值紧密对齐，那么该轮廓将被进一步评估和重新校准。

3.3. 传感器融合

针对障碍物遮挡导致的点云失准问题，本文采用扩展卡尔曼滤波(EKF)算法来融合多个传感器的数据。在预测阶段，我们首先利用惯性测量单元(IMU)提供的初始信息，然后通过EKF算法进行定位。这一过程不仅考虑了单个LiDAR的数据，还结合了LiDAR、WiFi与深度相机数据的融合结果。

传统地，卡尔曼滤波器主要用于线性系统，但在实际应用中，我们常常需要处理非线性系统。为了应对这种情况，扩展卡尔曼滤波器(EKF)被开发出来。在EKF中，系统的状态方程可以表示为式(3.11)：

$X_{k+1}=f\left(X_k,U_k\right)+W_k$ ，$Z_k=h\left(X_k\right)+V_k$ (3.11)

在此背景下，$X_k$ 表示时刻k的系统状态。其中，$X_k$ 和$Y_k$ 分别表示x方向和y方向的线性加速度，${\theta }_k$ 表示角加速度，而${\left(X_w\right)}_k$ 表示时刻k的特征点信息。因此，$X_k$ 可以表示为式(3.12)：

$X_k={\left[{\ddot{X}}_k,{\ddot{Y}}_k,{\theta }_k,{\left({\ddot{X}}_w\right)}_k\right]}^{\text{T}}$ (3.12)

其中，${\ddot{X}}_k$ 在时间步k的x坐标的估计值。${\ddot{Y}}_k$ 在时间步k的y坐标的估计值。${\left({\ddot{X}}_w\right)}_k$ 在时间步k的全局坐标系中的位置估计值，其中w代表世界坐标系。$W_k$ 和$V_k$ 是零均值的高斯白噪声，$U_k$ 表示系统在时间k的输入量，而$Q_k$ 和$R_k$ 分别是$W_k$ 和$V_k$ 的协方差矩阵。

$E\left(u_k\right)=0$ ，$Q_k=E\left(w_k{w}_k^{\text{T}}\right)$ ，$E\left(V_k\right)=0$ ，$R_k=E\left(v_k{v}_k^{\text{T}}\right)$ (3.13)

在单个LiDAR的情况下，${\left(X_w\right)}_k$ 表示k时刻的LiDAR特征。在定位融合部分，${\left(X_w\right)}_k$ 不仅表示k时刻的LiDAR特征，还表示RGB-D图像的特征。考虑到传感器的噪声特性，我们将协方差矩阵R设置为diag (1.5)。而协方差矩阵Q决定了估计精度和收敛速度之间的权衡。在k时刻，状态预测的计算公式如式(3.14)所示：

${\widehat{X}}_{k+1|k}=f\left(X_{k|k}\right)+w_k$ , ${\widehat{P}}_{k+1|k}=A{\widehat{P}}_{k|k}{A}^{\text{T}}+Q_{k|k}$ (3.14)

这里，${\widehat{X}}_{k+1|k}$ 表示在时间步k给定观测数据的情况下，对时间步k + 1状态的预测估计值。${\widehat{P}}_{k+1|k}$ 表示在时间步k给定观测数据的情况下，对时间步k + 1的状态估计误差协方差矩阵的预测值。A是雅可比矩阵，表示为$A=\frac{\partial h}{\partial X_{k+1|k}}$ 。预测协方差在EKF更新阶段确定：

$K_{k+1|k}={\widehat{P}}_{k+1|k}{A}^{\text{T}}{\left(A{P}_{k+1|k}{A}^{\text{T}}+R_{k+1}\right)}^{-1}$ (3.15)

$X_{k+1|k}={\widehat{X}}_{k|k}+K_{k+1|k}{\left(Z_{k+1|k}-h_{k|k}\right)}^{-1}$ (3.16)

$P_{k+1|k}=\left(I-K_{k+1}A\right){\widehat{P}}_{k+1|k}$ (3.17)

在融合阶段，将更新后的LiDAR观测方程得到的状态量$X_{k|k}$ 和协方差矩阵$P_{k|k}$ 作为深度相机数据的预测状态量，并将预测的协方差矩阵用于下一次迭代的状态更新。这个迭代过程允许融合来自多个传感器的数据，从而有效地解决点云错位问题。

4. 试验结果与分析

为了验证多传感器融合定位的有效性，本研究开展了一系列全面的实验。实验内容主要包括对点对点迭代最近点(PL-ICP)算法的改进、对Otsu阈值方法的改进，以及融合定位过程的测试。实验采用的设备YAHBOOM的ROSMASTER X3小车，该装置配备了思岚A1激光雷达和Astra深度相机传感器，如图2所示。通过这些实验，旨在科学、有效地评估所提出方法的性能。

Figure 2. Equipment settings for experiments

图2. 用于进行实验的设备设置

本文采用以下三个主要性能指标来评估所提方法的有效性：

1) 定位精度：通过将融合后的传感器数据与地面真实数据进行对齐，计算两者之间的偏差来衡量定位的准确性。

2) 鲁棒性：评估系统在面对障碍物以及处于复杂、具有挑战性的环境条件下，能否保持准确定位的能力。

3) 处理速度：测量融合过程的运行时间，确保其能够在实时或可接受的时间范围内完成，以满足实际应用的需求。

4.1. 改进的PL-ICP

第一组实验聚焦于评估对传统点对点迭代最近点(PL-ICP)算法的改进。实验中，使用思岚A1激光雷达传感器从环境中捕获三维点云数据。思岚A1激光雷达的检测范围为0.15~12米，且可实现360˚全方位旋转。通过将改进后的PL-ICP算法与传统PL-ICP算法的处理结果进行详细对比，验证了改进方法的显著性能提升，如图3所示。观察图3(A)和图3(B)可以发现，改进后的PL-ICP算法在处理遮挡场景时，尤其是在渲染边缘部分，表现出了明显的优越性。该算法在形状对齐方面具有显著的稳定性，能够持续保持高精度水平。

Figure 3. The experimental results of traditional PL-ICP and improved PL-ICP were compared

图3. 传统PL-ICP和改进PL-ICP的试验结果

此外，本研究还对改进后的PL-ICP算法的收敛时间进行了全面分析，并将其与传统PL-ICP算法进行了对比。研究发现，传统PL-ICP算法的收敛时间为310毫秒，而改进后的PL-ICP算法仅需260毫秒即可完成收敛。这一显著的收敛时间减少，充分证明了改进算法在效率上的显著提升。

综上所述，通过视觉比较渲染效果，改进后的PL-ICP算法在处理复杂场景时的性能提升得到了显著强调。同时，收敛时间的定量分析进一步证实了改进算法在效率上的显著提升。这些改进使得该算法在对精度和效率都有较高要求的应用场景中，展现出良好的应用前景。

4.2. 改进的Otsu

为了评估改进的Otsu算法的有效性，本文导入了两个图像数据集(MNIST, CIFAR-10)，并对其处理速度进行了比较分析，分别采用了三种不同的算法：K-means算法、改进的Otsu算法和曲线变换算法。结果如图4所示，表明改进的Otsu算法在三种方法中具有明显的性能优势。值得注意的是，它显示出最短的每帧的平均处理时间。此外，当进行深入的比较分析时，可以明显看出，即使在相同的CPU上执行时，改进的Otsu算法在计算效率方面也具有显著的优势。事实上，该方法明显优于其他两种方法，运行效率提高了15%以上。

Figure 4. Time-consuming comparison of four algorithms

图4. 四种算法的耗时比较

此外，本文对在同一时间点的不同方法实现的分割效果进行了比较分析，如图5所示。在此图中，A对应RGB-D像素数据，B表示使用一个改进的Otsu算法得到的结果，C作为地面真实参考，D表示K-means方法的结果，E展示了曲线变换生成的结果。以地面真实参考C为基准，对图5的综合评估揭示了几个值得注意的观察结果。首先，在分析图5(D)时，可以明显看出，K-means算法仍然保留了强物体的扩展回波，并表现出大量的强背景噪声。另一方面，图5(B)表示改进的Otsu算法的结果，与地面真相非常一致，使其成为最接近标准的分割方法。然而，图5(E)显示了曲线变换的输出，它似乎过度地过滤掉了对象信息，导致了对象细节的显著丢失。

Figure 5. Comparison of denoising effects of different methods. Among them, (A) original RGB-D image; (B) The real value of the ground; (C) Improved Otsu; (D) curve transformation; (E) k-means

图5. 不同方法的去噪效果的比较。其中，(A) 原始RGB-D图像；(B) 地面真实值；(C) 改进Otsu；(D) 曲线变换；(E) k-means

总之，本研究在Linux操作系统上进行的实验表明，与现有其他方法相比，改进的Otsu算法展现出显著的性能优势。该算法在计算效率方面表现出色，尤其适合对运行时效率有较高要求的图像处理任务，展现出广阔的应用前景。同时，通过比较分析，改进的Otsu算法在精确分割目标的同时，能够有效保留目标的基本特征，进一步凸显了其在各类图像处理任务中的优越性，使其成为极具潜力的图像处理方法。

4.3. 基于融合的定位

为了验证所提出的多传感器融合定位算法的有效性，本文在同一场景和数据集中对改进的PL-ICP定位方法和基于融合的定位方法进行了严格的对比分析。验证结果如图6所示。仔细观察该图可以发现，在红线划定的区域内，包括整个场景的边缘和遮挡部分，本文提出的方法始终优于改进的PL-ICP方法，并且与地面真实数据高度一致。这一观察结果表明，本文的方法在定位精度和可见性方面表现出色，尤其是在面对具有挑战性的环境时。因此，这充分证明了本文的方法在构建高精度地图方面具有显著优势。

此外，本文对融合算法的定位精度进行了严格评估，结果如表1所示。在此次评估中，本文将基于传感器融合的SLAM方法、EKF SLAM方法以及单一改进的PL-ICP方法进行了对比分析，所用数据均采集自配备多种传感器的无人平台。表1中，“精度(m, rad)”一栏所表示的是初始位置偏差值，其单位分别为米和弧度。在评估过程中，地图分辨率被初始化为(0.05 m, 0.05 m)，且系统地将初始角度值在0˚~45˚范围内进行了调整。对表1中数据进行全面分析后，一个有趣的趋势逐渐显现：随着初始角度间隔的增大，改进的PL-ICP方法和传感器SLAM方法实现定位精度小于0.001 m的概率呈现出逐渐降低的趋势。然而，即便初始角度区间不断扩大，本文提出的方法在0.001 m阈值内，始终保持着超过99.7%的定位精度的高概率。表1最后两列的粗体数字清晰地展示了本文提出的传感器融合方法在面对此类问题时所具备的卓越鲁棒性。

Figure 6. The influence of ground real value and the improvement of SLAM and fusion by EKF

图6. 地面真实值的影响和EKF对SLAM和融合的改进

Table 1. Verification of positioning accuracy

表1. 定位精度的验证

实验组(m, rad)	精度区间	改进PL-ICP (%)	传感器SLAM (%)	EKF融合策略(%)
试验1 (0.05 m, 0.05 m, 2^˚)	<0.001	99.85	99.99	99.98
	(0.001, 0.005)	0.01	0	0
	(0.005, 0.01)	0.01	0	0.01
	(0.01, 0.05)	0.13	0.01	0.01
	>0.001	0.00	0.00	0.00
试验2 (0.05 m, 0.05 m, 4^˚)	<0.001	99.71	99.83	99.98
	(0.001, 0.005)	0.02	0.02	0
	(0.005, 0.01)	0.03	0.02	0.01
	(0.01, 0.05)	0.22	0.11	0.01
	>0.001	0.02	0.02	0.00
试验3 (0.05 m, 0.05 m, 8.6^˚)	<0.001	99.51	99.96	99.95
	(0.001, 0.005)	0.03	0.01	0.01
	(0.005, 0.01)	0.05	0.02	0.02
	(0.01, 0.05)	0.33	0.01	0.01
	>0.001	0.08	0.00	0.00
试验4 (0.05 m, 0.05 m, 17.2^˚)	<0.001	98.43	98.65	99.79
	(0.001, 0.005)	0.088	0.01	0.01
	(0.005, 0.01)	0.13	0.10	0.02
	(0.01, 0.05)	0.44	0.35	0.07
	>0.001	0.92	0.89	0.11
试验5 (0.05 m, 0.05 m, 32^˚)	<0.001	84.48	92.67	99.79
	(0.001, 0.005)	0.20	0.18	0.01
	(0.005, 0.01)	0.28	0.20	0.02
	(0.01, 0.05)	0.93	0.41	0.07
	>0.001	14.11	6.54	0.11

续表

试验6 (0.05 m, 0.05 m, 45˚)	<0.001	73.46	85.48	99.79
	(0.001, 0.005)	0.23	0.14	0.01
	(0.005, 0.01)	0.35	0.21	0.02
	(0.01, 0.05)	1.14	0.82	0.07
	>0.001	24.81	13.35	0.11

这些结果凸显了一个关键发现：本文的方法在融合定位精度上优于传感器SLAM方法和单一改进的PL-ICP方法。此外，本文的方法在可靠性和实用性方面也展现出更高水准，使其成为适用于不同场景、需要实现稳健且准确定位的应用程序的不二之选。

5. 结论

在本研究中，提出了一种用于室内定位系统的复杂多传感器融合算法。该算法已在配备有深度相机、WiFi、IMU和激光雷达扫描仪等多种传感器阵列的无人驾驶平台上进行了严格测试。通过全面的理论分析和广泛的实验验证，得出了以下显著的研究成果：

1) 改进的PL-ICP算法：本文提出了一种PL-ICP算法的改进版本。实验结果表明，该改进算法在处理遮挡问题时，不仅表现出更高的准确性和稳定性，还显著提升了计算效率，在相同条件下相比原算法，CPU加速比超过15%。

2) 一种改进的Otsu算法：本研究的另一项关键贡献是Otsu算法的优化。实验结果表明，改进后的Otsu算法能够精准地区分不同距离的目标物体，同时细致地保留目标图像的轮廓特征。此外，该算法在提取特征信息方面表现出色，能够获取更多的细节特征，这不仅增强了系统识别障碍物的能力，还促进了算法的稳健收敛。

3) 多传感器融合定位策略：本研究提出了一种先进的多传感器融合定位策略。实验结果表明，该策略在构建地图方面表现出色，不仅显著提高了遮挡场景下的目标识别能力，还有效缓解了姿态跟踪中的不确定性问题。未来，我们将致力于深入研究智能算法的集成，例如深度学习，以高效地引入多个传感器之间的环路闭合约束。这一研究方向旨在进一步提升导航和定位的精度与密度。

总而言之，本研究在室内定位系统领域取得了一项重要突破，提出了一系列改进的算法和融合策略，有望显著提升定位精度、效率和鲁棒性。未来的研究可以探索如何将深度学习与现有的滤波算法(如EKF)相结合，通过学习不同传感器数据之间的复杂关系，深度学习模型能够提供更加精确和可靠的定位结果，以实现更高效的环路闭合和全局定位。我们对集成深度学习等先进技术的持续探索，彰显了我们致力于不断拓展导航与定位能力边界的决心。