激光视觉融合的改进图优化智能轮椅建图算法

戚开诚; 李超; 单新颖

doi:10.12404/j.issn.1671-1815.2501003

科学技术与工程 ›› 2026, Vol. 26 ›› Issue (3) :1087-1097 DOI: 10.12404/j.issn.1671-1815.2501003

论文：自动化技术、计算机技术

激光视觉融合的改进图优化智能轮椅建图算法

戚开诚 ¹^,² ,
李超 ¹^,² ,
单新颖 ³^,^*

作者信息 +

Improved Graph Optimization Intelligent Wheelchair Mapping Algorithm Based on Laser Vision Fusion

QI Kai-cheng ¹^,² ,
LI Chao ¹^,² ,
SHAN Xin-ying ³^,^*

Author information +

文章历史 +

摘要

为了改进传统图优化建图算法在建图时环境检测能力差、特征提取不明显以及累积误差大等问题,提出一种激光与视觉融合的改进图优化建图算法。首先,标定激光雷达与深度相机,完成数据处理与融合初步消除传感器误差与数据冗余;其次,采用引入距离评价函数的协方差分析(principal component analysis,PCA)方法并结合自适应阈值设定方法选取点云特征点;然后,使用IMU(inertial measurement unit)预积分技术获得初始位姿变换矩阵完成初步扫描匹配,在此基础上采用点对面ICP(iterative closest point)算法完成最终扫描匹配,得到最终位姿变换矩阵;最后,采用蒙特卡洛定位(Monte Carlo localization,MCL)方法并结合里程计数据完成回环检测,消除累积误差。使用智能轮椅在实际场景进行建图验证,结果表明提出的改进图优化算法相比于传统图优化算法有更好的环境检测能力,并且在大环境可以有效减少累积误差,相比于传统图优化算法有一定的改进与提升。

Abstract

An improved graph optimization-based mapping algorithm was proposed to address the limitations of traditional graph optimization methods, including poor environmental perception, indistinct feature extraction, and large cumulative errors. In this approach, a LiDAR and a depth camera was first calibrated, and their data was processed and fused to preliminarily reduce sensor errors and data redundancy. A distance evaluation function was then incorporated into principal component analysis (PCA), and an adaptive threshold was set to select point cloud feature points. The initial pose transformation matrix was obtained using IMU pre-integration for preliminary scan matching, after which a point-to-plane iterative closest point (ICP) algorithm was applied to derive the final pose transformation matrix. Loop closure detection was performed using Monte Carlo localization (MCL) combined with odometry data to eliminate cumulative errors. Experiments were conducted using a smart wheelchair in real-world scenarios. The results indicate that the proposed algorithm exhibits superior environmental perception compared to traditional graph optimization methods and effectively reduces cumulative errors in large-scale environments, demonstrating noticeable improvements over conventional approaches.

Graphical abstract

关键词

智能轮椅 / 激光与视觉 / 图优化 / 特征提取 / 点对面ICP / 蒙特卡洛定位

Key words

smart wheelchairs / laser & vision / graph optimization / feature extraction / point-to-face ICP / Monte Carlo localization

引用本文

引用格式 ▾

戚开诚, 李超, 单新颖. 激光视觉融合的改进图优化智能轮椅建图算法[J]. 科学技术与工程, 2026, 26(3): 1087-1097 DOI:10.12404/j.issn.1671-1815.2501003

QI Kai-cheng, LI Chao, SHAN Xin-ying. Improved Graph Optimization Intelligent Wheelchair Mapping Algorithm Based on Laser Vision Fusion[J]. Science Technology and Engineering, 2026, 26(3): 1087-1097 DOI:10.12404/j.issn.1671-1815.2501003

登录浏览全文

4963

注册一个新账户忘记密码

近年来,随着人口老龄化趋势日益明显,智能轮椅作为一种代步工具,对提高老年人的生活质量和独立性至关重要^[1-2]。智能轮椅搭载老年人在陌生环境中自主运行时,安全问题是首要的,这就必然需要一种关键技术,即同时定位与建图(simultaneous localization and mapping,SLAM)。SLAM技术是利用轮椅上携带的传感器获取环境信息,并对自身位姿进行估计构建出全局地图^[3],是实现路径规划与自主导航的重要前提。

SLAM建图常用的方法分为滤波SLAM算法和图优化SLAM算法^[4]。滤波SLAM算法主要是通过贝叶斯估计框架对系统状态进行估计,它会导致系统误差不断被累积并且实时性低,最终难以得到精确的全局地图^[5];图优化SLAM算法一般由数据输入与处理、扫描匹配、回环检测和后端位姿优化组成,通过数据输入与处理完成环境点云数据的获取,扫描匹配和回环检测完成局部地图更新与回环误差消除,后端位姿优化最终消除累积误差,从而得到精确的全局地图^[6]。因此,图优化SLAM算法在建图研究中得到了广泛的应用。

