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

申请/专利权人：深圳华芯信息技术股份有限公司

摘要：本发明的多子图激光雷达定位方法、系统以及终端，利用激光雷达传感器、里程计和陀螺仪采集的数据构建至少两个子图，并利用当前激光雷达的扫描点与已建好的子图进行扫描匹配，结合多子图可以获得高精度位姿，减少累计误差；利用当前激光雷达扫描的环境信息与已建立的子图进行闭环检测，从而建立约束，消除定位累积误差，解决了累积误差难以消除的问题；并且当机器人受到动态物体干扰而导致错误定位时，开启重定位线程或融合定位线程，有效解决了机器人长时间不知道自己的位置，失去了自主行为能力的问题，增加了机器人的灵活性，还可以有效解决定位错误的情况，增加了机器人工作的稳定性和定位的精准性。

主权项：1.一种多子图激光雷达定位方法，其特征在于，应用于移动机器人，其上设有激光雷达传感器、里程计和陀螺仪，所述方法包括：基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建的地图坐标系，分别构建当前环境下的至少两个局部子图以及一全局子图，以获得由各子图组成的二维地图；其中，所述二维地图包括：各障碍物在所述地图坐标系下的坐标位置信息；该步骤具体包括：以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建地图坐标系；将所述激光雷达传感器采集的当前环境下各障碍物的位置信息转换为同一机器人坐标系下，再转换为世界坐标系，以获得所述激光雷达传感器采集的当前环境下各障碍物的运动畸变去除位置信息；基于激光雷达帧阈值，依次构建一或多个局部子图，以获得由各局部子图以及全局子图组成的二维地图；其中，全局子图从开始到建图结束都一直存在，用来进行扫描匹配获取位姿；局部子图只进行插入障碍物操作并更新；当激光雷达到达帧到达一定阈值时，不再插入障碍物信息了；将所述激光雷达传感器当前时刻采集的障碍物的位置信息与二维地图中的全局子图所对应的各障碍物在地图坐标系下的坐标位置信息分别进行扫描匹配，以获得所述机器人当前时刻的最优位姿；根据所述最优位姿获得所述机器人当前的最终位姿，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息以所述最终位姿插入至各局部子图和全局子图中，以更新所述二维地图以及各局部子图以及全局子图的轨迹信息；其中，该步骤具体包括：基于定位判断条件，判断在定位过程中是否出现定位不准的情况；若未出现定位不准的情况，则将所述最优位姿作为所述机器人的最终位姿；若出现定位不准的情况，则将当前时刻的预测位姿作为当前时刻的最终位姿；将所述激光雷达传感器当前时刻采集的障碍物的位置信息以所述最终位姿插入至各子图中，以更新所述二维地图以及各子图的轨迹信息；接收到对应障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存；其中，所述最终二维地图包括：各障碍物在所述地图坐标系下的最终坐标位置信息以及各子图的最终轨迹信息；当所述机器人重新启动时，加载保存的所述最终二维地图，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息与所述二维地图中的全局子图的各障碍物在所述地图坐标系下的最终坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿以及具有当前轨迹信息的局部子图以及全局子图；利用具有上一轨迹信息的局部子图对当前轨迹信息中的轨迹节点以及具有当前轨迹信息的完成的旧局部子图和上一轨迹信息已保存的轨迹节点进行闭环检测，建立闭环约束；其中，该步骤具体包括：对具有当前轨迹信息的局部子图的数量进行检验，当所述局部子图数量大于两个时，将最先构建的子图删除掉；当前轨迹下的最新插入的两个局部子图与当前轨迹节点建立观测约束；利用具有上一轨迹信息的各局部子图对当前轨迹信息中的轨迹节点进行闭环检测，并建立闭环约束；完成状态下的旧局部子图和上一轨迹的所有已保存的轨迹节点进行闭环检测，并建立闭环约束；当具有当前轨迹信息的局部子图所对应的轨迹节点的个数大于优化阈值时，开始计算约束，进行优化求解，获取优化后的当前轨迹下的局部子图的全局位姿和轨迹节点的全局位姿，并更新当前时刻的最优位姿，以获得当前时刻的最终位姿；其中，该步骤具体包括：当具有当前轨迹信息的子图所对应的轨迹节点个数大于优化阈值时，添加优化变量，构建残差，开始计算约束；获取优化后的当前轨迹下的局部子图的全局位姿和轨迹节点的全局位姿，并进行更新；基于更新后的具有当前轨迹信息的局部子图所对应的全局位姿获得转移矩阵，并根据所述转移矩阵更新所述当前时刻的最优位姿，以获得当前时刻的最终位姿；其中，将所述激光雷达传感器当前时刻采集的障碍物的位置信息与所述二维地图中的全局子图所对应的各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿的方式包括：基于由所述里程计以及陀螺仪实时采集的机器人的位姿信息和或上一时刻的最优位姿，计算获得机器人的加速度以及线速度，以获得机器人的位姿变化信息；根据所述机器人的位姿变化信息，获得当前时刻的预测位姿；将所述预测位姿作为当前时刻激光雷达扫描的中心位姿且以设定的第一搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上局部子图中各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分。

全文数据：

权利要求：

百度查询： 深圳华芯信息技术股份有限公司 多子图激光雷达定位方法、系统以及终端