CN113538410B 一种基于3d激光雷达和uwb的室内slam建图方法 （广东工业大学）

一种基于3D激光雷达和UWB的室内SLAM建图本发明公开了一种基于3D激光雷达和UWB的室内SLAM建图方法，在室内场景中部署UWB定位索该室内场景区域，利用融合激光雷达数据和UWB数据的SLAM算法生成所探索区域的地图。具UWB定位坐标系之间的相对位姿变换；S2、利用UWB数据为激光里程计的帧间匹配提供初始值；S3、利用UWB数据为SLAM后端位姿图添加约束；21.一种基于3D激光雷达和UWB的室内SLAM建图方法，其特征在于，在室内场景中部署所述步骤S1确定3D激光SLAM坐标系和UWB定位坐标系之间的相对位姿变换的具体过程S1-2、在SLAM算法中融入UWB数据之前，分别利用UWB定位算法和仅使用激光数据的激光SLAM坐标系下(L)的三维运动轨迹pi={=(,残,碳)3通过式(11)即可获取包括3D激光SLAM坐标系下的UWB定位所述步骤S3利用UWB数据为SLAM后端位姿图添若当前传感器的位置处于由各个UWB基站若当前传感器的位置处于由各个UWB基站节点构成的包络面区域外，则按以下方法处ez=lldneon-ll(s)4点pointi到激光雷达中心0的距离，dj代表K中除去pointi的其他点到激光雷达中心0的距为编码矩阵M生成一个行数相同的编码列向量v接着确定Mselect和当前时刻的关键帧的编码矩阵Mcurrent之间的列平移数shift，求取5式(26)中，和pleet为对应编码矩将当前关键帧点云和查找到的历史关键帧点云进行点2.根据权利要求1所述的一种基于3D激光雷达和UWB的室内SLAM建图方法，其特征在3.根据权利要求1所述的一种基于3D激光雷达和UWB的室内SLAM建图方法，其特征在6[0005]动态的室内地图构建主要是基于即时定位与地图构建技术(simultaneous[0008]本发明的目的在于克服现有技术的不足，提供一种基于3D激光雷达和UWB的室内[0010]一种基于3D激光雷达和UWB的室内SLAM建图方法，在室内场景中部署UWB定位系7[0016]进一步地，所述步骤S1确定3D激光SLAM坐标系和UWB定位坐标系之间的相对位姿8T2-xT1。[0047]利用获取得到的旋转矩阵R和位移向量tl，将位于UWB坐标系下的UWB基站节点投[0052]若当前传感器的位置处于由各个UWB基站节点构成的包络面区域外，则按以下方9[0056]将t时刻测得的最近的基站坐标xmean作为路标加入到SLAM的后端位姿图，并根据代表点pointi到激光雷达中心O的距离,dj代表K中除去pointi的其他点到激光雷达中心O的[0070]式(22)中，L1和L2分别表示水平面上，距离激光雷达中心O的最大距离和最小距[0071]根据划分的空间区域生成一个规格为的编码矩阵M，编码矩阵M的每[0072]在SLAM算法运行的过程中，将生成的每个关键帧的对应的编码矩阵M存入数据库[0088]2)激光SLAM算法在使用前，不需要通过测量的手段获取部署的每个UWB基站节点相对于初始时刻的激光雷达的空间位置(即不需要在算法前通过手动测量手段获取UWB坐标系和初始激光SLAM坐标系之间的位姿变换)，可以在算法运行后根据获取的激光SLAM和UWB定位的轨迹坐标点求取两个坐标系之间的相对位姿变换，从而将UWB基站坐标等UWB数室内场景中部署UWB定位系统，并通过携带3D激光雷达传感器的机器人探索该室内场景区[0101]本实施例在室内场景中部署UWB定位系统，并通过携带3D激光雷达传感器的人或机器人探索该室内区域，利用融合激光雷达数据和UWB数据的SLAM算法生成所探索区域的个基站之间的测距信息即可解算得到标签节点在U可以在初始时刻下，测量3D激光雷达与UWB各个基站节点之间的距离信息，从而解算得到站节点之间的空间相对几何信息，即只能通过各个基站节点之间的测距信息初始化得到[0106]针对事先不知道UWB坐标系和3D激光SLAM坐标系之间相对位姿变换的场景，本实[0118]由于3D激光雷达的工作频率(一般为10Hz)相比于UWB定位更低，且由于SLAM采用[0141]本实施例利用UWB定位给出的两个相邻激光点云帧对应的UWB定位位置xT1和xT2，[0144]利用上述获取得到的旋转矩阵R和位移向量tl，将位于UWB坐标系下的UWB基站节[0146]若当前传感器的位置处于由各个UWB基站节点构成的包络面内(图4所示)，则UWB[0159]传统激光SLAM算法(如LeGO-LOAM等算法)常用的回环检测方式是直接法：即根据当前时刻的关键帧位姿Pcurrent的位置分量xcurrent，在历史关键帧数据库中阈值要求内的候选历史关键帧，即无法找到满足式(13)的目标关键帧(虽然实际是存在代表点pointi到激光雷达中心O的距离,dj代表K中除去pointi的其他点到激光雷达中心O的[0179]式(22)中，L1和L2分别表示水平面上，距离激光雷达中心O的最大距离和最小距[0181]在SLAM算法运行的过程中，将生成的每个关键帧的对应的编码矩阵M存入数据库[0188]接着还需要确定Mselect和当前时刻的关键帧的编码矩阵Mcurrent之间的列平移数为jcurrent，然后遍历Mselect中的每一个列向量，找到一个最相似的列向量，记所在的列为[0197]以上所述之