Graph Based SLAM 基本原理

作者 | Alex

01 引言

SLAM 基本框架大致分为两大类：基于概率的方法如 EKF, UKF, particle filters 和基于图的方法 。基于图的方法本质上是种优化方法，一个以最小化对环境的观测误差为目标的优化问题。至今仍是主流的框架的核心，karto，cartographer，hector 等都是基于优化的。这种框架 20 年前就已经兴起，比如著名的 Atlas，今天依然是主流。

Atlas 初衷是设计一个通用框架，以便在其中实验各种建图算法。目的就是通过建立小块的局部地图，再用这些小地图拼成一整块大地图。用“鬼话”说就是：在小尺度区域和有限计算下局部建图算法基础上，建立全局一致的地图。

Atlas 的名字来源于古希腊神话，众所周知的神王宙斯只是第三代，第一代是他的爷爷乌拉诺斯 ，宙斯和他爸爸克罗诺斯连续上演了儿子灭了老子、篡位夺权的的狗血剧，到宙斯时代奥林匹斯山上的坐次才基本排定。想当年爸爸克罗诺斯干掉爷爷乌拉诺斯赢得胜利后，巨人族首领泰坦由于跟着爷爷混，站错了队，败者为寇嘛，秋后算账的时候他和他的一众喽啰统统被打入地狱--黑暗深渊塔尔塔洛斯。其中一个叫 Atlas 的巨人，大概是因为个儿特别大、特别有劲儿，被挑出来去西方站在地母盖娅（Gaia）身上，擎住天父乌拉诺斯，天父就是天空啦，把他托在那儿(相当于囚禁)不能下来捣乱。简而言之，Atlas 是那个站在地球上托举天空的巨人，就象中国古代神话里的盘古。

好了回来说 Atlas 的 SLAM 框架，框架的思想一般是将局部坐标系表示为图上的节点，这个局部坐标系可以是某些 landmark(路标)，相邻坐标系之间的变换表示为边。目的要使所有的边加起来最短。

本文介绍一下最简版的基于图的 SLAM 框架，然而麻雀虽小五脏俱全。理解了以后，可以在此基础上搭建自己的 SLAM 系统的。

02 先看看演示结果

蓝线：ground truth.打算走出这样的轨迹

黑线：航位推算.

航位推算法是在知道当前时刻位置的条件下，通过测量移动的距离和方位，推算下一时刻位置的方法，并实施。

红线：算法估计的运动轨迹

反正每个人的路都要自己走，还只能学习那么有限的时间，学习结束后，走出的结果怎么样不太好说，红色就是能走出的最好结果了。

同一个程序运行两次的结果不一样，这是由于每次都加入随机噪声，会导致估计结果不同，实际应用中也是这样。

03 流程图及代码注释

1.程序入口： main()函数：

• 给定路标点：5 个星星

• 开着小车观赏这 5 个点。xTrue：groundtruth 从(0,0)点开始，真实轨迹 xDR 也从(0,0)开始。

◦ 函数 u = calc_input()计算需要的控制向量：速度=1m/s,角速度=0.1 rad/s 。

◦ 并做一次观测 xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)

• 共仿真 SIM_TIME = 100 秒，每隔 DT = 2 秒，对环境观察一次，对小车进行一次操作。

◦ 首先需要计算一次控制向量。

◦每隔show_graph_d_time=20秒调用一次x_opt = graph_based_slam(hxDR, hz)，共 5 次，根据当前位置和历史轨迹推断的最佳路径，如图中的红线所示。

