姿态与运动学表示总结

本小节介绍了传统的旋转平移矩阵、四元数、对偶四元数以及鼎立四元数的定义以及运动学表示。

旋转平移矩阵

定义与性质

旋转矩阵 $\mathbf{R}$ 是一个用于表示三维空间中绕原点的旋转的正交矩阵。它有以下性质：

• 行列式：$\det (\mathbf{R})=1$，意味着它是特殊正交矩阵，属于旋转群 $SO(3)$。

• 逆矩阵：旋转矩阵的逆等于它的转置，即 ${\mathbf{R}}^{-1}={\mathbf{R}}^{\top }$。

• 正交性：所有行向量和列向量都是单位向量，并且相互正交。

平移向量 $\mathbf{t}$ 是一个三维向量，用于表示空间中的直线平移。它可以定义为：

$$\mathbf{t}=\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]$$

其中 $t_x,t_y,t_z$ 分别是沿着 $x$, $y$, 和 $z$ 轴的平移量。

刚体变换包括一个旋转和一个平移，可以用齐次坐标表示为 $4\times 4$ 矩阵：

$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0& 1\end{array}\right]$$

其中 $\mathbf{R}$ 是旋转矩阵，$\mathbf{t}$ 是平移向量，$0$ 是 $1\times 3$ 的零向量。这个变换矩阵可以用来将一个点 $\mathbf{p}$ 从一个坐标系变换到另一个坐标系：

$${\mathbf{p}}^{\prime }=\mathbf{Tp}$$

这里 $\mathbf{p}$ 和 ${\mathbf{p}}^{\prime }$ 都是齐次坐标下的点。

运动学表示

在一个SINS系统中，传统方法对姿态的描述包括旋转、速度、位置，并通过IMU所采集的数据为载体系的角速度与加速度。对于传统方法，我们的系统状态量为 $${\chi }_i=[{\mathbf{R}}_{b_i}^e,{\mathbf{v}}_i^e,{\mathbf{t}}_i^e],$$ 其中${\chi }_i$为$i$时刻姿态表示的状态，${\mathbf{R}}_{b_i}^e$为载体相对于参考系的旋转矩阵，${\mathbf{v}}_i^e$为载体相对参考系的速度，${\mathbf{t}}_i^e$为相对参考系的位移。设在$i$时刻时IMU所测得的角速度为${\omega }_i^b$，加速度为${\mathbf{a}}_i^b$，则从时刻$i$到$i+1$的状态推算需要计算状态随时间变化的导数，其中，${\mathbf{v}}_i^e$与${\mathbf{t}}_i^e$的计算易得： $${\dot{\mathbf{v}}}_i^e={\mathbf{R}}_{b_i}^e{\mathbf{a}}_i^b-\mathbf{g},$$ $${\dot{\mathbf{t}}}_i^e={\mathbf{v}}_i^e,$$ 其中$\mathbf{g}$为重力加速度。在SLAM系统中，地球的自转通常可以被忽略，因此我对重力模型相较于论文中进行了简化。旋转矩阵的求导相对复杂，首先这里先介绍罗德里格斯公式（Rodrigues’ rotation formula）。

罗德里格斯公式提供了一个绕单位向量 $\mathbf{u}=(u_x,u_y,u_z)$ 旋转角度 $\theta$ 的旋转矩阵 $\mathbf{R}$。公式如下：

