CN114047766A - 面向室内外场景长期应用的移动机器人数据采集系统及方法 - Google Patents

Abstract

本发明提供了一种面向室内外场景长期应用的移动机器人数据采集方法及系统，包括：步骤S1：利用联合标定算法计算异构传感器之间的位姿变化关系，实现多传感器的标定；步骤S2：通过标定后的传感器采集环境信息数据；步骤S3：使用多轨迹融合算法将不同场景的轨迹真值进行融合得到全场景真值，全场景真值是获取的环境信息数据对照的定位基准。本发明具有精度高、成本低的特点，可用于移动机器人的位姿跟踪、环境建模等研究。

Description

面向室内外场景长期应用的移动机器人数据采集系统及方法

技术领域

本发明涉及机器人数据采集平台领域，具体地，涉及面向室内外场景长期应用的移动机器人数据采集系统及方法，更为具体地，涉及一种机器人长期、室内外多场景数据采集平台，包括其中的传感器标定算法与真值获取方案。

背景技术

移动机器人通过传感器对外界进行感知，进而完成自身的定位、对环境的建图，并最终实现上层的路径规划、自主导航等任务。事实上，移动机器人进行实测的难度与成本相对较高，大多数研究均是基于数据集与仿真平台进行的。为了提供符合真实环境的测试平台。如何尽可能地提供全面、准确的传感器数据：传感器数据与外参标定信息，以及环境与自身定位的真实值作为研究参考是数据集采集研究的重点。

本发明首先提出了一个支持长期、室内外采集的多传感器采集平台，并针对长期数据采集过程中的两大难点：高精度标定算法和全场景定位真值，设计了一种异构传感器在线联合标定算法和定位真值获取系统。其中，在定位真值研究中，本发明重点考虑了室内环境的特点，并结合室外高精度实时动态全球定位系统，设计了一种低成本的高精度室内外真值系统；并在此基础上，设计了一种基于定位能力的多轨迹真值融合算法，提升了全场景真值的鲁棒性。

目前大多数多传感器的标定方案是基于特殊标定板[1-3]，在不受干扰的环境中实现两两离线标定。然而，这种标定方法需要在相对独立的环境进行标定。此外，在现实生活中，尤其是机器人的长期数据采集以及工作场景中，由于传感器的退化、运动过程中的震动、结构形变等因素，传感器之间的相对位姿也会发生动态变化，而这些问题都会造成传统基于离线方案的标定失真，从而无法提供准确的定位、建图、导航等服务。本发明提出了一种可以运行在实时采集系统、支持动态调整的外参估计算法。该算法通过对整个标定过程建立状态空间模型，将传感器外参作为参数向量，然后使用PaRIS(Particle-based RapidIncremental Smoother，PaRIS)算法实现在线估计与调整。

室外环境通常使用高精度实时动态全球定位系统(Real-Time Kinematic GlobalPositioning System,RTK-GPS)作为定位真值[4,5]，而针对室内丢失RTK-GPS信息的场景，研究人员通常选用高精度三维激光传感器作为定位真值[5]。但考虑到本发明所涉及的数据集存在高人流的室内场景，传统的激光定位方案会受到人群的遮挡、造成定位丢失，本发明提出了一种倾斜视角的真值采集方案。该方案通过将二维激光测距传感器呈一定倾斜角摆放，使其能够观测相对稳定的天花板和地面信息，降低了动态干扰，提高了传统离散地图的建图、定位精度。此外，本发明还通过对整个地图进行连续高斯过程建模，避免了栅格之间的独立性假设[6]，有效地克服了量化误差，能更精确地还原地图。最后，与传统基于蒙特卡洛的定位算法[7-9]不同，当环境较大时需要较高的粒子数目，难以达到实时。本发明基于容积卡尔曼滤波，做到固定时间的状态估计，在保持较高精度的同时大幅提升了算法实时性。

多轨迹融合作为数据集真值是业界普遍做法[5]，与传统方案相比[10,11]，本发明通过引入定位能力指标[12]，将轨迹生成分为：高精度RTK-GPS轨迹和多传感器融合轨迹。并采用自监督的方案训练对齐位姿点数，实现高精度、高鲁棒的真值定位融合。

基于前人的工作，本发针对一种长期、室内外多场景数据采集平台中的标定与真值获取任务，提出了(1)一种异构传感器在线联合标定算法；(2)一种低成本的高精度室内外真值系统；(3)一种基于定位能力的多轨迹真值融合算法。

[1].Guindel C,Beltrán J,Martín D,et al.Automatic extrinsiccalibration for lidar-stereo vehicle sensor setups[C]//2017IEEE 20thinternational conference on intelligent transportation systems(ITSC).IEEE,2017:1-6.

[2].Huang W,Liu H.Online initialization and automatic camera-IMUextrinsic calibration for monocular visual-inertial SLAM[C]//2018 IEEEInternational Conference on Robotics and Automation(ICRA).IEEE,2018:5182-5189.

[3].Kühner T,Kümmerle J.Extrinsic multi sensor calibration underuncertainties[C]//2019 IEEE Intelligent Transportation Systems Conference(ITSC).IEEE,2019:3921-3927.

[4].Geiger A,Lenz P,Stiller C,et al.Vision meets robotics:The kittidataset[J].The International Journal of Robotics Research,2013,32(11):1231-1237.

[5].Carlevaris-Bianco N,Ushani A K,Eustice R M.University of MichiganNorth Campus long-term vision and lidar dataset[J].The International Journalof Robotics Research,2016,35(9):1023-1035.

[6].Hess W,Kohler D,Rapp H,et al.Real-time loop closure in 2D LIDARSLAM[C]//2016 IEEE International Conference on Robotics and Automation(ICRA).IEEE,2016:1271-1278.

[7].Peng G,Zheng W,Lu Z,et al.An improved AMCL algorithm based onlaser scanning match in a complex and unstructured environment[J].Complexity,2018,2018.

[8].Dellaert F,Fox D,Burgard W,et al.Monte carlo localization formobile robots[C]//Proceedings 1999 IEEE International Conference on Roboticsand Automation(Cat.No.99CH36288C).IEEE,1999,2:1322-1328.

[9].Thrun S,Fox D,Burgard W,et al.Robust Monte Carlo localization formobile robots[J].Artificial intelligence,2001,128(1-2):99-141.

[10].Qin T,Li P,Shen S.Vins-mono:A robust and versatile monocularvisual-inertial state estimator[J].IEEE Transactions on Robotics,2018,34(4):1004-1020.

[11].Li M,Mourikis A I.High-precision,consistent EKF-based visual-inertial odometry[J].The International Journal of Robotics Research,2013,32(6):690-711.

[12].Behnam Irani,Jingchuan Wang and Weidong Chen,“A localizabilityconstraint-based path planning method for autonomous vehicles,”IEEE Trans.onIntelligent Transportation Systems,20(7):2593-2604,2019.

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种面向室内外场景长期应用的移动机器人数据采集方法及系统。

根据本发明提供的一种面向室内外场景长期应用的移动机器人数据采集方法，包括：

步骤S1：利用联合标定算法计算异构传感器之间的位姿变化关系，实现多传感器的标定；

步骤S2：通过标定后的传感器采集环境信息数据；

步骤S3：使用多轨迹融合算法将不同场景的轨迹真值进行融合得到全场景真值，全场景真值是获取的环境信息数据对照的定位基准。

优选地，所述步骤S1采用：分析多传感器联合标定过程中的时空变化关系，进行联合建模，采用前向传播与反向平滑相结合的算法实现多传感器的高精度标定。

优选地，所述步骤S1采用：

步骤S1.1：定义

和

为可测空间；

步骤S1.2：对机器人在t∈[0,T]的位姿

与机器人观测模型y_t建立6自由度位姿的马尔可夫模型，x_t和y_t分别为马尔可夫模型中的状态变量和观测变量，初始状态转移分布为p(x|x_t-1,u_t)，为了简化步骤，忽略外部输入u_t；考虑到机器人位姿与观测模型y_t的关系，认为观测变量

与状态变量x_t满足条件独立性假设，定义观测模型为p_θ(y_t|x_t),其中θ为标定参数向量；假设x_t满足q_t-1分布，y_t满足g_t:θ分布，完整概率模型构建如下：

x_t～q_t-1 (1)

y_t～g_t:θ (2)

其中，定义

分别表示在s≤t≤T的状态变量和观测；x_t表示t时刻机器人的6自由度位姿；q_t-1表示状态转移分布p(x|x_t-1,u_t)所对应的概率密度；g_t；θ表示观测模型p_θ(y_t|x_t)所对应的概率密度；x_s表示s时刻机器人的6自由度位姿；y_s表示s时刻观测变量；u_t表示外部输入；

步骤S1.3：通过正切滤波求取预测分布π_t；θ以及梯度

基于改进粒子滤波的方法实现对标定外参θ估计；

其中，π_t；θ表示对x_t+1的预测分布，η_t；θ为π_t；θ的梯度；

表示微分算子；

步骤S1.4：定义对于任意s≤s'，x_s:s'在y_0:t下的条件分布为φ_s:s'|t；θ＝p_θ(x_s:s'|y_0:t)；

使用缩写φ_t:θ表示滤波分布φ_s:s'|t；θ；基于滤波递归，对于t∈N和任意定义在x上的函数

则

π_t+1；θf＝φ_t；θQ_θf (4)

其中，N表示自然数；φ_t；θ为t时刻，参数为θ的滤波分布，π_t+1；θ为t+1时刻，参数为θ的预测分布，Q_θ为转移分布P(x_t|x_t-1)；

在反向平滑过程中，定义反向核

T_t；θ＝p_θ(x_0:t-1|x_t,y_0:t)，联合平滑分布φ_0:t|t；θ表示为：

φ_0:t|t；θ＝φ_t；θT_t；θ (5)

其中，T_t；θ表示已知0到t时刻的观测y_0:t和t时刻的位姿x_t情况下，0到t-1时刻的状态x_0:t-1的联合概率分布；

定义需优化的目标函数满足

其中

其中，log(g_s；θ(x_s))表示为g_s；θ(x_s)的对数；

对于T_t；θh_t通过下式递归计算：

其中，h_t是一个加性函数，

步骤S1.5：在传统粒子滤波的基础上，考虑了正向传播以及反向平滑问题，设计

为带权重的粒子集；在滤波过程中，粒子通过运动模型进行正向传播，并动态分配观测权重：

其中，φ_t；θ通过

求得，其中

而q_t和g_t+1分别为之前所得的态转移分布的密度函数和观测模型的概率密度函数；

表示为t时刻粒子集合中的第i个粒子，

表示为它的权重，

表示t+1时刻第i个粒子的索引，

后向核

的估计由下式确定：

其中，

表示表示为s时刻第i个粒子的权重，

表示为t时刻粒子集合中的第j个粒子，

表示为t时刻粒子集合中的第l个粒子，

表示它所对应的权重，

根据PaRIS算法，

的粒子估计为：

其中，

为反向索引采样数，它远远小于权重粒子集的大小

为t+1时刻，第i个粒子的第j个反向索引；

针对外参θ_t，使用RML算法对参数θ估计，使其满足

其中

其中，p(x₀)表示初始状态概率，p(y₀|x₀)表示初始状态概率下的观测概率，p(y_l+1|x_l+1)表示l+1时刻状态概率下的观测概率，p(x_l+1|x_l)表示l时刻到l+1时刻的状态转移概率，

定义对数似然函数l_θ(y_0:t)＝logL_θ(y_0:t)，采用Robbins-Monro算法求取近似：

其中，γ_t表示为学习率，

其中，

分解为：

优选地，所述步骤S2采用：通过标定后的外部传感器和标定后的内部传感器采集各种环境信息的数据；

所述外部传感器包括三维激光传感器、二维激光测距传感器、可见光摄像机以及深度摄像机；

所述内部传感器包括里程计以及IMU。

优选地，所述步骤S3采用：通过不同场景下的环境特征分析建立不同场景真值系统；所述不同场景真值系统包括室外轨迹真值系统和室内轨迹真值系统；

所述室外轨迹真值系统是由RTK-GPS提供的；

所述室内轨迹真值系统是使用激光雷达算法提供的。

优选地，使用一种基于高斯过程回归地图的SLAM算法提供室内轨迹真值；

对环境进行高精度的建图；与常用概率占据栅格地图对环境进行描述不同，采用一种连续描述方式，减少了各个栅格之间是相互独立假设带来的量化误差；

首先通过八叉树存储每个被激光扫描到的占据点，获得对数级别的查询时间；然后通过高斯过程回归，依据已经有的点对空间上任意的位置的占据情况进行预测；

具体包括：连续高斯过程建图和基于容积卡尔曼滤波的前端位姿估计；

连续高斯过程建图步骤：基于空间上各点之间的占据情况是符合高斯联合分布的假设，整个地图由高斯过程描述，控制高斯过程的变量是空间位置(x,y)；对任意一个测试点预测的方式为：

f(x^*)＝N(μ,σ) (15)

其中，

其中，x^*代表的是测试点，即空间上任意一点；X代表的是训练点，即扫描到的点，y表示对应的占据情况；n是训练点的数目；σ_n是整体的方差；k为核函数，用于衡量空间上两个点之间距离的远近；I表示单位矩阵，选用RBF径向基函数作为核函数：

其中，c表示尺度因子，x和x'表示任意两个空间点，

前端位姿估计步骤：基于容积卡尔曼滤波的前端位姿估计；具体包括：前端预测建模、特征点提取、时间更新以及量测更新；

前端预测建模步骤：利用高精度、连续的地图表示方式，GPR-SLAM的前端位姿预测为：

其中，T_ζ代表位姿变换，h_k则是激光的第k个观测；

特征点提取步骤：在机器上设置一个向上的相机，用于记录天花板上的特征点；在GPR-SLAM的过程中，利用ORB算法提取每一帧上的特征点，并利用优化后的位姿，形成特征点地图，记作M_v；

时间更新步骤：包含滤波初始化、容积点计算、容积点传播；

滤波初始化：通过容积卡尔曼滤波融合激光与相机的信息达到更好的定位效果，将机器人的平面坐标记作x，滤波初始化为：

其中，E(x)表示期望，x₀表示初始估计，上标T表示矩阵的转置，P₀表示初始协方差矩阵；

容积点计算：利用Cholesky分解

计算容积点η：

其中，S_k表示信息矩阵，

表示信息矩阵的转置，η_i为第i个容积点，[1]_i为第[1]_i维上的单位向量；

容积点传播：

其中，T_k表示投影矩阵，

表示上一次迭代的估计；

T_k通过PnP算法求解，记

求解方程为：

其中，P₁,…,P_N来自特征地图，[u₁ v₁],…,[u_N v_N]是对应的像素坐标，其对应关系可以通过RANSAC算法获取；

状态量

以及误差协方差预测P_k+1|k：

其中，Q是预测步的协方差；

量测更新步骤：包含容积点计算、容积点传播、传播量测预测以及量测更新；

容积点计算：使用Cholesky将P_k+1|k分解为

则可以通过S_k+1|k与容积点η以及状态量

进行预测：

容积点传播：寻找观测上被占据概率最大的距离：

其中，

为观测模型，

是预测状态上的期望预测结果，

表示在第n条观测上被占据概率最大的距离，n为总的观测数；

传播量测预测：

观测更新：首先，计算量测误差

以及互协方差

增益更新：滤波增益K_k+1、估计

和最终协方差P_k+1:

优选地，所述多轨迹融合算法采用：

步骤S3.1：考虑到数据集的采集场景中缺失高精度RTK-GPS真值信息，采用基于定位能力分析的多轨迹融合算法获取高精度真值；

使用Fisher信息矩阵反应机器人在不同环境下所获得的定位能力，从而分析环境对后续联合标定的精度影响；针对激光传感器，其定义为如下形式：

考虑到每条激光扫面线都可以认为是互不相关的，d(p_t,z_t)表示在为位姿p_t时的定位概率密度函数，r_iE表示第i条激光扫描线的期望测量距离，σ²则为估计方差；z_t表示空间观测；上标T表示矩阵的转置；E_z表示期望；N₀表示激光束的数量；上标L表示激光雷达；

针对相机模型，定义

