第三篇 坐标转换关系T平滑

一 背景

在SLAM系统中或者自动驾驶定位系统中，需要视觉感知元素和地图元素进行匹配，然后将感知元素旋转到地图上或者将地图元素旋转到感知元素表达的坐标系。这种转换关系使用T表达，每一帧感知都会对应一个T，帧与帧之间存在着抖动，需要进行平滑。
载体坐标和感知元素在 smooth DR坐标下表达，地图元素在地图坐标下表达，需要平滑的地图坐标到smooth坐标之间的转换关系T如下表达。
$$T_{\_smooth\_map}$$

二 位姿矩阵平滑定义变量

不能直接平滑T_smooth_map，因为位置和姿态的计算是相互关联的，并不是相互独立的过程。可以将载体在地图中的位置和姿态设置为状态量。姿态的计算是一个非线性过程，所以采用误差卡尔曼滤波。

2.1 状态量

载体在地图中的位置状态量：
$$T_{\_map\_rig}$$
状态误差量定义如下:
$$\begin{gathered} R_{true}=R\ast \delta R \\ {t}_{true}=t-\delta t \end{gathered}$$
状态误差向量定义如下
$$st\_vec\_err=[\delta x\delta y\delta z\delta {\varphi }_x\delta {\varphi }_y\delta {\varphi }_z]$$
后三维是姿态误差的等效旋转矢量

2.2 状态量更新

$$\begin{array}{rl}{T}_{\_map\_rig(k+1)}(update)& =T_{\_map\_rig(k)}\ast T_{\_rig(k)\_rig(k+1)}\\ & =T_{\_map\_rig(k)}\ast T_{\_rig(k)\_smooth}\ast T_{\_smooth\_rig(k+1)}\\ & =T_{\_map\_rig(k)}\ast T_{\_smooth\_rig(k)}.inv()\ast T_{\_smooth\_rig(k+1)}\end{array}$$

2.3 观测量

2.3.1 观测量

通过匹配运算观测到地图坐标系到smooth系之间的变换关系。
$$T_{\_smooth\_map}$$
求出载体在地图中的位姿矩阵，进而直接求出载体在地图中的位姿。
$$T_{\_map\_rig}(observation)=T_{\_smooth\_map}.inv()\ast T_{\_smooth\_rig}$$
注：通过上式可以看出，T_smooth_map的平滑转为T_map_rig的平滑。后者的位置和姿态是相互独立的。平滑T_map_rig之后，求出T_smooth_map。

2.3.2 观测状态更新带来的误差量

从误差量的定义触发， 通过观测，更新状态的误差量为：
$$\begin{gathered} \delta R_{\_update\_observation}=R_{\_map\_rig(update)}.inv()\ast R_{\_map\_rig(observation)} \\ \delta t_{\_update\_observation}=t_{\_map\_rig(update)}-t_{\_map\_rig(observation)} \end{gathered}$$
利用上式求出观测误差向量如下。
$$obs\_vec\_err=[\delta x\delta y\delta z\delta {\varphi }_x\delta {\varphi }_y\delta {\varphi }_z]$$

三 平滑求解方程

平滑过程采用误差卡尔曼滤波(ESKF)。

3.1状态转移方程

$$\begin{gathered} st\_vec\_err(k+1)=A\ast st\_vec\_err(k)+w_k \\ std(w_k)=\Delta s\ast [0.000020.000020.00020.0030.0030.003] \end{gathered}$$
A是单位矩阵，w_k是过程噪声。

3.2 观测方程

$$obs\_vec\_err(k+1)=C\ast st\_vec\_err(k)+v_k$$
C是单位矩阵，v_k是观测噪声。

3.3 校正状态

利用状态转移方程和观测方程就可以求出
$$st\_vec\_err=[\delta x\delta y\delta z\delta {\varphi }_x\delta {\varphi }_y\delta {\varphi }_z]$$
得到
$$\begin{gathered} \delta R \\ \delta t \end{gathered}$$
从而 根据误差量的定义，
$$\begin{gathered} R_{\_correct}=R_{\_map\_rig(update)}\ast \delta R \\ {t}_{\_correct}=R_{\_map\_rig(update)}-\delta t \end{gathered}$$

四 直接从T角度观测和更新(不建议)

另外一种角度计算也可以直接从T的角度触发定义误差量。
姿态误差量定义为
$$\begin{gathered} T_{\delta R}=\left[\begin{array}{cc}\delta R& 0\\ 0& 1\end{array}\right] \\ {T}_{true}=T\ast T_{\delta R} \end{gathered}$$
位置误差量定义为
$$\begin{gathered} T_{\delta t}=\left[\begin{array}{cc}1& -\delta t\\ 0& 1\end{array}\right] \\ {T}_{true}=T_{\delta t}\ast T \end{gathered}$$
因此校正过程可以直接得出
$$T_{\_map\_rig}=T_{\delta t}\ast T\ast T_{\delta R}$$

五 最后求出

$$T_{\_smooth\_map}=T_{\_smooth\_rig}\ast T_{\_map\_rig}.inv()$$