$$\mathbf{R}=(\cos \theta )\mathbf{I}+(\sin \theta )\mathbf{K}+(1-\cos \theta ){\mathbf{K}}^2,$$ 其中 $\mathbf{I}$ 是单位矩阵，$\mathbf{K}$ 是关于 $\mathbf{u}$ 的反对称矩阵，定义为： $$\mathbf{K}=\left(\begin{array}{ccc}0& -u_z& u_y\\ u_z& 0& -u_x\\ -u_y& u_x& 0\end{array}\right)=\mathbf{u}\times .$$ 对于旋转矩阵求导，根据导数定义： $${\dot{\mathbf{R}}}_{b_i}^e=\underset{\Delta t\to 0}{\lim }\frac{{\mathbf{R}}_{b_i}^e(t+\Delta t)-{\mathbf{R}}_{b_i}^e(t)}{\Delta t}.$$ 在极短的时间内，假设从$i$到$i+1$时刻，可以得到 $${\mathbf{R}}_{b_i}^e(t+\Delta t)={\mathbf{R}}_{b_{i+1}}^e={\mathbf{R}}_{b_i}^e(t){\mathbf{R}}_{b_{i+1}}^{b_i},$$ 其中，${\mathbf{R}}_{b_{i+1}}^{b_i}$为载体在$i+1$时刻相对于$i$时刻的相对旋转矩阵。当旋转在极短时间内为小量时，利用罗德里格斯公式，对${\mathbf{R}}_{b_{i+1}}^{b_i}$进行近似，得到： $${\mathbf{R}}_{b_{i+1}}^{b_i}\approx \mathbf{I}+{\theta }_{b_{i+1}}^{b_i}{\mathbf{K}}_{b_{i+1}}^{b_i}.$$ 将（8）代入（6）中得到： $${\dot{\mathbf{R}}}_{b_i}^e=\underset{\Delta t\to 0}{\lim }\frac{{\mathbf{R}}_{b_i}^e(t)(\mathbf{I}+{\theta }_{b_{i+1}}^{b_i}{\mathbf{K}}_{b_{i+1}}^{b_i})-{\mathbf{R}}_{b_i}^e(t)}{\Delta t}=\underset{\Delta t\to 0}{\lim }\frac{{\mathbf{R}}_{b_i}^e(t){\theta }_{b_{i+1}}^{b_i}{\mathbf{K}}_{b_{i+1}}^{b_i}}{\Delta t},$$ 显然， $$\underset{\Delta t\to 0}{\lim }\frac{{\theta }_{b_{i+1}}^{b_i}{\mathbf{K}}_{b_{i+1}}^{b_i}}{\Delta t}={\omega }_i^b\times .$$ 因此，旋转矩阵对时间的导数为： $${\dot{\mathbf{R}}}_{b_i}^e={\mathbf{R}}_{b_i}^e{\omega }_i^b\times .$$

综上，传统的姿态状态表示方式需要$3\times 3+3\times 1+3\times 1=15$维来描述。在旋转、平移、速度这三个量上的表示相对独立。

状态空间	运动学表示
${\chi }_i=[{\mathbf{R}}_{b_i}^e,{\mathbf{v}}_i^e,{\mathbf{t}}_i^e]$	${\dot{\mathbf{R}}}_{b_i}^e={\mathbf{R}}_{b_i}^e{\omega }_i^b\times$ ; ${\dot{\mathbf{v}}}_i^e={\mathbf{R}}_{b_i}^e{\mathbf{a}}_i^b-\mathbf{g}$ ; ${\dot{\mathbf{t}}}_i^e={\mathbf{v}}_i^e$

传统运动学表示

传统四元数

定义与性质

四元数是复数的一个扩展，用于在三维空间中表示旋转。它由一个实部和三个虚部组成，可以表示为 $q=a+bi+cj+dk$，其中 $a,b,c,d\in \mathbb{R}$，并且 $i,j,k$ 是四元数的虚单位。

四元数的基本性质包括：

• 四元数的加法和实数的加法类似，遵循交换律和结合律。

• 四元数的乘法不遵循交换律，但遵循结合律。

• 四元数的虚单位满足以下关系： $$\begin{array}{rl}{i}^2& =j^2=k^2=ijk=-1,\\ ij& =k,ji=-k,\\ jk& =i,kj=-i,\\ ki& =j,ik=-j.\end{array}$$

• 四元数的共轭 $q^{\ast }$ 定义为 $q^{\ast }=a-bi-cj-dk$。

• 四元数的模（或范数）$\Vert q\Vert$ 定义为 $\Vert q\Vert =\sqrt{a^2+b^2+c^2+d^2}$。

• 四元数的逆 $q^{-1}$（如果 $q$ 不是零四元数）定义为 $q^{-1}=\frac{q^{\ast }}{\Vert q{\Vert }^2}$。

• 一个单位四元数满足 $\Vert q\Vert =1$。

两个四元数 $q_1=a_1+b_{1}i+c_{1}j+d_{1}k$ 和 $q_2=a_2+b_{2}i+c_{2}j+d_{2}k$ 的加法定义为：