为第i个相机位姿时的相机信息矩阵，具体表达如下：

其中，m_i表示相机模型在第i个位姿时与地图特征点匹配的数量；J_i,j表示第i个相机模型与第j个地图点的观测Jacobian矩阵；上标T表示矩阵的转置；上标C表示相机；

步骤S3.2：在获得各自定位能力后，采用动态权重的方法调整轨迹融合时各个轨迹的可信度，从而实现一种鲁棒、高精度的定位真值；

当RTK-GPS处于高精度固定解时，使用RTK-GPS作为真值位姿；

当RTK-GPS处于差分浮点解时，首先选择差分浮点解两端一定数量高精度固定解作为对齐点，然后采用绝对旋转的封闭解法求取旋转矩阵、平移矩阵以及尺度因子；通过旋转、平移和缩放激光传感器、相机、惯性传感器融合算法的轨迹替换RTK-GPS浮点解，实现真值轨迹融合；

当RTK-GPS处于其他状态时，仅使用激光传感器、相机、惯性传感器融合定位位姿作为定位真值；

对齐点数量选取，我们采用基于自监督学习的方式获取；

训练数据准备：首先在无遮挡环境或者有额外高精度追踪设备辅助的情况下获取高精度轨迹真值，然后设计随机掩网遮挡其中不定长度轨迹；

模型输入与输出定义：输入为各个传感器的可信度、RTK-GPS状态S_GPS，以及掩网长度N：

输出则为前后两端所估计的对齐点数[P_start P_end]；

其中，P_start表示起始端参与对齐的位姿数，P_end表示结束端参与对齐的位姿数；

采用全连接的深度网络作为训练模型，损失函数采用平均距离误差：

其中，L表示距离误差，x_i表示被遮盖前的真实位姿，

表示对齐后所估计的位姿。

根据本发明提供的一种面向室内外场景长期应用的移动机器人数据采集系统，包括：

模块M1：利用联合标定算法计算异构传感器之间的位姿变化关系，实现多传感器的标定；

模块M2：通过标定后的传感器采集环境信息数据；

模块M3：使用多轨迹融合算法将不同场景的轨迹真值进行融合得到全场景真值，全场景真值是获取的环境信息数据对照的定位基准。

优选地，所述模块M1采用：分析多传感器联合标定过程中的时空变化关系，进行联合建模，采用前向传播与反向平滑相结合的算法实现多传感器的高精度标定；

所述模块M1采用：

模块M1.1：定义

和

为可测空间；

模块M1.2：对机器人在t∈[0,T]的位姿

与机器人观测模型y_t建立6自由度位姿的马尔可夫模型，x_t和y_t分别为马尔可夫模型中的状态变量和观测变量，初始状态转移分布为p(x|x_t-1,u_t)，为了简化步骤，忽略外部输入u_t；考虑到机器人位姿与观测模型y_t的关系，认为观测变量

与状态变量x_t满足条件独立性假设，定义观测模型为p_θ(y_t|x_t),其中θ为标定参数向量；假设x_t满足q_t-1分布，y_t满足g_t:θ分布，完整概率模型构建如下：

x_t～q_t-1 (1)

y_t～g_t:θ (2)

其中，定义

分别表示在s≤t≤T的状态变量和观测；x_t表示t时刻机器人的6自由度位姿；q_t-1表示状态转移分布p(x|x_t-1,u_t)所对应的概率密度；g_t；θ表示观测模型p_θ(y_t|x_t)所对应的概率密度；x_s表示s时刻机器人的6自由度位姿；y_s表示s时刻观测变量；u_t表示外部输入；

模块M1.3：通过正切滤波求取预测分布π_t；θ以及梯度

基于改进粒子滤波的方法实现对标定外参θ估计；

其中，π_t；θ表示对x_t+1的预测分布，η_t；θ为π_t；θ的梯度；

表示微分算子；

模块M1.4：定义对于任意s≤s'，x_s:s'在y_0:t下的条件分布为φ_s:s'|t；θ＝p_θ(x_s:s'|y_0:t)；

使用缩写φ_t:θ表示滤波分布φ_s:s'|t；θ；基于滤波递归，对于t∈N和任意定义在x上的函数

则

π_t+1；θf＝φ_t；θQ_θf (4)

其中，N表示自然数；φ_t；θ为t时刻，参数为θ的滤波分布，π_t+1；θ为t+1时刻，参数为θ的预测分布，Q_θ为转移分布P(x_t|x_t-1)；

在反向平滑过程中，定义反向核

T_t；θ＝p_θ(x_0:t-1|x_t,y_0:t)，联合平滑分布φ_0:t|t；θ表示为：

φ_0:t|t；θ＝φ_t；θT_t；θ (5)

其中，T_t；θ表示已知0到t时刻的观测y_0:t和t时刻的位姿x_t情况下，0到t-1时刻的状态x_0:t-1的联合概率分布；

定义需优化的目标函数满足

其中

其中，log(g_s；θ(x_s))表示为g_s；θ(x_s)的对数；

对于T_t；θh_t通过下式递归计算：

其中，h_t是一个加性函数，

模块M1.5：在传统粒子滤波的基础上，考虑了正向传播以及反向平滑问题，设计

为带权重的粒子集；在滤波过程中，粒子通过运动模型进行正向传播，并动态分配观测权重：

其中，φ_t；θ通过

求得，其中

而q_t和g_t+1分别为之前所得的态转移分布的密度函数和观测模型的概率密度函数；

表示为t时刻粒子集合中的第i个粒子，

表示为它的权重，

表示t+1时刻第i个粒子的索引，

后向核

的估计由下式确定：

其中，

表示表示为s时刻第i个粒子的权重，

表示为t时刻粒子集合中的第j个粒子，

表示为t时刻粒子集合中的第l个粒子，

表示它所对应的权重，

根据PaRIS算法，

的粒子估计为：

其中，

为反向索引采样数，它远远小于权重粒子集的大小

为t+1时刻，第i个粒子的第j个反向索引；

针对外参θ_t，使用RML算法对参数θ估计，使其满足

其中

其中，p(x₀)表示初始状态概率，p(y₀|x₀)表示初始状态概率下的观测概率，p(y_l+1|x_l+1)表示l+1时刻状态概率下的观测概率，p(x_l+1|x_l)表示l时刻到l+1时刻的状态转移概率，

定义对数似然函数l_θ(y_0:t)＝logL_θ(y_0:t)，采用Robbins-Monro算法求取近似：

其中，γ_t表示为学习率，

其中，

分解为：

优选地，所述模块M2采用：通过标定后的外部传感器和标定后的内部传感器采集各种环境信息的数据；

所述外部传感器包括三维激光传感器、二维激光测距传感器、可见光摄像机以及深度摄像机；

所述内部传感器包括里程计以及IMU；

所述模块M3采用：通过不同场景下的环境特征分析建立不同场景真值系统；所述不同场景真值系统包括室外轨迹真值系统和室内轨迹真值系统；

所述室外轨迹真值系统是由RTK-GPS提供的；

所述室内轨迹真值系统是使用激光雷达算法提供的；

使用一种基于高斯过程回归地图的SLAM算法提供室内轨迹真值；

对环境进行高精度的建图；与常用概率占据栅格地图对环境进行描述不同，采用一种连续描述方式，减少了各个栅格之间是相互独立假设带来的量化误差；

首先通过八叉树存储每个被激光扫描到的占据点，获得对数级别的查询时间；然后通过高斯过程回归，依据已经有的点对空间上任意的位置的占据情况进行预测；

具体包括：连续高斯过程建图和基于容积卡尔曼滤波的前端位姿估计；

连续高斯过程建图步骤：基于空间上各点之间的占据情况是符合高斯联合分布的假设，整个地图由高斯过程描述，控制高斯过程的变量是空间位置(x,y)；对任意一个测试点预测的方式为：

f(x^*)＝N(μ,σ) (15)

其中，

其中，x^*代表的是测试点，即空间上任意一点；X代表的是训练点，即扫描到的点，y表示对应的占据情况；n是训练点的数目；σ_n是整体的方差；k为核函数，用于衡量空间上两个点之间距离的远近；I表示单位矩阵，选用RBF径向基函数作为核函数：

