加速度计和陀螺仪——姿态位置求解

上边的描述虽然绕口，但是常见的三维笛卡尔坐标系的向上z轴、向右y轴、向前x轴的坐标系也是标准的右手坐标系。具体可以使用 “大拇指朝向x轴、食指朝向y轴、中指朝向z轴”的判断方式，并且右手手指自然弯曲方向为绕x轴旋转的正方向——逆时针方向。同理，“大拇指朝向y轴、食指朝向z轴、中指朝向x轴”“大拇指朝向z轴、食指朝向x轴、中指朝向y轴”也可以判断逆时针为旋转正方向。

（注：很多的坐标系建立不是依照这个标准右手坐标系建立的，并且旋转的正方向也没有给定，所以之后使用欧拉法时要尤为注意坐标系和旋转正方向！！！）

常见描述两坐标系间关系的方法：

三参数法——欧拉角法
四参数法——四元数法
九参数法——方向余弦法

可参考：四元数、欧拉角和方向余弦的定义及关系_方向余弦欧拉角四元数-CSDN博客

角度

航向角 $\psi$ （Yaw）：是飞行器绕z轴的旋转，也就是飞行器的机头在水平面上的左右偏转。
俯仰角 $\theta$ （Pitch）：是飞行器绕y轴的旋转，也就是飞行器的机头在垂直面上的上下倾斜。
滚转角 $\phi$ （Roll）：是飞行器绕x轴的旋转，也就是飞行器机身的左右翻滚。

具体上述角度的直观图像表示参考：

（虽然在飞行器中 $\psi$ 、 $\theta$ 、 $\phi$ 的物理含义相同，但是参考链接1中的坐标系是标准的右手坐标系，我们使用的是常见直角坐标系，和参考连接2相同）

（根据不同的xyz轴的位置定义，会导致绕轴旋转的角度定义的不同）

数据处理（以四旋翼为例）

可以先观看后文第二节算法部分，以便理解

为什么同时使用加速度计和陀螺仪？

答：单独使用加速度计会存在抖动噪声较大，无法直接使用；单独使用陀螺仪不易受到抖动影响，但测得的数据是角速度，为获取实际姿态需要进行积分，但是长时间的积分行为会导致积分值逐渐偏离实际值。

姿态求解

假设从全局坐标系中转化到局部坐标系的四元数为 $Q=q_0+q_1*\vec{i}+q_2*\vec{j}+q_3*\vec{k}$ ，旋转矩阵为 $C_b^n$ 形式。这里的 $Q$ 时使用陀螺仪数据来获取的，也就是说用陀螺仪数据积分获取当前姿态（即局部坐标系）。更新规则为：四元数微分方程的推导和解算实现 - 知乎

$\frac{dQ}{dt}=\frac{1}{2}Q\otimes \begin{bmatrix}0\\ gyr_x\\gyr_y\\gyr_z \end{bmatrix} =\frac{1}{2} M(gyr)*Q$

$Q(t+T) = Q(t) + T*\frac{dQ}{dt}$

其中 $T$ 为采样时间间隔

加速度计获取的数据是局部坐标系下的加速度数据acc_x、acc_y、acc_z，所以需要将该加速度数据转化到全局坐标系下。

首先进行加速度数据归一化

$\begin{aligned} & acc = \sqrt{acc_x^2 + acc_y^2 + acc_z^2} \\ & a_x = \frac{acc_x}{acc}\\ & a_y = \frac{acc_y}{acc}\\ &a_z = \frac{acc_z}{acc} \end{aligned}$

将重力加速度从全局坐标系中转化到局部坐标系，以重力单位向量为例

$G=0+0\vec{i}+0\vec{j}-1\vec{z}=\begin{bmatrix}0\\0\\0\\-1 \end{bmatrix}$

$G'=C_b^n*G = \begin{bmatrix}0\\2q_0q_3+2q_1q_2\\2q_2q_3-2q_0q_1 \\q_0^2-q_2^2-q_2^2+q_3^2 \end{bmatrix}$