$$q_1+q_2=(a_1+a_2)+(b_1+b_2)i+(c_1+c_2)j+(d_1+d_2)k$$

四元数的乘法则较为复杂，其定义为：
$$\begin{array}{rl}{q}_1\otimes q_2=& (a_1{a}_2-b_1{b}_2-c_1{c}_2-d_1{d}_2)\\ & +(a_1{b}_2+b_1{a}_2+c_1{d}_2-d_1{c}_2)i\\ & +(a_1{c}_2-b_1{d}_2+c_1{a}_2+d_1{b}_2)j\\ & +(a_1{d}_2+b_1{c}_2-c_1{b}_2+d_1{a}_2)k\end{array}$$

四元数在三维空间中表示旋转时在计算上比旋转矩阵更高效，减少了旋转矩阵的维数冗余。

运动学表示

传统四元数在SLAM中一般与平移向量和速度向量联合使用。平移向量和速度向量的时间导数在上一小节已经讨论过，下面讨论传统四元数对时间的导数。

设$q_{b_i}^e$为载体相对于参考系的旋转四元数。 对于旋转四元数求导，根据导数定义： $${\dot{q}}_{b_i}^e=\underset{\Delta t\to 0}{\lim }\frac{q_{b_i}^e(t+\Delta t)-q_{b_i}^e(t)}{\Delta t}.$$ 在极短的时间内，假设从$i$到$i+1$时刻，可以得到 $$q_{b_i}^e(t+\Delta t)=q_{b_{i+1}}^e=q_{b_i}^e(t)q_{b_{i+1}}^{b_i},$$ 其中，$q_{b_{i+1}}^{b_i}$为载体在$i+1$时刻相对于$i$时刻的相对旋转四元数。与旋转矩阵相似，给定一个单位旋转轴 $\mathbf{u}=(u_x,u_y,u_z)$ 和一个旋转角 $\theta$，相应的单位四元数 $q$ 可以表示为：

$$q=\cos \left(\frac{\theta }{2}\right)+(u_{x}i+u_{y}j+u_{z}k)\sin \left(\frac{\theta }{2}\right),$$

其中 $\theta$ 是旋转角，$\mathbf{u}$ 是旋转轴，且 $u_x,u_y,u_z$ 是 $\mathbf{u}$ 的坐标。这里 $i,j,k$ 是四元数的虚单位。该四元数 $q$ 表示绕轴 $\mathbf{u}$ 旋转 $\theta$ 弧度的旋转。 当在极短时间内旋转一个小角度时，$q_{b_{i+1}}^{b_i}$可近似表示为 $$q_{b_{i+1}}^{b_i}\approx 1+(u_{x}i+u_{y}j+u_{z}k)\frac{{\theta }_{b_{i+1}}^{b_i}}{2}$$ 将（14）代入（12）中得到： $${\dot{q}}_{b_i}^e=\underset{\Delta t\to 0}{\lim }\frac{q_{b_i}^e(t)(1+(u_{x}i+u_{y}j+u_{z}k)\frac{{\theta }_{b_{i+1}}^{b_i}}{2})-q_{b_i}^e(t)}{\Delta t}=\underset{\Delta t\to 0}{\lim }\frac{q_{b_i}^e(t)\otimes [0;\mathbf{u}\frac{{\theta }_{b_{i+1}}^{b_i}}{2}]}{\Delta t},$$ 显然， $$\underset{\Delta t\to 0}{\lim }\frac{[0;\mathbf{u}\frac{{\theta }_{b_{i+1}}^{b_i}}{2}]}{\Delta t}=[0;\frac{{\omega }_i^b}{2}].$$ 因此， $${\dot{q}}_{b_i}^e=q_{b_i}^e\otimes [0;\frac{{\omega }_i^b}{2}].$$

综上，传统四元数可以高效表示旋转，用4维代替了旋转矩阵的9维对旋转进行了表示。

对偶四元数

定义与性质

对偶四元数是一种数学工具，用于在三维空间中同时表示旋转和平移。一个对偶四元数可以写为 $\hat{q}=q+ϵq^{\prime }$，其中 $q$ 是标准四元数部分，用于表示旋转，而 $q^{\prime }$ 是对偶部分，用于表示平移，且 $ϵ$ 是对偶单位，满足 ${ϵ}^2=0$ 但 $ϵ\ne 0$。 一个对偶四元数 $\hat{q}$ 由一个实四元数 $q$ 和一个对偶四元数 $ϵq^{\prime }$ 组成：

