阅读说明：本技术 一种基于灯光标定信息融合的激光slam方法及装置 (Laser SLAM method and device based on lamplight calibration information fusion ) 是由 王强 曾勇 高川琦 卢镇宇 于 2020-07-09 设计创作，主要内容包括：本发明公开一种基于灯光标定信息融合的激光SLAM方法、装置及计算机可读存储介质,属于激光SLAM与Li-Fi信息传输技术领域,解决了现有技术中位姿漂移过大及建立的地图准确性较低的技术问题。一种基于灯光标定信息融合的激光SLAM方法,包括以下步骤：获取最优链路链接,根据所述最优链路链接,确定基于Li-Fi的帧间齐次变换矩阵；获取数据结合帧数据,对所述数据结合数据进行配准,得到配准后的数据结合帧数据,确定基于激光的帧间齐次变换矩阵,获取融合Li-Fi和激光数据帧之间的位姿变换矩阵,根据所述融合Li-Fi和激光数据帧之间的位姿变换矩阵及配准后的数据结合帧数据,对当前帧与地图进行匹配并更新地图。抑制了位姿漂移,提高了建立的地图的准确性。(The invention discloses a laser SLAM method and device based on lamplight calibration information fusion and a computer readable storage medium, belongs to the technical field of laser SLAM and Li-Fi information transmission, and solves the technical problems of overlarge pose drift and lower accuracy of an established map in the prior art. A laser SLAM method based on lamplight calibration information fusion comprises the following steps: obtaining an optimal link, and determining a homogeneous transformation matrix between frames based on Li-Fi according to the optimal link; acquiring data combination frame data, registering the data combination data to obtain the registered data combination frame data, determining a laser-based interframe homogeneous transformation matrix, acquiring a pose transformation matrix between a fusion Li-Fi and a laser data frame, and matching a current frame with a map and updating the map according to the pose transformation matrix between the fusion Li-Fi and the laser data frame and the registered data combination frame data. The pose drift is inhibited, and the accuracy of the established map is improved.)

一种基于灯光标定信息融合的激光SLAM方法及装置

技术领域

本发明涉及激光SLAM与Li-Fi信息传输技术领域，尤其是涉及一种基于灯光标定信息融合的激光SLAM方法、装置及计算机可读存储介质。

背景技术

即时定位与地图构建技术(Simultaneous localization and mapping，SLAM)是移动机器人在未知环境中进行探索、侦查、定位导航等各项任务的基础和关键，而激光SLAM作为搭载激光雷达的主体，能在运动中获得环境状态信息，估计自身或机器人的位姿，同时建立并更新周围的环境地图；SLAM的信息处理方式可分为滤波法和图优化法；由于二维激光SLAM的定位、映射和导航算法比视觉SLAM更成熟，因此二维激光SLAM更适合室内移动机器人；实际上，机器人外部运行环境是无法预测的，不同环境的不确定程度不同，机器人获取的信息也受传感器限制，因此，在几何特征不明显的环境或复杂的环境中，移动机器人很难通过单个传感器了解自身精确位置和周围环境，为了使移动机器人能在未知环境中稳定运行，基于多传感器结合的激光SLAM方案便成为了新的研究热点。

近年来，发光二极管由于其长寿命、高亮度和快响应的特性被广泛用于区域照明。可见光通信技术(Light Fidelity，Li-Fi)则是基于LED的新一代通信技术，它通过快速开关LED达到通信的目的；而基于可见光通信技术的室内定位系统，如RSS由于无需借助电磁波通信，可实现室内高精度定位；在SLAM中，回环检测是一个递推过程，即根据传感器信息判断机器人当前所处位置，并与全局地图中的上一位置进行比较。回环检测是未知环境地图创建中的一个难点，如，移动机器人在长走廊等特征点不明显的环境中运行时，传感器可获得的环境特征信息少，回环检测的难度增加，位姿漂移过大；移动机器人在存在大量相同或相似物体的复杂环境中，如存在相似的桌椅、墙角等物体时，会造成感知歧义，在对这些相似特征点进行重复判断和概率估计时，要处理的数据会成倍增加，导致建立的地图准确性较低。

