[发明专利]坐标系转换方法、装置及包含该装置的多源融合SLAM系统

权利要求书

1.用于多源融合SLAM系统的坐标系转换方法，其中，所述多源融合SLAM系统至少包括第一SLAM系统和第二SLAM系统，所述用于多源融合SLAM系统的坐标系转换方法包括：

通过所述第一SLAM系统获得运动载体在第一坐标系内的第一运动轨迹；

通过所述第二SLAM系统获得所述运动载体在第二坐标系内的第二运动轨迹；

基于所述第一运动轨迹和所述第二运动轨迹确定所述第一坐标系与所述第二坐标系之间的坐标系转换参数；以及

根据所述坐标系转换参数在所述第一坐标系与所述第二坐标系之间进行坐标系转换。

2.如权利要求1所述的用于多源融合SLAM系统的坐标系转换方法，其中，

对所述第一运动轨迹和所述第二运动轨迹同时进行采样，以获得来自所述第一运动轨迹的多个第一采样点以及对应的来自所述第二运动轨迹的多个第二采样点，并且所述多个第一采样点和所述多个第二采样点均不少于4个；

对所述多个第一采样点和所述多个第二采样点进行中心化处理，以获得多个第一中心化采样点以及多个第二中心化采样点；以及

对所述多个第一中心化采样点和所述多个第二中心化采样点进行正交普氏分析，以确定所述坐标系转换参数。

3.如权利要求2所述的用于多源融合SLAM系统的坐标系转换方法，其中，

所述坐标系转换参数包括所述第一坐标系与所述第二坐标系之间的旋转转换矩阵。

4.如权利要求3所述的用于多源融合SLAM系统的坐标系转换方法，其中，通过以下确定所述旋转转换矩阵，

构造

通过奇异值分解，获得U、Σ和V；以及

基于U、Σ和V获得

其中，

其中，

表示所述多个第一中心化采样点对应的矩阵，

表示所述多个第二中心化采样点对应的矩阵，

U表示第一中间矩阵，Σ表示第二中间矩阵，V表示第三中间矩阵，以及

表示所述旋转转换矩阵。

5.如权利要求4所述的用于多源融合SLAM系统的坐标系转换方法，其中，通过将X_L去除x_L，means获得以及通过将X_U去除x_U,means获得

其中，X_L表示所述多个第一采样点对应的矩阵，

其中，和表示来自所述第一运动轨迹的第i个采样点的坐标，i＝1…m，m≥4，

x_L,means表示X_L的均值列向量，

其中，X_U表示所述多个第二采样点对应的矩阵，

其中，和表示来自所述第二运动轨迹的第i个采样点的坐标，

x_U,means表示X_U的均值列向量。

6.如权利要求2所述的用于多源融合SLAM系统的坐标系转换方法，其中，

所述坐标系转换参数包括所述第一坐标系与所述第二坐标系之间的位姿转换矩阵。