基于全局特征描述子的激光SLAM回环检测方法( 三 ) _图像配准

计算得到的角度分量
，
，
，均以35维的向量表示，最后20维数据为当前帧点云
全部点的尺度分量
。
本文提出的全局描述子计算方法以点云中心建立相对坐标系进行计算，具有旋转不变性和平移不变性，如图3所示。图3(a)为原始点云
及其旋转45°、平移10 m后的点云，经旋转和平移变换后，如图3(b)和3(c)所示，点云的全局特征描述子不发生变化。其中：
为描述子
的维度信息；
为对应维度的数值信息。旋转不变性和平移不变性说明，对于同一场景下，激光雷达传感器处于不同角度和位置时，仍能进行准确地进行回环检测。
图3 旋转和平移不变性实验
使用点云全局特征描述子的皮尔逊相关系数[15]作为衡量点云相似度的标准，其计算公式如下：
式中：
，
为归一化后的点云全局特征描述子；
和
分别为
和
元素的平均值向量；
为数学期望；
为方差；
为相关系数，其取值范围为[0，1] 。当
接近于1时，说明特征描述子为正相关关系，点云的相似度越高[16] 。
1.3 位姿变换
对筛选后相似度大于阈值的当前帧点云
和历史相似帧点云
进行相对位姿变换的计算。相对位姿的计算使用点云的边角特征和平面特征配准方法，即使用三维点的曲率计算公式[13]计算点云不同位置处的光滑度，以光滑度为依据提取点云的边角特征和平面特征，只对边角特征和平面特征进行点云配准，提高了计算速度。
为减少点云的误匹配概率，使用曲率估计方法对点云配准效果进行验证。当前帧边角点和平面点分别在相似帧边角特征和平面特征中搜索匹配点，对相似帧的特征进行主成分分析，计算其特征值
。
其中，当点云为边角特征时，扫描点按照线方向分布；为平面特征时，扫描点呈现平面式分布，在垂直平面方向上的方差最小。边角特征和平面特征应分别满足如下关系式：
对于配准后的相似帧点云，构建需要求解的目标函数，使用-法迭代求解使目标函数最小的位姿变换，其目标函数如下：
式中：上标L和W分别为局部和世界坐标系；上划线-为测量值，~为估计值；
为当前边角特征点云
中一点
在激光雷达局部坐标系下的坐标；
为当前平面特征点云
中一点
在激光雷达局部坐标系下的坐标；
，
分别为相似帧边角特征点云在世界坐标系下距离
最近的两点坐标；
，
，
分别为相似帧平面特征在世界坐标系下距离
最近的三点坐标；
，
分别为边角和平面特征的目标函数，其含义为配准点到线或平面的距离；
和
为待求解变量，分别为当前坐标系到世界坐标系下的旋转四元数估计值和平移向量估计值。
边角特征和平面特征的配准如图4所示，其中：黑色点云为当前帧；绿色点云为历史相似帧；黑色加粗点为当前帧边角特征点坐标
和平面特征点坐标
；绿色加粗点分别为相似帧的边角特征点
，
构成直线，以及平面特征点
，
，
构成平面。
图4 边角特征和平面特征点云配准
1.4 因子图优化
通过位姿变换方法可以获得当前关键帧与历史相似帧之间的相对位姿变换，以此建立新的回环约束，构建因子图如图5所示。其中：
为测量函数；
为测量噪声协方差矩阵；因子图由一系列不同时刻位姿测量值
和地图路标点
的观测量构成。每条实线为1次前后帧之间的位姿累积测量事件，每条虚线为1次检测到的回环约束测量事件，将因子图表达转换为线性化的约束的线性矩阵