基于新的图优化方法的拓扑地图创建*

黄宏前石朝侠王燕清

（1.南京理工大学计算机科学与工程学院 南京 210094）（2.南京晓庄学院信息工程学院 南京 211171）

1 引言

SLAM是移动机器人领域研究的热点。机器人需要通过自主导航和决策，完成相应的任务。而闭环检测和检测之后的地图优化也是相当重要的一环。仅使用里程计航迹推算信息的SLAM系统，误差随着时间的积累增加导致创建的地图误差会越来越大。所以实现闭环检测和地图优化是一个非常重要的内容。

在SLAM发展过程中，实现闭环检测和地图优化的方法很多，如图优化理论算法［1～3］。图通常是由节点和边构成，在SLAM领域中，图的节点通常是由机器人的位姿构成，图中的边通常是由当前时刻和上一时刻的位姿变换关系构成的，或者是由视觉里程计计算出来的位姿转换矩阵构成的。图构建好之后就需要优化节点位姿去满足图中边的约束，这就是SLAM中主流的图优化方法。图优化方法应用在各个地方，Olson等［4］使用了随机梯度下降的方法来求解优化的非线性方程，利用两个位姿之间的边和它们之间的变换关系来恢复机器人的轨迹，可以在初始值较差的情况下取得较好的结果。翁潇文等［5］利用激光传感器获得观测数据来建立边，根据图优化理论进行匹配优化求解出误差最小的位姿。在三维的空间中［6～8］，利用相机模型，通过在相邻图像之间匹配获得的位姿变换和提取图像的特征点放在一起创建成图进行优化，利用闭环检测和捆绑约束BA的方法，可以消除由于里程计漂移造成的误差问题。拓扑地图因简洁而不需要大量计算的特点，在很多方面也有应用。Hou等［9］将真实扫描的地图表示为拓扑地图，顶点表示面积，边线表示通道，利用Voronoi图生成表示环境的拓扑地图。Sren等［10］利用拓扑地图来检测房间并且用与真实地面图的拓扑进行匹配，形成映射，完成房间的检测。在车辆领域，也可以使用一种基于拓扑地图构建和场景识别方法来对车辆进行定位［11］，使用图像序列生成拓扑图，利用Extended-HCT方法进行语义描述和特征提取来对车辆进行识别和定位。

可见，图优化在地图创建和优化方面是很重要的一环。图优化的方法在大部分情况下是非常有效的。但是如果系统中仅有里程计航迹推算信息，是很难对地图进行调整和优化。本文针对这个问题，提出了一种新的图优化方法，只采用里程计航迹推算信息，引入机械臂逆向运动学模型减小位姿跟踪和地图创建误差，并使用模拟退火算法实现变步长快速迭代，完成拓扑地图的创建。

2 基于机械臂逆向运动学的图优化方法

2.1 拓扑图边及节点定义

拓扑地图是由一系列的边和节点构成的。在本文中，节点的定义为地图中路口的位姿。在每一个位姿中，都有包含当前路口的全局坐标（x，y），当前路口的分支个数branch_num、每一个分支相对当前路口机器人的偏角大小orientation［branch_num］以及节点对应的每一个分支所看到的图像image［branch_num］。其中orientation里面存放的是当前路口的所有分支相对于机器人的偏角，Image存放着当前机器人在该路口中央所看到的各个分支的图像。在本文中，存在四种类型的路口，分别是十字路口、T字路口、直角路口和单向路口。在每一种路口类型中，都会有当前的位姿信息。机器人每到达一个路口节点之后，会使用环绕摄像头来检测消失点从而判断路口分支的数量，然后在每一个分支场景中提取特征点作为当前分支的场景描述，从而建立节点的拓扑场景地图［12］。如图1所示，图中十字路口节点处的四处分支分别对应四张特征场景图。拓扑地图中的边信息，是两个路口节点之间的路径。从边的信息可以得到相对应的两个节点的信息。

图1 拓扑场景地图

拓扑地图是由节点和边来表示，边表示相连的两个节点之间的有一条相通的路径，这与机械臂模型很相似，机械臂模型由杆和关节连接而成。可以借鉴机械臂运动学规划思想应用到拓扑地图的创建优化中。所以本文引入了机械臂逆向运动学的思想来对创建的拓扑地图进行优化。

2.2 机械臂逆向运动学

在本文中，拓扑地图的优化调整主要借鉴了机械臂运动学的规划控制原理。机械臂运动学［13～16］主要包括两个方面，一是正向运动学，已知机械臂中所有杆的长度和各个关节的转角，求末端结构所处的一个位置；二是逆向运动学，知道了末端结构所处的位置，最终需要求出各个关节所转动的角度分别是多少。在机械臂的运动学中，讨论和使用最多的还是逆向运动学。

假设现在末端结构的位置（x，y），那么根据机械臂的结构，假设机械臂有N个关节，那么根据机器人运动学正解的方法可以得出：

式中，x和y表示末端结构的位置，θi表示各个关节所旋转之后的角度，Li表示每一个机械臂杆的长度。如果将末端结构x和y的改变量大小设置为ΔX，θi的变化量设置为Δq，那么上面式（1）、（2）可以写为

其中J为

将机器人路径看成机械臂，由于里程计的误差，机器人生成的路径中不仅方向会发生偏移，两个路口之间的距离也会有偏差。所以，需要将每个节点路口的偏角θ和两个路口节点之间距离L都当成自变量来进行求解。此时，式（4）中的雅可比矩阵就变为

自变量q变为

假设现在需要求雅格比矩阵中的某一列的微分，那么有

其中e表示末端结构的位姿，qi表示某一列的旋转角度和杆的伸缩长度。如果对qi增加微小的改变量，那么就能计算末端结构的位移量：

对于解析法，本文使用差分代替微分求解，那么式（5）变换为

求机械臂的逆解时，本文用到了最速下降法的思想，一是避免变量太多时解析法无法计算，二是可以沿着函数值下降最快的方向搜索。求取末端结构的位姿，本文的误差函数就是当前值与目标值的欧式距离：

式中Xtarget表示目标点位姿，f（q）表示末端位置关于自变量q的函数。机械臂逆解中使用梯度下降法，该方法可以求得角度和杆伸缩的变化值Δq：

式中的α表示迭代搜索的步长。

3 模拟退火算法迭代策略

模拟退火算法［17］是启发示算法的一种，使用贪心的算法，来搜索达到全局的最优点。该算法来源于固体退火原理，温度升高的时候，内部分子的会变为无序状；当温度下降下来的时候，分子又会取向于有序。模拟算法的基本思想的基本流程如下：

Step 1初始化温度T，初始解状态S，以及迭代次数L。

Step 2迭代L次，重复步骤3到步骤6的过程，直到搜索到最优解。

Step 3在当前解基础上面产生新解S'。

Step 4计算当前增量Δs=f（s’）-f（s），其中f（s）为模拟退火算法的代价函数，在本文中的代价函数f（s）指的是当前位姿X与目标位姿Xtarget的欧式距离大小。

Step 5若Δs＜0则接受当前解S'作为当前的新解；否则以一定概率P接受该解作为当前解。其中P为

Step 6如果满足条件，则输出当前解作为最优解，结束程序。

Step 7 T减少，并且保证T＞0。

在利用梯度下降法去迭代结果的时候，本文中的步长可以使用模拟退火算法来实现变长，加快算法的速度。在该模拟退火算法的基础上，每次在不接受新解的时候，会将当前的步长α减半，进行一个二分的折半搜索策略，可以加快搜索的进度。

4 仿真实验及分析

4.1 实验平台及数据

本文实验是在自主研发的一个仿真平台上面实现的。该平台是基于C++语言开发的一个机器人仿真平台，地图数据是本校的俯瞰图，可以看到地图的每一条道路和每一个路口地点，平台上有机器人在地图中真实的行走路径，也有机器人行走时创建的理论地图。每一个机器人都有自己的一个里程计信息。

