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

申请/专利权人：合肥哈工图南智控机器人有限公司

摘要：本发明提供一种用于机器人室内外多传感器少参数的位姿优化方法、定位方法，基于ceres优化剔除点云数据的地面点云的面点云；将当前点和其n个邻居点，拟合出了一个平面；建立了定位系统的抽象模型，调用ceres库中的solve函数来求解对应的定位结果x0。重复定位计算直至收敛到一定阈值，得到Xrobot，Map之间的变换关系，即当前时刻的定位数据。本发明基于ceres去除地面点云特征点迭代优化方法，明显地提高实时定位算法处理速度；对算力要求小，实时性高，鲁棒性高；可兼容白天和夜晚以及光线不好的雨雾天气。

主权项：1.一种用于机器人室内外多传感器少参数的位姿优化方法，其特征在于，包括以下步骤：步骤1.对机器人搭载的激光雷达获取的测量数据进行预处理，得到点云数据；并基于ceres优化剔除点云数据的地面点云的面点云；步骤2.遍历去除地面点云和平面点云之后的点云数据，对每个点进行处理，将当前点根据用户给定的初始定位数据或上一次计算结果作为输入，将激光雷达坐标系转换到Map坐标系；步骤3.使用k-d树在Map中找到当前点的最近的n个邻居点；如果最远的邻居点与当前点的距离小于阈值m，则继续处理，否则跳过当前点；基于n个邻居点拟合出了一个平面；步骤4.计算当前点到平面的距离dist，并计算当前点在平面上的投影点point_proj；步骤5.构建定位模型：创建一个代价函数，代价函数定义了一个名为CostPlan的结构体，在CostPlan结构体中，operator函数被配置成接收两个参数：constT*PRi：为指向6维向量的指针，表示机器人的位姿，6维向量的前三个元素是位置，后三个元素是旋转的对数映射；T*residual：为指向3维向量的指针，用于存储计算出的残差；operator首先将输入的当前点和投影点转换为模板类型T，然后将输入的位姿映射为一个6维向量，使用基于C++和Eigen库开发的Sophus库，其中用来表示三维旋转群SO3类将旋转部分的对数映射转换为四元数，然后计算出从世界坐标系到局部坐标系的旋转和平移，最后计算出当前点在Map上的位置，并计算出当前点到投影点的距离，将其保存到残差中；步骤6.为了使得残差的平方和最小，调用ceres库中的solve函数来求解，得到相应的定位结果x0；步骤7.将获取的定位结果x0输出，作为步骤2的输入，重复以上步骤2-步骤7直至收敛到一定阈值，得到机器人位置姿态与Map之间的变换关系，即当前时刻的定位数据。

全文数据：

权利要求：

百度查询： 合肥哈工图南智控机器人有限公司 用于机器人室内外多传感器少参数的位姿优化方法、定位方法