SLAM问题中,机器人自身的状态估计主要有两种方式: 基于滤波器的方法(Kalman filter, Particle filter等)和基于非线性优化的方法,目前主流的SLAM方案均使用了非线性优化方法
状态估计
经典SLAM模型主要由运动模型和观测模型组成,其方程如下
xk=f(xk−1,uk)+wkzk,j=h(yj,xk)+vk,j {x}_{k}=f\left({x}_{k-1}, {u}_{k}\right)+{w}_{k}\\ {z}_{k, j}=h\left({y}_{j}, {x}_{k}\right)+{v}_{k, j} xk=f(xk−1,uk)+wkzk,j=h(yj,xk)+vk,j
在传统的基于滤波器的方案中其状态估计分为两个步骤: 运动更新与观测更新,从概率学的角度看,是在已知输入数据uuu和观测数据zzz的条件下,计算状态xxx的条件概率分布,x={
x1,...,xN,y1,...,yM}x=\lbrace x_1,...,x_N,y_1,...,y_M \rbracex={
x1,...,xN,y1,...,yM},包括了机器人自身位姿以及环境中的路标点,利用贝叶斯法则:
P(x∣z)=P(z∣x)P(x)P(z) P(x|z)=\frac{P(z|x)P(x)}{P(z)} P(x∣z)=P(z)P(z∣x)P(x)
其中P(x∣z)P(x|z)P(x∣z)为后验概率,P(z∣x)P(z|x)P(z∣x)为似然,P(x)P(x)P(x)为先验,此时的目标为求解一个xxx使得后验概率最大
xMAP∗=argmaxP(x∣z)=argmaxP(z∣x)P(x) {x}^{*}_{M A P}=\arg \max P({x} | {z})=\arg \max P({z} | {x}) P({x}) xMAP∗=argmaxP(x∣z)=argmaxP(z∣x)P(x)
未知先验的情况下,求解最大似然问题,即在什么状态下最可能产生当前的观测数据
xMLE∗=argmaxP(z∣x) x^*_{MLE}=\arg \max P(z|x) xMLE∗=argmaxP(z∣x)
对上述的观测模型zk,j=h(yj,xk)+vk,jz_{k,j}=h(y_j,x_k)+v_{k,j}zk,j=h(yj,xk<