[发明专利]基于双窗口的激光里程计方法、系统、装置

权利要求书

1.一种基于双窗口的激光里程计方法，其特征在于，该方法包括：

步骤S100，服务机器人通过激光雷达传感器感知环境，获取周围环境在激光雷达传感器当前帧坐标系下的原始三维点云数据；

步骤S200，从原始三维点云数据中提取特征，并构建特征集，作为第一特征集；

步骤S300，若激光雷达传感器当前帧是第1帧，则将其作为关键帧，并利用第一特征集构建第一环境特征集，构建后，跳转至步骤S100，否则，跳转至步骤S400；所述关键帧为从激光雷达传感器获取的所有帧中按设定的位姿间距阈值筛选的帧；

步骤S400，根据激光雷达传感器当前帧及其外部大窗口中的关键帧对应的坐标系与世界坐标系的坐标转换关系，将第一特征集、第一环境特征集转换到世界坐标系中，分别得到第二特征集、第二环境特征集；

步骤S500，基于预设的特征关联规则，计算第二特征集中的特征在第二环境特征集中的候选关联特征，进而确定出最佳关联特征；

步骤S600，基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数；利用LM算法求解位姿优化函数，得到激光雷达传感器当前帧坐标系到世界坐标系的优化后位姿，并基于优化后的位姿更新激光雷达传感器当前帧坐标系到世界坐标系的位姿；

步骤S700，循环执行步骤S400-步骤S600 K次，循环完成后，计算更新后的激光雷达传感器当前帧坐标系到世界坐标系的位姿、最近关键帧坐标系到世界坐标系的位姿之间的间距，若该间距大于等于设定的位姿间距阈值，则将当前帧作为新的关键帧，并根据第二特征集的特征及其在第二环境特征集中的最佳关联特征，利用第一特征集更新第一环境特征集，跳转至步骤S800，否则，跳转至步骤S100；其中，K为预设的循环次数；

步骤S800，基于第一环境特征集，构建内部小窗口中的关键帧的多帧位姿优化函数；利用LM算法求解多帧位姿优化函数，得到内部小窗口中的所有关键帧的坐标系到世界坐标系的优化后位姿，进而更新内部小窗口中的所有关键帧的坐标系到世界坐标系的位姿，更新后，跳转至步骤S100。

2.根据权利要求1所述的基于双窗口的激光里程计方法，其特征在于，基于预设的特征关联规则，计算第二特征集中的特征在第二环境特征集中的候选关联特征，进而确定出最佳关联特征，其方法为：

对于第二特征集P₂中的特征首先利用PCL点云库中的KD-tree算法在第二环境特征集Q₂中寻找满足点点距离约束即的特征，构成特征集Ω_dd，其中‖·‖是向量欧式范数，r是第一预设阈值；

在特征集Ω_dd中进一步筛选出满足点面距离约束即的特征，构成的候选关联特征集Ω_c，其中(p，n)∈Ω_dd，d为第二预设阈值；

在候选关联特征集Ω_c中寻找使得最小的特征，记为其中(p′，n′)∈Ω_c，为第二特征集P₂中的特征在第二环境特征集Q₂中的最佳关联特征。

3.根据权利要求2所述的基于双窗口的激光里程计方法，其特征在于，基于第二特征集的特征及其在第二环境特征集中的最佳关联特征，构建位姿优化函数，其方法为：

其中，R_wl和t_wl分别是T_wl的旋转矩阵和平移向量，T_wl是激光雷达传感器当前帧L_l的坐标系到世界坐标系W的位姿，是传感器当前帧L_l的坐标系到世界坐标系W的优化后位姿，p_a是第二特征集P₂中的特征在第一特征集P₁中对应特征的位置，和分别是特征在第二环境特征集Q₂中的最佳关联特征的位置和方向，是矩阵转置算子，‖·‖表示向量欧式范数，为位姿所在的流形空间，|P₂|为第二特征集P₂中的特征数量，a表示第二特征集P₂中的特征编号。