则重力单位向量在局部坐标系中的分量为

$u_x=2q_0q_3+2q_1q_2\\ u_y=2q_2q_3-2q_0q_1 \\ u_z=q_0^2-q_2^2-q_2^2+q_3^2$

但是单独使用陀螺仪数据会导致随着时间变化积分数据会逐渐偏离实际值，为此使用加速度计数据进行补偿更新。使用陀螺仪数据获取的重力单位向量和加速度计的重力单位向量进行叉乘，求取两向量间的偏差值（为什么能进行叉乘，因为 $u_x\;u_y\;u_z$ 和 $a_x\;a_y\;a_z$ 都是在局部坐标系中，只是说两个获取来源不一样，存在偏差）

$e_x\vec{i}+e_y\vec{j}+e_z\vec{k} =(a_x\vec{i}+a_y\vec{j}+a_z\vec{k} ) \times (u_x\vec{i}+u_y\vec{j}+u_z\vec{k})\\ =\begin{bmatrix}i &j &k \\a_x & a_y & a_z\\ u_x & u_y & u_z \end{bmatrix}=(a_yu_z-a_zu_y)\vec{i} + (a_zu_x-a_xu_z)\vec{j} + (a_xu_y-a_yu_x)\vec{k}$

为什么叉乘可以表示误差？

答案来自kimiAI助手，点击链接查看和 Kimi 智能助手的对话 https://kimi.moonshot.cn/share/ct50ci10522hogo2v18g

垂直性：叉乘的结果是一个垂直于两个原始向量的向量。这意味着，如果两个向量不平行，它们的叉乘结果将指向它们之间的“偏差”方向。

大小与角度的关系：叉乘的大小等于两个向量的大小乘以它们之间夹角的正弦值。公式为 ∥A×B∥=∥A∥∥B∥sin⁡θ，其中 $\theta$ 是两个向量之间的夹角。当两个向量完全平行时，夹角为0，正弦值为0，叉乘结果为零向量，表示没有偏差；当两个向量不平行时，存在一个非零的角度，叉乘结果将有一个非零的大小，表示两个向量之间的偏差。

方向的判定：叉乘结果的符号（正或负）可以告诉我们一个向量旋转到另一个向量的方向。如果结果为正，则表示一个向量逆时针旋转到另一个向量；如果结果为负，则表示顺时针旋转。这可以用来表示两个向量之间的相对方向偏差。

误差表示：在某些应用中，如无人机算法，叉乘被用来表示两个向量之间的误差。例如，如果两个向量分别代表两个不同的参考系中同一物理量的方向，它们的叉乘可以表示这两个参考系之间的偏差。

物理意义：在姿态估计中，单位向量的叉积在物理意义上可以看作是轴角，轴角本身刻画了旋转，也就是叉乘的结果可以看成是旋转误差

有了偏差之后，我们可以对陀螺仪数据进行PI环节调节，以减小误差

$gyr_x=gyr_x+K_p*e_x+K_i*\int{e_x}\\ gyr_y=gyr_y+K_p*e_y+K_i*\int{e_y}\\ gyr_z=gyr_z+K_p*e_z+K_i*\int{e_z}$

并使用调节后的 $gyr_x\; gyr_y\; gyr_z$ 来更新 $Q$ 值。

至此，四元数Q已经获得。也就是说3个欧拉角也可以得到，姿态信息已得到。

位置求解

那么如何获取在全局坐标系中的位置？在全局坐标系中使用加速度计进行积分获得速度，在积分获得具体位置。

首先将加速度计的测得的加速度数据 $acc_x\; acc_y\;acc_z$ 从局部坐标系转化到全局坐标系，旋转矩阵为 $C_n^b$ 的形式

