[发明专利]一种机器人三维点云地图构建与扩充方法

权利要求书

1.一种机器人三维点云地图构建与扩充方法，其特征在于，包括：

步骤1，通过扩展卡尔曼滤波融合多传感器信息测量得到当前机器人的位姿估计变换去除原始三维点云数据的运动失真，同时确定激光雷达扫描匹配前的先验位姿；

所述步骤1中，利用卡尔曼滤波融合多传感器信息，获取所述机器人的状态信息X：

X＝[p^T,q^T,v^T]^T (1)；

所述式(1)中，p为机器人位姿的三维位置向量；q为单位四元数，能生成旋转矩阵R∈SO(3)；v为三维速度向量；

将结合所述三维位置向量p与所述旋转矩阵R∈SO(3)得到的位姿估计变换T＝R|p∈SE(3)通过线性插值方式去除原始三维点云数据的运动失真；

通过所述位姿估计变换T将所述机器人的当前坐标系转换到地图坐标系下，得出激光雷达扫描匹配前的先验位姿T⁰；

步骤2，从去除运动失真后的三维点云数据提取特征关键帧，将所述特征关键帧与生成的局部点云地图进行扫描匹配，根据匹配结果对所述先验位姿进行修正获得激光雷达后验位姿，通过所述激光雷达后验位姿构建得到实时三维点云地图，保存所述实时三维点云地图作为离线三维点云地图，同时保存特征关键帧与对应的后验位姿；

所述步骤2中，从当前去除运动失真后的三维点云数据的扫描帧提取平面特征点F^p与边缘特征点F^e，提取的平面特征点F^p与边缘特征点F^e组成特征帧，当特征帧相对应的位姿估计变换超过位置1米和旋转角度10°的变化阈值时，提取该特征帧作为特征关键帧并舍弃未超过所述变化阈值的全部扫描帧；

步骤3，通过所述步骤2中已保存的特征关键帧与对应的后验位姿建立离线地图姿态图；在当前时刻接收到继续提取的特征关键帧时，对该特征关键帧建立里程计姿态约束和回环姿态约束，将该特征关键帧作为一个带有里程计姿态约束和回环姿态约束的状态节点，添加到所述离线地图姿态图中，对所述离线三维点云地图进行扩充；

所述步骤3中，通过以下方式通过所述步骤2中已保存的特征关键帧与对应的后验位姿建立离线地图姿态图：

将所述步骤2中已保存的每个特征关键帧作为一个状态节点x′，利用各特征关键帧对应的后验位姿建立状态节点之间的姿态空间约束形成所述离线地图姿态图T′_i-1,i＝(T′_i-1)^-1T′_i (9)；所述式(9)中，T′_i-1,i表示第i-1个特征关键帧作为的状态节点x′_i-1到第i个特征关键帧作为的状态节点x′_i的姿态空间约束；T′_i-1与T′_i分别为离线地图姿态图中第i-1与i个特征关键帧对应的后验位姿；

在当前时刻接收到继续提取的特征关键帧时，使用所述步骤2中扫描匹配方法建立该特征关键帧的里程计姿态约束；利用基于欧式距离的KNN搜索算法使该特征关键帧与所述离线三维点云地图进行扫描匹配建立回环姿态约束；以建立里程计姿态约束和回环姿态约束的特征关键帧作为一个状态节点，添加到所述离线地图姿态图中，对所述离线三维点云地图进行扩充；

所述里程计姿态约束的建立方式为：用当前接收的特征关键帧对应的后验位姿建立相邻状态节点之间的姿态空间约束，其中，所述离线地图姿态图的最后一个状态节点x′_n到当前继续建图的第一个状态节点x₀之间的空间约束P_n,0由所述机器人在离线地图坐标系中的初始位置确定；所述初始位置通过以下方式确定：在需要扩充三维点云地图时，加载离线三维点云地图后，在激光雷达第一次定位或扫描之前，根据已保存的所述离线三维点云地图原点的绝对测量信息，确定机器人在该离线三维点云地图坐标系中的初始位置；

所述回环姿态约束的建立方式为：用基于欧式距离的KNN搜索算法在离线地图姿态图各状态节点中搜索并提取与当前状态节点最近的一个特征关键帧；当判断该特征关键帧与当前状态节点在位姿上距离小于预定阈值时，提取该特征关键帧前后m个特征关键帧生成局部离线三维点云地图，与该特征关键帧进行扫描匹配得到该特征关键帧与当前状态节点之间的变换关系，在所述离线地图姿态图中添加回环姿态约束C_n,i。