发明内容

有鉴于此，本发明提供了一种基于灯光标定信息融合的激光SLAM方法、装置及计算机可读存储介质，解决现有技术中位姿漂移过大及建立的地图准确性较低的技术问题。

一方面，本发明提供了一种基于灯光标定信息融合的激光SLAM方法，包括以下步骤：

获取LED的标定坐标、LED发射机的编码以及接受机的估计坐标，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接；

根据所述最优链路链接，获取基于Li-Fi的当前数据帧，根据所述基于Li-Fi的当前数据帧，确定基于Li-Fi的帧间齐次变换矩阵；

获取当前激光帧数据，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合帧数据，对所述数据结合数据进行配准，得到配准后的数据结合帧数据，根据所述配准后的结合帧数据，确定基于激光的帧间齐次变换矩阵；

根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，根据所述融合Li-Fi和激光数据帧之间的位姿变换矩阵及配准后的数据结合帧数据，对当前帧与地图进行匹配并更新地图。

进一步地，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接，具体包括，建立最优链路链接机制模型，通过所述最优链路链接机制模型，最优链路链接，其中，最优链路链接机制模型为

T_xi＝(x_i,y_i,i)，(x_i,y_j)为LED的标定坐标，i＝1,2,3是LED发射机编码，R_xt＝(x_e,y_e,t)，t是观测时刻，(x_e,y_e)为接受机的估计坐标，E＝(1,1,0)^T，

是Li-Fi链路数量。

进一步地，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合数据，具体包括，根据所述Li-Fi齐次变换矩阵T₁将当前激光帧数据进行映射变换得到虚拟帧数据，对虚拟帧数据做空间相邻顺序预处理，得到预处理后的虚拟帧数据，将当前激光帧数据变换到参考帧坐标系下，得到参考帧数据，将参考帧数据加入到虚拟帧数据中得到数据结合帧数据。

进一步地，将当前激光帧数据变换到参考帧坐标系下，得到参考帧数据，具体包括，通过帧间齐次变换将当前激光帧数据变换到参考帧坐标系下，所述帧间齐次变换的变化关系为

其中，T_j＝(x_j,y_j,θ_j)∈R²×[0,2π]是位姿变换矩阵，j＝1时，T_j为基于Li-Fi的帧间齐次变换矩阵，j＝2时，T_j为基于激光的帧间齐次变换矩阵，表示数据点q_l经过变换矩阵T_j变换到q_l,f，f(T_j,q_l)为对应的映射变换函数，q_l＝(q_lx,q_ly)^T为观测坐标。

进一步地，确定基于Li-Fi的帧间齐次变换矩阵与基于激光的帧间齐次变换矩阵，具体包括，通过相对变换求解公式，确定基于Li-Fi的帧间齐次变换矩阵T₁与基于激光的帧间齐次变换对应的位姿变换矩阵T₂，所述相对变换求解公式为

其中，

为第k次迭代的位姿变换矩阵，

表示数据点q_l经过变换矩阵变换到q_l,f，

进一步地，对所述数据结合数据进行配准，具体包括，计算当前数据与相邻数据的欧式距离，取欧式距离最小的两个数据作为配准数据，实现对所述数据结合数据的配准。

进一步地，根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，具体包括，根据基于Li-Fi的帧间齐次变换矩阵、基于激光的帧间齐次变换矩阵及基于概率的权重系数，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵。

进一步地，基于灯光标定信息融合的激光SLAM方法，还包括，根据基于Li-Fi数据的观测概率模型获取基于Li-Fi数据的观测概率，根据基于激光数据的观测概率模型获取基于激光数据的观测概率，根据基于Li-Fi数据的观测概率和所述基于激光数据的观测概率获取基于概率的权重系数，其中，基于Li-Fi数据的观测概率模型为

基于激光数据的观测概率模型为

其中，δ_t是t时刻观测的高斯白噪声，均值为0，方差为Q_t，x_t是t时刻的机器人位姿，z_t,1是t时刻机器人基于Li-Fi最优链路的观测量，p(z_t,1|x_t)为位姿x_t下对于观测量z_t,1的观测概率；

