作者|Krunal Kshirsagar
编译|Flin
起源|Medium
什么是SLAM？
即时定位与地图构建（simultaneous localization and mapping，简写成SLAM），用于环境模型（map）的并行构建，以及在其中挪动的机器人的状态估算。换句话说，SLAM为你提供了一种实时跟踪机器人在世界上的地位、并辨认地标（例如建筑物，树木，岩石和其余世界特色）的地位的办法。除了本地化之外，咱们还心愿建设机器人环境的模型，这样咱们就有了一个物体的概念，以及围绕机器人的地标，以便咱们能够应用此地图数据来确保机器人在世界各地挪动时走在正确的路线上。因而，构建地图的要害是机器人自身可能会因为其静止不确定性而失去对其地位的跟踪，因为不存在现有的地图，并且咱们正在用机器人并行构建地图。而这就是SLAM发挥作用的中央。
SLAM的工作：
同时定位和地图绘制（SLAM）的根底是从机器人的传感器和随时间推移的静止中收集信息，而后应用无关测量和静止的信息来重建世界地图。在这种状况下，咱们将机器人定位在2D网格世界中，因而，基于图的SLAM办法通过提取原始传感器测量值来结构简化的预计问题。这些原始测量值将替换为图中的边缘，而后能够将其视为虚构测量值。
假如咱们有一个机器人和初始地位 x0 = 0 和 y0 = 0 。对于此示例，为了放弃简略，咱们并不关怀方向。让咱们假如机器人在X方向上向右挪动了10。所以，在现实世界中，你会理解到 x1，静止后的地位与x0 + 10雷同，即x1 = x0 + 10，同理，y1与y0雷同。
如图，机器人在x方向上的位移为10：
然而依据卡尔曼滤波器（https://medium.com/@krunalksh...） 和其余各种机器人技术，咱们曾经晓得地位实际上是不确定的。因而，与其假如咱们的XY坐标系中的机器人准确地向右挪动了10个地位，不如了解成它在x1 = x0 + 10静止更新后的理论地位是以（10,0）为核心的高斯分布，然而机器人也可能在其余中央。
如图：静止更新后，高斯以机器人的地位为核心
这是x变量的高斯的数学公式：
当这两个条件雷同时，与其将x1设置为x0+10，不如用高斯函数来示意，此时高斯函数达到峰值。。因而，如果你减去x1-x0-10，把它变成一个正方形，而后将其转换为高斯，咱们将失去与x1和x0相干的概率分布。咱们能够对y做同样的转换。依据咱们的静止y不变，因而y1和y0尽可能凑近。
这两个高斯的乘积当初是咱们的约束条件。指标是在地位x0为（0,0）的状况下最大化地位x1的可能性。因而，Graph SLAM所做的是，它应用一系列此类约束条件来定义概率。 假如咱们有一个在某个空间中挪动的机器人，GRAPH SLAM会收集其初始地位（0,0），最后也称为“初始束缚”，而后收集许多绝对束缚，这些绝对束缚会将每个机器人姿势与之前的机器人姿势相关联作为相对运动束缚。例如，让咱们应用机器人能够在各个地位看到的地标，这是每次机器人看到地标时的绝对测量束缚。因而，Graph SLAM收集这些束缚，以便找到最可能的机器人门路配置以及地标地位，即映射过程。
实作
生成环境：
咱们将生成一个带有地标的2D世界网格，而后通过将机器人搁置在该世界中，并在肯定数量的工夫步长上挪动和感应来生成数据。实例化的机器人在世界中挪动和感知时，将收集数据。咱们的SLAM函数将把这些数据作为输出。因而，让咱们首先创立此数据，并摸索它如何代表咱们的机器人进行静止和传感器测量。
SLAM输出：
除了数据之外，咱们的slam函数还具备：
- N：机器人将要挪动和感应的工夫步数。
- num_landmarks：世界上的地标数量。
- world_size：你的世界的大小（w / h）。
- motion_noise：与静止相干的噪声；静止的更新置信度应为
1.0/motion_noise. - measurement_noise：与测量/传感相干的噪声；测量的更新权重应为
1.0/measurement_noise.
import numpy as npfrom helpers import make_data#slam的实现应该应用以下输出#请随便更改这些输出值并查看其响应形式！# 世界参数num_landmarks = 5 # number of landmarksN = 20 # time stepsworld_size = 100.0 # size of world (square)# 机器人参数measurement_range = 50.0 # range at which we can sense landmarksmotion_noise = 2.0 # noise in robot motionmeasurement_noise = 2.0 # noise in the measurementsdistance = 20.0 # distance by which robot (intends to) move each iteratation # make_data实例化一个机器人，并为给定的世界大小和给定数量的地标生成随机地标data = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance)让咱们编写两个次要性能，这些性能能够使机器人到处挪动，帮忙定位地标并在2D地图上测量它们之间的间隔：
- Move：尝试按dx，dy移动机器人。
- Sense：返回可见范畴边疆标的x和y间隔。
class robot: #move function def move(self, dx, dy): x = self.x + dx + self.rand() * self.motion_noise y = self.y + dy + self.rand() * self.motion_noise if x < 0.0 or x > self.world_size or y < 0.0 or y > self.world_size: return False else: self.x = x self.y = y return True #sense function def sense(self): measurements = [] for landmark_index, landmark in enumerate(self.landmarks): landmark_distance_x = landmark[0] landmark_distance_y = landmark[1] random_noise = self.rand() cal_dx = self.x - landmark_distance_x + random_noise * self.measurement_noise cal_dy = self.y - landmark_distance_y + random_noise * self.measurement_noise is_not_in_measurement_range = self.measurement_range == -1 if(is_not_in_measurement_range) or ((abs(cal_dx) <= self.measurement_range) and (abs(cal_dy) <= self.measurement_range)): measurements.append([landmark_index, cal_dx, cal_dy]) return measurementsOmega 和 Xi：
为了实现Graph SLAM，引入了一个矩阵和一个向量（别离为和xi）。矩阵是正方形的，标有所有机器人姿态（xi）和所有地标。例如，每次进行察看时，当你在两个姿态之间挪动某个间隔dx，并能够关联这两个地位时，能够将其示意为这些矩阵中的数值关系。
让咱们编写函数，以便它为机器人的起始地位返回omega和xi束缚。咱们尚不晓得的所有值都应应用0进行初始化。咱们能够假如咱们的机器人以100％的置信度在世界的正中间开始。
def initialize_constraints(N, num_landmarks, world_size): ''' This function takes in a number of time steps N, number of landmarks, and a world_size, and returns initialized constraint matrices, omega and xi.''' middle_of_the_world = world_size / 2 ## 倡议：在变量中定义和存储约束矩阵的大小（行/列） rows, cols = 2*(N + num_landmarks), 2*(N + num_landmarks) ## TODO: 用两个初始“strength”值定义约束矩阵Omega omega = np.zeros(shape = (rows, cols)) ## 咱们机器人最后的x，y地位 #omega = [0] omega[0][0], omega[1][1] = 1,1 ## TODO: Define the constraint *vector*, xi ## 假如机器人以100％的置信度在世界的正中间开始。 #xi = [0] xi = np.zeros(shape = (rows, 1)) xi[0][0] = middle_of_the_world xi[1][0] = middle_of_the_world return omega, xi通过静止和测量值进行更新：
## slam承受6个参数并返回mu，## mu是机器人穿过的整个门路（所有x，y姿态）和所有地标地位def slam(data, N, num_landmarks, world_size, motion_noise, measurement_noise): ## TODO: 应用你的初始化创立约束矩阵 omega, xi = initialize_constraints(N, num_landmarks, world_size) ## TODO:遍历数据中的每个工夫步骤 for time_step in range(len(data)): ## 每次迭代时获取所有的静止和测量数据 measurement = data[time_step][0] motion = data[time_step][1] x dx = motion[0] # 本次沿x挪动的间隔 dy = motion[1] # 本次沿y挪动的间隔 #假如机器人在这个工夫从（x0，y0）挪动到（x1，y1） #omega的偶数列对应于x值 x0 = (time_step * 2) #x0 = 0,2,4,... x1 = x0 + 2 #x1 = 2,4,6,... # omega 的奇数列对应于y值 y0 = x0 + 1 #y0 = 1,3,5,... y1 = y0 + 2 #y1 = 3,5,7,... actual_m_noise = 1.0/measurement_noise actual_n_noise = 1.0/motion_noise ## TODO: 更新约束矩阵/向量（Omega/xi）以解释所有*measurements* ## 这应该是一系列思考测量噪声的附加值 for landmark in measurement: lM = landmark[0] # 地标 id dx_lM = landmark[1] # 沿x与以后地位拆散 dy_lM = landmark[2] # 沿y与以后地位拆散 L_x0 = (N*2) + (lM*2) # 偶数列有x个地标值 L_y0 = L_x0 + 1 # 奇数列有y个地标值 # 更新对应于x0和Lx0之间测量值的omega值 omega[x0][x0] += actual_m_noise omega[L_x0][L_x0] += actual_m_noise omega[x0][L_x0] += -actual_m_noise omega[L_x0][x0] += -actual_m_noise # 更新对应于y0和Ly0之间测量值的omega值 omega[y0][y0] += actual_m_noise omega[L_y0][L_y0] += actual_m_noise omega[y0][L_y0] += -actual_m_noise omega[L_y0][y0] += -actual_m_noise # 更新X0和LX0之间的测量值对应的xi值 xi[x0] -= dx_lM/measurement_noise xi[L_x0] += dx_lM/measurement_noise # 更新y0和Ly0之间的测量值对应的xi值 xi[y0] -= dy_lM/measurement_noise xi[L_y0] += dy_lM/measurement_noise ## TODO: 更新约束矩阵/向量（omega/XI），以解释从（x0，y0）到（x1，y1）和静止噪声的所有*静止*。 omega[x0][x0] += actual_n_noise omega[x1][x1] += actual_n_noise omega[x0][x1] += -actual_n_noise omega[x1][x0] += -actual_n_noise omega[y0][y0] += actual_n_noise omega[y1][y1] += actual_n_noise omega[y0][y1] += -actual_n_noise omega[y1][y0] += -actual_n_noise xi[x0] -= dx/motion_noise xi[y0] -= dy/motion_noise xi[x1] += dx/motion_noise xi[y1] += dy/motion_noise ## TODO: 在遍历所有数据之后 ## 计算姿态和地标地位的最佳估计值 ##应用公式，omega_inverse * Xi inverse_of_omega = np.linalg.inv(np.matrix(omega)) mu = inverse_of_omega * xi return mu机器人的姿态和地标：
让咱们打印函数产生的预计姿态和界标地位。咱们定义了一个提取姿态和地标地位，并将它们作为本人的独自列表返回。
def get_poses_landmarks(mu, N): # 创立一个姿态列表 poses = [] for i in range(N): poses.append((mu[2*i].item(), mu[2*i+1].item())) # 创立一个地标列表 landmarks = [] for i in range(num_landmarks): landmarks.append((mu[2*(N+i)].item(), mu[2*(N+i)+1].item())) # 返回实现的列表 return poses, landmarks def print_all(poses, landmarks): print('\n') print('Estimated Poses:') for i in range(len(poses)): print('['+', '.join('%.3f'%p for p in poses[i])+']') print('\n') print('Estimated Landmarks:') for i in range(len(landmarks)): print('['+', '.join('%.3f'%l for l in landmarks[i])+']')# 调用你的slam实现，并传入必要的参数mu = slam(data, N, num_landmarks, world_size, motion_noise, measurement_noise)# 打印出地标和姿态后果if(mu is not None): # 获取姿态和地标列表 # 并打印进去 poses, landmarks = get_poses_landmarks(mu, N) print_all(poses, landmarks)如图：预计的机器人姿态和地标
可视化构建的世界：
# 导入函数from helpers import display_world# 显示最终世界！# 定义图形大小plt.rcParams["figure.figsize"] = (20,20)# 查看姿态是否已创立if 'poses' in locals(): # 打印出最初一个姿态 print('Last pose: ', poses[-1]) # 显示机器人的最初地位和地标地位 display_world(int(world_size), poses[-1], landmarks)如图：输出量
在Github（https://github.com/Noob-can-C...） 上查看代码。
原文链接：https://medium.com/@krunalksh...
欢送关注磐创AI博客站：
http://panchuang.net/
sklearn机器学习中文官网文档：
http://sklearn123.com/
欢送关注磐创博客资源汇总站：
http://docs.panchuang.net/