$\begin{bmatrix}0\\acc_x' \\acc_y'\\acc_z' \end{bmatrix} = C_n^b * \begin{bmatrix}0\\acc_x \\acc_y\\acc_z \end{bmatrix}$

全局坐标系中速度分量

$v_{x} = v_{x0} + \int acc_x'\\ v_{y} = v_{y0} + \int acc_y'\\ v_{z} = v_{z0} + \int acc_z'$

全局坐标系中位置分量

$x = x_0 + \int v_x\\ y = y_0 + \int v_y\\ z = z_0 + \int v_z$

2. 算法

欧拉角法

基本思想就是将两个坐标系的变换分解为绕三个不同的坐标轴的三次连续转动组成的序列。欧拉角的旋转规定为连续两次旋转，必须绕着不同的转动轴旋转，常见的旋转顺序有围绕2轴或3轴的各6种顺序，一共有12种旋转顺规，且不同的旋转顺序会产生不同的效果。

$XYX\;\;\; XZX\;\;\;YXY\;\;\;YZY\;\;\;ZXZ\;\;\;ZYZ\\ XYZ\;\;\;XZY\;\;\;YXZ\;\;\;YZX\;\;\;ZXY\;\;\;ZYX$

三维笛卡尔坐标系中，以环绕z轴的偏航角 $\psi$ 、环绕y轴的俯仰角 $\theta$ 、环绕x轴的滚转角 $\phi$ 来旋转

针对向量 $V={[V_x\;V_y\; V_z]}^T$

考虑绕Z轴旋转 $\psi$ ：得到新向量 $V^{\psi}={[V_x^{\psi}\;V_y^{\psi}\; V_z^{\psi}]}^T$

$V_z^{\psi} = V_z \\ {V_x^{\psi}}^2 + {V_y^{\psi}}^2 = {V_x}^2 + {V_x}^2 = {V_{xy}}^2\\ V_x = V_{xy}*\cos(\alpha) \;\;\;\;\;\;\;\;\;\;\;\;\;\; V_y = V_{xy}*\sin(\alpha) \\ V_x^{\psi} = V_{xy}*\cos(\alpha+\psi) \;\;\;\;\;\; V_y^{\psi} = V_{xy}*\sin(\alpha+\psi)$

得

$\begin{bmatrix}V_x^{\psi}\\ V_y^{\psi}\\V_z^{\psi} \end{bmatrix} = \begin{bmatrix}\cos(\psi) & -\sin(\psi)& 0\\ \sin(\psi)& \cos(\psi) & 0\\ 0 & 0 & 1 \end{bmatrix}* \begin{bmatrix}V_x\\ V_y\\V_z \end{bmatrix}$

$V^{\psi} = C^{\psi}*V$

同理，考虑绕Y轴旋转 $\theta$ ：得到新向量 $V^{\theta}={[V_x^{\theta}\;V_y^{\theta}\; V_z^{\theta}]}^T$

$\begin{bmatrix}V_x^{\theta}\\ V_y^{\theta}\\V_z^{\theta} \end{bmatrix} = \begin{bmatrix}\cos(\theta) & 0 & \sin(\theta)\\ 0 & 1 & 0 \\ -\sin(\theta)& 0 & \cos(\theta) \end{bmatrix}* \begin{bmatrix}V_x\\ V_y\\V_z \end{bmatrix}$

$V^{\theta} = C^{\theta}*V$

考虑绕X轴旋转 $\phi$ ：得到新向量 $V^{\phi}={[V_x^{\phi}\;V_y^{\phi}\; V_z^{\phi}]}^T$

$\begin{bmatrix}V_x^{\phi}\\ V_y^{\phi}\\V_z^{\phi} \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(\phi) & -\sin(\phi) \\ 0 & \sin(\phi) &\cos(\phi) \end{bmatrix}* \begin{bmatrix}V_x\\ V_y\\V_z \end{bmatrix}$

$V^{\phi} = C^{\phi}*V$