图优化SLAM算法的数据输入一般由二维激光雷达完成,但是单激光雷达在检测环境时容易出现扫描死角,从而导致智能轮椅在运行时可能发生碰撞;并且激光雷达在障碍物较近时得到的点云数据比较密集,容易造成环境特征缺失的情况。针对此缺陷,张伟伟等^[7]通过融合激光与视觉点云数据提高了环境检测能力,但是由于数据过于复杂,难以提取到准确的环境特征;汤青等^[8]采用基于最大似然估计的随机采样一致性(maximum likelihood estimator sample consensus,MLESAC)算法对点云数据进行预处理,但是提取的点云特征不明显;陆军等^[9]提出了一种多尺度加权法向投影均值差的点云特征提取方法,通过设定阈值完成特征点提取且特征效果明显,但是阈值设定过于依赖经验,对各种环境缺乏自适应性。

图优化SLAM算法的扫描匹配主要通过ICP(iterative closest point)方法完成,通过将连续的两帧点云数据最近点的欧式距离最小化,不断调整相对位姿直至达到收敛得到最终相对位姿。刘畅等^[10]提出了融合ICP与曼哈顿世界假说的位姿扫描匹配方法,提高了匹配精度与建图质量;徐晓苏等^[11]提出了一种基于改进广义迭代最近点(generalized iterative closest point,GICP)配准的扫描匹配方法,提高了匹配与定位精度;李峰等^[12]提出使用基于K4PCS(key 4 point congruent sets,)算法进行初步配准,然后使用ICP算法进行最终配准,匹配精度也明显得到提升。上述研究皆提高了扫描匹配的精度,但是当匹配的点云位姿变化量较大时,匹配误差增大,构建的地图会出现累积误差。

图优化SLAM算法的回环检测主要采用基于特征匹配的方法完成。Olson^[13]提出了多分辨率的闭环检测方法,将激光点云和多分辨率的地图进行特征匹配以完成回环检测;传统图优化Cartographer算法采用低分辨率扫描匹配的方法完检测,并且以分支定界方法完成回环检测的加速^[14]。上述研究虽然成功的检测到了回环,但是在特征单一的大环境中,基于特征匹配的方法容易出现错误回环,从而导致出现建图累计误差。

因此,为解决传统图优化算法中存在的上述问题,提出一种激光与视觉融合的改进图优化算法。该算法在数据输入与处理环节采用激光与视觉融合采集环境信息;并提出一种基于协方差分析方法的自适应特征点提取方法,在扫描匹配环节使用IMU(inertial measurement unit)预积分技术获取初始位姿,然后采用点对面ICP方法获取最终相对位姿;在回环检测环节使用基于蒙特卡洛定位的方法并结合里程计数据正确检测地图中的回环,减少累积误差;最后使用智能轮椅实验平台验证本文改进算法相对于传统图优化算法的优越性和有效性。

1 改进图优化算法整体框架

本文提出的整体优化算法框架如图1所示,主要分为以下步骤。

(1)传感器数据输入与处理。首先,标定传感器、获取三维点云数据并将其降维处理,基于贝叶斯方法融合得到激光视觉融合点云数据;其次,采用引用距离评价函数的协方差分析方法从融合点云数据中提取局部邻域的特征点。与传统固定阈值设定方法相比,提出一种自适应的阈值设定方法,通过其协方差矩阵的特征值求取融合点云的特征变化量,然后与其平均特征变化量对比选择出更具代表性的特征点,从而提高后续扫描匹配的精度与准确性;最后,使用IMU预积分技术为扫描匹配提供两帧融合点云的初始位姿,提升位姿的初步精度。

(2)前端建图。首先,根据特征提取后特征点云与IMU提供的初始位姿估计,采用点对面ICP算法完成扫描匹配,从而得到智能轮椅最终的相对位姿变化。与传统点对点ICP算法相比,点对面ICP算法不容易受到大位姿变化的影响,误差更小,可以提供一个更准确的初始位姿估计;其次,使用运动滤波器过滤掉静止状态下的相对位姿关系,得到运动状态下的相对位姿关系,并将其转换为局部位姿图;最后,采用基于蒙特卡洛定位的方法对一帧帧局部位姿图进行回环检测,以消除位姿图所累积的误差,提高地图的一致性与准确性。与传统图优化算法中基于特征匹配的方法相比,计算复杂度低并且在特征不明显的稀疏环境中有较好的效果。