def main():…...# RFID positions [x, y, yaw]路标点坐标#这里路标是固定的#通常路标需要满足 4 个条件：# 1.易于被重复观测# 2.不同路标点之间有明显的区别# 3.环境中应该有足够数量的路标点# 4.路标点应该是稳定的，或者说是静态的，如醒目的建筑、交通标识等RFID = np.array([[10.0, -2.0, 0.0],[15.0, 10.0, 0.0],[3.0, 15.0, 0.0],[-5.0, 20.0, 0.0],[-5.0, 5.0, 0.0]])# State Vector [x y yaw v]'xTrue = np.zeros((STATE_SIZE, 1))#ground truthxDR = np.zeros((STATE_SIZE, 1)) # 航位推算.也就是程序中小车真实的轨迹while SIM_TIME >= time:#仿真 100 秒...time += DT#计时，每两秒做给小车一个控制信号，并观察一次环境d_time += DTu = calc_input()#产生控制信号xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)hz.append(z)if d_time >= show_graph_d_time:#每隔 20 秒，调用一次 graph_based_slam ,估计一次最佳轨迹x_opt = graph_based_slam(hxDR, hz)#通过真实轨迹和到路标的距离产生最佳轨迹……

2.observation()观测函数

在 main()函数中每 2 秒调用一次，有两个作用：

①调用 xTrue = motion_model(xTrue, u)产生 ground truth 数据(轨迹和控制都没有噪声)

②调用 xd = motion_model(xd, ud) ，在观测数据基础上添加噪声(轨迹和控制都有噪声)，用上一时刻的轨迹、真实轨迹和控制量和路标会产生下一个观测。实际应用中，观测比较复杂。观测数据通常来自各种传感器，如相机(单目、或双目)、imu、雷达、声纳、GPS 等，需要对传感器做标定，从不同传感器取回的数据还需要融合。传感器融合也是当前的研究热点。

def observation(xTrue, xd, u, RFID):xTrue = motion_model(xTrue, u)#无噪声轨迹和控制、调用运动模型、为下一时刻生成 groundtruth 轨迹# add noise to gps x-yz = np.zeros((0, 4))for i in range(len(RFID[:, 0])):#这个循环是对小车到路标点之间的距离、角度添加噪声dx = RFID[i, 0] - xTrue[0, 0]#RFID 是 main()中定义的路标点坐标，是固定的dy = RFID[i, 1] - xTrue[1, 0]#xTrue 是 ground truth 轨迹d = math.hypot(dx, dy)#sqrt(dx^2+dy^2)#求距离，勾股定理angle = pi_2_pi(math.atan2(dy, dx)) - xTrue[2, 0]#反余切求角度phi = pi_2_pi(math.atan2(dy, dx))#角度转化为弧度if d <= MAX_RANGE:#MAX_RANGE = 30dn = d + np.random.randn() * Q_sim[0, 0] # add noiseangle_noise = np.random.randn() * Q_sim[1, 1]angle += angle_noisephi += angle_noisezi = np.array([dn, angle, phi, i])#拼成数组z = np.vstack((z, zi))# add noise to input 对控制信号添加噪声ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0]ud2 = u[1, 0] + np.random.randn() * R_sim[1, 1]ud = np.array([[ud1, ud2]]).Txd = motion_model(xd, ud)#用真实轨迹和噪声控制信号，调用运动模型生成下一时刻产生有噪声轨迹return xTrue, z, xd, ud#

3.motion_model 运动模型

飞行器、车辆、机器人都有自己的运动模型，用于描述自己独特的运动方式，如何将各种控制量转换为各种运动：如何产生旋翼、车轮，行走的腿的位移或旋转、直行、转弯、翻转等操作。

4.graph_based_slam(hxDR, hz)

这个函数是框架的核心。

函数的输入是历史数据包括：①真实轨迹②从真实轨迹各点对路标的观测输出最优轨迹。

def graph_based_slam(x_init, hz):print("start graph based slam")z_list = copy.deepcopy(hz)x_opt = copy.deepcopy(x_init)nt = x_opt.shape[1]n = nt * STATE_SIZE for itr in range(MAX_ITR):edges = calc_edges(x_opt, z_list)#计算总的代价函数H = np.zeros((n, n))b = np.zeros((n, 1))for edge in edges:H, b = fill_H_and_b(H, b, edge)#抽取信息矩阵# to fix originH[0:STATE_SIZE, 0:STATE_SIZE] += np.identity(STATE_SIZE)dx = - np.linalg.inv(H) @ b#将总代价与每个点的参数关联起来，计算需要修正的量for i in range(nt):x_opt[0:3, i] += dx[i * 3:i * 3 + 3, 0]#修正真实轨迹，得到最优轨迹diff = dx.T @ dxprint("iteration: %d, diff: %f" % (itr + 1, diff))if diff < 1.0e-5:breakreturn x_opt