我们以 $ZYX$ 的旋转顺序为例，进行欧拉角法中两坐标系之间的转化

$V' = C_{\phi} C_{\theta} C_{\psi} V \\ =\begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(\phi) & -\sin(\phi) \\ 0 & \sin(\phi) &\cos(\phi) \end{bmatrix} \begin{bmatrix}\cos(\theta) & 0 & \sin(\theta)\\ 0 & 1 & 0 \\ -\sin(\theta)& 0 & \cos(\theta) \end{bmatrix} \begin{bmatrix}\cos(\psi) & -\sin(\psi)& 0\\ \sin(\psi)& \cos(\psi) & 0\\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix}V_x\\ V_y\\V_z \end{bmatrix} \\ = \begin{bmatrix}\cos(\theta) & 0 & \sin(\theta)\\ \sin(\phi)\sin(\theta) & \cos(\phi) & -\sin(\phi)\cos(\theta) \\ -\cos(\phi)\sin(\theta)& \sin(\phi) & \cos(\phi)\cos(\theta) \end{bmatrix} \begin{bmatrix}\cos(\psi) & -\sin(\psi)& 0\\ \sin(\psi)& \cos(\psi) & 0\\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix}V_x\\ V_y\\V_z \end{bmatrix} \\ =\begin{bmatrix} \cos(\theta)\cos(\psi) & - \cos(\theta)\sin(\psi) & \sin(\theta) \\ \sin(\phi)\sin(\theta)\cos(\psi)+\cos(\phi)\sin(\psi) & -\sin(\phi)\sin(\theta)\sin(\psi)+\cos(\phi)\cos(\psi)& -\sin(\phi)\cos(\theta) \\ -\cos(\phi)\sin(\theta)\cos(\psi) + \sin(\phi)\sin(\psi) & \cos(\phi)\sin(\theta)\sin(\psi) + \sin(\phi)\cos(\psi) & \cos(\phi)\cos(\theta) \end{bmatrix} \begin{bmatrix}V_x\\ V_y\\V_z \end{bmatrix}\\ =R_b^n * V$

可以得到转移矩阵 $R_b^n$ （与下述的四元数法中 $C_b^n$ 的右下角3*3矩阵内容相对应）

为什么欧拉角法需要绕3各轴旋转而不是绕两个轴旋转？

答：对于任意的向量V（除X轴上的向量），通过环绕X、Y轴的两次旋转，可以将V变化到三维空间中任意的一个新向量V'。如果V是X轴上的向量，环绕X轴的旋转无法起到效果，会导致一个自由度的丢失。为了规避这种情况需要进行三次旋转，但是仍然也会有旋转轴和向量重合的情况导致自由度的丢失，因此采用欧拉角法的三次旋转角度也有要求，要不等于90°，避免上边的旋转轴和向量重合的情况。

四元数法

由四个元组成的数

$\begin{aligned} &Q(q_0,q_1,q_2,q_3)=q_0 + q_1*\vec{i} + q_2*\vec{j} + q_3*\vec{k} \\ &P(p_0,p_1,p_2,p_3)=p_0 + p_1*\vec{i} + p_2*\vec{j} + p_3*\vec{k}\\ &R(r_0,r_1,r_2,r_3)=r_0 + r_1*\vec{i} + r_2*\vec{j} + r_3*\vec{k} \end{aligned}$

运算关系为：

$R(r_0,r_1,r_2,r_3) = Q(q_0,q_1,q_2,q_3) \otimes P(p_0,p_1,p_2,p_3) \\ = (q_0 + q_1*\vec{i} + q_2*\vec{j} + q_3*\vec{k}) \otimes (p_0 + p_1*\vec{i} + p_2*\vec{j} + p_3*\vec{k}) \\ =(q_0p_0 - q_1p_1 - q_2p_2 - q_3p_3) \\ + (q_0p_1+q_1p_0+q_2p_3-q_3p_2)*\vec{i} \\ + (q_0p_2-q_1p_3+q_2p_0+q_3p_1)*\vec{j} \\ + (q_0p_3+q_1p_2-q_2p_1+q_3p_0)*\vec{k}$

