浅谈基于卡尔曼滤波器的姿态解算（一）

争取35岁退休

已于 2024-04-24 21:44:28 修改

阅读量2.7k

点赞数 29

分类专栏：数据融合和控制算法文章标签：算法线性代数矩阵概率论

于 2024-04-21 22:31:47 首次发布

本文链接：https://blog.csdn.net/m0_37835056/article/details/137972765

版权

数据融合和控制算法专栏收录该内容

16 篇文章

订阅专栏

传送门

前言
卡尔曼滤波器
基于卡尔曼滤波器的姿态解算
总结

前言

通常，在实践工程中，对于机器人姿态解算的算法有很多，如Mahony互补滤波算法、卡尔曼滤波算法。并且，这些算法都是基于IMU惯性传感器的数据完成的。即根据IMU数据中的三轴加速度计和三轴角速度（基于机体坐标系），根据坐标系转换关系来实现姿态解算。（默认读者均了解IMU的工作原理，减少相关赘述。）

卡尔曼滤波器

卡尔曼滤波器作为最优预测的数据融合算法，能够将加速度计和陀螺仪数据更好的结合起来，得到理想姿态数据。
卡尔曼滤波器之前做过一篇推导的博客，链接卡尔曼滤波器
这里不做重复赘述，直接给出卡尔曼滤波器的结论：
对于线性状态系统
$\dot{\bold{X}}=A\bold{X}+B\bold{U}\\ \bold{Y}=C\bold{X}$
Step1：用上一次的后验误差协方差矩阵计算先验误差协方差矩阵
${\color{red} P_{k+1}^{-}=A P_{k} A^{T}+Q}$
Step2：先验估计
${\color{red} \hat{x}_{k+1}^{-}=A\hat{x}_{k}+Bu_{k}}$
Step3：计算卡尔曼增益
${\color{red} K_{k+1}=\frac{P_{k+1}^{-}H^{T}}{HP_{k+1}^{-}H^{T}+R}}$
Step4：后验估计
${\color{red} \hat{x}_{k+1}=\hat{x}_{k+1}^{-}+K_{k+1}(z_{k+1}-H\hat{x}_{k+1}^{-})}$
Step5：计算后验误差协方差矩阵
${\color{red} P_{k+1}=(I-K_{k+1}H)P_{k+1}^{-}}$

基于卡尔曼滤波器的姿态解算

卡尔曼滤波器使用方法

对于卡尔曼滤波器的使用，无论是否对卡尔曼滤波器熟悉掌握，通常只需要明确以下两个点，再套公式就可以完成整个数据融合的过程。

状态转移关系（先验估计），明确状态转移方程相关矩阵 $A, B$ 以及系统噪声 $Q$
状态测量值（通过传感器观测得到），明确观测值 $\boldsymbol{z_{k}}$ 和测量噪声 $R$
在给定初始的后验协方差矩阵 $P_{0}$ 后，经过一定时间的迭代，卡尔曼滤波器即可收敛到最优区间，实现数据融合和预测作用。

IMU数据解析

相关的公式在之前的博客中都做过推导，这里直接给结论

