Deviation Matrix Based for 3D SLAM Pose Graph Optimization
-
摘要:
位姿图优化(pose graph optimization，PGO)是3D SLAM(simultaneous localization and mapping)后端优化方法之一，其精确求解依赖于良好的初始值。针对PGO噪声数据集初始化，首先提出一种新的PGO目标公式——CN(chordal with noise)模型，此模型考虑噪声影响下产生的旋转偏差，将偏差矩阵设为参数；其次，提出ORDM(optimize rotation with the deviation matrix)算法求解CN模型，此算法在位姿图子图中，分别建立关于偏差矩阵的相对旋转测量方程，最终将CN模型化为矩阵形式，并采用线性最小二乘求出偏差矩阵的封闭解，以此修正旋转方向。实验证明，ORDM算法在面对PGO噪声数据集时，较为鲁棒，具有一定的可伸缩性；与迭代初始化算法相比，可对应较差的初始化场景。
Abstract:Pose graph optimization (PGO) is a back-end optimization method for 3D simultaneous localization and mapping, and its accuracy depends on good initial values. We present a chordal with noise (CN) model, which is a new PGO target formula for initializing PGO noise datasets. This proposed model considers the rotation deviation caused by noise, and the deviation matrix is set as a parameter. The optimize rotation with the deviation matrix (ORDM) algorithm is proposed to solve the CN model. The algorithm builds the relative rotation measurement equation of the deviation matrix in the subgraph of the pose graph and finally models the CN into the matrix form. A closed solution of the deviation matrix is obtained by using linear least squares to correct the direction of rotation. Our experimental results show that the ORDM algorithm is robust and scalable in PGO noise data sets. In addition, the ORDM algorithm can correspond to poor initialization scenarios when compared with the iterative initialization algorithm.
-
Keywords:
- pose graph optimization (PGO) /
- 3D SLAM /
- niose /
- deviation matrix /
- linear least square
-
0. 引言
随着全球老龄化趋势的不断加速，人口老龄化问题已经成为发达和发展中国家共同面对的挑战[1]。随着年龄的增长，人的活动能力逐渐下降。对于存在活动障碍的人群，长时间坐着不动会导致下肢肌肉萎缩和身体机能迅速下降，给个人健康带来更为严重的损害[2-4]。
为了帮助这一人群维持独立性并减少日常活动中的困难，智能助行器作为一种重要的辅助工具得到了广泛应用。MOREIRA等[5]证明了助行器是一种有效的康复工具，可以帮助行动不便者恢复和保持其运动能力，提高其生活质量。MARTINS等[6]对智能助行器进行了介绍，相较于传统助行器，它具有众多优势，如电机驱动、动态支撑、步态和姿态稳定、用户友好、多功能、可适应性、减轻医护人员的工作负担以及提高用户独立性等。
作为辅助康复机器人，清楚用户的运动状态至关重要[7-9]。姚瀚晨[10]设计和搭建了一个搭载激光雷达的跟随型移动机器人系统，提出了一种基于支持向量描述的目标检测方法，可以在多人场景下识别跟随目标的双腿。AGUIRRE等[11]利用一个单目相机和2维激光雷达来检测和跟踪人。BOZORGI等[12]提出了一种基于2维激光和3维相机的用户轨迹跟踪方法，通过多传感器数据的集成和滤波，提高了对用户运动状态检测的鲁棒性和准确性。HAN等[13]设计了一种基于力敏电阻的力传感机制，用于测量用户和机器人之间的交互力，并通过分析交互力和用户意图力/力矩的关系，估计用户的运动状态。然而，基于力传感器估计用户运动意图的方法并不能保证用户在使用助行器时的安全。例如，当使用者有前倾的摔倒趋势时，会导致用户与机器的相互作用力发生急剧的变化，使用传统导纳运动控制器的控制系统会认为用户具有前向的运动速度突变，从而造成危险。因此，助行器在对用户的运动状态进行估计时，需要同时检测用户下肢的位置，并根据此来控制助行器的速度和方向[14]。而且，在人类行走时，手臂和腿部是协调运动的[15-17]。STEPHENSON等[18]认为上肢运动在步行中起着重要的作用，应该被纳入步行康复的训练中。对于不能在没有外部支撑的情况下步行的患者，使用滑动手柄是一种有效的替代自然摆臂的方法。2维激光雷达以其显著的距离测量精度被广泛应用于用户检测和定位，将激光雷达安装在机器人平台上，可以提供用户膝盖位置的距离信息[19]。所以，将上肢信息与下肢信息融合进行估计从而获得更准确的用户运动状态估计，具有可行性。XU等[20]提出了一种基于多传感器的用户运动意图识别算法，该算法比传统的基于力传感器的阻抗控制算法有更好的性能和准确度，能够让用户感觉更自然和舒适。
在传感器采集到用户的信息后，需要进行滤波处理，才能为后续的对用户运动状态的估计提供良好基础，但是数据处理的过程会对数据采集的实时性产生一定影响[21]。因此，可以使用Informer对未来状态进行预测，从而降低数据处理对实时性产生的影响。CHEN等[22]提出了一种结构信息器方法，可以实现准确有效的目标长期轨迹预测。LANGE等[23]提出了首个结合遮挡推理和轨迹预测的端到端环境预测框架。研究结果表明，Scene Informer算法在Waymo Open Motion数据集上的表现优于现有方法，尤其在部分可观测环境下展示了更高的准确性和鲁棒性。
本文的目标是对使用者的手臂和腿的信息进行处理，获得更准确的运动状态，并用Informer提高实时性，使用户获得更舒适的使用体验。实验结果表明，本文提出的方法能够很好地估计出使用者的运动状态，使助行器起到更好的助行效果。
1. 实验平台
智能助行器如图 1所示，包含了显示屏、3维力传感器、二维激光雷达、驱动轮、全向轮、金属框架等部件。显示屏用于显示设备状态信息，驱动轮用于控制智能助行器的运动，3维力传感器用于测量用户与机器的相互作用力，而2维激光雷达用于测量用户与机器的相对位置。
本文研究涉及的坐标系均在雷达坐标系OL-XLYL下。雷达坐标系遵循右手定则，其坐标系的原点位于其圆心，并且正方向遵循图 2中所示的方向。
![]()
图 2 系统坐标系及用户左右腿坐标Fig. 2. System coordinate system and user′s left and right leg coordinates智能助行器为三轮结构，前轮为全向轮，后两个轮为驱动轮，将智能助行器坐标(Xr，Yr)定义为两个驱动轮连线的中心点。用户的左腿坐标定义为(Xml，Yml)，右腿坐标定义为(Xmr，Ymr)，所以，用户坐标的定义为
2. 用户运动意图估计算法设计
2.1 算法说明
本文首先通过传感器系统获取了用户运动状态数据。对数据进行处理后，再对用户的线速度和角速度进行了估计，为解决由数据滞后引起的问题，本文引入了Informer模型，有效地弥补了滤波过程中的延迟，确保最终输出的用户运动意图更加准确和可靠。本研究的算法流程图见图 3。
2.2 用户作用力数据采集
本文使用3维力传感器估计使用者的运动意图。规定使用者在使用智能助行器时将使用“趴握式”的姿势。
作用在握把上的力如图 4所示，左右手作用在握把上的力分别为F1和F2 3维力传感器受到的力为Fs，3维力传感器提供的支反力与Fs大小相同，方向相反，记为-Fs。作用在y方向的合力为0，求得作用在3维力传感上的力为
(1) ![]()
图 4 趴握式姿势及3维力传感受力情况Fig. 4. The lying grip posture and the force applied to the 3D force sensor将式(1)变形得到：Fs=F1+F2。
2.3 用户相对位置数据采集
使用者在使用智能助行器的时候，本文有如下假设：
1) 使用者的手自始至终都是放在握把上的。
2) 使用者的腿部不会产生相互遮挡。
3) 使用者在使用助行器时只能进行平滑旋转，而不会出现横向的移动。
2维激光雷达采集用户膝盖部位的点云数据，在筛选和聚类这些点云数据之后，计算出其对应的圆心位置，得到用户腿部位置。
本文使用文[20]的方法计算圆心，在获得了各自对应的坐标之后，根据Y轴坐标的大小差异，将其中Y轴坐标更大的成员标记为右腿，而Y轴坐标更小的成员则被标记为左腿。
2.4 用户行走状态数据处理
本文将用户的行走状态定义为
(2) 其中，Vleg为使用者行走时的线速度，ωF、ωL分别为使用3维力传感器和2维激光雷达得到的意图角速度估计。
α为通过3维力传感器估计出的使用者的角加速度，其计算方法如下：
(3) 其中，τ是使用者的手作用在智能助行器的握把上，从而产生的扭矩，其大小为τ=rFs；I为使用者的转动惯量，其大小为I=mr2；Fs为3维力传感采集到的使用者对智能助行器握把的Y方向的作用力的大小，m为智能助行器的质量。如图 5所示，以智能助行器坐标(Xr，Yr)为圆心，r为该圆的半径，r的大小为r=d+Xr，d为2维激光雷达到3维力传感器的X方向上的距离。
使用者下肢意图角速度的计算方法为
(4) 其中，t为现在的时刻；t*为上一采样时间；Δt为间隔时间，Δt=t-t*。使用者在0时刻的角速度ω0等于0，即ω0=0。
如图 6所示，θL为激光雷达得到的用户意图转角。
用户的意图转角的计算方法如式(5)所示：
(5) 将其进行微分，得到用户相对意图角速度ωL(t)：
(6) 用户的运动可以用下面的状态转移矩阵表示：
(7) 其中，w为增量矩阵，记录用户状态变量的变化值，其定义如为w =[ΔVleg ΔωF ΔωL]T。
ΔVleg为用户线速度的变化值，△ωF、△ωL分别为3维力传感器和2维激光雷达测到的用户角速度的变化值，本文将3种变化值视为高斯噪声。系统的观测方程为
(8) ε为观测误差向量，ε=[εleg εF εL]T，εleg为距离测量噪声，εF为角速度测量噪声，εL为速度测量噪声。
系统的方差矩阵为
σV2(t)为用户线速度估计的方差，σωf2(t)为由3维力传感估计的用户意图角速度的方差，σωl2(t)为由2维激光雷达估计的用户意图角速度的方差。
v为过程噪声矩阵：v=[σv2 σaf2 σal2]T，σv2是速度变化的方差，σaf2是角加速度变化的方差，σal2是角加速度变化的方差。接下来进入卡尔曼滤波循环，在t时刻执行预测，然后在t+1时刻进行测量更新：
1) 预测：
系统的状态转移模型为
(9) 
(10) 其中，
2) 测量更新：
(11) K为卡尔曼增益矩阵：
(12) 
(13) I为单位矩阵。反复进行预测和测量更新步骤，以持续估计用户意图。
图 7和图 8呈现了原始速度和角速度与经卡尔曼滤波处理后的速度和角速度之间的对比。可以明显观察到，在原始信号中存在明显的跳变，而经过卡尔曼滤波后，速度和角速度的信号变得相对平滑。这种滤波效果有助于提高用户运动意图信息的稳定性和一致性，为后续工作提供了更可靠的基础。通过这一步骤的优化，本文有效降低了信号的噪音水平，为进一步的分析和研究奠定了更为可靠的数据基础。
2.5 用户意图线速度估计
XU等[20]对用户线速度的估计，需要使用用户双腿的全局坐标，不可避免地引入了里程计误差对用户线速度估计的影响。本文通过使用腿部距离的变化来计算线速度的大小，只需使用用户与助行器的相对位置信息，成功地避免了助行器里程计引入的误差，实现了更准确的用户意图线速度估计。
在使用2维激光雷达得到用户腿部在X方向的距离后，对其进行微分，并对结果取绝对值，得到用户的线速度Vmtemp(t)，计算方法如式(14)所示：
(14) 使用腿部X方向距离变换的速率来确定用户线速度虽然可以得到更准确的线速度大小，但是并不能对用户的线速度方向进行估计，所以需要引入文[20]对用户线速度的估计Vglobal(t)来提供用户线速度的方向。
综上，对用户线速度的估计为
(15) 2.6 用户意图角速度估计
在脑卒中患者中，下肢行动困难是常见的问题。若仅使用3维力传感器对其意图角速度进行估计，可能导致错误的反馈。以用户向左摔倒为例，此时用户相对于助行器会产生向左的力。若仅依赖3维力传感器进行估计，助行器可能会错误地向左运动，而事实上用户并没有向左的运动意图，从而引发潜在危险。
因此，为解决此问题，本文提出了用于估计用户意图角速度的多传感器融合估计算法。
由于用户的行走具有一定的不平衡性，即在行走过程中，用户Y方向的坐标容易出现偏移，会使估计结果不准确。为了降低行走中小幅度偏移对传感器对用户意图角速度估计的影响，并确保快速旋转运动被及时识别，本文引入了权重函数g(x)：
(16) 该权重函数可以根据传感器数据的大小来调整其对角速度估计影响的大小，从而更好地平衡精确性和实时性。通过权重函数，可以降低行走过程中的微小变化对用户意图角速度估计的影响，同时在用户进行快速旋转运动时保持敏感度。
在2.4节中，对这2个传感器估计出的用户意图角速度进行了分别的滤波，分别得到对用户角速度的估计ωF、ωL。通过3维力传感器和2维激光雷达对用户意图角速度估计得到的方差分别为σωf2、σωl2，它们被用于计算权重系数，通过方差和权重函数的结合，得到了权重系数，进而利用这一系数来融合3维力传感器和2维激光雷达对用户意图角速度的估计，具体的传感器融合方式为
当g(ωF)≠0且g(ωL)≠0时：
(17) 其中，η为权重系数，对用户角速度的估计为
(18) 为了避免奇异解，当g(ωF)=0且g(ωL)=0时：
(19) 通过第3节中的实验结果可以表明，相较于仅使用力传感器进行的估计，本文提出的融合方法在准确性和安全性方面均表现出色，可用于对用户运动意图的估计。
2.7 Informer预测
为了解决由于滤波引入的滞后性问题，本研究采用了Informer模型，旨在提高对未来用户运动意图角速度和线速度的估计的准确性和实时性。Informer模型是一种强大的神经网络模型，特别设计用于时间序列数据的处理，它利用自注意力机制，能够自适应地捕获时间序列数据中的依赖关系，并处理多维特征序列。它的编码器-解码器结构允许同时考虑历史观测和未来预测，从而提高了时间序列数据的建模和预测能力。
在本研究中，本文进行了有针对性的超参数调整，主要集中在学习率和模型训练轮数上。具体设置如下：
·学习率：本文尝试了不同的学习率值，包括0.000 1、0.001、0.01和0.1，以找到最佳的学习率设置。最终，本文选择了学习率为0.001，因为它在训练过程中表现出最好的性能。
·训练轮数：本文进行了多次实验，分别读取了20、50、100、200和300轮训练的结果。结果显示，模型在经过20轮训练后收敛，后续增加的训练轮数对性能没有显著提升。
为了保证稳定性，将Informer优化后的参数进行一阶滤波，输出最终的速度估计：
(20) 其中，Vi(t)、ωi(t)为Informer预测出t时刻的用户意图速度与角速度，α=0.3，V*(t)、ω*(t)为对用户意图行走速度的最终估计。
2.8 用户运动意图
用户运动意图方向及其意图行走速度被称为用户运动意图。本文将t时刻对用户运动意图的2维估计表示为
(21) 其中，V*(t)和ω*(t)为对用户的意图行走速度估计，其计算方法已由前文给出。θ(t)为用户运动意图方向，其计算方法为
(22) 其中，mod表示取余，θ(0)=0，即每次启动时的方向作为基准方向，通过对用户行走角速度进行积分，得到用户运动意图方向。
3. 实验结果及讨论
3.1 实验设备介绍
按图 1搭建实验平台，搭建好的效果如图 9所示，用笔记本处理雷达消息，并将速度信息发回给智能助行器驱动左右轮。实验使用的雷达型号为ydlidar G4，扫描频率为10 Hz；扫描范围为360°。3维力传感器型号为LZ-SW66/27，量程X、Y轴为100 kg；Z轴为200 kg。
3.2 用户运动意图估计实验
为验证本文提出的用户运动估计算法的卓越性能，本节将其与使用全局坐标进行的用户意图线速度估计以及仅使用力传感进行的用户意图角速度估计进行比较。在图 10(a)中，本文将使用全局坐标获得的用户线速度估计方法标记为方法1，而将本文提出的用户线速度估计方法标记为方法2。对方法1和方法2得到的用户线速度估计值进行积分，得到最终的行走距离。
![]()
图 10 用户运动意图估计准确度实验Fig. 10. Experiment on the accuracy of human motion intention estimation通过进行同一试验者行走3 m的重复测试5次，本文通过式(23)计算准确度来评估两种方法。
(23) 测试结果如表 1所示。实验结果表明，相较于方法1，本文提出的方法在准确度上提高了10%。
表 1 线速度估计准确度实验Tab. 1. Linear velocity estimation accuracy experiment序号 方法1估计的使用者行走距离/m 准确度/% 方法2估计的使用者行走距离/m 准确度/% 1 3.3 90 3.06 98 2 2.92 97.4 3.02 99.3 3 3.30 90 2.96 98.6 4 3.85 71.6 3.05 98.3 5 3.21 93 3.06 98 平均值 3.316 88.4 3.03 98.4 在图 10(b)中，对使用力传感估计得到的用户角速度和使用本文提出的多传感器融合估计得到的用户角速度进行积分，得到最终的旋转角度。通过对同一试验者进行向左旋转90°的重复测试5次，测试结果如表格所示，本文通过式(24)计算准确度来评估两种方法。
(24) 如表 2所示，相较于仅使用力传感的方法，本文提出的方法在准确度上提高了14.1%。
表 2 角速度估计准确度实验Tab. 2. Angular velocity estimation accuracy experiment序号 力传感估计得到的使用者旋转角度/rad 准确度/% 多传感器融合估计得到的使用者旋转角度/rad 准确度/% 1 -1.82 84.1 -1.58 99.4 2 -1.83 83.5 -1.52 96.8 3 -1.82 84.1 -1.55 98.7 4 -1.96 75.2 -1.74 89.2 5 -1.80 85.4 -1.59 98.8 平均值 -1.85 82.5 -1.60 96.6 为进行助行器的安全验证，本文让同一试验者用手向左推助行器，但腿部不动，模拟用户向左摔倒的情况。图 10(c)展示了在此情况下估计出的用户意图角速度的积分。通过多次重复实验并计算平均误差，结果见表 3所示。相较于只使用3维力传感的估计，本文提出的算法显示出更小的平均误差，从而说明本文的方法在提高安全性方面更为可靠。
表 3 安全性实验Tab. 3. Safety experiments单位: rad 序号 力传感估计用户角速度的累计误差 多传感器融合估计用户角速度的累计误差 1 -0.33 -0.03 2 -0.23 -0.01 3 -0.16 -0.04 4 -0.47 -0.03 5 -0.36 -0.01 平均误差 -0.31 -0.02 为了更直观地体现出本文的多传感器融合算法对用户角速度估计的准确性，本文进行了用户运动轨迹估计实验。如图 11所示，本研究让实验者沿着地砖接缝进行行走，形成一个倒“L”形路线。
在采集到相关数据后，分别使用不同的方法输出用户运动轨迹估计进行对比，本文使用式(25)输出用户运动轨迹：
(25) 其中，线速度v由使用本文提出的用户意图线速度估计方法得到。在分别使用3维力传感器和本文提出的多传感器融合估计算法估计用户意图角速度后，通过式(22)分别得到用户意图方向θ。最终输出的用户运动轨迹如图 12、图 13所示。
![]()
图 12 仅用3维力传感器的用户运动轨迹估计Fig. 12. User motion trajectory estimation using only 3D force sensors![]()
图 13 多传感器融合方法用户运动轨迹估计Fig. 13. User motion trajectory estimation using a multi-sensor fusion approach此外，在使用助行器时，用户无法实现直角转弯。因此，可以通过起点和终点位置的变化来判断角速度估计的准确性。通过对比图 12、图 13，可以发现，相较于仅使用力传感器进行的运动估计，本文提出的多传感器融合运动意图方法能够更准确地估计用户的角速度，从而获得更精准的用户运动轨迹。
3.3 Informer性能实验
在本文的实验中，Informer模型表现出了卓越的性能，图 14和图 15分别展示了速度预测和角速度预测的最终效果图。本文使用MAE(mean absolute error)、MSE(mean squared error)、RMSE(root mean squared error)作为模型的评估标准：MAE衡量了模型的平均绝对误差，较低的MAE表示模型的平均误差相对较小；MSE表示均方误差，较低的MSE表示模型的误差分布相对紧凑；RMSE是MSE的平方根，给出了与实际值的均方误差的相对大小，较低的RMSE表示模型的误差相对较小。
线速度预测结果：
MAE：0.076 174 82；
MSE：0.013 809 74；
RMSE：0.117 514 83。
角速度预测结果：
MAE：0.097 954 683；
MSE：0.017 413 788；
RMSE：0.131 961 32。
在本文的模型应用中，两组预测结果所获得的MAE、MSE及RMSE的数值均呈现出相对较低的水平。这意味着本文的模型预测结果与实际观测值之间的误差很小。这一结果明确地表明，在本研究中，Informer模型展现出了令人满意的性能。
3.4 使用舒适度实验
为了验证本文的用户运动意图估计算法的有效性，本文将对用户的相对运动意图估计发送给智能助行器，将其转换为智能助行器的左右轮速度，实现对使用者的实时跟随，智能助行器可以通过跟随使用者，为使用者提供适当的支撑力，从而减少腿部用力，所以可以通过跟随的效果体现出意图估计算法的效果。将使用本文提出的用户相对运动意图估计的方法称为“多传感器融合方法”，将Informer优化用户相对运动意图估计的方法称为“Informer优化方法”，将使用全局坐标估计用户线速度、仅用3维力传感器估计用户角速度的方法称为“传统控制方法”。
在表面肌电信号(surface ElectroMyoGraphy，sEMG)信号分析中，通常使用从时域、频域和时频域提取特征作为分析指标。时域特征，如均方根(root mean square，RMS)、平均肌电值(average rectified value，ARV)、或者平均绝对值(mean absolute value，MAV)等，常用于测量肌纤维收缩强度，以及检测肌肉激活的开始和结束时间[24]。本文采集了大腿外侧肌肉(vastus lateralis obliquus，VLO)和大腿内侧肌肉(vastus medialis obliquus，VMO)的肌电信号，其信号图分别如图 16、图 17所示，通过均方根(RMS)来体现腿部的发力情况。
由于老年人的行动能力有限，在研发阶段进行实验会出现不可预料的风险，本文找了3个受试者，年龄在22~24岁，已获得书面知情同意书。为了模拟行动不便人群使用智能助行器，本文的实验中，只用右腿发力行走。
本文将使用3名受试者使用智能助行器，分别使用传统控制方法、多传感器融合方法、Informer优化方法进行实验，在实验室的同一区域沿着同一口字形路线进行行走10 m。观察右腿的肌电信号，通过分析右腿的发力情况，从而得到智能助行器的助力效果。
通过表 4，通过VLO和VMO的肌电信号的均方根分析腿部的发力情况，实验表明，3名受试者在使用本文提出的方法的时候，右腿的发力均体现出不同程度的减小的情况，并且Informer模型的引入使用户能够更轻松地使用助行器，这表明本文提出的方法有效地减轻了下肢肌肉的负担。
表 4 肌电信号指标分析Tab. 4. Analysis of electromyographic signal indexes单位: μV 受试者编号 肌电信号 传统控制 多传感器融合 Informer优化 1 VMO 65.19 36.23 35.82 VLO 40.80 23.94 13.47 2 VMO 32.34 27.18 26.35 VLO 30.85 25.63 24.58 3 VMO 26.82 20.85 19.38 VLO 27.71 27.13 24.65 4. 结论
为了更加准确地估计用户运动意图，本文首先对3维力传感器和2维激光雷达采集到地数据进行处理，本文通过用户腿部相对位置信息估计用户意图线速度，通过融合3维力传感器和2维激光雷达对用户角速度的估计得到最终的用户意图角速度估计。并且引入Informer模型改善数据处理造成的滞后问题。
相较于使用全局坐标的用户意图线速度估计方法，本文提出的用户意图线速度估计方法的准确率提高了10%。相对于仅使用3维力传感器的用户意图角速度估计方法，本文提出的方法对用户意图角速度的估计准确度提高了14.1%。此外，与仅使用3维力传感器进行角速度估计的方法相比，本文的方法具有更好的安全性。
由于临床医学实验的复杂性，本文所提出的方法尚未在脑卒中患者的步行康复训练中进行应用。在未来的工作中，本文计划与医院合作，展开相关的应用研究。
-
![]()
图 3 相对测量边数、位姿点数、噪声分别对算法运行时间的影响
Figure 3. The influence of relative measurement edge number, pose number and noise on algorithm running time is discussed
![]()
图 4 SPT算法、RS算法、ORDM算法对Sphere_a的轨迹优化对比
Figure 4. Comparison of Sphere_a trajectory optimization by SPT, RS and ORDM algorithm
![]()
图 5 SPT算法、RS算法、ORDM算法对Torus的轨迹优化对比
Figure 5. Comparison of Torus trajectory optimization by SPT, RS and ORDM algorithm
![]()
图 6 SPT算法、RS算法、ORDM算法对Garage的轨迹优化对比
Figure 6. Comparison of Garage trajectory optimization by SPT, RS and ORDM algorithm
![]()
图 7 SPT算法、RS算法、ORDM算法对Cubicle的轨迹优化对比
Figure 7. Comparison of Cubicle trajectory optimization by SPT, RS and ORDM algorithm
算法1：ORDM：Optimize Rotation with The Deviation Matrix 输入： 
输出： ti，Ri，∀i∈{1，2，…，n}，f 1：For all ek∈E，k∈{1，2，…，m} let 
End for 2：H=[H1，H2，…，Hn] 3：Q=[Q1，Q2，…，Qm] 4：Ψ=(HTΩH)-1HTΩQ 5：For all i∈{1，2，…，n} do 
End for 6：Ri，i∈{1，2，…，n} 7：t=(ATΛA)-1ATΛD，根据式(12)计算ti←第i行of t 8：将Ri，ti，i∈{1，2，…，n}代入式(3)，计算f 9：return Ri，ti，f 
下载: 导出CSV
表 1 数据集参数
Table 1 Dataset parameter
数据集 Mean(θnoise) Std(θnoise) m n Shpere_a 0.270 7 0.116 8 647 2 200 Torus 0.053 9 0.023 9 047 5 000 Garage 0.001 5 0.001 1 6 275 1 661 Cubicle 0.006 4 0.014 8 16 869 5 750
下载: 导出CSV
表 2 在噪声影响下，各算法的优化函数值
Table 2 The optimization function value of each algorithm under the influence of noise
噪声标准差/rad 方法 Sphere_a(f) Torus(f) Garage(f) Cubicle(f) Chordal 3.866×106 7.736×104 2.62×103 2.300×104 0.1 ORDM+T 3.854×106 7.678×104 2.22×103 2.246×104 ORDM+C 3.852×106 7.691×104 2.43×103 2.245×104 Chordal 6.250×106 2.501×105 9.23×103 9.981×104 0.2 ORDM+T 6.223×106 2.464×105 8.54×103 9.826×104 ORDM+C 6.219×106 2.454×105 8.59×103 9.810×104 Chordal 1.031×107 5.974×105 1.70×104 2.576×105 0.3 ORDM+T 1.026×107 5.853×105 1.56×104 2.553×105 ORDM+C 1.025×107 5.853×105 1.63×104 2.516×105
下载: 导出CSV
表 3 在SPT初始化下ORDM与RS算法比较
Table 3 Comparison of ORDM and RS algorithms under SPT initialization
数据集 SPT ORDM RS f f t/s f t/s Shpere_a 6.818×107 3.068×106 0.423 5.345×106 1.421 Torus 9.514×106 2.466×104 0.474 3.956×105 2.014 Garage 2.738×104 1.415 0.211 1.066×104 0.407 Cubicle 4.022×105 8.354×102 1.201 7.962×104 1.652 注：加黑数据表示算法最优
下载: 导出CSV
-
[1] 刘浩敏, 章国锋, 鲍虎军. 基于单目视觉的同时定位与地图构建方法综述[J]. 计算机辅助设计与图形学学报, 2016, 28(6): 855-868. doi: 10.3969/j.issn.1003-9775.2016.06.001 LIU H M, ZHANG G F, BAO H J. A survey of monocular simultaneous localization and mapping[J]. Journal of Computer-Aided Design & Computer Graphics, 2016, 28(6): 855-868. doi: 10.3969/j.issn.1003-9775.2016.06.001
[2] BRIALES J, GONZALEZ-JIMENEZ J. Initialization of 3D pose graph optimization using lagrangian duality[C]//2017 IEEE International Conference on Robotics and Automation. Piscataway, USA: IEEE, 2017: 5134-5139.
[3] GRISETTI G, KÜMMERLE R, STACHNISS C, et al. A tutorial on graph-based SLAM[J]. IEEE Intelligent Transportation Systems Magazine, 2010, 2(4): 31-43. doi: 10.1109/MITS.2010.939925
[4] ILA V, POLOK L, SOLONY M, et al. SLAM++-A highly efficient and temporally scalable incremental SLAM framework[J]. The International Journal of Robotics Research, 2017, 36(2): 210-230. doi: 10.1177/0278364917691110
[5] ALOISE I, GRISETTI G. Chordal based error function for 3-D pose-graph optimization[J]. IEEE Robotics and Automation Letters, 2019, 5(1): 274-281.
[6] KÜMMERLE R, GRISETTI G, STRASDAT H, et al. g2o: A general framework for graph optimization[C]//2011 IEEE International Conference on Robotics and Automation. Piscataway, USA: IEEE, 2011: 3607-3613.
[7] 郝亚东, 张奇志, 周亚丽. 基于高斯牛顿的局部优化SLAM系统[J]. 传感器世界, 2018, 24(3): 7-11. https://www.cnki.com.cn/Article/CJFDTOTAL-CGSJ201803002.htm HAO Y D, ZHANG Q Z, ZHOU Y L. Local optimization SLAM system based on Gauss-Newton method[J]. Sensor World, 2018, 24(3): 7-11. https://www.cnki.com.cn/Article/CJFDTOTAL-CGSJ201803002.htm
[8] DOU X, HUO Y, LIU Y, et al. An unmanned aerial vehicle pose estimation system based on SLAM[C/OL]//IOP Conference Series: Materials Science and Engineering. London, UK: IOP Publishing, 2019[2021-10-08]. https://www.reasearchgate.net/publication/335081524. DOI: 10.1088/1757-899X/569/4/042036.
[9] 曾俊飞, 杨海清, 吴浩. 面向三维重建的自适应列文伯格-马夸尔特点云配准方法[J]. 计算机学报, 2020, 47(3): 137-142. https://www.cnki.com.cn/Article/CJFDTOTAL-JSJA202003024.htm ZENG J F, YANG H Q, WU H. Adaptive Levenberg-Marquardt cloud registration method for 3D reconstruction[J]. Computer Science, 2020, 47(3): 137-142. https://www.cnki.com.cn/Article/CJFDTOTAL-JSJA202003024.htm
[10] DELLAERT F. Factor graphs and GTSAM: A hands-on introduction[R]. Georgia, USA: Georgia Institute of Technology, 2012.
[11] KONOLIGE K, GRISETTI G, KÜMMERLE R, et al. Efficient sparse pose adjustment for 2D mapping[C]//2010 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2010: 22-29.
[12] CARLONE L, ARAGUES R, CASTELLANOS J A, et al. A fast and accurate approximation for planar pose graph optimization[J]. The International Journal of Robotics Research, 2014, 33(7): 965-987. doi: 10.1177/0278364914523689
[13] GRISETTI G, STACHNISS C, GRZONKA S, et al. A tree parameterization for efficiently computing maximum likelihood maps using gradient descent[C/OL]//Robotics: Science and Systems. 2007[2022-11-11]. https://ieeeplore.ieee.org/xpl/articleDetail.jsp?arnumber=6280136. DOI: 10.15607/RSS.2007.Ⅲ.009.
[14] HU G, KHOSOUSSI K, HUANG S. Towards a reliable SLAM back-end[C]//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2013: 37-43.
[15] GUADAGNINO T, DI GIAMMARINO L, GRISETTI G. HiPE: Hierarchical initialization for pose graphs[J]. IEEE Robotics and Automation Letters, 2021, 7(1): 287-294.
[16] HARSÁNYI K, KISS A, SZIRÁNYI T, et al. MASAT: A fast and robust algorithm for pose-graph initialization[J]. Pattern Recognition Letters, 2020, 129: 131-136. doi: 10.1016/j.patrec.2019.11.010
[17] BRIALES J, GONZALEZ-JIMENEZ J. Cartan-sync: Fast and global SE (d)-synchronization[J]. IEEE Robotics and Automation Letters, 2017, 2(4): 2127-2134. doi: 10.1109/LRA.2017.2718661
[18] PARRA A, CHNG S F, CHIN T J, et al. Rotation coordinate descent for fast globally optimal rotation averaging[C]//IEEE/CVF Conference on Computer Vision and Pattern Recognition. Piscataway, USA: IEEE, 2021: 4298-4307.
[19] MARTINEC D, PAJDLA T. Robust rotation and translation estimation in multiview reconstruction[C]//2007 IEEE Conference on Computer Vision and Pattern Recognition. Piscataway, USA: IEEE, 2007: 1-8.
[20] CARLONE L, TRON R, DANⅡLIDIS K, et al. Initialization techniques for 3D SLAM: A survey on rotation estimation and its use in pose graph optimization[C]//2015 IEEE International Conference on Robotics and Automation. Piscataway, USA: IEEE, 2015: 4597-4604.
[21] NASIRI S M, MORADI H, HOSSEINI R. A linear least square initialization method for 3d pose graph optimization problem[C]// 2018 IEEE International Conference on Robotics and Automation. Piscataway, USA: IEEE, 2018: 2474-2479.
[22] NASIRI S M, HOSSEINI R, MORADI H. Novel parameterization for Gauss-Newton methods in 3-D pose graph optimization[J]. IEEE Transactions on Robotics, 2020, 37(3): 780-797.
[23] BRIALES J, GONZALEZ-JIMENEZ J. Fast global optimality verification in 3D SLAM[C]//2016 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2016: 4630-4636.
[24] ROSEN D M, CARLONE L, BANDEIRA A S, et al. SE-Sync: A certifiably correct algorithm for synchronization over the special Euclidean group[J]. The International Journal of Robotics Research, 2019, 38(2/3): 95-125.
[25] SORKINE-HORNUNG O, RABINOVICH M. Least-squares rigid motion using svd[J]. Computing, 2017, 1(1): 1-5.
计量
- 文章访问数: 37
- HTML全文浏览量: 57
- PDF下载量: 15
