mpu6050姿态解算与卡尔曼滤波（4）姿态解算

qiguizhe

已于 2024-04-29 22:17:31 修改

阅读量2.1k

点赞数 7

CC 4.0 BY-SA版权

分类专栏：姿态解算卡尔曼滤波文章标签： mpu6050 姿态解算卡尔曼滤波

于 2024-04-17 22:53:59 首次发布

本文链接：https://blog.csdn.net/qiguizhe/article/details/137890330

姿态解算同时被 2 个专栏收录

5 篇文章

订阅专栏

卡尔曼滤波

3 篇文章

订阅专栏

一、状态方程
状态变量选取为姿态四元数 $X=[q_0,q_1,q_2,q_3]^T$ 。则根据四元数微分方程
$[q˙0q˙1q˙2q˙3]=12[0−ωnbxb−ωnbyb−ωnbzbωnbxb0ωnbzb−ωnbybωnbyb−ωnbzb0ωnbxbωnbzbωnbyb−ωnbxb0][q0q1q2q3]\begin{bmatrix}\dot q_0\\\dot q_1\\\dot q_2\\\dot q_3\end{bmatrix}=\frac{1}{2} \begin{bmatrix} 0&-\omega_{nbx}^b&-\omega_{nby}^b&-\omega_{nbz}^b\\ \omega_{nbx}^b&0&\omega_{nbz}^b&-\omega_{nby}^b\\ \omega_{nby}^b&-\omega_{nbz}^b&0&\omega_{nbx}^b\\ \omega_{nbz}^b&\omega_{nby}^b&-\omega_{nbx}^b&0\\ \end{bmatrix}\begin{bmatrix}q_0\\q_1\\q_2\\q_3\end{bmatrix}$
取一阶近似
$[q0q1q2q3]k=(I+Δt2[0−ωnbxb−ωnbyb−ωnbzbωnbxb0ωnbzb−ωnbybωnbyb−ωnbzb0ωnbxbωnbzbωnbyb−ωnbxb0]k−1)[q0q1q2q3]k−1\begin{bmatrix} q_0\\ q_1\\ q_2\\ q_3\end{bmatrix}_{k}= \left( I+\frac{\Delta t}{2} \begin{bmatrix} 0&-\omega_{nbx}^b&-\omega_{nby}^b&-\omega_{nbz}^b\\ \omega_{nbx}^b&0&\omega_{nbz}^b&-\omega_{nby}^b\\ \omega_{nby}^b&-\omega_{nbz}^b&0&\omega_{nbx}^b\\ \omega_{nbz}^b&\omega_{nby}^b&-\omega_{nbx}^b&0\\ \end{bmatrix}_{k-1} \right)\begin{bmatrix}q_0\\q_1\\q_2\\q_3\end{bmatrix}_{k-1}$
所以状态转移矩阵为
$Φk/k−1=I+Δt2[0−ωnbxb−ωnbyb−ωnbzbωnbxb0ωnbzb−ωnbybωnbyb−ωnbzb0ωnbxbωnbzbωnbyb−ωnbxb0]k−1\Phi_{k/k-1}=I+\frac{\Delta t}{2} \begin{bmatrix} 0&-\omega_{nbx}^b&-\omega_{nby}^b&-\omega_{nbz}^b\\ \omega_{nbx}^b&0&\omega_{nbz}^b&-\omega_{nby}^b\\ \omega_{nby}^b&-\omega_{nbz}^b&0&\omega_{nbx}^b\\ \omega_{nbz}^b&\omega_{nby}^b&-\omega_{nbx}^b&0\\ \end{bmatrix}_{k-1}$
对于系统噪声 $W_{k-1}$ 可以简单考虑为系统噪声直接作用在状态 $X$ 上，也即
$[q0q1q2q3]k=(I+Δt2[0−ωnbxb−ωnbyb−ωnbzbωnbxb0ωnbzb−ωnbybωnbyb−ωnbzb0ωnbxbωnbzbωnbyb−ωnbxb0]k−1)[q0q1q2q3]k−1+Wk−1\begin{bmatrix} q_0\\ q_1\\ q_2\\ q_3\end{bmatrix}_{k}= \left( I+\frac{\Delta t}{2} \begin{bmatrix} 0&-\omega_{nbx}^b&-\omega_{nby}^b&-\omega_{nbz}^b\\ \omega_{nbx}^b&0&\omega_{nbz}^b&-\omega_{nby}^b\\ \omega_{nby}^b&-\omega_{nbz}^b&0&\omega_{nbx}^b\\ \omega_{nbz}^b&\omega_{nby}^b&-\omega_{nbx}^b&0\\ \end{bmatrix}_{k-1} \right)\begin{bmatrix}q_0\\q_1\\q_2\\q_3\end{bmatrix}_{k-1}+W_{k-1}$
事实上角速度 $ωnbb\omega_{nb}^b$ 的误差 $Δω\Delta\omega$ 构成了系统噪声，有兴趣可以自行推导其噪声输入矩阵 $Γk−1\Gamma_{k-1}$ 。
二、测量方程
选取测量值为mpu6050测量得到的加速度 $Z_k=a$ ，在mpu6050自身无运动加速度的情况下其测量值
$g]Ta=C_n^b*\begin{bmatrix}0\ 0\ g \end{bmatrix}^T$
根据四元数与姿态矩阵的转换，得到
$[axayaz]=g[2(q1q3−q0q2)2(q2q3+q0q1)q02−q12−q22+q32]\begin{bmatrix} a_x\\a_y\\a_z\end{bmatrix}=g\begin{bmatrix} 2(q_1q_3-q_0q_2)\\2(q_2q_3+q_0q_1)\\q_0^2-q_1^2-q_2^2+q_3^2\end{bmatrix}$
线性化和离散化
$[axayaz]k=g[2(q1q3−q0q2)2(q2q3+q0q1)q02−q12−q22+q32]k−1+g∗[−2q22q3−2q02q12q12q02q32q22q0−2q1−2q22q3](Xk−X^k−1)+Vk\begin{bmatrix} a_x\\a_y\\a_z\end{bmatrix}_{k}=g\begin{bmatrix} 2(q_1q_3-q_0q_2)\\2(q_2q_3+q_0q_1)\\q_0^2-q_1^2-q_2^2+q_3^2\end{bmatrix}_{k-1}+g*\begin{bmatrix} -2q_2&2q_3&-2q_0&2q_1\\2q_1&2q_0&2q_3&2q_2\\2q_0&-2q_1&-2q_2&2q_3\end{bmatrix}\left( X_k-\hat X_{k-1}\right)+V_k$
到这里我们就获得了线性化的状态方程和测量方程了，按照博客(2)http://t.csdnimg.cn/EYpJ7里标准卡尔曼滤波的公式即可进行滤波，即可获得融合陀螺和加速度计的姿态四元数了。
三、 $X$ 和 $P$ 的初始化
通常情况下，卡尔曼滤波器是收敛的，因此滤波器的初值 $X$ 和 $P$ 可以设置的比较随意。但是对于本文设计的这个滤波器，测量值 $Z_k$ 只包含姿态的一部分信息，因此滤波器状态 $X$ 中必定有发散的部分。简单假设当 $X≈[1q1q2q3]TX\approx\begin{bmatrix} 1&q_1&q_2&q_3\end{bmatrix}^T$ 时， $q_1$ 、 $q_2$ 即对应俯仰和滚转角，而 $q_3$ 对应方位角，在滤波过程中 $q_3$ 是发散的。尽管滤波器估计出的姿态中方位角是发散的，我们仍然可以取初始化时的方位角为0，这样滤波器估计出的方位角就是相对初始时的方位，在短时间内它仍是可信赖的信息。
对于 $X$ 的初始化可以参考严恭敏老师的《捷联惯导算法与组合导航原理》7.5.4节。以下内容均摘抄自该部分。
记姿态阵 $C_b^n$ 的三个行向量为 $C_1$ ， $C_2$ ， $C_3$ ，即
$Cbn=[C1C2C3]C_b^n=\begin{bmatrix} C_1\\C_2\\C_3\end{bmatrix}$
根据加速度测量值 $a=[axayaz]Ta=\begin{bmatrix} a_x&a_y&a_z\end{bmatrix}^T$ 构造 $C_b^n$ 的一种简便方法如下：
（1）构造 $C3=[C31C32C33]=[axayaz]/∣a∣C_3= \begin{bmatrix} C_{31}&C_{32}&C_{33}\end{bmatrix}= \begin{bmatrix} a_x&a_y&a_z\end{bmatrix}/|a|$ ;
（2）如果 $∣C31∣>0.5\left| C_{31}\right|>0.5$ ，则构造 $C2′=[C32−C310]C_2'=\begin{bmatrix} C_{32}&-C_{31}&0\end{bmatrix}$ ，否则构造 $C2′=[0C33−C32]C_2'=\begin{bmatrix} 0&C_{33}&-C_{32}\end{bmatrix}$ ，归一化得到 $C2=C2′/∣C2′∣C_2=C_2'/\left| C_2'\right|$ 。
（3）构造 $C_1=C_2×C_3$ 。
这样就可以得到初始姿态矩阵 $C_b^n$ ，然后可以得到初始姿态四元数 $Q$ ，作为滤波器状态 $X$ 的初值。
对于 $P$ 阵，由于初始方位角被设定为0，它是我们极其确定的一个值，因此 $P_{44}$ 的初值应当被设置为很小，一般的可以取 $< 1 E - 7$ ， $P_{11}$ 、 $P_{22}$ 、 $P_{33}$ 的初值可以设置为 $1 E - 2$ ，在滤波过程中会收敛。
四、 $Q$ 和 $R$ 的设置
对于卡尔曼滤波器，系统噪声 $Q$ 和测量噪声 $R$ 至关重要。
首先考虑 $R$ ，由于测量量选取的就是加速度计测量的加速度，因此 $R$ 就是加速度测量误差 $Δa\Delta a$ 的协方差。考虑三轴加速度计为互相独立的测量，因此 $R$ 为对角矩阵。对于mpu6050这样的低精度imu，可以直接取静态情况下各轴加速度计测量值的3倍标准差的平方作为 $R$ 的对角线元素。
对于 $Q$ 阵，陀螺的测量误差构成了系统噪声。同样取静态情况下陀螺测量值的3倍标准差的平方作为陀螺测量的方差阵，再乘上滤波周期 $Δt\Delta t$ 的平方，即可作为 $Q$ 的对角线元素。然后观察滤波器的运行情况调整 $Q$ 。调整原则是如果想使滤波器更多的信赖陀螺测量则减小 $Q$ ，如果想使滤波器更多的信赖加表测量则增大 $Q$ 。