首先，定义惯性系和机体系的坐标变换矩阵，由惯性系到机体系的坐标系转换矩阵为 $\boldsymbol{R}$ （这里是重要的前提）
$\boldsymbol{R}=\boldsymbol{R_{X}(\phi)}\boldsymbol{R_{Y}(\theta)}\boldsymbol{R_{Z}(\psi)}=\left[\begin{array}{ccc} C_{\psi} C_{\theta} & S_{\psi} C_{\theta} &-S_{\theta} \\ C_{\psi} S_{\theta} S_{\phi}-S_{\psi} C_{\phi} & S_{\psi} S_{\theta} S_{\phi}+C_{\psi} C_{\phi} & C_{\theta} S_{\phi} \\ C_{\psi} S_{\theta} C_{\phi}+S_{\psi} S_{\phi} &S_{\psi} S_{\theta} C_{\phi}-C_{\psi} S_{\phi}& C_{\theta} C_{\phi} \end{array}\right]$
接着，对于机体系下角速度向量 $\begin{bmatrix}p\\q\\r\end{bmatrix}$ ，对于惯性系下欧拉角向量 $\begin{bmatrix}\boldsymbol{\phi}\\\boldsymbol{\theta}\\\boldsymbol{\psi}\end{bmatrix}$ ，满足以下关系（这里是通过陀螺仪所能得到的欧拉角变化趋势（欧拉角导数））
$\begin{array}{ll} {\left[\begin{array}{c} \dot{\boldsymbol{\phi}} \\ \dot{\boldsymbol{\theta}} \\ \dot{\boldsymbol{\psi}} \end{array}\right]=\left[\begin{array}{ccc} 1 & S_{\phi} T_{\theta} & C_{\phi} T_{\theta} \\ 0 & C_{\phi} & -S_{\phi} \\ 0 & S_{\phi} / C_{\theta} & C_{\phi} / C_{\theta} \end{array}\right]\left[\begin{array}{c} p \\ q \\ r \end{array}\right]} \end{array}$
最后，对于加速度数据而言，在静止状态或匀速运动状态下，加速度计采样得到的三轴数据为重力加速度在机体坐标系中的表示。
对于惯性系下重力加速度向量为 $\begin{bmatrix}0\\0\\-g\end{bmatrix}$ ，在机体系下为 $\begin{bmatrix}a_{x}\\a_{y}\\a_{z}\end{bmatrix}$ 。
根据坐标系转换的几何关系，可以得到以下关系
$\boldsymbol{\phi}=atan(a_{y}/a_{z})\\ \boldsymbol{\theta}=-atan(a_{x}/\sqrt{a_{y}^{2}+a_{z}^{2}})$
这里的几何关系结论是按照从Z->Y->X的顺序进行旋转变换得到的，可以自己动手绘制一下按照这个顺序进行旋转变换的坐标系，帮助了解。

卡尔曼滤波器模型

先假设初始的欧拉角为 $\begin{bmatrix}\boldsymbol{\phi}\\\boldsymbol{\theta}\\\boldsymbol{\psi}\end{bmatrix}=\begin{bmatrix}0\\0\\0\end{bmatrix}$ ，因为一开始的数据可能存在非常大的偏差，因此初始的后验协方差矩阵可以给的大一点，根据实际情况调整。

