[发明专利]定位方法、装置、电子设备和存储介质

权利要求书

1.一种定位方法，其特征在于，所述方法包括：

检测到激光定位失效后，通过视觉定位得到视觉定位粒子集合；

基于激光定位空间与视觉定位空间之间的映射关系，将所述视觉定位粒子集合映射到激光定位空间，得到激光定位粒子集合；其中，根据以下方法获得所述映射关系：

分别采用激光定位方法和视觉定位方法构建预设环境的地图，得到激光定位方法对应的第一地图以及视觉定位方法对应的第二地图；

对所述第一地图和所述第二地图进行匹配处理，得到所述映射关系；

获取所述第一地图中的位姿点集P和所述第二地图中的位姿点集Q；

根据中心化公式对所述位姿点集P、所述位姿点集Q中的位姿进行中心化；其中，所述中心化公式为：

在所述中心化公式中：p_i为所述位姿点集P中的第i个粒子，q_i为所述位姿点集Q中的第i个粒子，所述位姿点集P和所述位姿点集Q中的粒子数量均为n，i和n均为正整数，所述为所述位姿点集P中心化后的结果，所述为所述位姿点集Q中心化后的结果；

根据第一地图更新公式更新所述第一地图，并根据第二地图更新公式更新所述第二地图：

所述第一地图更新公式为：其中，p_i为所述位姿点集P中的第i个粒子，p′_i为所述p_i的更新结果；

所述第二地图更新公式为：其中，q_i为所述位姿点集Q中的第i个粒子，q′_i为所述q_i的更新结果；

采用所述激光定位粒子集合，得到用于表示激光定位的位置信息。

2.根据权利要求1所述的方法，其特征在于，所述对所述第一地图和所述第二地图进行匹配处理，得到所述映射关系，包括：

针对所述第一地图和所述第二地图，采用相同建图起始点，在世界坐标系中确定所述第一地图和所述第二地图之间的旋转矩阵、位移矩阵和尺度因子；

根据所述旋转矩阵、所述位移矩阵和所述尺度因子得到所述第一地图和所述第二地图之间的相似变换矩阵作为所述映射关系。

3.根据权利要求2所述的方法，其特征在于，所述方法还包括：

根据以下方法确定所述旋转矩阵：

根据矩阵构建公式构建矩阵M；所述矩阵构建公式为其中，T表示转置矩阵；

对所述矩阵M进行奇异值分解，得到分解后的矩阵V，矩阵U；

根据分解后的矩阵V，矩阵U，得到旋转矩阵R^*，其中：R^*＝VU^T，T表示转置矩阵。

4.根据权利要求2所述的方法，其特征在于，所述方法还包括：

根据以下方法确定所述位移矩阵：

根据位移矩阵公式确定所述位移矩阵t^*；

所述位移矩阵公式为：

其中：t^*为位移矩阵，R^*为旋转矩阵。

5.根据权利要求2所述的方法，其特征在于，所述方法还包括：

根据以下方法确定所述尺度因子：

根据所述第一地图更新公式以及所述第二地图更新公式获取尺度因子s；

所述尺度因子公式为：

其中：s为尺度因子，p′_i为所述位姿点集P中的第i个粒子的更新结果，q′_i为所述位姿点集Q中的第i个粒子的更新结果，| || |表示范数，所述位姿点集P和所述位姿点集Q中的粒子数量均为n。