$$\hat{q}=q+ϵq^{\prime }$$

其中 $q=a+bi+cj+dk$ 是旋转部分，$q^{\prime }=a^{\prime }+b^{\prime }i+c^{\prime }j+d^{\prime }k$ 是平移部分，$a,b,c,d,a^{\prime },b^{\prime },c^{\prime },d^{\prime }$ 是实数。 对偶四元数的基本性质包括：

• 加法：两个对偶四元数的加法遵循逐元素加法规则。

• 乘法：两个对偶四元数的乘法不遵循交换律，但遵循结合律和分配律。

• 共轭：对偶四元数的共轭定义为 ${\hat{q}}^{\ast }=q^{\ast }+ϵq^{\prime \ast }$。

• 模：对偶四元数的模定义为 $|\hat{q}|=|q|+ϵ\frac{q\cdot q^{\prime }}{|q|}$。

对偶四元数能够紧凑地表示三维空间中的刚体变换（旋转和平移）。

运动学表示

在一个SINS系统中，对姿态的描述包括旋转、速度、位置，并通过IMU所采集的数据为载体系的角速度与加速度。对于对偶四元数方法，我们的系统状态量为 $${\chi }_i=[{\hat{q}}_{b_i}^e,{\mathbf{v}}_i^e],$$ 其中${\chi }_i$为$i$时刻姿态表示的状态，${\hat{q}}_{b_i}^e$为载体相对于参考系的对偶四元数，其中包含了旋转四元数$q_{b_i}^e$，及其对偶部分，其对偶部分可以抽象理解为平移的表示，可以表示为$\frac{1}{2}{\mathbf{t}}_i^e\otimes q_{b_i}^e$, ${\mathbf{t}}_i^e$为相对参考系的位移。对偶四元数形式如下： $${\hat{q}}_{b_i}^e=q_{b_i}^e+\frac{ϵ}{2}{\mathbf{t}}_i^e\otimes q_{b_i}^e,$$ ${\mathbf{v}}_i^e$为载体相对参考系的速度。设在$i$时刻时IMU所测得的角速度为${\omega }_i^b$，加速度为${\mathbf{a}}_i^b$，则从时刻$i$到$i+1$的状态推算需要计算状态随时间变化的导数，其中，${\mathbf{v}}_i^e$与${\mathbf{t}}_i^e$的计算易得： $${\dot{\mathbf{v}}}_i^e=q_{b_i}^e\otimes {\mathbf{a}}_i^b\otimes {q_{b_i}^e}^{\ast }-\mathbf{g},$$ $${\dot{\mathbf{t}}}_i^e={\mathbf{v}}_i^e,$$

对于对偶四元数的时间导数可以表示为：

$${\dot{\hat{q}}}_{b_i}^e={\dot{q}}_{b_i}^e+\frac{ϵ}{2}({\mathbf{t}}_i^e\otimes q_{b_i}^e{)}^{\prime },$$ 利用复合函数求导规则可以得到 $${\dot{\hat{q}}}_{b_i}^e={\dot{q}}_{b_i}^e+\frac{ϵ}{2}({\dot{\mathbf{t}}}_i^e\otimes q_{b_i}^e+{\mathbf{t}}_i^e\otimes {\dot{q}}_{b_i}^e).$$ 将式（18）与式（22）代入（24）可以得到 $${\dot{\hat{q}}}_{b_i}^e=q_{b_i}^e\otimes [0;\frac{{\omega }_i^b}{2}]+\frac{ϵ}{2}({[0;\mathbf{v}}_i^e]\otimes q_{b_i}^e+{\mathbf{t}}_i^e\otimes q_{b_i}^e\otimes [0;\frac{{\omega }_i^b}{2}]).$$

整理并化简上式可得 $${\dot{\hat{q}}}_{b_i}^e={\hat{q}}_{b_i}^e\otimes \frac{1}{2}({\mathbf{\omega }}_i^b+ϵ{\mathbf{v}}_i^e),$$