其中，c表示尺度因子，x和x'表示任意两个空间点，

前端位姿估计步骤：基于容积卡尔曼滤波的前端位姿估计；具体包括：前端预测建模、特征点提取、时间更新以及量测更新；

前端预测建模步骤：利用高精度、连续的地图表示方式，GPR-SLAM的前端位姿预测为：

其中，T_ζ代表位姿变换，h_k则是激光的第k个观测；

特征点提取步骤：在机器上设置一个向上的相机，用于记录天花板上的特征点；在GPR-SLAM的过程中，利用ORB算法提取每一帧上的特征点，并利用优化后的位姿，形成特征点地图，记作M_v；

时间更新步骤：包含滤波初始化、容积点计算、容积点传播；

滤波初始化：通过容积卡尔曼滤波融合激光与相机的信息达到更好的定位效果，将机器人的平面坐标记作x，滤波初始化为：

其中，E(x)表示期望，x₀表示初始估计，上标T表示矩阵的转置，P₀表示初始协方差矩阵；

容积点计算：利用Cholesky分解

计算容积点η：

其中，S_k表示信息矩阵，

表示信息矩阵的转置，η_i为第i个容积点，[1]_i为第[1]_i维上的单位向量；

容积点传播：

其中，T_k表示投影矩阵，

表示上一次迭代的估计；

T_k通过PnP算法求解，记

求解方程为：

其中，P₁,…,P_N来自特征地图，[u₁ v₁],…,[u_N v_N]是对应的像素坐标，其对应关系可以通过RANSAC算法获取；

状态量

以及误差协方差预测P_k+1|k：

其中，Q是预测步的协方差；

量测更新步骤：包含容积点计算、容积点传播、传播量测预测以及量测更新；

容积点计算：使用Cholesky将P_k+1|k分解为

则可以通过S_k+1|k与容积点η以及状态量

进行预测：

容积点传播：寻找观测上被占据概率最大的距离：

其中，

为观测模型，

是预测状态上的期望预测结果，

表示在第n条观测上被占据概率最大的距离，n为总的观测数；

传播量测预测：

观测更新：首先，计算量测误差

以及互协方差

增益更新：滤波增益K_k+1、估计

和最终协方差P_k+1:

所述多轨迹融合算法采用：

模块M3.1：考虑到数据集的采集场景中缺失高精度RTK-GPS真值信息，采用基于定位能力分析的多轨迹融合算法获取高精度真值；

使用Fisher信息矩阵反应机器人在不同环境下所获得的定位能力，从而分析环境对后续联合标定的精度影响；针对激光传感器，其定义为如下形式：

考虑到每条激光扫面线都可以认为是互不相关的，d(p_t,z_t)表示在为位姿p_t时的定位概率密度函数，r_iE表示第i条激光扫描线的期望测量距离，σ²则为估计方差；z_t表示空间观测；上标T表示矩阵的转置；E_z表示期望；N₀表示激光束的数量；上标L表示激光雷达；

针对相机模型，定义

为第i个相机位姿时的相机信息矩阵，具体表达如下：

其中，m_i表示相机模型在第i个位姿时与地图特征点匹配的数量；J_i,j表示第i个相机模型与第j个地图点的观测Jacobian矩阵；上标T表示矩阵的转置；上标C表示相机；

模块M3.2：在获得各自定位能力后，采用动态权重的方法调整轨迹融合时各个轨迹的可信度，从而实现一种鲁棒、高精度的定位真值；

当RTK-GPS处于高精度固定解时，使用RTK-GPS作为真值位姿；

当RTK-GPS处于差分浮点解时，首先选择差分浮点解两端一定数量高精度固定解作为对齐点，然后采用绝对旋转的封闭解法求取旋转矩阵、平移矩阵以及尺度因子；通过旋转、平移和缩放激光传感器、相机、惯性传感器融合算法的轨迹替换RTK-GPS浮点解，实现真值轨迹融合；

当RTK-GPS处于其他状态时，仅使用激光传感器、相机、惯性传感器融合定位位姿作为定位真值；

对齐点数量选取，我们采用基于自监督学习的方式获取；

训练数据准备：首先在无遮挡环境或者有额外高精度追踪设备辅助的情况下获取高精度轨迹真值，然后设计随机掩网遮挡其中不定长度轨迹；

模型输入与输出定义：输入为各个传感器的可信度、RTK-GPS状态S_GPS，以及掩网长度N：

输出则为前后两端所估计的对齐点数[P_start P_end]；

其中，P_start表示起始端参与对齐的位姿数，P_end表示结束端参与对齐的位姿数；

采用全连接的深度网络作为训练模型，损失函数采用平均距离误差：

其中，L表示距离误差，x_i表示被遮盖前的真实位姿，

表示对齐后所估计的位姿。

与现有技术相比，本发明具有如下的有益效果：

1、通过数据采集平台，收集了可用于室内外场景长期应用的数据集；

2、通过所提出的标定算法，实现了多传感器之间的联合标定，降低了两两标定过程中的误差传递问题，解决了传感器在长期运动过程中的的退化、位移问题；

3、针对不同环境特点，设计了不同的真值算法，特别的，针对室内环境，通过所设计的一种低成本的高精度真值算法，获取了可信的室内真值；

4、通过所设计的一种多轨迹融合算法，达到了全场景下的轨迹真值获取。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明的整体架构图。

图2为多传感器采集平台的架构图，其中，1-二维激光测距传感器、2-可见光相机、3-三维激光传感器、4-深度相机、5-RTK-GPS、6-里程计。

图3为高精度室内外真值采集系统的架构图。

图4为多轨迹真值融合算法的架构图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例1

本发明提供的一种面向室内外场景长期应用的移动机器人数据采集系统，如图1至4所示，包括：集合多传感器融合的移动机器人数据采集平台、异构传感器在线联合标定算法、高精度室内外真值系统、一种多轨迹真值融合算法部分；

多传感器采集平台是采集系统的硬件主体，负责收集可用于科学研究的传感器数据；集成了三维激光传感器、二维激光测距传感器、可见光相机、深度相机等外部传感器，里程计、IMU等内部传感器等，支持各种环境信息的数据采集；

异构传感器在线联合标定算法提供了各个传感器之间的高精度外参关系，提供传感器之间的位姿变化关系；本算法主要通过分析多传感器联合标定过程中的时空变化关系，进行联合建模，采用前向传播与反向平滑相结合的算法，从而达到了多传感器的高精度标定，解决了移动机器人在长期运行过程中，传感器发生的退化、位移等问题；

高精度室内外真值系统：用于采集平台在室内外环境导航时，提供高精度位姿真值；通过不同场景下的环境的特征分析，建立了两套高精度真值系统，室外主要使用高精度RTK-GPS提供，室内部分则设计了一种经过精确标定的激光雷达算法。

多轨迹真值融合算法：用于解决单一定位算法在部分环境中运行时的位姿精度下降的问题。通过对不同算法在各个环境下的精度特征分析，设计一种室内外、多真值轨迹融合算法，从而提供全场景真值。

本发明所设计的室内外多场景下长期运行时的数据采集平台及其关键技术，具有精度高、成本低的特点，可用于移动机器人的位姿跟踪、环境建模等研究。

具体地，所述多传感器采集平台包括传感器感知模块与外部观测模块：

传感器感知模块：所述传感器可以细分为外部环境感知传感器和内部运动感知传感器两类。外部环境感知传感器：主要指相机、激光传感器等直接从外部采集数据的传感器。这类传感器可以提供丰富的环境信息，可用来进行定位、建图、环境建模等研究，但由于容易受到外部环境的干扰，可能造成精度下降、定位丢失等现象。内部运动感知传感器：主要指里程计、惯性测量单元(Inertial Measurement Unit,IMU)等直接获取位姿信息的传感器，不容易受外部干扰，但误差容易累计膨胀。

外部观测模块：主要指高精度实时动态全球定位系统(Real-Time KinematicGlobal Positioning System,RTK-GPS)，在无遮挡环境下可以提供厘米级别的定位精度，通常用来作为室外环境的真值。但其在室内或受到遮挡的地方，容易发生定位丢失的现象。

