申请/专利权人：苏州观瑞汽车技术有限公司

申请日：2023-08-04

公开（公告）日：2023-12-01

公开（公告）号：CN116698046B

主分类号：G01C21/20

分类号：G01C21/20;G01C21/00;G01S17/86;G01S17/89;G01C21/16;G01C22/00

优先权：

专利状态码：有效-授权

法律状态：2023.12.01#授权;2023.09.22#实质审查的生效;2023.09.05#公开

摘要：本发明公开了一种物业室内服务机器人建图定位和回环检测方法，涉及机器人图像定位检测技术领域，包括：采集机器人摄像前端数据并进行预处理；构建相邻状态量之间的约束关系，加入后端状态估计优化中；进行最优匹配优化姿态约束，获取回环检测结果。本发明提供的物业室内服务机器人建图定位和回环检测方法通过激光雷达和视觉耦合的回环检测，有效修正系统的轨迹偏移，使地图具有更好的一致性。融合点云信息以保证机器人在复杂室内环境下的可靠定位，且在定位异常时能及时报警，并可以实时或离线建图，要求回环正常、无重影，且回环异常时可手动纠正。

主权项：1.一种物业室内服务机器人建图定位和回环检测方法，其特征在于，包括：采集机器人摄像前端数据并进行预处理；构建相邻状态量之间的约束关系，加入后端状态估计优化中；进行最优匹配优化姿态约束，获取回环检测结果；所述采集机器人摄像前端数据并进行预处理包括获取机器人激光雷达摄像信息在激光雷达点云中提取结构特征信息，计算激光雷达里程计，以第一帧激光雷达坐标系为基准构建一个局部地图坐标系使第一帧激光雷达坐标系与系的转换关系为单位矩阵，将设为激光雷达里程计坐标系，后续的点云帧的最新姿态在系下进行表示；基于每个点在所属环的曲率信息在一帧点云中提取平面点psurf和边缘点pedge，曲率小于预设阈值的平坦点被视为平面点，曲率大于阈值的点被视为边缘点，若当前特征点相邻环的邻近点不属于平面特征，当前特征点也不视为平面点；以IMU推算姿态作为帧到局部地图匹配的初值，将当前帧与局部地图进行配准，得到激光雷达里程计位姿li表示第i帧激光雷达坐标系；积分项替换包括：对恢复尺度信息进行积分项替换，得到位置，速度和姿态的预积分量，表示为： 其中，为位置预积分量，为速度预积分量，为姿态预积分量，表示差乘，表示b坐标系下由t到k时间的转换矩阵，t表示从第k到第k+1的时间，和分别是以随机游走方式建模的加速度和角速度偏置，和分别表示为IMU自身坐标系的带有噪声的加速度和角速度，为IMU坐标系在世界坐标系下的姿态；k为当前时刻，k+1时刻系统状态量的估计值表示为： 分别表示IMU坐标第k+1时刻系在世界坐标系下的位置、速度、姿态，分别为IMU坐标系第k时刻在世界坐标系下的位置、速度、姿态，Δtk表示时间差，gw表示在世界坐标系下的重力加速度向量，表示k时间由b坐标系到w坐标系之间的转换矩阵，na和ng分别表示采用高斯白噪声进行建模的加速度和角速度IMU测量噪声；构建视觉重投影残差约束包括计算特征点残差与雅可比矩阵，计算特征点归一化坐标的实际值与理论值差值，表示为： 其中，1表示特征点pk在第k帧图像归一化平面上的坐标，是特征点在第j帧图像上理论归一化平面坐标，和是特征点在第i和j帧图像上实际测量得到的归一化平面坐标，为视觉重投影残差方程，表示深度信息，i表示首次被观测到的帧序列号，和表示状态量估值，和表示相机与IMU的外参，和分别表示状态实际值；完成残差求解后，求解雅克比矩阵，表示为： 其中，表示雅克比矩阵求解结果，x表示与当前视觉重投影约束的状态量，rc表示IMU视觉约束下观测值与待优化状态量之间的残差；将姿态从特殊欧式群T∈SE3分解为特殊正交群R∈SO3和平移向量t∈R3，表示为： 其中，rL表示IMU激光雷达约束下观测值与待优化状态量之间的残差，i和j为相邻两组关键帧的序列号，和为IMU到相机坐标系之间的转换矩阵，表示旋转矩阵到代数之间的转换，为激光雷达在i时间配准得到的里程计相对位姿，为激光雷达在j时间配准得到的里程计相对位姿，和表示IMU在滑窗内下的第j个平移位置和观测位置，为激光雷达在i时间配准得到的里程计相对位姿；完成残差计算后，对i时刻的旋转矩阵以及j时刻的旋转矩阵和位置求导，表示为： 其中，和分别表示特殊正交群SO3下在φ处的右雅可比和左雅克比，I3×3表示单位矩阵。

全文数据：

权利要求：

百度查询： 苏州观瑞汽车技术有限公司 一种物业室内服务机器人建图定位和回环检测方法及系统

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD