·292· 智能系统学报 第13卷 估计。所以这里只考虑状态变量和观测方程，构建 3.2图的建立与优化 出目标函数： 上述SLAM问题全部抽象成节点与边，节点代 (xy,2=zkj-g(k,y） (4) 表了优化变量，包括pose节点和point节点。边代 E(X)= e(,y,z)'2'e(xy2） (5) 表了误差项约束条件，包括pose-point约束和pose 化EC pose约束。pose-point约束依赖于相机观测产生的 X=arg min E(X) (6) 约束，如图3(a)所示。pose-pose约束则依赖于系统 式中：x表示第k时刻的机器人位姿，也称为图优化 中闭环检测产生的约束，如图3b)、(c)所示，可有效 的一个pose节点；y,表示第k时刻机器人所能观测 地抑制系统的累积误差，从而得到信息一致的 到的路标，也称为图优化的一个point节点；，表示 地图。 pose节点与point节点的约束；特别地，在闭环检测 优化过程中，代表pose节点与pose节点的约束； (xy,)为由两顶点组成的边所产生的向量误差 函数，代表两顶点满足约束条件z,的程度，当它的 值为0时，表示两顶点完全满足匹配约束。 目标函数的物理意义：这里假设了误差函数e() (a)相机观测约束下的位姿优化 符合高斯分布(O,2),则SLAM问题的贝叶斯概率描 述为，求解适合的(x,y),使系统最有可能产生当前的 观测数据Z,即最大似然(MLE): (x,y)'=argmax p(ax.y) (7) I.V 最终所求的(，y,)的二次型（式(6）)，实质 上是在求解最大似然。即在假设噪声是高斯分布之 后，将整个SLAM问题等效为一个最小二乘问题。 进一步，利用李代数的扰动模型，求得目标函数关 b)局部闭环约束下的位姿优化 于pose和point的导数J和J,可对此最小二乘问题 求解。以此建立最终的求解方程： 6 e(x+6,y+6,)≈e(x,y)+J6x+J,6, (8) H=∑JFJ, (9) b=∑JFg'e, (10) 环 H6=-b (11) 式中：J代表相机微小运动后，像素的变化；J,代表 (©)全局闭环约束下的位姿优化 空间点微小运动后，像素的变化。式(11)代表了考 图3图的建立与优化 虑所有的变量与边的Full SLAM问题的求解。结 Fig.3 Establishment and optimization of graph 合SLAM问题的稀疏性，选用合适的梯度下降方 法，用求得的6用于状态变量X的更新，通过多次 4闭环检测下的点云优化 迭代，最终求得满足目标方程的状态变量X的收敛 在基于位姿增量估计的SLAM建图中，会产生 值。可借助通用图优化工具g2o来解决上述非线 误差的累积，则在长时间的大范围SLAM建图中， 性最小二乘问题，求解过程如图2所示。 就需要闭环检测对位姿和生成的点云进行优化。 构建图 确定目标 求雅克比 求增量方程 RGBD-SLAM算法是在局部回环检测后，只使用局 G=/VE 函数E(X) 矩阵J H&=-b 部回环优化后的位姿作为三维地图点云的重建。 状态更新 优化后的 ORB-SLAM算法则是在运动跟踪过程中，时刻检测 Xx=X十6 位姿 并维护关键帧之间的点云共视关系，同时实现对点 图2优化求解流程 云优化。但这种方法只适用于规模有限的稀疏点 Fig.2 Process of optimizing solution 云，并不适用于稠密化点云的优化。基于此，提出估计。所以这里只考虑状态变量和观测方程，构建 出目标函数： e(xk , yj ,zk, j)=zk, j −g(xk , yj) (4) E(X) = ∑ (k, j)∈C e(xk , yj ,zk, j) TΩk j −1 e(xk , yj ,zk, j) (5) X ∗ = argmin X E(X) (6) xk yj zk, j zk, j e(xk , yj ,zk j) zk, j 式中： 表示第 k 时刻的机器人位姿，也称为图优化 的一个 pose 节点； 表示第 k 时刻机器人所能观测 到的路标，也称为图优化的一个 point 节点； 表示 pose 节点与 point 节点的约束；特别地，在闭环检测 优化过程中， 代表 pose 节点与 pose 节点的约束； 为由两顶点组成的边所产生的向量误差 函数，代表两顶点满足约束条件 的程度，当它的 值为 0 时，表示两顶点完全满足匹配约束。 (0,Ω) (x, y) 目标函数的物理意义：这里假设了误差函数 e(·) 符合高斯分布 ，则 SLAM 问题的贝叶斯概率描 述为，求解适合的 ，使系统最有可能产生当前的 观测数据 Z，即最大似然 (MLE)： (x, y) ∗ = argmax x,y p(z|x, y) (7) e(xk , yj ,zk j) Jx Jy 最终所求的 的二次型 (式 (6))，实质 上是在求解最大似然。即在假设噪声是高斯分布之 后，将整个 SLAM 问题等效为一个最小二乘问题。 进一步，利用李代数[9]的扰动模型，求得目标函数关 于 pose 和 point 的导数 和 ，可对此最小二乘问题 求解。以此建立最终的求解方程： e(x+δx , y+δy) ≈ e(x, y)+ Jxδx + Jyδy (8) H = ∑ i Ji TΩ −1 Ji (9) b = ∑ i Ji TΩ −1 ei (10) Hδx = −b (11) Jx Jy δx 式中： 代表相机微小运动后，像素的变化； 代表 空间点微小运动后，像素的变化。式 (11) 代表了考 虑所有的变量与边的 Full SLAM 问题的求解。结 合 SLAM 问题的稀疏性，选用合适的梯度下降方 法 [10] ，用求得的 用于状态变量 X 的更新，通过多次 迭代，最终求得满足目标方程的状态变量 X 的收敛 值。可借助通用图优化工具 g2o[11]来解决上述非线 性最小二乘问题，求解过程如图 2 所示。 a/cۇٴⅮ䯱 ⴕ䭡J ⟢ᔭᰠ᫜ XK+1=XK喎δ * ⶚჆Ⱊᴳ ܩ᪜E(X) Ⅾ්䛻᫥⼷ Hδ=−b ⮰ऺࡂф ѹ༫ Ჰᐦప G={V,E} 图 2 优化求解流程 Fig. 2 Process of optimizing solution 3.2 图的建立与优化 上述 SLAM 问题全部抽象成节点与边，节点代 表了优化变量，包括 pose 节点和 point 节点。边代 表了误差项约束条件，包括 pose-point 约束和 pose￾pose 约束。pose-point 约束依赖于相机观测产生的 约束，如图 3(a) 所示。pose-pose 约束则依赖于系统 中闭环检测产生的约束，如图 3(b)、(c) 所示，可有效 地抑制系统的累积误差，从而得到信息一致的 地图。 (a) Ⱔᱦ㻮≷㏒᲋̷⮰ѹ༫фࡂ (b) ᅬ䘔䬙⣛㏒᲋̷⮰ѹ༫фࡂ 2 8 3 4 5 6 9 7 1 10 䬙⣛ 䬙⣛ 1 2 3 4 5 6 7 (c) ڔᅬ䬙⣛㏒᲋̷⮰ѹ༫фࡂ 图 3 图的建立与优化 Fig. 3 Establishment and optimization of graph 4 闭环检测下的点云优化 在基于位姿增量估计的 SLAM 建图中，会产生 误差的累积，则在长时间的大范围 SLAM 建图中， 就需要闭环检测对位姿和生成的点云进行优化。 RGBD-SLAM 算法是在局部回环检测后，只使用局 部回环优化后的位姿作为三维地图点云的重建。 ORB-SLAM 算法则是在运动跟踪过程中，时刻检测 并维护关键帧之间的点云共视关系，同时实现对点 云优化。但这种方法只适用于规模有限的稀疏点 云，并不适用于稠密化点云的优化。基于此，提出 ·292· 智 能 系 统 学 报 第 13 卷