K为激光传感器采集的激光观测数据的编号，为t时刻采集的编号为K的激光观测数据，z_t，2是t时刻机器人观测到的激光数据集，m是占用概率地图，p(z_t,2|x_t,m)为位姿x_t下对于观测量z_t,2的观测概率。

另一方面，本发明还提供了一种基于灯光标定信息融合的激光SLAM装置，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现如上述任一技术方案所述的基于灯光标定信息融合的激光SLAM方法。

另一方面，本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机该程序被处理器执行时，实现如上述任一技术方案所述的基于灯光标定信息融合的激光SLAM方法。

与现有技术相比，本发明的有益效果包括：通过获取LED的标定坐标、LED发射机的编码以及接受机的估计坐标，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接；根据所述最优链路链接，获取基于Li-Fi的当前数据帧，根据所述基于Li-Fi的当前数据帧，确定基于Li-Fi的帧间齐次变换矩阵；获取当前激光帧数据，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合帧数据，对所述数据结合数据进行配准，得到配准后的数据结合帧数据，根据所述配准后的结合帧数据，确定基于激光的帧间齐次变换矩阵；根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，根据所述融合Li-Fi和激光数据帧之间的位姿变换矩阵及配准后的数据结合帧数据，对当前帧与地图进行匹配并更新地图；抑制了位姿漂移，提高了建立的地图的准确性。

附图说明

图1为本发明实施例1所述的基于灯光标定信息融合的激光SLAM方法的其流程示意图；

图2为本发明实施例1所述的距离-区域模型示意图；

图3为本发明实施例1所述的机器人示意图；

图4为本发明实施例1所述的Li-Fi的链路链接数量与使用RSS室内定位得到机器人的定位误差关系图；

图5为本发明实施例1所述的raw odometry及pIC方法建立的环境地图；

图6为本发明实施例1所述的基于灯光标定信息融合的激光SLAM方法建立的环境地图；

图7为本发明实施例1所述的基于灯光标定信息融合的激光SLAM方法和Cartographer方法的回环精度、召回率对比的示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

实施例1

本发明实施例提供了一种基于灯光标定信息融合的激光SLAM方法，其流程示意图，如图1所示，所述方法包括以下步骤：

S1、获取LED的标定坐标、LED发射机的编码以及接受机的估计坐标，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接；

S2、根据所述最优链路链接，获取基于Li-Fi的当前数据帧，根据所述基于Li-Fi的当前数据帧，确定基于Li-Fi的帧间齐次变换矩阵；

S3、获取当前激光帧数据，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合帧数据，对所述数据结合数据进行配准，得到配准后的数据结合帧数据，根据所述配准后的结合帧数据，确定基于激光的帧间齐次变换矩阵；

S4、根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，根据所述融合Li-Fi和激光数据帧之间的位姿变换矩阵及配准后的数据结合帧数据，对当前帧与地图进行匹配并更新地图。

优选的，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接，具体包括，建立最优链路链接机制模型，通过所述最优链路链接机制模型，最优链路链接，其中，最优链路链接机制模型为

T_xi＝(x_i,y_i,i)，(x_i,y_j)为LED的标定坐标，i＝1,2,3是LED发射机编码，R_xt＝(x_e,y_e,t)，t是观测时刻，(x_e,y_e)为接受机的估计坐标，E＝(1,1,0)^T，

是Li-Fi链路数量；

需要说明的是，通过筛选Li-Fi数据，保证最优链路链接，接收端接收到的Li-Fi数据会有多个，由于Li-Fi数据的准确性与接受端和发射端之间的空间距离有关，所以要筛选出与接收端距离最近并且被接收到的Link_n个发射端发出的Li-Fi数据，Link_n值与具体环境有关；

优选的，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合数据，具体包括，根据所述Li-Fi齐次变换矩阵T₁将当前激光帧数据进行映射变换得到虚拟帧数据，对虚拟帧数据做空间相邻顺序预处理，得到预处理后的虚拟帧数据，将当前激光帧数据变换到参考帧坐标系下，得到参考帧数据，将参考帧数据加入到虚拟帧数据中得到数据结合帧数据；