这里 ${\mathbf{\omega }}_i^b$ 和 ${\mathbf{v}}_i^e$ 分别被表示为四元数形式，即实部为零。若记${\hat{\omega }}_i^b=({\mathbf{\omega }}_i^b+ϵ{\mathbf{v}}_i^e)$，那么对偶四元数对时间的求导形式可与传统四元数相同： $${\dot{\hat{q}}}_{b_i}^e={\hat{q}}_{b_i}^e\otimes \frac{1}{2}{\hat{\omega }}_i^b.$$

综上，对偶四元数将旋转和平移统一表示在一个对偶四元数中，但是对于速度状态仍然单独考虑。

状态空间	运动学表示
${\chi }_i=[{\hat{q}}_{b_i}^e,{\mathbf{v}}_i^e]$	${\dot{\hat{q}}}_{b_i}^e={\hat{q}}_{b_i}^e\otimes \frac{1}{2}{\hat{\omega }}_i^b$ ; ${\dot{\mathbf{v}}}_i^e=q_{b_i}^e\otimes {\mathbf{a}}_i^b-\mathbf{g}$

对偶四元数运动学表示

鼎立四元数

定义与性质

鼎立四元数（TridentQs）在对偶四元数的基础上扩展了一维虚部，定义为 $\breve{q}=q+{ϵ}_1{q}^{\prime }+{ϵ}_2{q}^{\prime \prime }$，其中 $q,q^{\prime },q^{\prime \prime }$ 是常规的四元数，对偶单位 ${ϵ}_1,{ϵ}_2$ 满足 ${ϵ}_1^2={ϵ}_2^2={ϵ}_1{ϵ}_2=0$，且 ${ϵ}_1\ne 0$ 和 ${ϵ}_2\ne 0$。

对于任意两个鼎立四元数 ${\breve{q}}_1=q_1+{ϵ}_1{q}_1^{\prime }+{ϵ}_2{q}_1^{\prime \prime }$ 和 ${\breve{q}}_2=q_2+{ϵ}_1{q}_2^{\prime }+{ϵ}_2{q}_2^{\prime \prime }$，算术运算如下：

• 加法： ${\breve{q}}_1+{\breve{q}}_2=q_1+q_2+{ϵ}_1(q_1^{\prime }+q_2^{\prime })+{ϵ}_2(q_1^{\prime \prime }+q_2^{\prime \prime })$

• 乘法： ${\breve{q}}_1\otimes {\breve{q}}_2=q_1\otimes q_2+{ϵ}_1(q_1\otimes q_2^{\prime }+q_1^{\prime }\otimes q_2)+{ϵ}_2(q_1\otimes q_2^{\prime \prime }+q_1^{\prime \prime }\otimes q_2)$

运动学表示

在一个SINS系统中，对姿态的描述包括旋转、速度、位置，并通过IMU所采集的数据为载体系的角速度与加速度。对于对偶四元数方法，我们的系统状态量为 $${\chi }_i=[{\breve{q}}_{b_i}^e],$$ 其中${\chi }_i$为$i$时刻姿态表示的状态，${\hat{q}}_{b_i}^e$为载体相对于参考系的对偶四元数，其中包含了旋转四元数$q_{b_i}^e$，及其两个对偶部分，第一个对偶部分抽象表示了载体速度$\frac{1}{2}{\mathbf{v}}_i^e\otimes q_{b_i}^e$，${\mathbf{t}}_i^e$为相对参考系的载体速度，第二个对偶部分抽象表示了载体平移，可以表示为$\frac{1}{2}{\mathbf{t}}_i^e\otimes q_{b_i}^e$, ${\mathbf{t}}_i^e$为相对参考系的载体位移。鼎立四元数形式如下： $${\breve{q}}_{b_i}^e=q_{b_i}^e+\frac{{ϵ}_1}{2}{\mathbf{v}}_i^e\otimes q_{b_i}^e+\frac{{ϵ}_2}{2}{\mathbf{t}}_i^e\otimes q_{b_i}^e,$$

对于对偶四元数的时间导数可以表示为：

