一. 状态估计问题
经典的SLAM/模型由2个方程构成:
如上式所示:上面的为运动方程;下面的为观测方程。
参数:
k:时刻
xk:k时刻的相机位姿。(用变换矩阵或李代数表示)
uk:输入(运动传感器的读数)
yj: 相机位姿为xk时,观测点的路标点
zk,j:相机位姿为xk时,对路标yj进行了一次观测,对应到图像上的像素位置。
wk:噪声项
vk,j:噪声项
f() 、h(): 某函数
本讲主要讲观测方程:
根据第五讲的知识:假设在xk处对路标进行了一次观测,对应到图像上的像素位置zk,j,则观测方程可表示:
注1:K为相机内参;s为像素点的距离(现实空间的观测点在相机坐标系下的Z坐标),zk,j和yj皆为齐次坐标的形式。其中exp(&^)=T(变换矩阵),&为六维向量。
&^即为将六维向量转换为四维矩阵。yj为世界坐标系下的坐标的齐次表示。
注2:通常认为两个噪声项wk,vk,j都满足零均值的高斯分布:
我们的工作就是:希望通过带有噪声的数据z和u,推断出位姿x和地图y。这构成了一个状态估计问题。
对机器人的状态估计问题,就是求已知输入数据u和观测数据z的条件下,计算状态x的条件概率分布。即:
当没有测量运动的传感器,只有一张张图象时,即只考虑观测方程时,相当于估计P(x | z)的条件概率分布。根据贝叶斯:
注1:P(x | z)称为后验概率。P(x)称为先验。P(z | x)称为似然。
注2:分母部分与估计量x无关,所以可以略去。
注3:直接求后验很难,但求一个状态最优