需要说明是，进行相邻帧匹配，根据激光雷达返回的质量信号Q可以得到测距结果的可信度，由于激光雷达的工作特点，当实际测量距离超过一定值L时，质量Q会下降，测距信息变得不可靠，为了减少测量误差，需要对每帧点云数据进行筛选，舍弃低质量数据点，然后通过迭代方式结合Li-Fi信息配准点集；通过距离—区域模型加速最近邻点的查找以及回环检测时的数据预处理，在增加数据帧信息维的同时保证系统的实时性；

具体实施时，基于Li-Fi的数据点进行配准，由于灯源信息是人为标定的，灯源投影信息是相对准确的，机器人接收到同一灯源的信号相同，而测量、信号处理及计算引起的误差已经在Li-Fi位置概率模型中有所体现，基于位置概率的Li-Fi数据帧的齐次变换是准确的；计算出基于Li-Fi的齐次变换矩阵T₁，并依据变换矩阵T₁将当前帧数据(S_new,i＝{q_m,m＝1,L,M_j}，表示第i帧数据集中的m个观测量，包括基于Li-Fi和激光的观测量)，进行映射变换得到虚拟帧，以加快基于激光的点云数据的相对变换矩阵T₂的求解，并对虚拟帧中的数据做空间相邻顺序排列的预处理，虚拟帧形式如下

其中，f为映射变换函数，它通过帧间齐次变换将当前帧(目标帧)中的数据点q_l变换到参考帧(S_ref,i＝{p_n,n＝1,L,N_j}中，表示第i帧数据集中的n个观测量)坐标系下，得到数据点q_l,f；

优选的，将当前激光帧数据变换到参考帧坐标系下，得到参考帧数据，具体包括，通过帧间齐次变换将当前激光帧数据变换到参考帧坐标系下，所述帧间齐次变换的变化关系为

其中，T_j＝(x_j,y_j,θ_j)∈R²×[0,2π]是位姿变换矩阵，j＝1时，T_j为基于Li-Fi的帧间齐次变换矩阵，j＝2时，T_j为基于激光的帧间齐次变换矩阵，表示数据点q_l经过变换矩阵T_j变换到q_l,f，f(T_j,q_l)为对应的映射变换函数，q_l＝(q_lx,q_ly)^T为观测坐标；

将参考帧数据集加入到虚拟帧中，得到数据结合帧，如式下

其中，p_l′为参考帧中的第l′个数据。

一个具体实施例中，根据距离-区域模型从当前数据中心向数据轴的两端搜索，图2为距离-区域模型示意图，即首先对帧数据进行空间相对原点距离排序，再对一个数据的相邻域的数据进行遍历排序，根据排序结果，计算当前数据与相邻数据的欧式距离，距离最小的两个数据即为对应配准数据；通过距离-区域模型来减少数据匹配的计算量，可以加快寻找最近邻点，在帧-图匹配时也能更快寻找出关键帧，图2中，空白圆圈表示融合帧中的数据点，三角形表示参考帧中的数据点，涂黑圆圈表示灯光标定信息，第一行代表虚拟帧，第二行代表数据融合帧，箭头表示距离值增大的方向；

优选的，确定基于Li-Fi的帧间齐次变换矩阵与基于激光的帧间齐次变换矩阵，具体包括，通过相对变换求解公式，确定基于Li-Fi的帧间齐次变换矩阵T₁与基于激光的帧间齐次变换对应的位姿变换矩阵T₂，所述相对变换求解公式为

其中，为第k次迭代的位姿变换矩阵，

表示数据点q_l经过变换矩阵

变换到数据点q_l,f，

需要说明是，在找到使得激光点云数据对应点之间匹配度最高的的齐次变换点后，通过改变变换矩阵使得对应点之间的距离平方和最小；

优选的，对所述数据结合数据进行配准，具体包括，计算当前数据与相邻数据的欧式距离，取欧式距离最小的两个数据作为配准数据，实现对所述数据结合数据的配准。