得到矩阵形式为

$\begin{bmatrix}r_0\\r_1\\r_2\\r_3 \end{bmatrix} = \begin{bmatrix}q_0p_0 - q_1p_1 - q_2p_2 - q_3p_3 \\q_0p_1+q_1p_0+q_2p_3-q_3p_2 \\q_0p_2-q_1p_3+q_2p_0+q_3p_1 \\q_0p_3+q_1p_2-q_2p_1+q_3p_0 \end{bmatrix} \\=\begin{bmatrix}q_0 &-q_1&-q_2&-q_3\\ q_1 &q_0 & -q_3 &q_2\\ q_2 &q_3 &q_0 &-q_1 \\q_3 &-q_2 & q_1 & q_0 \end{bmatrix} \begin{bmatrix}p_0\\ p_1\\ p_2\\ p_3 \end{bmatrix} =M(Q)*P\\ =\begin{bmatrix}p_1 &-p_1 &-p_2 &-p_3\\ p_1 &p_0 & p_3 &-p_2\\ p_2 &-p_3 & p_0 &p_1\\ p_3 & p_2 & -p_1 &p_0 \end{bmatrix} \begin{bmatrix}q_0\\ q_1\\q_2\\q_3 \end{bmatrix}=M'(P)*Q$

即有

$R=Q\otimes P = M(Q)*P = M'(P)*Q$

如何使用四元数法进行坐标变换？

假设存在坐标系n和坐标系b，使用四元数 $q_b^n=q_0+q_1\vec{i} + q_2\vec{j} + q_3\vec{k}$ 和共轭四元数 $(q_b^n)^*=q_0-q_1\vec{i} - q_2\vec{j} - q_3\vec{k}$ 来表示从坐标系b到坐标系n的旋转，那么通过四元数旋转变换得到的向量 $r_n={\begin{bmatrix}0 &r_x^n &r_y^n &r_z^n \end{bmatrix}}^T$ 将是 $r_b={\begin{bmatrix}0 &r_x^b &r_y^b &r_z^b \end{bmatrix}}^T$ 在坐标系n中的表示。

$r_n=q_b^n * \otimes r_b \otimes {q_b^n}^* = q_b^n \otimes (r_b \otimes {q_b^n}^*) \\= q_b^n \otimes (M'({q_b^n}^*)*r_b )= M(q_b^n)*(M'({q_b^n}^*)*r_b )\\ = \begin{bmatrix}q_0 &-q_1&-q_2&-q_3\\ q_1 &q_0 & -q_3 &q_2\\ q_2 &q_3 &q_0 &-q_1 \\q_3 &-q_2 & q_1 & q_0 \end{bmatrix} * \begin{bmatrix}q_0 &q_1&q_2&q_3\\ -q_1 &q_0 & -q_3 &q_2\\ -q_2 &q_3 &q_0 &-q_1 \\-q_3 &-q_2 & q_1 & q_0 \end{bmatrix} * r_b \\ =C_b^n * r_b$

其中定义的四元数旋转矩阵为坐标系b到坐标系n的旋转矩阵 $C_b^n$ 为

$C_b^n=\begin{bmatrix}q_0^2+q_1^2+q_2^2+q_3^2 &0&0&0\\ 0 &q_0^2+q_1^2-q_2^2-q_3^2 & 2q_1q_2-2q_0q_3 &2q_1q_3+2q_0q_2\\ 0 &2q_0q_3+2q_1q_2 &q_0^2-q_1^2+q_2^2-q_3^2 &2q_2q_3-2q_0q_1 \\0 &2q_1q_3-2q_0q_2 & 2q_0q_1+2q_2q_3 & q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix}$