具体地，所述的异构传感器在线联合标定算法负责提供各个传感器之间位姿外参变化关系，提供多传感器融合算法的标定基础，具体包括：状态空间建模步骤和外参估计步骤：

模块M1：基于状态空间模型对传感器标定过程进行时空序列建模。

具体地，模块M1具体包括可测空间定义、马尔可夫建模。

可测空间定义：为简化说明，以两个传感器为例，定义

和

为可测空间。需要说明的是：在多传感器联合标定的情况下，仅需对观测模型进行高维扩展。

马尔可夫建模：对机器人在t∈[0,T]的位姿

建立6自由度位姿的马尔可夫模型，其中初始状态转移分布为p(x|x_t-1,u_t)，为了简化说明步骤，忽略系统输入u_t。考虑到机器人位姿与观测模型的关系，认为观测模型

与隐变量x_t满足条件独立性假设，定义观测模型为p_θ(y_t|x_t),其中θ为标定参数向量。假设x_t满足q_t-1分布，y_t满足g_t:θ分布，完整概率模型构建如下：

x_t～q_t-1

y_t～g_t:θ

为了方便表述，定义

分别表示在s≤t≤T的状态变量和观测。

模块M2：首先通过正切滤波求取预测分布

以及梯度

然后基于改进粒子滤波的方法实现对标定外参θ估计。

所述模块M2包括：预测分布建模、外参估计。

预测分布建模：定义对于任意s≤s'，x_s:s'在y_0:t下的条件分布为φ_s:s'|t；θ＝p_θ(x_s:s'|y_0:t)。

为了方便叙述，使用缩写φ_t:θ表示滤波分布φ_s:s'|t；θ。基于滤波递归，对于t∈N和f∈F(χ)，下面的式子成立：

π_t+1；θf＝φ_t；θQ_θf

此外，定义

T_t；θ＝p_θ(x_0:t-1|x_t,y_0:t)，联合平滑分布

φ_0:t|t；θ可以表示为：

φ_0:t|t；θ＝φ_t；θT_t；θ

定义目标函数

其中

对于T_t；θh_t可通过下式递归计算：

考虑

与函数

外参估计：在传统粒子滤波的基础上，考虑了正向传播以及反向平滑问题，设计

为带权重的粒子集。在滤波过程中，粒子通过运动模型进行正向传播，并动态分配观测权重：

然后，φ_t；θ可以通过

求得，其中

此外，后向核

的估计由下式确定：

那么，根据PaRIS(Particle-based Rapid Incremental Smoother，PaRIS)算法，

的粒子估计为：

特别地，针对外参θ_t，使用RML(Recursive Maximum Likelihood,RML)算法对参数θ估计，使其满足

其中

定义对数似然函数l_θ(y_0:t)＝logL_θ(y_0:t)，采用Robbins-Monro算法求取近似：

其中，

可分解为：

具体地，所述高精度室内外真值采集系统主要考虑了不同环境下的特征，室外环境可以通过高精度RTK-GPS提供，室内环境由于受到障碍物的遮挡，无法接收GPS信号，使用一种基于高斯过程回归地图和容积卡尔曼滤波的真值求取算法；

所述高精度室内外真值采集系统由室内真值与室外真值两部分组成：

室外真值模块：主要由RTK-GPS采集信息提供，基于该信息解算出移动平台实时位姿作为系统真值；

室内真值模块：由低成本室内特征信息采集系统与室内真值求解算法两部分组成。

低成本室内特征信息采集系统：考虑到室内缺乏GPS信号的原因，采用较为便宜的二维激光测距传感器、惯性传感器和可见光相机作为室内环境特征的采集。考虑到在室内环境中激光信号、相机画面容易受到人群的遮挡，将激光测距传感器、可见光相机布设在一定高度、且呈一定角度布置，使其能够观察一定高度之上的固定信息，从而提升了对人群干扰的鲁棒性。

室内真值求解算法：使用一种基于高斯过程回归地图的SLAM(Simultaneouslocalization and Mapping,SLAM)算法提供室内轨迹真值。对环境进行高精度的建图。与常用概率占据栅格地图对环境进行描述不同，采用一种连续描述方式，减少了各个栅格之间是相互独立假设带来的量化误差。首先通过八叉树存储每个被激光扫描到的占据点，获得对数级别的查询时间。然后通过高斯过程回归，依据已经有的点对空间上任意的位置的占据情况进行预测。

具体地，所述室内真值求解算法包括：连续高斯过程建图和基于容积卡尔曼滤波的前端位姿估计。

连续高斯过程建图：基于空间上各点之间的占据情况是符合高斯联合分布的假设，整个地图可以由高斯过程描述，控制高斯过程的变量是空间位置(x,y)。对任意一个测试点预测的方式为：

f(x^*)＝N(μ,σ)

其中，

这里，x^*代表的是测试点，即空间上任意一点；X代表的是训练点，即扫描到的点，y是它们对应的占据情况；n是训练点的数目；σ_n是整体的方差；σ为核函数，用于衡量空间上两个点之间距离的远近。特别地，这里可选为RBF径向基函数：

前端位姿估计：基于容积卡尔曼滤波的前端位姿估计。

具体地，所述前端位姿估计包括：前端预测建模、特征点提取、时间更新、量测更新。

前端预测建模：利用这种高精度、连续的地图表示方式，GPR-SLAM的前端位姿预测为：

其中，T_ζ代表位姿变换，h_k则是激光的第k个观测。

特征点提取：同时，在机器上设置一个向上的相机，用于记录天花板上的特征点，这些特征点处于静态，往往较为稳定。在GPR-SLAM的过程中，利用ORB(Oriented FAST andRotated BRIEF，ORB)算法提取每一帧上的特征点，并利用优化后的位姿，形成特征点地图，记作M_v。

时间更新：包含滤波初始化、容积点计算、容积点传播。

滤波初始化：通过容积卡尔曼滤波(Cubature Kalman filter,CKF)融合激光与相机的信息达到更好的定位效果，将机器人的平面坐标记作x，滤波初始化为：

容积点计算：利用Cholesky分解

计算容积点η：

其中，η_i为第i个容积点，[1]_i为第[1]_i维上的单位向量。

容积点传播：

T_k可以通过PnP(Perspective-n-Point,PnP)算法求解，记

那么其求解方程为：

其中，P₁,…,P_N来自特征地图，[u₁ v₁],…,[u_N v_N]是对应的像素坐标，其对应关系可以通过RANSAC(RANdom SAmple Consensus,RANSAC)算法获取。

状态量以及误差协方差预测：

其中，Q是预测步的协方差。

量测更新：包含容积点计算、容积点传播、传播量测预测、量测更新。

容积点计算：

容积点传播：

传播量测预测：

观测更新：其中，量测误差以及互协方差计算：

增益更新：

具体地，所述多轨迹真值融合算法包括：轨迹可信度评估和多轨迹融合策略。特别地，针对轨迹融合过程中的的对齐点选取，采用了一种自监督的方式获取。

轨迹可信度评估：考虑到数据集的采集场景中(室内穿梭、受到障碍物遮挡时)缺失高精度RTK-GPS真值信息，采用基于定位能力分析的多轨迹融合算法获取高精度真值。具体使用Fisher信息矩阵反应机器人在不同环境下所获得的定位能力，从而分析环境对后续联合标定的精度影响。针对激光传感器，其定义为如下形式：

考虑到每条激光扫面线都可以认为是互不相关的，d(p_t,z_t)表示在为位姿p_t时的定位概率密度函数，r_iE表示第i条激光扫描线的期望测量距离，σ²则为估计方差。

针对相机模型，我们定义

为第i个相机位姿时的相机信息矩阵，其具体表达如下：

其中m_i表示相机模型在第i个位姿时与地图特征点匹配的数量，J_i,j则表示第i个相机模型与第j个地图点的观测Jacobian矩阵。

多轨迹融合策略：在获得各自定位能力后，采用动态权重的方法调整轨迹融合时各个轨迹的可信度，从而实现一种鲁棒、高精度的定位真值。具体来说：

当RTK-GPS处于高精度固定解时，使用RTK-GPS作为真值位姿。