5.calc_edge()

建立图—graph.轨迹上每个点以及每个路标都是图上的节点，路标到轨迹点观测误差构成边--代价函数。

def calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1,angle1, d2, angle2, t1, t2):edge = Edge()tangle1 = pi_2_pi(yaw1 + angle1)tangle2 = pi_2_pi(yaw2 + angle2)tmp1 = d1 * math.cos(tangle1)tmp2 = d2 * math.cos(tangle2)tmp3 = d1 * math.sin(tangle1)tmp4 = d2 * math.sin(tangle2)edge.e[0, 0] = x2 - x1 - tmp1 + tmp2edge.e[1, 0] = y2 - y1 - tmp3 + tmp4edge.e[2, 0] = 0Rt1 = calc_rotational_matrix(tangle1)Rt2 = calc_rotational_matrix(tangle2)sig1 = cal_observation_sigma()sig2 = cal_observation_sigma()edge.omega = np.linalg.inv(Rt1 @ sig1 @ Rt1.T + Rt2 @ sig2 @ Rt2.T)#权矩阵edge.d1, edge.d2 = d1, d2edge.yaw1, edge.yaw2 = yaw1, yaw2edge.angle1, edge.angle2 = angle1, angle2edge.id1, edge.id2 = t1, t2return edge

6.calc_edges(hxDR, hz)

计算轨迹上每个点对代价函数的贡献，计算轨迹上所有点产生的总的代价函数。

def calc_edges(x_list, z_list):edges = [] cost = 0.0z_ids = list(itertools.combinations(range(len(z_list)), 2))for (t1, t2) in z_ids:x1, y1, yaw1 = x_list[0, t1], x_list[1, t1], x_list[2, t1]x2, y2, yaw2 = x_list[0, t2], x_list[1, t2], x_list[2, t2]if z_list[t1] is None or z_list[t2] is None:continue # No observationfor iz1 in range(len(z_list[t1][:, 0])):for iz2 in range(len(z_list[t2][:, 0])):if z_list[t1][iz1, 3] == z_list[t2][iz2, 3]:d1 = z_list[t1][iz1, 0]angle1, phi1 = z_list[t1][iz1, 1], z_list[t1][iz1, 2]d2 = z_list[t2][iz2, 0]angle2, phi2 = z_list[t2][iz2, 1], z_list[t2][iz2, 2]edge = calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1, angle1, d2, angle2, t1, t2)edges.append(edge)cost += (edge.e.T @ edge.omega @ edge.e)[0, 0]print("cost:", cost, ",n_edge:", len(edges))return edges

7.信息矩阵与代价函数

信息矩阵是用来表达不确定性的，将总的代价函数与每个变量的梯度关联起来：

需要通过信息矩阵来完成：最优轨迹的原则:

为了最小化总的代价函数，需要如何更新变量：

信息矩阵就是协方差矩阵的逆。

def fill_H_and_b(H, b, edge):#计算信息矩阵和更新的量A, B = calc_jacobian(edge)id1 = edge.id1 * STATE_SIZEid2 = edge.id2 * STATE_SIZEH[id1:id1 + STATE_SIZE, id1:id1 + STATE_SIZE] += A.T @ edge.omega @ AH[id1:id1 + STATE_SIZE, id2:id2 + STATE_SIZE] += A.T @ edge.omega @ BH[id2:id2 + STATE_SIZE, id1:id1 + STATE_SIZE] += B.T @ edge.omega @ AH[id2:id2 + STATE_SIZE, id2:id2 + STATE_SIZE] += B.T @ edge.omega @ B b[id1:id1 + STATE_SIZE] += (A.T @ edge.omega @ edge.e)b[id2:id2 + STATE_SIZE] += (B.T @ edge.omega @ edge.e)return H, b在 graph_based_slam()函数中：for i in range(nt):x_opt[0:3, i] += dx[i * 3:i * 3 + 3, 0]#更新实际轨迹的每一点，得到最优轨迹