(3)后端位姿优化。根据回环检测生成的带有约束关系的位姿子地图,采用基于稀疏姿态调整(sparse pose adjustment, SPA)方法来完成后端优化。该方法采用LM(levenberg marquardt)算法作为框架,利用位姿约束求解位姿约束的最优估计对地图进行修正,从而得到全局一致二维栅格地图。

2 具体改进方法

2.1 传感器标定

为了使传感器获取的环境信息与实际环境信息完全匹配,通过标定深度相机获取其内参与畸变参数,降低其成像误差^[15];为了获取深度相机与激光雷达之间的三维坐标变换关系,接着完成深度相机与激光雷达的联合标定获取二者之间的外参数,将数据统一到相同坐标系下。

深度相机遵循小孔成像模型,即空间中的三维点p=[x,y,z]与它的投影像素平面一点的坐标P=(u,v,1)关系为

(1)sp=K[

R T

式(1)中:s为比例因子;K为相机内参矩阵;R为其他坐标系与相机坐标系之间的旋转矩阵;T为平移矩阵。[

R T

]统称为相机的外参。本文中采用张正友标定法^[16]来标定相机,以棋盘格为标定物,通过获取各个不同角度的棋盘格图片,使用MATLAB自带的标定工具箱完成标定,获取标定参数。

相机与雷达之间的外参模型可以表示为

(2)

x K y K z K

R T 01 x S y S z S

式(2)中:(x_K,y_K,z_K)为目标点在深度相机下的坐标;(x_S,y_S,z_S)为目标点在雷达下的坐标;通过代入深度相机与雷达数据,即可求得二者之间的外参数。

2.2 降维与融合

深度相机在标定完后,主要通过采集彩色图和深度图从而可以获取周围环境的三维点云数据,进一步便可以提供三维环境信息。通过设置相关参数(帧率、颜色格式、深度模式),采集彩色图与深度图,如图2所示,然后将其进行融合处理后转换成三维点云数据,如图3所示。

通过下载雷达驱动程序包,设置激光雷达的相关参数,完成三维环境点云数据的获取,可视化后如图4所示。

由于所构建的地图是便于路径规划的二维栅格地图,所以需要将获取的三维点云数据转换成二维点云数据。使用开源pointcloud_to_laserscan功能包完成三维点云数据的降维。

在获得深度相机与激光雷达的三维点云数据后,为了获得更精确的观测数据,采用贝叶斯推断方法将降维后的雷达和相机点云数据进行融合,步骤如下:

步骤1 定义模型,设激光雷达点云数据为Z_l,深度相机点云数据为Z_d,融合后的点云数据为Z_f,则激光雷达与深度相机的观测模型可分别表示为p(Z_l|θ_l)与p(Z_l|θ_d),其中,θ_l与θ_d为相关测量模型参数。

步骤2 数据预处理,主要包括滤波降噪处理。

步骤3 定义先验概率,设定激光雷达先验概率为p(Z'_l|θ'_l),深度相机先验概率为p(Z'_l|θ'_d)。

步骤4 观测更新,通过结合先验概率和观测数据,可以得到更新后的概率分布,即后验概率,其贝叶斯公式为

(3)p(Z_f|Z'_l,Z'_d)=

p (Z' l | Z f) p (Z' d | Z f) p (Z f) p (Z' l) p (Z' d)

式(3)中:p(Z_f|Z'_l,Z'_d)为后验概率;p(Z'_l|Z_f)与p(Z'_d|Z_f)为激光雷达和深度相机的似然度;p(Z_f)为先验概率。式(3)通过计算似然度和先验概率的乘积,并进行归一化,得到更新后的后验概率,将观测数据融合到先验概率中,从而得到更准确的估计结果。

步骤5 通过将先验概率和观测数据的后验概率进行融合操作,得到融合后的点云数据Z_f。

2.3 特征点提取

在点云中提取关键特征点常用的方法是基于协方差分析法,该方法主要是通过估计点云的法向量和曲率来筛选特征点^[17],随机选择点云中一点x_i,而x_i₁,x_i₂,…,x_in为x_i的邻域点,则x_i的邻域协方差矩阵可以表示为

(4)E=

1 n ∑ j = 1 n

(x_ij-

x i ¯

)

(x i j - x i ¯) T

式(4)中:

x i ¯

为n个邻域点的质心。但是该协方差矩阵存在一些局限性:当邻域点距离中心点x_i较远时,由于数据可靠性降低,导致在法向量或曲率变化较大时估计不够准确。因此引入距离评价函数优化了此协方差矩阵,具体公式为

(5)E_x=

1 n ∑ j = 1 n

(x_ij-

x i ¯

)

(x i j - x i ¯) T e - d i / r 2

式(5)中:e为距离评价函数,该函数随着d_i的增大而递减;d_i为x_i与邻域点的距离;r为邻域的半径,其邻域示意图如图5所示。

然后求式(3)的特征值与特征向量,记特征值为λ₁与λ₂,对应的特征向量为v₁和v₂,通过对比特征值的大小,选取最小特征值所对应的特征向量为x_i的法向量e,假设最小特征值为λ₁,所以x_i处的邻域特征变化可以表示为

(6)ε(x_i)=

2 λ 1 λ 1 + λ 2

当ε(x_i)越大,则点在邻域内特征变化越大,该点便可以被选作为特征点,因此通过设定适当的阈值δ,来选取满足ε(x_i)>δ条件的点作为特征点。

传统的阈值设定方法为根据经验设定固定阈值,该方法具有计算速度快、效果稳定的优点,但是需要根据场景变化不断调整,导致适应性下降,在不同场景下还容易出现特征点不明显的情况。因此采用一种自适应的阈值设定方法,通过计算邻域内的平均特征变化作为点云所选点的阈值,由于邻域内每一个点的特征变化都不同,所以阈值也会根据邻域中所选点的特征变化而自适应的调整,其平均特征变化表示为

(7)δ_i=

1 n ∑ i = 1 n

ε(x_i)

因此,当所选点ε(x_i)>δ_i时,则认为该点为点云中的特征点,反之,则去除该点。

2.4 IMU预积分

IMU角速度

ω ∧ t

和加速度

a ∧ t

测量模型表示为

(8)

ω ∧ t = ω t + b ω t + n ω a ∧ t = a t + b a t + n a

式(8)中:ω_t和a_t分别为角速度和加速度的先验估计;b_ωt和b_at分别为角速度与加速度的偏置分量;n_ω和n_a分别为角速度与加速度的噪声分量。

由于IMU的采样频率比激光雷达和深度相机的帧数据快,所以需要在单帧融合数据里对IMU数据进行积分处理,主要就是依据IMU数据计算位姿、速度以及旋转量,其计算公式为

(9)

α b k + 1 b k = ∫ ∫ t k t k + 1 R t b k (a ∧ t - b a t - n a) d t 2 β b k + 1 b k = ∫ t k t k + 1 R t W (a ∧ t - b a t - n a) d t γ b k + 1 b k = ∫ t k t k + 1 12 Ω (ω ∧ t - b ω t - n ω) γ t b k d t

式(9)中:${\alpha }_{{b}_{k+1}}^{{b}_{k}}$、${\beta }_{{b}_{k+1}}^{{b}_{k}}$、${\gamma }_{{b}_{k+1}}^{{b}_{k}}$分别为位姿、速度和旋转量的预积分量;b_k为IMU对应的坐标系;${R}_{t}^{{b}_{k}}$和${R}_{t}^{W}$为t时刻本体坐标系相对于IMU坐标系和世界坐标系的旋转矩阵;Ω为四元数的微分矩阵函数;${\gamma }_{t}^{{b}_{k}}$为本体坐标系下t时刻到k时刻的变换矩阵。

由于IMU的采样频率过高,如果将所有时刻的偏置都优化计算则计算量过大,且预积分量只与测量值和偏置有关,所以当偏置变化较小时,可以假设短时间内IMU偏置不变,计算公式为

(10)

p k + 1 = p k + v k + 12 a ∧ k ¯ t 2 v k + 1 = v k + a ∧ k ¯ t q k + 1 = q k ⊗ 2 ω ∧ k ¯ t

式(10)中:p_k、v_k和q_k分别为k时刻的位姿、速度和旋转量;⊗代表四元数乘法;

a ∧ k ¯

、

ω ∧ k ¯

分别为加速度与角速度的平均后验估计,其计算公式为

(11)

a ∧ k ¯ = 12 [q k (a ∧ k - b a k) + q k + 1 (a ∧ k + 1 - b a k + 1) - 2 g W] ω ∧ k ¯ = 12 (ω ∧ k + ω ∧ k + 1) - b ω k

式(11)中:g^W为世界坐标系下的重力加速度。

2.5 扫描匹配

由于点对点ICP算法存容易受到点云位姿变化量的影响,所以使用该算法对连续的两帧点云信息进行扫描匹配测试,如图6与图7所示。

由图6实验结果可知,当连续两帧融合点云信息位姿变化量比较小时,使用点对点ICP匹配算法可以得到较好的匹配效果。但是当连续两帧融合点云位姿变化量很大时,由图7可知,基准点云与待配准点云的匹配效果大打折扣,从而造成轮椅的相对运动位姿变化出现很大的误差。因此决定采用点对面ICP算法完成扫描匹配,并且使用IMU提供初始位姿以增强点对面ICP算法的匹配精度,具体思路如下。

首先使用IMU预积分技术获取两个点云帧的相对位姿,因为使用IMU预积分技术得到的相对位姿不会存在传统扫描匹配中局部极值造成的误匹配问题,所以此相对位姿可作为连续两帧融合点云的初步相对位姿估计,为后续的扫描匹配提供初始化的位姿变换矩阵,提高了后续扫描匹配的精度;然后在接收到经特征点提取后的激光雷达与深度相机融合点云后,以IMU预估的相对位姿为初步位姿进行初步匹配,最后采用点对面ICP算法进行最终匹配,获得最终相对位姿变化,完成扫描匹配。其实现步骤如下:

步骤1 初始化,选取两次特征点提取后的融合点云,分别为目标点云集G={G₁,G₂,…,G_n}和源点云集S={S₁,S₂,…,S_n},其中的所有点皆为三维坐标点,维度为3×1,IMU预积分技术获取的初始变换矩阵为M=(R,T),R为旋转矩阵,维度为3×3,T为平移矩阵,维度为3×1,最大迭代次数为z。

步骤2 初步配准,利用初始变换矩阵M对源点云集中的每一个点S_i进行初始变换,得到初始估计源点云集S'={S'₁,S'₂,…,S'_n}。