当RTK-GPS处于差分浮点解时，首先选择差分浮点解两端一定数量高精度固定解作为对齐点，然后采用K.P.Horn所提出的绝对旋转的封闭解法求取旋转矩阵、平移矩阵以及尺度因子。通过旋转、平移和缩放激光传感器、相机、惯性传感器融合算法的轨迹替换RTK-GPS浮点解，实现真值轨迹融合。

当RTK-GPS处于其他状态时，仅使用激光传感器、相机、惯性传感器融合定位位姿作为定位真值。

特别地，对齐点数量选取，我们采用基于自监督学习的方式获取，具体来说：

训练数据准备：首先在无遮挡环境或者有额外高精度追踪设备辅助(激光追踪仪)的情况下获取高精度轨迹真值，然后设计随机掩网(Mask)遮挡其中不定长度轨迹。

模型输入与输出定义：输入为各个传感器的可信度、RTK-GPS状态S_GPS，以及掩网(Mask)长度N：

输出则为前后两端所估计的对齐点数[P_start P_end]。

模型与损失函数：采用全连接的深度网络作为训练模型，损失函数采用平均距离误差：

本发明提供的面向室内外场景长期应用的移动机器人数据采集系统，可以通过本发明提供的面向室内外场景长期应用的移动机器人数据采集方法中的步骤流程实现。本领域技术人员，可以将所述面向室内外场景长期应用的移动机器人数据采集方法理解为面向室内外场景长期应用的移动机器人数据采集系统的一个优选例。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种面向室内外场景长期应用的移动机器人数据采集方法，其特征在于，包括：

步骤S1：利用联合标定算法计算异构传感器之间的位姿变化关系，实现多传感器的标定；

步骤S2：通过标定后的传感器采集环境信息数据；

步骤S3：使用多轨迹融合算法将不同场景的轨迹真值进行融合得到全场景真值，全场景真值是获取的环境信息数据对照的定位基准。

2.根据权利要求1所述的面向室内外场景长期应用的移动机器人数据采集方法，其特征在于，所述步骤S1采用：分析多传感器联合标定过程中的时空变化关系，进行联合建模，采用前向传播与反向平滑相结合的算法实现多传感器的高精度标定。

3.根据权利要求2所述的面向室内外场景长期应用的移动机器人数据采集方法，其特征在于，所述步骤S1采用：

步骤S1.1：定义(X,

)和(Y,

)为可测空间；

步骤S1.2：对机器人在t∈[0,T]的位姿

与机器人观测模型y_t建立6自由度位姿的马尔可夫模型，x_t和y_t分别为马尔可夫模型中的状态变量和观测变量，初始状态转移分布为p(x|x_t-1,u_t)，为了简化步骤，忽略外部输入u_t；考虑到机器人位姿与观测模型y_t的关系，认为观测变量

与状态变量x_t满足条件独立性假设，定义观测模型为p_θ(y_t|x_t),其中θ为标定参数向量；假设x_t满足q_t-1分布，y_t满足g_t:θ分布，完整概率模型构建如下：

x_t～q_t-1 (1)

y_t～g_t:θ (2)

其中，定义

分别表示在s≤t≤T的状态变量和观测；x_t表示t时刻机器人的6自由度位姿；q_t-1表示状态转移分布p(x|x_t-1,u_t)所对应的概率密度；g_t；θ表示观测模型p_θ(y_t|x_t)所对应的概率密度；x_s表示s时刻机器人的6自由度位姿；y_s表示s时刻观测变量；u_t表示外部输入；

步骤S1.3：通过正切滤波求取预测分布π_t；θ以及梯度

基于改进粒子滤波的方法实现对标定外参θ估计；

其中，π_t；θ表示对x_t+1的预测分布，η_t；θ为π_t；θ的梯度；

表示微分算子；

步骤S1.4：定义对于任意s≤s'，x_s:s'在y_0:t下的条件分布为φ_s:s'|t；θ＝p_θ(x_s:s'|y_0:t)；

使用缩写φ_t:θ表示滤波分布φ_s:s'|t；θ；基于滤波递归，对于t∈N和任意定义在x上的函数

则

π_t+1；θf＝φ_t；θQ_θf (4)

其中，N表示自然数；φ_t；θ为t时刻，参数为θ的滤波分布，π_t+1；θ为t+1时刻，参数为θ的预测分布，Q_θ为转移分布P(x_t|x_t-1)；

在反向平滑过程中，定义反向核

T_t；θ＝p_θ(x_0:t-1|x_t,y_0:t)，联合平滑分布φ_0:t|t；θ表示为：

φ_0:t|t；θ＝φ_t；θT_t；θ (5)

其中，T_t；θ表示已知0到t时刻的观测y_0:t和t时刻的位姿x_t情况下，0到t-1时刻的状态x_0:t-1的联合概率分布；

定义需优化的目标函数满足

其中

其中，log(g_s；θ(x_s))表示为g_s；θ(x_s)的对数；

对于T_t；θh_t通过下式递归计算：

其中，h_t是一个加性函数，

步骤S1.5：在传统粒子滤波的基础上，考虑了正向传播以及反向平滑问题，设计

为带权重的粒子集；在滤波过程中，粒子通过运动模型进行正向传播，并动态分配观测权重：

其中，φ_t；θ通过

求得，其中

而q_t和g_t+1分别为之前所得的态转移分布的密度函数和观测模型的概率密度函数；

表示为t时刻粒子集合中的第i个粒子，

表示为它的权重，

表示t+1时刻第i个粒子的索引，

后向核

的估计由下式确定：

其中，

表示表示为s时刻第i个粒子的权重，

表示为t时刻粒子集合中的第j个粒子，

表示为t时刻粒子集合中的第l个粒子，

表示它所对应的权重，

根据PaRIS算法，

的粒子估计为：

其中，

为反向索引采样数，它远远小于权重粒子集的大小

为t+1时刻，第i个粒子的第j个反向索引；

针对外参θ_t，使用RML算法对参数θ估计，使其满足

其中

其中，p(x₀)表示初始状态概率，p(y₀|x₀)表示初始状态概率下的观测概率，p(y_l+1|x_l+1)表示l+1时刻状态概率下的观测概率，p(x_l+1|x_l)表示l时刻到l+1时刻的状态转移概率，

定义对数似然函数l_θ(y_0:t)＝logL_θ(y_0:t)，采用Robbins-Monro算法求取近似：

其中，γ_t表示为学习率，

其中，

分解为：

4.根据权利要求1所述的面向室内外场景长期应用的移动机器人数据采集方法，其特征在于，所述步骤S2采用：通过标定后的外部传感器和标定后的内部传感器采集各种环境信息的数据；

所述外部传感器包括三维激光传感器、二维激光测距传感器、可见光摄像机以及深度摄像机；

所述内部传感器包括里程计以及IMU。

5.根据权利要求1所述的面向室内外场景长期应用的移动机器人数据采集方法，其特征在于，所述步骤S3采用：通过不同场景下的环境特征分析建立不同场景真值系统；所述不同场景真值系统包括室外轨迹真值系统和室内轨迹真值系统；

所述室外轨迹真值系统是由RTK-GPS提供的；

所述室内轨迹真值系统是使用激光雷达算法提供的。

6.根据权利要求5所述的面向室内外场景长期应用的移动机器人数据采集方法，其特征在于，使用一种基于高斯过程回归地图的SLAM算法提供室内轨迹真值；

对环境进行高精度的建图；与常用概率占据栅格地图对环境进行描述不同，采用一种连续描述方式，减少了各个栅格之间是相互独立假设带来的量化误差；

首先通过八叉树存储每个被激光扫描到的占据点，获得对数级别的查询时间；然后通过高斯过程回归，依据已经有的点对空间上任意的位置的占据情况进行预测；

具体包括：连续高斯过程建图和基于容积卡尔曼滤波的前端位姿估计；

连续高斯过程建图步骤：基于空间上各点之间的占据情况是符合高斯联合分布的假设，整个地图由高斯过程描述，控制高斯过程的变量是空间位置(x,y)；对任意一个测试点预测的方式为：

f(x^*)＝N(μ,σ) (15)

其中，