$s t e p 1$ ：先验估计
先读取到陀螺仪的角速度数据 $\begin{bmatrix}p\\q\\r\end{bmatrix}$ ，转换成单位 $r a d / s$ ，代入以下公式得到欧拉角的导数 $\begin{bmatrix}\dot{\boldsymbol{\phi}} \\ \dot{\boldsymbol{\theta}} \\ \dot{\boldsymbol{\psi}}\end{bmatrix}$ ：
$\begin{array}{ll} {\left[\begin{array}{c} \dot{\boldsymbol{\phi}} \\ \dot{\boldsymbol{\theta}} \\ \dot{\boldsymbol{\psi}} \end{array}\right]=\left[\begin{array}{ccc} 1 & S_{\phi} T_{\theta} & C_{\phi} T_{\theta} \\ 0 & C_{\phi} & -S_{\phi} \\ 0 & S_{\phi} / C_{\theta} & C_{\phi} / C_{\theta} \end{array}\right]\left[\begin{array}{c} p \\ q \\ r \end{array}\right]} \end{array} \tag{1}$
将欧拉角的导数 $\begin{bmatrix}\dot{\boldsymbol{\phi}} \\ \dot{\boldsymbol{\theta}} \\ \dot{\boldsymbol{\psi}}\end{bmatrix}$ 作为系统状态转移方程的输入向量 $\bold{U}$ ，即
$\bold{U}=\begin{bmatrix}\dot{\boldsymbol{\phi}} \\ \dot{\boldsymbol{\theta}} \\ \dot{\boldsymbol{\psi}}\end{bmatrix} \tag{2}$
设离散周期为 $\boldsymbol{T}$ ，最终可以得到离散的系统状态转移方程（实际使用均是离散系统）：
$\bold{X_{k+1}}=A\bold{X_{k}}+B\bold{U_{k}}$
展开后得到
$\begin{bmatrix}\boldsymbol{\phi_{k+1}} \\ \boldsymbol{\theta_{k+1}} \\ \boldsymbol{\psi_{k+1}}\end{bmatrix} \ =\begin{bmatrix}1 & 0&0 \\ 0&1&0 \\ 0&0&1\end{bmatrix}\ \begin{bmatrix}\boldsymbol{\phi_{k}} \\ \boldsymbol{\theta_{k}} \\ \boldsymbol{\psi_{k}}\end{bmatrix}\ +\begin{bmatrix}T & 0&0 \\ 0&T&0 \\ 0&0&T\end{bmatrix}\ \begin{bmatrix}\dot{\boldsymbol{\phi_{k}}} \\ \dot{\boldsymbol{\theta_{k}}} \\ \dot{\boldsymbol{\psi_{k}}}\end{bmatrix} \tag{3}$
其中，矩阵 $A$ 是三阶单位矩阵，矩阵 $B$ 是三阶单位矩阵乘以 $T$ ，即
$A=\begin{bmatrix}1 & 0&0 \\ 0&1&0 \\ 0&0&1\end{bmatrix}，B=\begin{bmatrix}T & 0&0 \\ 0&T&0 \\ 0&0&T\end{bmatrix} \tag{4}$
最终，得到了先验估计计算公式：
$\begin{bmatrix}\boldsymbol{\hat{\phi}_{k+1}^{-}} \\ \boldsymbol{\hat{\theta}_{k+1}^{-}} \\ \boldsymbol{\hat{\psi}_{k+1}^{-}}\end{bmatrix} \ =\begin{bmatrix}1 & 0&0 \\ 0&1&0 \\ 0&0&1\end{bmatrix}\ \begin{bmatrix}\boldsymbol{\hat{\phi}_{k}} \\ \boldsymbol{\hat{\theta}_{k}} \\ \boldsymbol{\hat{\psi}_{k}}\end{bmatrix}\ +\begin{bmatrix}T & 0&0 \\ 0&T&0 \\ 0&0&T\end{bmatrix}\ \begin{bmatrix}\dot{\boldsymbol{\phi_{k}}} \\ \dot{\boldsymbol{\theta_{k}}} \\ \dot{\boldsymbol{\psi_{k}}}\end{bmatrix} \tag{5}$
再根据卡尔曼滤波器的公式结论计算先验估计协方差矩阵：
$P_{k+1}^{-}=A P_{k} A^{T}+Q \tag{6}$
$s t e p 2$ ：状态测量值
对于状态测量值与系统状态，满足以下关系：
$z_{k}=H\bold{X}_{mk}$
通过加速度计数据转换得到的姿态角向量 $z_{k}$ 为
$z_{k}=\begin{bmatrix}\boldsymbol{\phi_{mk}} \\ \boldsymbol{\theta_{mk}} \\ \boldsymbol{\psi_{mk}}\end{bmatrix}\$
通过读取到的三轴加速度计数据 $\begin{bmatrix}a_{x}\\a_{y}\\a_{z}\end{bmatrix}$ ，可以计算得到以下信息：
$\boldsymbol{\phi_{mk}}=atan(a_{y}/a_{z})\\ \boldsymbol{\theta_{mk}}=-atan(a_{x}/\sqrt{a_{y}^{2}+a_{z}^{2}}) \tag{7}$
重力加速度在加速度计的数据无法解算得到航向角！
最终得到测量转移矩阵 $H$ 为：
$H=\begin{bmatrix}1 & 0&0 \\ 0&1&0 \\ 0&0&0\end{bmatrix}\$
$s t e p 3$ ：后验估计
根据前两步的推导，可以得到系统状态的先验估计（公式5）和先验协方差矩阵（公式6）。
接下来需要计算卡尔曼增益：
$K_{k+1}=\frac{P_{k+1}^{-}H^{T}}{HP_{k+1}^{-}H^{T}+R} \tag{8}$
计算后验估计：
$\hat{x}_{k+1}=\hat{x}_{k+1}^{-}+K_{k+1}(z_{k+1}-H\hat{x}_{k+1}^{-}) \tag{9}$
以及计算后验估计协方差矩阵：
$P_{k+1}=(I-K_{k+1}H)P_{k+1}^{-} \tag{10}$

总结

使用IMU进行基于卡尔曼滤波器的姿态解算，整个过程可以概括为：

读取IMU三轴角速度数据和三轴加速度数据；
通过公式1计算得到欧拉角导数；
通过公式5计算先验估计，并且通过公式6计算先验估计协方差；
通过公式7计算得到系统状态测量值，再通过公式8计算卡尔曼增益，将二者代入公式9计算后验估计，最终通过公式10计算出后验估计的协方差，完成整个卡尔曼滤波器的计算周期。

下一篇博客将使用实际IMU传感器完成基于卡尔曼滤波器的姿态解算，完成后将实现源码共享
感谢阅读！