[发明专利]一种基于灯光标定信息融合的激光SLAM方法及装置

权利要求书

1.一种基于灯光标定信息融合的激光SLAM方法，其特征在于，包括以下步骤：

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

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

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

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

2.根据权利要求1所述的基于灯光标定信息融合的激光SLAM方法，其特征在于，根据所述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，Link_n是Li-Fi链路数量。

3.根据权利要求1所述的基于灯光标定信息融合的激光SLAM方法，其特征在于，根据当前激光帧数据及基于Li-Fi的帧间齐次变换矩阵，获取数据结合帧数据，具体包括，根据所述Li-Fi齐次变换矩阵T₁将当前激光帧数据进行映射变换得到虚拟帧数据，对虚拟帧数据做空间相邻顺序预处理，得到预处理后的虚拟帧数据，将当前激光帧数据变换到参考帧坐标系下，得到参考帧数据，将参考帧数据加入到虚拟帧数据中得到数据结合帧数据。

4.根据权利要求3所述的基于灯光标定信息融合的激光SLAM方法，其特征在于，将当前激光帧数据变换到参考帧坐标系下，得到参考帧数据，具体包括，通过帧间齐次变换将当前激光帧数据变换到参考帧坐标系下，所述帧间齐次变换的变化关系为

其中，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为观测坐标。

5.根据权利要求4所述的基于灯光标定信息融合的激光SLAM方法，其特征在于，确定基于Li-Fi的帧间齐次变换矩阵与基于激光的帧间齐次变换矩阵，具体包括，通过相对变换求解公式，确定基于Li-Fi的帧间齐次变换矩阵T₁与基于激光的帧间齐次变换对应的位姿变换矩阵T₂，所述相对变换求解公式为

其中，为第k次迭代的位姿变换矩阵，表示数据点q_l经过变换矩阵变换到数据点q_l,f，