- slam过程的基本方程:
x k = f ( x k − 1 , u k , w k ) \boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}, \boldsymbol{w}_{k}\right) xk=f(xk−1,uk,wk) (运动方程)
z k , j = h ( y j , x k , v k , j ) \boldsymbol{z}_{k, j}=h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}, \boldsymbol{v}_{k, j}\right) zk,j=h(yj,xk,vk,j) (观测方程)
由此推导出slam问题,已知运动测量的读数u和传感器的读数z,如何求解定位问题(x)和建图问题(y)
-
旋转向量到旋转矩阵的过程为罗德里格斯公式, 如下:
R = cos θ I + ( 1 − cos θ ) n n T + sin θ n ∧ R=\cos \theta I+(1-\cos \theta) n n^{T}+\sin \theta n^{\wedge} R=cosθI+(1−cosθ)nnT+sinθn∧
其反变换如下:
θ = arccos ( tr ( R ) − 1 2 ) \theta=\arccos \left(\frac{\operatorname{tr}(R)-1}{2}\right) θ=arccos(2tr(R)−1)
转轴n为矩阵R的特征向量。给出一个旋转矩阵,即可使用这种方法求出旋转轴和旋转角。 -
欧拉角:欧拉角有多重模式,最常用的是偏航-俯仰-滚转。方法如下:
绕物体的Z 轴旋转,得到偏航角yaw;
绕旋转之后的Y 轴旋转,得到俯仰角pitch;
绕旋转之后的X 轴旋转,得到滚转角roll。 -
四元数:
q = [ cos θ 2 , n x sin θ 2 , n y sin θ 2 , n z sin θ 2 ] T q=\left[\cos \frac{\theta}{2}, n_{x} \sin \frac{\theta}{2}, n_{y} \sin \frac{\theta}{2}, n_{z} \sin \frac{\theta}{2}\right]^{T} q=[cos2θ,nxsin2θ,nysin2θ,nzsin2θ]T
两个相反的四元数表示同一种旋转。 -
罗德里格斯公式实际上是一种指数映射,将旋转向量组成的空间映射到旋转矩阵。
-
李代数(旋转矩阵)通过指数映射变为李群(旋转向量),李群通过对数映射变为李代数。不过通常并不这样做,而是有更加简单的代数方法。
三维旋转的对应变换关系
三维变换的对应变换关系 -
假定对某个旋转R,对应的李代数为ϕ。我们给它左乘一个微小旋转,记作ΔR,对应的李代数为Δϕ,根据bch近似,可以写成:
exp ( Δ ϕ ∧ ) exp ( ϕ ∧ ) = exp ( ( ϕ + J l − 1 ( ϕ ) Δ ϕ ) ∧ ) \exp \left(\Delta \phi^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\left(\phi+J_{l}^{-1}(\phi) \Delta \phi\right)^{\wedge}\right) exp(Δϕ∧)exp(ϕ∧)=exp((ϕ+Jl−1(ϕ)Δϕ)∧)
反之,则有:
exp ( ( ϕ + Δ ϕ ) ∧ ) = exp ( ( J l Δ ϕ ) ∧ ) exp ( ϕ ∧ ) = exp ( ϕ ∧ ) exp ( ( J r Δ ϕ ) ∧ ) \exp \left((\phi+\Delta \phi)^{\wedge}\right)=\exp \left(\left(J_{l} \Delta \phi\right)^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\phi^{\wedge}\right) \exp \left(\left(J_{r} \Delta \phi\right)^{\wedge}\right) exp((ϕ+Δϕ)∧)=exp((JlΔϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((JrΔϕ)∧)