其中，x^*代表的是测试点，即空间上任意一点；X代表的是训练点，即扫描到的点，y表示对应的占据情况；n是训练点的数目；σ_n是整体的方差；k为核函数，用于衡量空间上两个点之间距离的远近；I表示单位矩阵，选用RBF径向基函数作为核函数：

其中，c表示尺度因子，x和x'表示任意两个空间点，

前端位姿估计步骤：基于容积卡尔曼滤波的前端位姿估计；具体包括：前端预测建模、特征点提取、时间更新以及量测更新；

前端预测建模步骤：利用高精度、连续的地图表示方式，GPR-SLAM的前端位姿预测为：

其中，T_ζ代表位姿变换，h_k则是激光的第k个观测；

特征点提取步骤：在机器上设置一个向上的相机，用于记录天花板上的特征点；在GPR-SLAM的过程中，利用ORB算法提取每一帧上的特征点，并利用优化后的位姿，形成特征点地图，记作M_v；

时间更新步骤：包含滤波初始化、容积点计算、容积点传播；

滤波初始化：通过容积卡尔曼滤波融合激光与相机的信息达到更好的定位效果，将机器人的平面坐标记作x，滤波初始化为：

其中，E(x)表示期望，x₀表示初始估计，上标T表示矩阵的转置，P₀表示初始协方差矩阵；

容积点计算：利用Cholesky分解

计算容积点η：

其中，S_k表示信息矩阵，

表示信息矩阵的转置，η_i为第i个容积点，[1]_i为第[1]_i维上的单位向量；

容积点传播：

其中，T_k表示投影矩阵，

表示上一次迭代的估计；

T_k通过PnP算法求解，记

求解方程为：

其中，P₁,…,P_N来自特征地图，[u₁ v₁],…,[u_N v_N]是对应的像素坐标，其对应关系可以通过RANSAC算法获取；

状态量

以及误差协方差预测P_k+1|k：

其中，Q是预测步的协方差；

量测更新步骤：包含容积点计算、容积点传播、传播量测预测以及量测更新；

容积点计算：使用Cholesky将P_k+1|k分解为

则可以通过S_k+1|k与容积点η以及状态量

进行预测：

容积点传播：寻找观测上被占据概率最大的距离：

其中，

为观测模型，

是预测状态上的期望预测结果，

表示在第n条观测上被占据概率最大的距离，n为总的观测数；

传播量测预测：

观测更新：首先，计算量测误差

以及互协方差

增益更新：滤波增益K_k+1、估计

和最终协方差P_k+1:

7.根据权利要求1所述的面向室内外场景长期应用的移动机器人数据采集方法，其特征在于，所述多轨迹融合算法采用：

步骤S3.1：考虑到数据集的采集场景中缺失高精度RTK-GPS真值信息，采用基于定位能力分析的多轨迹融合算法获取高精度真值；

使用Fisher信息矩阵反应机器人在不同环境下所获得的定位能力，从而分析环境对后续联合标定的精度影响；针对激光传感器，其定义为如下形式：

考虑到每条激光扫面线都可以认为是互不相关的，d(p_t，z_t)表示在为位姿p_t时的定位概率密度函数，r_iE表示第i条激光扫描线的期望测量距离，σ²则为估计方差；z_t表示空间观测；上标T表示矩阵的转置；E_z表示期望；N₀表示激光束的数量；上标L表示激光雷达；

针对相机模型，定义

为第i个相机位姿时的相机信息矩阵，具体表达如下：

其中，m_i表示相机模型在第i个位姿时与地图特征点匹配的数量；J_i,j表示第i个相机模型与第j个地图点的观测Jacobian矩阵；上标T表示矩阵的转置；上标C表示相机；

步骤S3.2：在获得各自定位能力后，采用动态权重的方法调整轨迹融合时各个轨迹的可信度，从而实现一种鲁棒、高精度的定位真值；

当RTK-GPS处于高精度固定解时，使用RTK-GPS作为真值位姿；

当RTK-GPS处于差分浮点解时，首先选择差分浮点解两端一定数量高精度固定解作为对齐点，然后采用绝对旋转的封闭解法求取旋转矩阵、平移矩阵以及尺度因子；通过旋转、平移和缩放激光传感器、相机、惯性传感器融合算法的轨迹替换RTK-GPS浮点解，实现真值轨迹融合；

当RTK-GPS处于其他状态时，仅使用激光传感器、相机、惯性传感器融合定位位姿作为定位真值；

对齐点数量选取，我们采用基于自监督学习的方式获取；

训练数据准备：首先在无遮挡环境或者有额外高精度追踪设备辅助的情况下获取高精度轨迹真值，然后设计随机掩网遮挡其中不定长度轨迹；

模型输入与输出定义：输入为各个传感器的可信度、RTK-GPS状态S_GPS，以及掩网长度N：

输出则为前后两端所估计的对齐点数[P_start P_end]；

其中，P_start表示起始端参与对齐的位姿数，P_end表示结束端参与对齐的位姿数；

采用全连接的深度网络作为训练模型，损失函数采用平均距离误差：

其中，L表示距离误差，x_i表示被遮盖前的真实位姿，

表示对齐后所估计的位姿。

8.一种面向室内外场景长期应用的移动机器人数据采集系统，其特征在于，包括：

模块M1：利用联合标定算法计算异构传感器之间的位姿变化关系，实现多传感器的标定；

模块M2：通过标定后的传感器采集环境信息数据；

模块M3：使用多轨迹融合算法将不同场景的轨迹真值进行融合得到全场景真值，全场景真值是获取的环境信息数据对照的定位基准。

9.根据权利要求8所述的面向室内外场景长期应用的移动机器人数据采集系统，其特征在于，所述模块M1采用：分析多传感器联合标定过程中的时空变化关系，进行联合建模，采用前向传播与反向平滑相结合的算法实现多传感器的高精度标定；

所述模块M1采用：

模块M1.1：定义(X,

)和(Y,

)为可测空间；

模块M1.2：对机器人在t∈[0,T]的位姿

与机器人观测模型y_t建立6自由度位姿的马尔可夫模型，x_t和y_t分别为马尔可夫模型中的状态变量和观测变量，初始状态转移分布为p(x|x_t-1,u_t)，为了简化步骤，忽略外部输入u_t；考虑到机器人位姿与观测模型y_t的关系，认为观测变量

与状态变量x_t满足条件独立性假设，定义观测模型为p_θ(y_t|x_t),其中θ为标定参数向量；假设x_t满足q_t-1分布，y_t满足g_t:θ分布，完整概率模型构建如下：

x_t～q_t-1 (1)

y_t～g_t:θ (2)

其中，定义

分别表示在s≤t≤T的状态变量和观测；x_t表示t时刻机器人的6自由度位姿；q_t-1表示状态转移分布p(x|x_t-1,u_t)所对应的概率密度；g_t；θ表示观测模型p_θ(y_t|x_t)所对应的概率密度；x_s表示s时刻机器人的6自由度位姿；y_s表示s时刻观测变量；u_t表示外部输入；

模块M1.3：通过正切滤波求取预测分布π_t；θ以及梯度

基于改进粒子滤波的方法实现对标定外参θ估计；

其中，π_t；θ表示对x_t+1的预测分布，η_t；θ为π_t；θ的梯度；

表示微分算子；

模块M1.4：定义对于任意s≤s'，x_s:s'在y_0:t下的条件分布为φ_s:s'|t；θ＝p_θ(x_s:s'|y_0:t)；

使用缩写φ_t:θ表示滤波分布φ_s:s'|t；θ；基于滤波递归，对于t∈N和任意定义在x上的函数

则

π_t+1；θf＝φ_t；θQ_θf (4)

其中，N表示自然数；φ_t；θ为t时刻，参数为θ的滤波分布，π_t+1；θ为t+1时刻，参数为θ的预测分布，Q_θ为转移分布P(x_t|x_t-1)；

在反向平滑过程中，定义反向核

T_t；θ＝p_θ(x_0:t-1|x_t,y_0:t)，联合平滑分布φ_0:t|t；θ表示为：

φ_0:t|t；θ＝φ_t；θT_t；θ (5)

其中，T_t；θ表示已知0到t时刻的观测y_0:t和t时刻的位姿x_t情况下，0到t-1时刻的状态x_0:t-1的联合概率分布；

