买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：杭州电子科技大学

摘要：本发明公开了一种基于3D激光雷达的多特征融合IGV定位与建图方法。主要包括数据采集处理、扫描匹配及局部地图构建、后端优化、回环检测四个过程；数据采集处理是对3D激光雷达传感器的数据处理；扫描匹配及局部地图构建是对处理过的激光点云数据采用帧‑子图的匹配方式，本发明利用具有角度、距离、反光强度等多特征信息的三维地标解算初始位姿，通过占据栅格地图，构建局部最优子图；后端优化对于不断迭代的子图，采用图的优化策略，用高斯牛顿法解决优化问题，并利用三维地标加速求解过程，从而消除累计误差；回环检测中存储所有轨迹，采用多分辨率地图，通过分支定界法加速计算，完成闭环检测。最终实现AGV的高精度定位和建图。

主权项：1.一种基于3D激光雷达的多特征融合IGV定位与建图方法，其特征在于，包括如下步骤：S1：将整个地图场景划分为大小相同的若干立方体，赋予每个立方体一定概率值，其中三维地标区域概率值赋予1，构建三维占据栅格地图；在地图中加入具有撕裂点、角点、直线、圆弧多种特征的三维地标，用3D激光雷达传感器对三维地标进行标定，根据实际距离和角度信息定义IGV初始位姿；S2：对3D激光雷达传感器获取的数据进行预处理，记3D激光雷达采集的激光点为C，其中包含时间戳、反光强度、距离和角度信息；步骤S1定义的IGV在世界坐标系下初始位姿为x,y,θ，那么扫描到的激光点在世界坐标系下的坐标xc，yc为： 其中，d为C中包含的距离信息，θ为C中包含的角度信息；激光点C的点云数据结构由xc、yc、时间戳、反光强度构成，设在k时刻在雷达坐标系L下获得的一帧点云为其中tk＝k为获取这一帧点云时的时间戳，为k时刻在雷达坐标系L下点云中第i个点的坐标；由此可以根据雷达扫描时间、扫描角度、角度分辨率实现对三维点云数据有序化；最后，对有序化的点云采用范围滤波、体素滤波、点云去噪去除不需要的点云，输出预处理后的点云数据；其中，范围滤波去除不在3D激光雷达场景内的点云；体素滤波使用部分点云代替整体点云并保存原点云特征，降低点云数量；点云去噪是去除范围滤波和体素滤波产生的噪声；S3：对预处理后的3D激光雷达传感器数据进行扫描匹配，对于处理后的3D激光雷达传感器数据采用帧-子图的插入方式，其中一帧由若干点云组成，子图由若干帧组成；当前帧插入子图之前，需要对扫描帧位姿和当前的子图进行优化，让帧中所有激光点落在当前子图中的概率和最大，则问题可转化为求解以下最小二乘问题： 其中，ξ为IGV位姿，函数M为占据栅格地图中某个坐标为障碍物的概率，函数Siξ为一帧中i号激光点照射到的障碍物在占据栅格地图中的坐标；最小二乘问题为局部最优问题，需要一个初始位姿估计，若此时IGV静止，初始位姿可由步骤S1解算得出，若IGV运行中，则由编码器和IMU预测提供初始位姿；由此求解可得得到对当前帧和子图的位姿变换关系矩阵Tξ如下： 其中，Rξ为旋转矩阵，tξ为平移矩阵，子图使用概率网格描述；S4：局部地图构建，对于3D激光雷达传感器持续扫描匹配，采用子图集策略，首先构建子图集合，每个子图集合由若干子图构成，子图集的最后两个子图，分别记为P1和P2，当前帧与P1匹配，并插入P1和P2中，由于子图由若干帧组成，当P2中的帧数量达到阈值数目后，标记P1已经完成，并建新的子图，将原来的P2记为新的P1，新的子图则记为P2，继续进行当前帧的插入过程；通过子图集策略，增加点云占据栅格代价函数、平移代价函数、旋转代价函数三种约束，构建局部最优子图；S5：后端优化，由于步骤S3中激光雷达传感器扫描帧仅与当前子图进行匹配，环境地图是由一系列子图构成的，因此会存在累积误差；对此，3D激光雷达传感器获取的扫描帧在插入子图时的位姿会被缓存到内存，当子图不再变化时，所有的扫描帧和子图都会被用来进行闭环检测；采用图的优化方式，以多特征三维地标和IGV位姿为顶点，以步骤S3得到的当前帧和子图的位姿变换关系矩阵和此时IGV观测的位姿变换关系的误差函数为边，总体优化问题变为若干条边加和的形式，构建目标优化函数如下： 上式中，函数E为误差函数,通过分别表示在一定约束条件下，子图的位姿和扫描帧的位姿，m为子图数量，n为扫描帧数量；相对位姿ξij表示扫描帧j在子图i中的匹配位置，与其相关的协方差矩阵Ωij共同构成优化约束；结合高斯牛顿法进行优化问题的快速求解，得到帧和子图优化后的转换关系；S6：轨迹位姿获取，在步骤S4构建局部地图的同时，结合运动控制量的输入，多特征三维地标作为观测量，使用IMU和编码器预测IGV位姿，获得IGV运行轨迹；S7：闭环检测，对于步骤S6获得的轨迹位姿进行存储，再次经过轨迹上某一点时，采用分支定界法快速遍历所有轨迹，得到历史帧；若当前帧和历史帧匹配成功，则在步骤S5中图优化问题中增加一条边，形成约束完成闭环检测。

全文数据：

权利要求：

百度查询： 杭州电子科技大学 一种基于3D激光雷达的多特征融合IGV定位与建图方法