$${\dot{\breve{q}}}_{b_i}^e={\dot{q}}_{b_i}^e+\frac{{ϵ}_1}{2}({\mathbf{v}}_i^e\otimes q_{b_i}^e{)}^{\prime }+\frac{{ϵ}_2}{2}({\mathbf{t}}_i^e\otimes q_{b_i}^e{)}^{\prime },$$ 利用复合函数求导规则可以得到 $${\dot{\breve{q}}}_{b_i}^e={\dot{q}}_{b_i}^e+\frac{{ϵ}_1}{2}({\dot{\mathbf{v}}}_i^e\otimes q_{b_i}^e+{\mathbf{v}}_i^e\otimes {\dot{q}}_{b_i}^e)+\frac{{ϵ}_2}{2}({\dot{\mathbf{t}}}_i^e\otimes q_{b_i}^e+{\mathbf{t}}_i^e\otimes {\dot{q}}_{b_i}^e).$$ 设在$i$时刻时IMU所测得的角速度为${\omega }_i^b$。在鼎立四元数的推导过程中，为了形式更加优美，设加速度为载体的纯加速度，不考虑重力的影响，可表示为${\mathbf{a}}_i^b$，则从时刻$i$到$i+1$的状态推算需要计算状态随时间变化的导数，其中，${\mathbf{v}}_i^e$与${\mathbf{t}}_i^e$的计算易得： $${\dot{\mathbf{v}}}_i^e=q_{b_i}^e\otimes {\mathbf{a}}_i^b\otimes {q_{b_i}^e}^{\ast },$$ $${\dot{\mathbf{t}}}_i^e={\mathbf{v}}_i^e,$$ 将式（32）及（33）代入（31）得到 $$\begin{array}{r}{\dot{\breve{q}}}_{b_i}^e=q_{b_i}^e\otimes [0;\frac{{\omega }_i^b}{2}]+\frac{{ϵ}_1}{2}(q_{b_i}^e\otimes {\mathbf{a}}_i^b\otimes {q_{b_i}^e}^{\ast }\otimes q_{b_i}^e+{\mathbf{v}}_i^e\otimes q_{b_i}^e(t)\otimes [0;\frac{{\omega }_i^b}{2}])+\\ \frac{{ϵ}_2}{2}({[0;\mathbf{v}}_i^e]\otimes q_{b_i}^e+{\mathbf{t}}_i^e\otimes q_{b_i}^e(t)\otimes [0;\frac{{\omega }_i^b}{2}]).\end{array}$$

通过四元数结合律可得 $$\begin{array}{r}{\dot{\breve{q}}}_{b_i}^e=q_{b_i}^e\otimes [0;\frac{{\omega }_i^b}{2}]+\frac{{ϵ}_1}{2}(q_{b_i}^e\otimes {\mathbf{a}}_i^b+{\mathbf{v}}_i^e\otimes q_{b_i}^e(t)\otimes [0;\frac{{\omega }_i^b}{2}])+\\ \frac{{ϵ}_2}{2}({[0;\mathbf{v}}_i^e]\otimes q_{b_i}^e+{\mathbf{t}}_i^e\otimes q_{b_i}^e(t)\otimes [0;\frac{{\omega }_i^b}{2}]).\end{array}$$

整理并化简上式可得 $${\dot{\breve{q}}}_{b_i}^e={\breve{q}}_{b_i}^e\otimes \frac{1}{2}({\mathbf{\omega }}_i^b+{ϵ}_1{\mathbf{a}}_i^b+{ϵ}_2{\mathbf{v}}_i^e),$$

这里 ${\mathbf{\omega }}_i^b$ 、 ${\mathbf{a}}_i^b$和 ${\mathbf{v}}_i^e$ 分别被表示为四元数形式，即实部为零。若记${\breve{\omega }}_i^b=({\mathbf{\omega }}_i^b+{ϵ}_1{\mathbf{a}}_i^b+{ϵ}_2{\mathbf{v}}_i^e)$，那么鼎立四元数对时间的求导形式可与传统四元数相同： $${\dot{\breve{q}}}_{b_i}^e={\breve{q}}_{b_i}^e\otimes \frac{1}{2}{\breve{\omega }}_i^b.$$

综上，鼎立四元数将旋转、速度和平移统一表示在一个鼎立四元数中，是目前位置最简洁的姿态表示方式。

状态空间	运动学表示
${\chi }_i=[{\breve{q}}_{b_i}^e]$	${\breve{q}}_{b_i}^e\otimes \frac{1}{2}{\breve{\omega }}_i^b$