定义需优化的目标函数满足

其中

其中，log(g_s；θ(x_s))表示为g_s；θ(x_s)的对数；

对于T_t；θh_t通过下式递归计算：

其中，h_t是一个加性函数，

模块M1.5：在传统粒子滤波的基础上，考虑了正向传播以及反向平滑问题，设计

为带权重的粒子集；在滤波过程中，粒子通过运动模型进行正向传播，并动态分配观测权重：

其中，φ_t；θ通过

求得，其中

而q_t和g_t+1分别为之前所得的态转移分布的密度函数和观测模型的概率密度函数；

表示为t时刻粒子集合中的第i个粒子，

表示为它的权重，

表示t+1时刻第i个粒子的索引，

后向核

的估计由下式确定：

其中，

表示表示为s时刻第i个粒子的权重，

表示为t时刻粒子集合中的第j个粒子，

表示为t时刻粒子集合中的第l个粒子，

表示它所对应的权重，

根据PaRIS算法，

的粒子估计为：

其中，

为反向索引采样数，它远远小于权重粒子集的大小

为t+1时刻，第i个粒子的第j个反向索引；

针对外参θ_t，使用RML算法对参数θ估计，使其满足

其中

其中，p(x₀)表示初始状态概率，p(y₀|x₀)表示初始状态概率下的观测概率，p(y_l+1|x_l+1)表示l+1时刻状态概率下的观测概率，p(x_l+1|x_l)表示l时刻到l+1时刻的状态转移概率，

定义对数似然函数l_θ(y_0:t)＝logL_θ(y_0:t)，采用Robbins-Monro算法求取近似：

其中，γ_t表示为学习率，

其中，

分解为：

10.根据权利要求8所述的面向室内外场景长期应用的移动机器人数据采集系统，其特征在于，所述模块M2采用：通过标定后的外部传感器和标定后的内部传感器采集各种环境信息的数据；

所述外部传感器包括三维激光传感器、二维激光测距传感器、可见光摄像机以及深度摄像机；

所述内部传感器包括里程计以及IMU；

所述模块M3采用：通过不同场景下的环境特征分析建立不同场景真值系统；所述不同场景真值系统包括室外轨迹真值系统和室内轨迹真值系统；

所述室外轨迹真值系统是由RTK-GPS提供的；

所述室内轨迹真值系统是使用激光雷达算法提供的；

使用一种基于高斯过程回归地图的SLAM算法提供室内轨迹真值；

对环境进行高精度的建图；与常用概率占据栅格地图对环境进行描述不同，采用一种连续描述方式，减少了各个栅格之间是相互独立假设带来的量化误差；

首先通过八叉树存储每个被激光扫描到的占据点，获得对数级别的查询时间；然后通过高斯过程回归，依据已经有的点对空间上任意的位置的占据情况进行预测；

具体包括：连续高斯过程建图和基于容积卡尔曼滤波的前端位姿估计；

连续高斯过程建图步骤：基于空间上各点之间的占据情况是符合高斯联合分布的假设，整个地图由高斯过程描述，控制高斯过程的变量是空间位置(x,y)；对任意一个测试点预测的方式为：

f(x^*)＝N(μ,σ) (15)

其中，

其中，x^*代表的是测试点，即空间上任意一点；X代表的是训练点，即扫描到的点，y表示对应的占据情况；n是训练点的数目；σ_n是整体的方差；k为核函数，用于衡量空间上两个点之间距离的远近；I表示单位矩阵，选用RBF径向基函数作为核函数：

其中，c表示尺度因子，x和x'表示任意两个空间点，

前端位姿估计步骤：基于容积卡尔曼滤波的前端位姿估计；具体包括：前端预测建模、特征点提取、时间更新以及量测更新；

前端预测建模步骤：利用高精度、连续的地图表示方式，GPR-SLAM的前端位姿预测为：

其中，T_ζ代表位姿变换，h_k则是激光的第k个观测；

特征点提取步骤：在机器上设置一个向上的相机，用于记录天花板上的特征点；在GPR-SLAM的过程中，利用ORB算法提取每一帧上的特征点，并利用优化后的位姿，形成特征点地图，记作M_v；

时间更新步骤：包含滤波初始化、容积点计算、容积点传播；

滤波初始化：通过容积卡尔曼滤波融合激光与相机的信息达到更好的定位效果，将机器人的平面坐标记作x，滤波初始化为：

其中，E(x)表示期望，x₀表示初始估计，上标T表示矩阵的转置，P₀表示初始协方差矩阵；

容积点计算：利用Cholesky分解

计算容积点η：

其中，S_k表示信息矩阵，

表示信息矩阵的转置，η_i为第i个容积点，[1]_i为第[1]_i维上的单位向量；

容积点传播：

其中，T_k表示投影矩阵，

表示上一次迭代的估计；

T_k通过PnP算法求解，记

求解方程为：

其中，P₁，…，P_N来自特征地图，[u₁ v₁],…,[u_N v_N]是对应的像素坐标，其对应关系可以通过RANSAC算法获取；

状态量

以及误差协方差预测P_k+1|k：

其中，Q是预测步的协方差；

量测更新步骤：包含容积点计算、容积点传播、传播量测预测以及量测更新；

容积点计算：使用Cholesky将P_k+1|k分解为

则可以通过S_k+1|k与容积点η以及状态量

进行预测：

容积点传播：寻找观测上被占据概率最大的距离：

其中，

为观测模型，

是预测状态上的期望预测结果，

表示在第n条观测上被占据概率最大的距离，n为总的观测数；

传播量测预测：

观测更新：首先，计算量测误差

以及互协方差

增益更新：滤波增益K_k+1、估计

和最终协方差P_k+1:

所述多轨迹融合算法采用：

模块M3.1：考虑到数据集的采集场景中缺失高精度RTK-GPS真值信息，采用基于定位能力分析的多轨迹融合算法获取高精度真值；

使用Fisher信息矩阵反应机器人在不同环境下所获得的定位能力，从而分析环境对后续联合标定的精度影响；针对激光传感器，其定义为如下形式：

考虑到每条激光扫面线都可以认为是互不相关的，d(p_t,z_t)表示在为位姿p_t时的定位概率密度函数，r_iE表示第i条激光扫描线的期望测量距离，σ²则为估计方差；z_t表示空间观测；上标T表示矩阵的转置；E_z表示期望；N₀表示激光束的数量；上标L表示激光雷达；

针对相机模型，定义

为第i个相机位姿时的相机信息矩阵，具体表达如下：

其中，m_i表示相机模型在第i个位姿时与地图特征点匹配的数量；J_i,j表示第i个相机模型与第j个地图点的观测Jacobian矩阵；上标T表示矩阵的转置；上标C表示相机；

模块M3.2：在获得各自定位能力后，采用动态权重的方法调整轨迹融合时各个轨迹的可信度，从而实现一种鲁棒、高精度的定位真值；

当RTK-GPS处于高精度固定解时，使用RTK-GPS作为真值位姿；

当RTK-GPS处于差分浮点解时，首先选择差分浮点解两端一定数量高精度固定解作为对齐点，然后采用绝对旋转的封闭解法求取旋转矩阵、平移矩阵以及尺度因子；通过旋转、平移和缩放激光传感器、相机、惯性传感器融合算法的轨迹替换RTK-GPS浮点解，实现真值轨迹融合；

当RTK-GPS处于其他状态时，仅使用激光传感器、相机、惯性传感器融合定位位姿作为定位真值；

对齐点数量选取，我们采用基于自监督学习的方式获取；

训练数据准备：首先在无遮挡环境或者有额外高精度追踪设备辅助的情况下获取高精度轨迹真值，然后设计随机掩网遮挡其中不定长度轨迹；

模型输入与输出定义：输入为各个传感器的可信度、RTK-GPS状态S_GPS，以及掩网长度N：

输出则为前后两端所估计的对齐点数[P_start P_end]；

其中，P_start表示起始端参与对齐的位姿数，P_end表示结束端参与对齐的位姿数；

采用全连接的深度网络作为训练模型，损失函数采用平均距离误差：

其中，L表示距离误差，x_i表示被遮盖前的真实位姿，

表示对齐后所估计的位姿。

Images