优选的，根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，具体包括，根据基于Li-Fi的帧间齐次变换矩阵、基于激光的帧间齐次变换矩阵及基于概率的权重系数，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵。

基于激光和基于Li-Fi的点云数据是由不同的传感器所测量的，通过观测概率估计来确定T₁和T₂的权重，以融合基于激光的点云信息和基于Li-Fi的点云信息，然后重建激光帧之间的变换，并得到融合Li-Fi数据帧和激光数据帧之间的位姿变换T^k，

其中，k是迭代次数，T₁ ^k、

分别为第k次迭代时的基于Li-Fi和激光的位姿变换，w_prop是基于概率的权重系数，其数学模型如式

其中，ρ_j是归一化系数，使得所有观测量z_t,j(包括z_t,1：Li-Fi数据和z_t,2：激光数据)的概率和为1，m是由观测量得到的占用概率地图；

优选的，所述基于灯光标定信息融合的激光SLAM方法还包括，根据基于Li-Fi数据的观测概率模型获取基于Li-Fi数据的观测概率，根据基于激光数据的观测概率模型获取基于激光数据的观测概率，根据基于Li-Fi数据的观测概率和所述基于激光数据的观测概率获取基于概率的权重系数，其中，基于Li-Fi数据的观测概率模型为

基于激光数据的观测概率模型为

其中，δ_t是t时刻观测的高斯白噪声，均值为0，方差为Q_t，x_t是t时刻的机器人位姿，z_t,1是t时刻机器人基于Li-Fi最优链路的观测量，p(z_t,1|x_t)为位姿x_t下对于观测量z_t,1的观测概率；

K为激光传感器采集的激光观测数据的编号，为t时刻采集的编号为K的激光观测数据，z_t，2是t时刻机器人观测到的激光数据集，m是占用概率地图，p(z_t,2|x_t,m)为位姿x_t下对于观测量z_t,2的观测概率；

一个具体实施例中，建立数据观测概率模型，利用Extended Kalman Filter(EKF)构造Li-Fi数据的虚拟一致性状态分量，对激光传感器的扫描信息进行状态向量增广做概率估计，建立基于Li-Fi数据的观测概率模型，如下式所示

其中，δ_t是t时刻观测的高斯白噪声，均值为0，方差为Q_t，x_t是t时刻的机器人位姿，z_t,1是t时刻机器人基于Li-Fi最优链路的观测量，p(z_t,1|x_t)为位姿xt下对于观测量z_t,1的观测概率；基于激光数据的观测概率模型如下

其中，

K为激光传感器采集的激光观测数据的编号，为t时刻采集的编号为K的激光观测数据，z_t，2是t时刻机器人观测到的激光数据集，m是地图(由观测量得到的占用概率地图)，p(z_t,2|x_t,m)为位姿x_t下对于观测量zt,2的观测概率。

需要说明的是，通过建立了接收端与LED发射端之间的最优链路链接，保证基于Li-Fi的数据集误差最小，通过基于Li-Fi和激光的相邻帧间的位姿变换矩阵，并通过建立的Li-Fi数据的观测概率以及激光数据的观测概率，重构帧间齐次变换矩阵得到T^k；相邻时刻机器人的位姿通过位姿变换T^k建立联系，从而确定不同时刻机器人的位姿，并结合观测数据计算出地图每个栅格的占用值；在当前帧与地图进行匹配时，将当前帧数据从本体坐标系变换到世界坐标系下，并调取当前帧的数据集S_new,i，与地图数据集SM做交集运算，若S_new,i∩SM＝S_new,i且集合元素数目不少于Link_n，说明当前帧Li-Fi链路无遮挡，否则说明取交集运算无意义，应该只依据激光点云数据进行配准；然后建立当前帧与子图的数据对应关系，具体步骤与上述对Li-Fi数据点及数据结合帧的配准相同，如果对应点之间的空间欧式距离小于阈值，则将当前帧定义为回环帧，并进行相对变换的求解，这时的两帧数据在时间上不一定相邻，但在空间上是相邻的，结合在相邻空间位置上采集的观测数据计算栅格占用值并更新地图；否则，进入下一帧与地图的匹配；