在实验中设定的参数初值分别是α=0.9，迭代的总次数为50次，模拟退火算法代价函数初始值为1000。本文采取的方法与原模拟退火算法稍有不同。本文中如果当前解不满足条件，则以当前的一定概率pcur接受该解，如果不接受该解，则将α的当前值做一个二分减半的操作，同时概率pcur是随着代价函数的变化量和温度T动态变化。来实现一个变步长的迭代算法。

4.2 创建地图及闭环检测

在创建拓扑地图时，机器人每到达一个节点，就进行闭合检测。闭环的方法很多［18～19］，但本文中的闭环利用到了相邻的两个节点之间里程计的误差范围大小，根据当前节点的位置，来推断下一个节点所在的范围，通过位姿的偏差范围、分支个数以及当前位姿相对于各个分支偏角的范围来进行判断是否为之前访问过的节点。如果确认闭环，则更新回环中的所有节点信息；否则将该节点放进自己的局部地图库中，继续漫游。

4.3 实验仿真

在实验过程中，采取在线地图优化的一个策略。在试验中，对创建的拓扑地图进行了优化，使用到了两种机械臂路径规划的算法思想来进行对比。一种是根据Cyclic Coordinate Descent（CCD）的启发式迭代搜索算法［20］，通过每次只改变一个关节的参数来减少误差，从该拓扑地图的末端开始调整优化，直到收敛或者误差到达最小值。另外一种就是本文中提出来的基于机械臂逆运动学方法的拓扑地图优化。结果如图2、图3所示。

图2 拓扑地图优化对比（一）

图3 拓扑地图优化对比（二）

虽然最后两种方法调整的结果差不多，但是过程却是不一样。由图4可知，在对十个位姿节点的闭环优化过程中，CCD方法、逆运动学方法以及加了模拟退火优化的逆运动学方法三种方法在原始图、第5次迭代和第10次迭代的结果可知，可以看出，在第5次和第10次的迭代结果中，CCD调整优化方法几乎是非常慢的并且调整的非常小，但本文使用的模拟退化算法变步长优化的机械臂逆运动学方法在第10次的时候可以快速的收敛到最后的结果，比没有加模拟退退火的机械臂逆运动学方法更加的快速。

图4 原始图（左）、第5次迭代（中）、第10次迭代（右）中间结果

本文中创建了两个地图，并且在机器人创建地图的同时，对到达的路口节点进行闭环检测，然后对环中的所有节点进行优化调整。可以看出，对于实际创建的拓扑地图，误差的积累越来越大。在只使用里程计的情况下，利用本文提出来的机械臂优化调整方法，可以调整优化已闭环的拓扑地图接近真实的拓扑地图结构。

4.4 机械臂方法各个位姿节点角度调整对比

图5表示在一个有十个位姿节点闭环的拓扑地图中，每条路径调整的弧度大小。可以看出来，在调整过程中，虽然结果可能差不多，但是本文方法在两个路口位姿节点之间的路径调整的幅度会比另外一种方法更小，并且调整的误差也会更加平均。

图5 十个位姿节点的闭环调整角度大小

4.5 模拟退火算法变步长对比

由图6可知，在本文实验下，使用了模拟退火算法实现变步长比固定步长的算法时间和迭代次数要短的多。虽然两者前面几次迭代都差不多一样，但是在后面迭代的时候，可以明显看出来变步长的算法更快的收敛。

图6 模拟退火变步长对比

5 结语

机器人的地图创建和优化是SLAM系统中重要的一环，特别是在闭环之后，误差调整更为重要。对于单里程计的SLAM系统，里程计漂移造成了地图创建偏差，本文针对该问题，提出了一种新的基于图优化方法的拓扑地图创建，可以很好地解决地图中各个节点之间的位姿误差问题，能够更好地将积累的误差平均的分摊到回环中的各个节点中，生成一个较为准确的地图。实验结果证明了该算法是有效可行的。

之后的研究中如果加入了场景匹配信息，那么将会极大地提高地图创建的准确性。