Graph SLAM
Graph SLAM 是众多SLAM方法中的一种
约束：
对于6个姿势：
- 一初始位置约束
- 5个附加相对运动约束
- 8个地标位置相对测量约束
所有这些加起来总共14个约束
考虑上图，有4个姿势(包括初始位置x0)和一个地标。我们可以用同样的数学方法，对给定的图像有5个总约束。
你可能注意到，不是所有的约束都会提供有用的信息。例如：图中X2没有和地标之间进行测量。
将两次约束相加：
添加地标：
将没有联系的矩阵置为0：
为了实现Graph SLAM ,引入了，该矩阵为方正，并标有所有机器人姿势
和所有地标
。当在两个姿势移动距离
,将这两个位置关联起来，可以将其表示为这些矩阵中的数值关系。下图为
的一个矩阵表示和
的一个向量表示：
为了确定姿势和地标位置
- 初始位置 -3
- 移动 5
- 移动 3
约束更新
当机器人移动一定量,需要更新约束矩阵，如下所示：
- 在
xt和xt+1交叉点的索引处将[[1, -1], [-1, 1]]添加到omega中 - 在
xt和xt+1行中将-dx和dx添加到xi中
import numpy as np
def mu_from_positions(initial_pos, move1, move2):
## TODO: construct constraint matrices
## and add each position/motion constraint to them
# Your code here
omega = np.zeros((3,3))
xi = np.zeros((3,1))
omega[0][0] = 1
# account for the first motion, dx = move1
xi[0] = initial_pos
omega += [[1., -1., 0.],
[-1., 1., 0.],
[0., 0., 0.]]
xi += [[-move1],
[move1],
[0.0]]
# account for the second motion
omega += [[0., 0., 0.],
[0., 1., -1.],
[0., -1., 1.]]
xi += [[0.],
[-move2],
[move2]]
# display final omega and xi
print('Omega: \n', omega)
print('\n')
print('Xi: \n', xi)
print('\n')
## TODO: calculate mu as the inverse of omega * xi
## recommended that you use: np.linalg.inv(np.matrix(omega)) to calculate the inverse
omega_inv = np.linalg.inv(np.matrix(omega))
mu = omega_inv*xi
return mu
# call function and print out `mu`
mu = mu_from_positions(-3, 5, 3)
print('Mu: \n', mu)
所以：
约束更新
创建和
矩阵，并根据运动和地标传感器测量来更新矩阵。
传感器测量
当感知到姿势和地标之间有一个距离dl，更新约束矩阵，如下所示：
- 在
与
交叉点索引处将
[[1, -1], [-1, 1]]添加到中
- 在
-dl与dl添加到中
import numpy as np
def mu_from_positions(initial_pos, move1, move2, Z0, Z1, Z2):
## construct constraint matrices
## and add each position/motion constraint to them
# initialize constraint matrices with 0's
# Now these are 4x4 because of 3 poses and a landmark
omega = np.zeros((4,4))
xi = np.zeros((4,1))
# add initial pose constraint
omega[0][0] = 1
xi[0] = initial_pos
# account for the first motion, dx = move1
omega += [[1., -1., 0., 0.],
[-1., 1., 0., 0.],
[0., 0., 0., 0.],
[0., 0., 0., 0.]]
xi += [[-move1],
[move1],
[0.],
[0.]]
# account for the second motion
omega += [[0., 0., 0., 0.],
[0., 1., -1., 0.],
[0., -1., 1., 0.],
[0., 0., 0., 0.]]
xi += [[0.],
[-move2],
[move2],
[0.]]
## TODO: Include three new sensor measurements for the landmark, L
# incorporate first sense
omega += [[1., 0., 0., -1.],
[0., 0., 0., 0.],
[0., 0., 0., 0.],
[-1., 0., 0., 1.]]
xi += [[-Z0],
[0.0],
[0.0],
[Z0]]
# incorporate second sense
omega += [[0., 0., 0., 0.],
[0., 1., 0., -1.],
[0., 0., 0., 0.],
[0., -1., 0., 1.]]
xi += [[0.],
[-Z1],
[0.],
[Z1]]
# incorporate third sense
omega += [[0., 0., 0., 0.],
[0., 0., 0., 0.],
[0., 0., 1., -1.],
[0., 0., -1., 1.]]
xi += [[0.],
[0.],
[-Z2],
[Z2]]
# display final omega and xi
print('Omega: \n', omega)
print('\n')
print('Xi: \n', xi)
print('\n')
## TODO: calculate mu as the inverse of omega * xi
## recommended that you use: np.linalg.inv(np.matrix(omega)) to calculate the inverse
omega_inv = np.linalg.inv(np.matrix(omega))
mu = omega_inv*xi
return mu
# call function and print out `mu`
mu = mu_from_positions(-3, 5, 3, 10, 5, 2)
print('Mu: \n', mu)
测量信任强度
传感器测量
当感测到姿势和地标之间的距离dl时，更新约束矩阵，如下所示：
- 在
xt和l交叉点的索引处将[[1, -1], [-1, 1]]添加到omega中 - 在
xt和l行中将-dl和dl添加到xi中
三个新的dl值作为函数Z0, Z1, Z2的新输入，如下所示。
运动
当机器人移动一定量dx时，更新约束矩阵时，如下所示：
- 在
xt和xt+1交叉点的索引处将[[1, -1], [-1, 1]]添加到omega中 - 在
xt和xt+1的行中将-dx和dx添加到xi中
更改以上代码，使最后一次传感器测量(Z2)的置信度非常高，将该测量的强度乘以5倍
import numpy as np
def mu_from_positions(initial_pos, move1, move2, Z0, Z1, Z2):
## construct constraint matrices
## and add each position/motion constraint to them
# initialize constraint matrices with 0's
# Now these are 4x4 because of 3 poses and a landmark
omega = np.zeros((4,4))
xi = np.zeros((4,1))
# add initial pose constraint
omega[0][0] = 1
xi[0] = initial_pos
# account for the first motion, dx = move1
omega += [[1., -1., 0., 0.],
[-1., 1., 0., 0.],
[0., 0., 0., 0.],
[0., 0., 0., 0.]]
xi += [[-move1],
[move1],
[0.],
[0.]]
# account for the second motion
omega += [[0., 0., 0., 0.],
[0., 1., -1., 0.],
[0., -1., 1., 0.],
[0., 0., 0., 0.]]
xi += [[0.],
[-move2],
[move2],
[0.]]
## Include three new sensor measurements for the landmark, L
# incorporate first sense
omega += [[1., 0., 0., -1.],
[0., 0., 0., 0.],
[0., 0., 0., 0.],
[-1., 0., 0., 1.]]
xi += [[-Z0],
[0.0],
[0.0],
[Z0]]
# incorporate second sense
omega += [[0., 0., 0., 0.],
[0., 1., 0., -1.],
[0., 0., 0., 0.],
[0., -1., 0., 1.]]
xi += [[0.],
[-Z1],
[0.],
[Z1]]
## This third sense is now *very confident* and
## we multiply everything by a strength factor of 5 instead of 1
# incorporate third sense
omega += [[0., 0., 0., 0.],
[0., 0., 0., 0.],
[0., 0., 5., -5.],
[0., 0., -5., 5.]]
xi += [[0.],
[0.],
[-Z2],
[Z2]]
# display final omega and xi
print('Omega: \n', omega)
print('\n')
print('Xi: \n', xi)
print('\n')
## calculate mu as the inverse of omega * xi
## recommended that you use: np.linalg.inv(np.matrix(omega)) to calculate the inverse
omega_inv = np.linalg.inv(np.matrix(omega))
mu = omega_inv*xi
return mu
# call function and print out `mu`
mu = mu_from_positions(-3, 5, 3, 10, 5, 1)
print('Mu: \n', mu)