从坐标系n到坐标系b的旋转，四元数 $q_n^b=q_0-q_1\vec{i} - q_2\vec{j} - q_3\vec{k}$ （假设 $q_n^b$ 和 $q_b^n$ 是单位四元数， $q_0^2+q_1^2+q_2^2+q_3^2 = 1$ ，则 $q_n^b$ 和 $q_b^n$ 是互逆的）。

$r_b=q_n^b * \otimes r_n \otimes {q_n^b}^* \\= q_n^b \otimes (M'({q_n^b}^*)*r_n )= M(q_n^b)*(M'({q_n^b}^*)*r_n )\\ = \begin{bmatrix}q_0 &q_1&q_2&q_3\\ -q_1 &q_0 & q_3 &-q_2\\ -q_2 &-q_3 &q_0 &q_1 \\-q_3 &q_2 & -q_1 & q_0 \end{bmatrix} * \begin{bmatrix}q_0 &-q_1&-q_2&-q_3\\ q_1 &q_0 & q_3 &-q_2\\ q_2 &-q_3 &q_0 &q_1 \\q_3 &q_2 & -q_1 & q_0 \end{bmatrix} * r_n \\ =C_n^b * r_n$

其中定义的四元数旋转矩阵为坐标系b到坐标系n的旋转矩阵 $C_b^n$ 为

$C_b^n=\begin{bmatrix}q_0^2+q_1^2+q_2^2+q_3^2 &0&0&0\\ 0 &q_0^2+q_1^2-q_2^2-q_3^2 & 2q_1q_2+2q_0q_3 &-2q_0q_3+2q_1q_2\\ 0 &-2q_0q_3+2q_1q_2 &q_0^2-q_1^2+q_2^2-q_3^2 &2q_2q_3+2q_0q_1 \\0 &2q_1q_3+2q_0q_2 & -2q_0q_1+2q_2q_3 & q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix}$

最终有转换矩阵 $C_n^b$ 和 $C_b^n$ 互逆

$C_n^b = {(C_b^n)}^{-1}$

欧拉角法和四元数法联系

旋转矩阵等价

$R_b^n = {C_b^n}_{[2:4, 2:4]}$

$\begin{aligned} \begin{bmatrix} \cos(\theta)\cos(\psi) & - \cos(\theta)\sin(\psi) & \sin(\theta) \\ \sin(\phi)\sin(\theta)\cos(\psi)+\cos(\phi)\sin(\psi) & -\sin(\phi)\sin(\theta)\sin(\psi)+\cos(\phi)\cos(\psi)& -\sin(\phi)\cos(\theta) \\ -\cos(\phi)\sin(\theta)\cos(\psi) + \sin(\phi)\sin(\psi) & \cos(\phi)\sin(\theta)\sin(\psi) + \sin(\phi)\cos(\psi) & \cos(\phi)\cos(\theta) \end{bmatrix}\\ = \begin{bmatrix}q_0^2+q_1^2-q_2^2-q_3^2 & 2q_1q_2-2q_0q_3 &2q_0q_3+2q_1q_2\\2q_0q_3+2q_1q_2 &q_0^2-q_1^2+q_2^2-q_3^2 &2q_2q_3-2q_0q_1 \\2q_1q_3-2q_0q_2 & 2q_0q_1+2q_2q_3 & q_0^2-q_2^2-q_2^2+q_3^2 \end{bmatrix} \end{aligned}$

则有欧拉角 $\psi\; \theta\;\phi$ 和四元数 $q_0\;q_1\;q_2\;q_3$ 的转换关系为

$\psi =\arctan(-\frac{2q_1q_2-2q_0q_3}{q_0^2+q_1^2-q_2^2-q_3^2}) \\ \theta = \arcsin(2q_0q_3+2q_1q_2)\\ \phi =\arctan(-\frac{2q_2 q_3 - 2q_0q_1}{q_0^2-q_1^2-q_2^2+q_3^2})$