一个具体实施例中，采用的装载光电传感器和180度HOKUYO激光传感器的Komodo机器人作为基于灯光标定信息融合的激光SLAM方法的载体，所述机器人示意图，如图3所示；Li-Fi的链路链接数量与使用RSS室内定位得到机器人的定位误差关系图，如图4所示，图4中横坐标为链路数量，纵坐标为每厘米的错误值，通过改变Li-Fi的链路链接数量，使用RSS室内定位得到机器人的定位误差，确定Link_n的值为4，

通过raw odometry、pIC以及基于灯光标定信息融合的激光SLAM方法按相同轨迹控制机器人行驶，建立环境地图，raw odometry及pIC方法建立的环境地图，如图5所示，图5左边为pIC方法建立的环境地图，图5右边为raw odometry方法建立的环境地图；基于灯光标定信息融合的激光SLAM方法建立的环境地图，如图6所示，可以看出，基于灯光标定信息融合的激光SLAM方法的视觉效果要比raw odometry、pIC更好，尤其是，它可以利用Li-Fi配准在实施环境中通透的地方进行帧配准，随后，通过标记起点和行驶路径，使机器人使用Cartographer和基于灯光标定信息融合的激光SLAM方法分别运行，每次都从同一点开始，然后返回到起点，重复多次，统计数据；基于灯光标定信息融合的激光SLAM方法和Cartographer方法(二维激光SLAM中主要的闭环检测方法)的回环精度、召回率对比的示意图，如图7所示，图7显示了基于灯光标定信息融合的激光SLAM方法(Li-Fi based SLAM)和Cartographer的回环精度和召回率；基于灯光标定信息融合的激光SLAM方法有着可靠的、稳定的综合性能，提高了回环检测的准确率，从而有效抑制位姿漂移。

实施例2

本发明实施例提供了一种基于灯光标定信息融合的激光SLAM装置，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现如上述实施例1所述的基于灯光标定信息融合的激光SLAM方法。

实施例3

本发明实施例提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机该程序被处理器执行时，实现如上述实施例1所述的基于灯光标定信息融合的激光SLAM方法。

本发明公开了一种基于灯光标定信息融合的激光SLAM方法、装置及计算机可读存储介质，通过获取LED的标定坐标、LED发射机的编码以及接受机的估计坐标，根据所述LED的标定坐标、LED发射机的编码以及接受机的估计坐标，获取最优链路链接；根据所述最优链路链接，获取基于Li-Fi的当前数据帧，根据所述基于Li-Fi的当前数据帧，确定基于Li-Fi的帧间齐次变换矩阵；获取当前激光帧数据，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合帧数据，对所述数据结合数据进行配准，得到配准后的数据结合帧数据，根据所述配准后的结合帧数据，确定基于激光的帧间齐次变换矩阵；根据基于Li-Fi的帧间齐次变换矩阵及基于激光的帧间齐次变换矩阵，获取融合Li-Fi和激光数据帧之间的位姿变换矩阵，根据所述融合Li-Fi和激光数据帧之间的位姿变换矩阵及配准后的数据结合帧数据，对当前帧与地图进行匹配并更新地图；抑制了位姿漂移，提高了建立的地图的准确性；

本发明所述技术方案构建了Li-Fi信息的虚拟一致状态，能提供更多环境信息，而距离区域模型的建立可以适当减少扫描匹配过程中的计算量；信息融合关闭机制得建立可以减少Li-Fi链路受阻塞的影响，从而使得系统更加平滑稳定；提出了用于Li-Fi数据校准的最短链路链接机制，使得系统链接Li-Fi链路保持最优，减少校准误差；能够利用激光和光电传感器采集的数据建立概率权重，来重建帧之间的变换，以减少数据关联误差；通过在走廊和室内办公环境的实验，验证了其能够在提高回环检测的准确率的同时及时发现回环帧、保证召回率，有效抑制位姿漂移。

以上所述本发明的具体实施方式，并不构成对本发明保护范围的限定。任何根据本发明的技术构思所做出的各种其他相应的改变与变形，均应包含在本发明权利要求的保护范围内。