步骤3 点面匹配,对于每个源点S'_i,在目标点云集G中找到对应的平面,计算到该平面的距离,并且使其最小化,建立点云之间的对应关系,即

(12)min=‖G_z-S'_z‖

步骤4 计算最优转换矩阵,通过奇异值分解法(singular value decomposition,SVD)计算最优的变换矩阵,以式(12)作为优化目标。

(1)分别将G与S'均值化为零均值点集,其中G_i与S'_i分别为点云集G与S'中的点,表达式为

(13)

G - i = G i - 1 n ∑ i = 1 n G i S -' i = S' i - 1 n ∑ i = 1 n S' i

(2)计算协方差矩阵H,即

(14)H=

G - i (S -' i) T

式(14)中:

G - i

和

S -

'_i分别为均值化后的点所组成的矩阵。

(3)SVD分解,即

(15)H=UΛV^T

式(15)中:U、V为正交矩阵;Λ为对角矩阵。

(4)计算最优旋转矩阵R₁与平移矩阵T₁,即

(16)

R 1 = V U T T 1 = 1 n ∑ i = 1 n G - i - R 1 1 n ∑ i = 1 n S -' i

步骤5 计算点云集配准误差,公式为

(17)E_dist(R₁,T₁)=

∑ i = 1 n

|R₁S'_i+T₁-S″_i|²

计算点云配准误差,其中S″_i为更新变换得到的新点云集;

步骤6 收敛判断:设定点云配准误差阈值ΔX,当点云配准误差大于ΔX,更新点云集S'_i为S″_i,继续执行步骤2;若连续两次的点云配准误差ΔE小于ΔX或者迭代次数等于z,停止迭代,返回最终的转换矩阵,完成扫描匹配。

2.6 回环检测

当完成扫描匹配后会生成一帧一帧的位姿约束图,随着时间的推移而产生累积误差,会使地图的一致性和准确性降低。因此需要引入回环检测,原理如图8所示。图8中圆点为建图过程中产生的关键扫描点,扫描点之间的实线为扫描匹配环节引入的相对位姿约束,可以看到在无回环检测环节时,扫描点的误差会随着运动距离的增加而不断累积;当存在回环检测环节时,它会检测是否存在于当前扫描点相似的扫描点,从而增加如图中虚线表示的回环约束,有效地降低扫描点的累积误差。

在特征单一的大环境中,基于特征匹配的方法来完成回环检测会产生累积误差,而蒙特卡洛定位方法可以适应各种环境特征,提高了系统在复杂环境中的鲁棒性,并且计算复杂度低,使得检测回环变得相对容易。因此采用蒙特卡洛定位的方法来进行回环检测,它通过在智能轮椅建图时可能的位置上进行随机采样,估计机器人的状态并判断是否发生了回环。其步骤如下:

步骤1 初始化,设置初始粒子数N;每个粒子的初始权重均匀分配;姿态表示为(x₀,y₀,θ₀),其中(x₀,y₀)为位置,θ₀为初始朝向角度,以弧度表示,设置初始姿态为(0,0,0);最大迭代次数为z。

步骤2 更新运动模型,根据智能轮椅里程计的运动数据对每一个粒子进行运动更新。

(1)角度更新,公式为

(18)θ_t=θ_t_-1+α

式(18)中:α为智能轮椅旋转角度。

(2)位置更新,公式为

(19)

x t = x t - 1 + d c o s θ t - 1 y t = y t - 1 + d s i n θ t - 1

式(19)中:d为智能轮椅运行的相对位移。

步骤3 更新粒子权重,根据智能轮椅的传感器数据更新每个粒子的权重,用传感器数据评估每个粒子与环境地图的匹配程度。

(1)权重更新,采用高斯分布表示传感器测量误差,所以使用高斯分布的概率密度函数来计算权重更新,即

(20)${w}_{t}^{i}$=$\frac{1}{\sqrt{{\left(2\pi \right)}^{k}\left|{\sigma }_{t}\right|}}$exp[-$\frac{1}{2}({z}_{t}-{\stackrel{\wedge }{z}}_{t}^{i}{)}^{T}$×σ^-1(z_t-${\stackrel{\wedge }{z}}_{t}^{i}$)]

式(20)中:${w}_{t}^{i}$为当前时刻第i个粒子的权重;k为传感器数据维度,此处取3;σ_t为当前时刻的协方差矩阵;z_t为当前时刻传感器数据;${\stackrel{\wedge }{z}}_{t}^{i}$为第i个粒子当前时刻的观测数据,可表示为

(21)${\stackrel{\wedge }{z}}_{t}^{i}$=h(${x}_{t}^{i}$,${y}_{t}^{i}$,${\theta }_{t}^{i}$,m)

式(21)中:h(·)为传感器模型;m为地图信息。

(2)权重归一化。

(22)

w - t i

w t i ∑ j = 1 N w t j

式(22)中:

w - t i

为第i个粒子归一化后的权重。

步骤4 重采样,根据粒子更新后的归一化权重,选择具有高权重的粒子,剔除低权重的粒子。

步骤5 回环检测,通过计算当前保存下来的高权重粒子的当前轨迹信息与历史轨迹信息的相似度来检测是否发生回环,其轨迹相似度公式为

(23)Sim_t=λ

(x t - x t - 1) 2 + (y t - y t - 1) 2

+(1-λ)|θ_t-θ_t_-1|

式(23)中:λ为比例系数。然后设定相似度阈值ΔE,此处取相似度分布的均值加上2倍标准差,如若相似度高于阈值ΔE,则认为发生了回环。

步骤6 迭代优化,若检测到回环,根据回环信息对地图信息进行调整,设定地图更新间隔或者达到最大迭代次数,停止迭代;否则返回步骤2。

3 实验验证与分析

本文实验环境有两种,分别为室内复杂小环境和室内长走廊大环境,如图9所示。使用了配备上位机处理器、深度相机、三维激光雷达、IMU以及轮式里程计的差速智能轮椅和ROS2 Humble系统下的Rviz可视化平台对提出的改进图优化算法进行实验验证,其相关硬件设备及功能信息如表1所示,智能轮椅实验平台如图10所示,实验验证主要从以下3个方面来探讨提出的改进图优化算法的有效性。

3.1 建图精度验证与分析

采用提出的改进图优化算法,人为控制的方式引导智能轮椅在两种实验环境中运动完成地图构建,如图11所示。从图11中可以看出,改进图优化算法所创建的两种环境下的栅格地图边界清晰,与实际环境也基本吻合,而且可以很好地表示环境信息。通过选取栅格地图的一些关键尺寸进行测量并与实际地图的对应尺寸对比,选取室内复杂小环境中的横边与竖边以及室内长走廊环境中的长度与宽度作为关键尺寸,得到的结果如表2所示。

由表2中数据可得,在室内小环境中构建地图的误差均值大约为4.85 cm,在室内长走廊中构建地图的误差均值大约为6.05 cm,而且与真实环境对比没有太大的变形和误差,具有良好的全局一致性,基本上满足智能轮椅自主导航的需求。

3.2 环境检测能力验证与分析

为了验证改进图优化算法的环境检测能力,选择在室内复杂小环境中布置一些低小障碍物,如图12所示,用来验证改进图优化算法在建图时是否可以检测到这些低小障碍物,是否弥补了传统图优化建图算法环境检测能力差的缺陷。

分别采用传统图优化Cartographer算法与改进图优化算法分别完成地图构建。如图13(a)中红色圈中所示,Cartographer算法建图由于单传感器感知环境的缺陷,导致构建出来的地图没有显现出低小障碍物信息,而且在图13(a)中蓝色圈内出现了建图误差,地图边界也不是很清晰。如图13(b)所示,改进图优化算法由于多传感器之间的信息互补,相对噪声减小,环境检测能力增强,可以看到红色圈内获取到了低小障碍物信息,蓝色圈内没有建图误差,并且明显看到地图的边界更加清晰,从而可以完整的显现出室内复杂小环境的全局一致地图。

3.3 累积误差验证与分析

为了验证本文提出的改进图优化建图算法在特征单一的大环境中的累积误差消除的效果以及建图的可行性,在室内长走廊环境中进行建图实验,并且与Cartographer算法进行对比。两种算法构建出的长走廊地图如图14所示,可以看出两种算法所建立的地图基本上都可以清晰的表示出周围环境的样貌。

Cartographer算法中在传感器数据输入处理、扫描匹配、回环检测环节中存在固有的缺陷,所以导致在构建地图时容易在末端出现累计误差,如图14(a)中红色圈内所示,误差的不断累积导致在长走廊栅格地图尽头出现边界不清晰以及倾斜的情况。而图14(b)为提出的改进图优化建图算法构建的室内长走廊栅格地图,在经过传感器数据融合和处理、优化后的扫描匹配与回环检测等环节之后,地图的累积误差明显得到改善,地图边界也更加的清晰,并且在室内长走廊栅格地图的尽头没有出现累积误差,得到了全局一致的栅格地图,如图14(b)中红色圈内所示。

为了更加清晰地对比Cartographer算法与提出的激光与视觉融合的改进图优化建图算法构建地图时产生地累积误差,以实际环境中测量的数据为真实值,选取10处特征点,测量10处特征点与轮椅建图起始点的距离值,将此测量值与建图显示软件Rviz中显示的图测值进行对比,计算其绝对误差,并将绝对误差取绝对值,所有数据均保留两位小数,两种算法的测试数据如表3所示,建图绝对误差的对比如图15所示。

由图15可知,传统图优化Cartographer算法随着距离的增加在构建地图时累计误差会不断被叠加,从而导致真实地图与算法估计地图之间的相对误差逐渐增大,无法得到全局一致栅格地图;而提出的改进图优化建图算法在1号与3号特征点时虽然绝对误差大于Cartographer算法,但是总体上改进图优化建图算法的绝对误差小于Cartographer算法,具有更好的精度与鲁棒性。在长走廊的近距离处,两种算法的绝对误差增长率基本持平,没有太大差别。但是在长走廊的尽头,即7~10号特征点处,Cartographer算法绝对误差增长速率非常高,而提出的改进图优化建图算法绝对误差较低,并且将绝对误差增长速率维持的较为平缓,极大地减少了使用Cartographer算法在简单大环境中运动建图时的累积误差。

4 结论

为了解决传统二维激光SLAM图优化算法环境检测能力差、特征点提取不明显以及产生累积误差等问题,对传统图优化算法进行改进,最终得到以下结论。

(1)首先通过标定传感器获取标定参数,然后完成点云数据获取和降维处理后得到栅格地图所需的二维点云数据,最后采用贝叶斯推断方法完成数据融合,解决了传统图优化算法的因环境点云数据不充分导致的环境检测能力差的问题。

(2)采用基于距离评价函数的协方差分析法求取点云特征点的特征变化,提出一种阈值设定方法,通过与点云特征变化对比选取点云特征点。该方法解决了传统图优化算法提取的点云特征点特征不明显的问题。

(3)首先利用IMU预积分技术得到初始位姿矩阵完成初步扫描匹配,然后使用点对面ICP算法完成最终扫描匹配以得到最终相对位姿,最后采用基于蒙特卡洛定位方法并结合里程计数据完成回环检测。该方法解决了传统图优化算法在特征单一的大环境中因前端建图精度不高而导致的累积误差较大的问题。

(4)使用智能轮椅完成实验验证,实验结果表明,相比于传统图优化算法,本文提出的改进图优化算法具有良好的建图精度、环境检测能力提高以及有效减少了建图累积误差,在特征单一的大环境中平均累积误差减少1.92 cm,最大减少5.43 cm。

参考文献

原文顺序 | 出版日期 | 本文引用

[1]	于欣平, 卢博扬. 中国人口老龄化现状及积极应对人口老龄化措施分析[J]. 经济师, 2023, 408(2): 21-22.

[2]	Yu Xinping, Lu Boyang. Active measures for current situation of population aging in China[J]. China Economist, 2023, 408(2): 21-22.

[3]	袁源强. 智能轮椅室内建图及自动导航技术研究[D]. 哈尔滨: 哈尔滨工业大学, 2017.

[4]	Yuan Yuanqiang. Research on mapping and automated navigation in indoor environment for intelligent wheelchair[D]. Harbin: Harbin Institute of Technology, 2017.

[5]	Smith R C, Cheeseman P. On the representation and estimation of spatial uncertainty[J]. International Journal of Robotics Research, 1986, 5(4): 56-68.

[6]	周治国, 曹江微, 邱顺帆. 3D激光雷达SLAM算法综述[J]. 仪器仪表学报, 2021, 42(9): 13-27.

[7]	Zhou Zhiguo, Cao Jiangwei, Qiu Shunfan. Overview of 3D lidar SLAM algorithms[J]. Chinese Journal of Scientific Instrument, 2021, 42(9): 13-27.

[8]	Murphy K, Russell S. Rao-blackwellised particle filtering for dynamic Bayesian networks[M]. Berlin: Springer, 2001: 176-183.

[9]	Lu F, Milions E. Globally consistent range scan alignment for environment mapping[J]. Autonomous Robots, 1997, 4(4): 333-349.

[10]	张伟伟, 陈超, 徐军. 融合激光与视觉点云信息的定位与建图方法[J]. 计算机应用与软件, 2020, 37(7): 114-119.

[11]	Zhang Weiwei, Chen Chao, Xu Jun. Localization and mapping method of fusion laser and visual point cloud information[J]. Computer Applications and Software, 2020, 37(7): 114-119.

[12]	汤青, 陈兵, 冯皓, 等. 基于图优化的三维激光雷达SLAM算法研究[J]. 电子质量, 2023(5): 20-25.

[13]	Tang Qing, Chen Bing, Feng Hao, et al. Research on 3D lidar SLAM algorithm based on graph optimization[J]. Electronics Quality, 2023(5): 20-25.

[14]	陆军, 邵红旭, 王伟, 等. 基于关键点特征匹配的点云配准方法[J]. 北京理工大学学报, 2020, 40(4): 409-415.

[15]	Lu Jun, Zhao Honxu, Wang Wei, et al. Point cloud registration method based on key point extraction with small overlap[J]. Transactions of Beijing Institute of Technology, 2020, 40(4): 409-415.

[16]	刘畅, 党淑雯, 陈丽. 改进位姿估计环节的ORB-SLAM稠密建图算法[J]. 科学技术与工程, 2024, 24(7): 2782-2789.

[17]	Liu Chang, Dang Shuwen, Chen Li. Improved pose estimation for dense mapping based on ORB-SLAM[J]. Science Technology and Engineering, 2024, 24(7): 2782-2789.

[18]	徐晓苏, 高佳誉, 周帅, 等. 基于改进GICP配准的激光-惯性SLAM算法[J]. 中国惯性技术学报, 2023, 31(8): 814-822.

[19]	Xu Xiaosu, Gao Jiayu, Zhou Shuai, et al. Lidar-inertial SLAM based on improved GICP registration[J]. Journal of Chinese Inertial Technology, 2023, 31(8): 814-822.

[20]	李峰, 孟飙. 基于K4PCS与ICP算法在点云配准中的应用[J]. 智能计算机与应用, 2024, 14(11): 138-143.

[21]	Li Feng, Meng Biao. Application of K4PCS and ICP algorithm in point cloud registration[J]. Intelligent Computer and Applications, 2024, 14(11): 138-143.

[22]	Olson E. M3RSM: many-to-many multi-resolution scan matching[C]// IEEE International Conference on Robotics and Automation (ICRA). Washington: IEEE, 2015: 5815-5821.

[23]	颜普, 曹昊宇, 雷为好. 基于改进图优化的激光SLAM算法[J]. 安徽大学学报(自然科学版), 2023, 47(4): 35-41.

[24]	Yan Pu, Cao Haoyu, Lei Weihao. Laser SLAM algorithm based on improved graph optimization[J]. Journal of Anhui University (Natural Science Edition), 2023, 47(4): 35-41.

[25]	姜文飞, 张小俊, 王金刚. 地下车库的雷达与相机融合即时定位与建图[J]. 科学技术与工程, 2022, 22(6): 2334-2342.

[26]	Jiang Wenfei, Zhang Xiaojun, Wang Jingang. Radar and camera fusion real-time positioning and mapping of underground garage[J]. Science Technology and Engineering, 2022, 22(6): 2334-2342.

[27]	迟德霞, 王洋, 宁立群, 等. 张正友法的摄像机标定试验[J]. 中国农机化学报, 2015, 36(2): 287-289, 337.

[28]	Chi Dexia, Wang Yang, Ning Liqun, et al. Experimental research of camera calibration based on Zhang’s method[J]. Journal of Chinese Agricultural Mechanization, 2015, 36(2): 287-289, 337.

[29]	陈华伟, 袁小翠, 吴禄慎, 等. 基于曲率突变分析的点云特征线自动提取[J]. 光学精密工程, 2019, 27(5): 1218-1228.

[30]	Chen Huawei, Yuan Xiaocui, Wu Lushen, et al. Automatic extraction of point cloud feature lines based on curvature mutation analysis[J]. Optics and Precision Engineering, 2019, 27(5): 1218-1228.