鼎立四元数运动学表示

TQ切比雪夫多项式迭代积分

在实际系统中，由于IMU采集的频率有限，因此数值积分存在计算误差。在常见的SLAM系统中，通常使用中点积分的方式来缓解积分数值误差。然而，在高速场景中，中点积分的性能也会迅速下降。论文中使用的切比雪夫多项式迭代积分方法可以实现更加准确的数值积分。接下来针对鼎立四元数的积分求解位姿进行讨论。

对于理想的位姿积分，积分过程为下式： $$\breve{q}(t)=\breve{q}(0)+{\int }_0^t\dot{\breve{q}}(t)dt.$$ 根据2.4小节讨论的鼎立四元数动力学，积分过程可以近似为： $$\breve{q}(t)=\breve{q}(0)+\frac{1}{2}{\int }_0^t\breve{q}(t)\otimes {\breve{\omega }}^b(t)dt.$$

传统中点积分法

对于传统中点法，采用中点处的斜率直接近似两个采样点间的变化。其离散化推导如下： $${\breve{q}}_{i+1}={\breve{q}}_i+\frac{1}{4}{\breve{q}}_i\otimes ({\breve{\omega }}_{i+1}^b+{\breve{\omega }}_i^b).$$ 这种计算方式效率很高，但是只用两个时刻前后的采样值进行近似误差较大。适合用于短时间低速运动又需要快速推算的场景下。

切比雪夫逼近迭代法

切比雪夫迭代法的思路是通过切比雪夫多项式逼近真值运动。由于需要应用切比雪夫多项式逼近，首先需要将时间归一化至[-1,1]之间。假设采样持续时间为$t_N$,每个采样时刻为$t_k$，令$t=t_N(1+\tau )/2$，则式（37）可变换为 $$\breve{q}(\tau )=\breve{q}(0)+\frac{t_N}{4}{\int }_{-1}^{\tau }\breve{q}(\tau )\otimes {\breve{\omega }}^b(\tau )d\tau .$$ 首先，对于由角速度、加速度和速度构成的${\breve{\omega }}^b(\tau )$进行切比雪夫多项式逼近，得到 $${\breve{\omega }}^b(\tau )=\sum _{j=0}^{n_{\omega }}{\breve{c}}_j{F}_j(\tau ),$$ 其中，$n_{\omega }$为多项式阶数，${\breve{c}}_j$为切比雪夫系数，$F_j(\tau )$为$j$阶切比雪夫多项式。

接着，对鼎立四元数所表示的状态进行切比雪夫逼近初始化，这里的初始化切比雪夫系数可以由传统中点法获得，得到 $$\breve{q}(\tau )=\sum _{i=0}^{n_q}{\breve{b}}_{l,i}{F}_i(\tau ),$$ 其中，$n_q$为多项式阶数，${\breve{b}}_{l,i}$为第$l$次迭代的切比雪夫系数，$F_i(\tau )$为$j$阶切比雪夫多项式。将式（39）和式（40）代入式（41）可以得到 $${\breve{q}}_{l+1}(\tau )=\breve{q}(0)+\frac{t_N}{4}{\int }_{-1}^{\tau }\sum _{i=0}^{n_q}{\breve{b}}_{l,i}{F}_i(\tau )\otimes \sum _{j=0}^{n_{\omega }}{\breve{c}}_j{F}_j(\tau )d\tau .$$ 将积分和求和顺序进行变换后得到 $${\breve{q}}_{l+1}(\tau )=\breve{q}(0)+\frac{t_N}{4}\sum _{i=0}^{n_q}{\breve{b}}_{l,i}\otimes \sum _{j=0}^{n_{\omega }}{\breve{c}}_j{\int }_{-1}^{\tau }{F}_i(\tau )F_j(\tau )d\tau .$$ 通过式（43）的计算得到$q_{l+1}(\tau )$的值后，将结果代回到式（41），再次对状态进行切比雪夫逼近，重复此操作不断迭代直到收敛。

由于鼎立四元数统一表示了旋转、位置和速度，因此在切比雪夫迭代过程中，只需要进行一次迭代运算，而传统的表示方法需要分别对旋转、速度和平移进行3次迭代，提升了计算效率。