三轴加速度计解算姿态（四元数）

Hugh_W

已于 2023-03-30 20:59:47 修改

阅读量2.5k

点赞数

分类专栏：姿态解算文章标签：线性代数

于 2023-03-30 20:50:19 首次发布

本文链接：https://blog.csdn.net/Hugh_W/article/details/129863480

版权

姿态解算专栏收录该内容

3 篇文章 1 订阅

订阅专栏

原理

当传感器载体静止时，加速度计只会输出重力加速度，可以凭此来计算载体的俯仰角和滚转角。

方法

假设导航坐标系为东北天，载体坐标系为右前上。

初始载体坐标系和导航坐标系重合，对应的四元数为q=[1,0,0,0]，使用此四元数表示载体在导航坐标系下的旋转。

先将四元数转为旋转矩阵
$C=\begin{bmatrix} 1-2y^{2}-2z^{2} & 2xy-2wz & 2wy+2xz \\ 2xy+2wz & 1-2x^{2}-2z^{2} & 2yz-2wx \\ 2xz-2wy & 2wx+2yz & 1-2x^{2}-2y^{2} \end{bmatrix}$
可以使用此旋转矩阵对载体在导航坐标系下进行旋转，旋转后的载体坐标系和导航坐标系，对于一相同向量有如下关系，推导参考3维旋转矩阵推导与助记。

$\begin{bmatrix} x^{'} \\ y^{'} \\ z^{'} \end{bmatrix} =C^{-1}\times \begin{bmatrix} x \\ y \\ z \end{bmatrix}$

可以使用此公式将导航坐标系中的向量映射到载体坐标系。 $C^{-1}$ 为旋转矩阵的逆矩阵，同样也是转置矩阵 $C^{T}$ 。

加速度计测量的重力向量为

$\begin{bmatrix} a_{xb}\\ a_{yb}\\ a_{zb} \end{bmatrix}$

需要将其转换到导航坐标系下，根据前面的旋转坐标系的公式可以得到

$\begin{bmatrix} a_{xb} \\ a_{yb} \\ a_{zb} \end{bmatrix} =C^{-1}\times \begin{bmatrix} a_{xn} \\ a_{yn} \\ a_{zn} \end{bmatrix}$

则

$\begin{bmatrix} a_{xn} \\ a_{yn} \\ a_{zn} \end{bmatrix} =C\times \begin{bmatrix} a_{xb} \\ a_{yb} \\ a_{zb} \end{bmatrix}$

其中 $a_{xb},a_{yb},a_{zb})$ 为加速度计在载体坐标系下测量的加速度向量， $a_{xn},a_{yn},a_{zn})$ 为转换到导航坐标系下的加速度向量。

现在，需要将测量的加速度向量在导航坐标系下的映射旋转到标准重力的方向即

$\begin{bmatrix} 0\\ 0\\ g \end{bmatrix}$

将这个旋转应用到载体上即为载体当前的姿态。

设 $\overrightarrow{v_{1}} =\begin{bmatrix} ax_{n} \\ ay_{n} \\ az_{n} \end{bmatrix}$ ， $\overrightarrow{v_{2}}=\begin{bmatrix} 0\\ 0\\ g \end{bmatrix}$ ， $\overrightarrow{v_{1}}$ 和 $\overrightarrow{v_{2}}$ 之间的夹角为 $\theta$

则旋转轴 $\overrightarrow{v}=\overrightarrow{v_{1}} \times \overrightarrow{v_{2}}$ ，然后绕此轴旋转 $\theta$ 。

根据 $\overrightarrow{v_{1}} \cdot \overrightarrow{v_{2}}=\left | \overrightarrow{v_{1}} \right | \left | \overrightarrow{v_{2}} \right | \cdot \cos(\theta)$ ，将 $\overrightarrow{v_{1}}$ 和 $\overrightarrow{v_{2}}$ 归一化，则 $\theta=\arccos(\overrightarrow{v_{1}} \cdot \overrightarrow{v_{2}} )$

根据四元数公式
$q^{'}=\begin{bmatrix} \cos(\frac{\theta}{2} ) & \overrightarrow{v} \times \sin(\frac{\theta}{2}) \end{bmatrix}$
可以得到此旋转对应的四元数，其中 $\overrightarrow{v}$ 需要归一化处理。

将初始四元数 $q$ 左乘 $q^{'}$ 就能得到载体当前姿态对应的四元数。

以下是使用Eigen3的C++示例

#include "Eigen/Core"
#include "Eigen/Geometry"
#include <cmath>

// 加速度计读取的数据，三个坐标轴，单位mg
float acc[3];

// 初始四元数
Eigen::Quaternionf quaternion = Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f);

while(1) {
	// 读取加速度计数据
	read_data(&acc);

	// 加速度计测量的加速度向量，单位转为g
	Eigen::Vector3f acceleration = Eigen::Vector3f(acc[0] / 1000.0f, acc[1] / 1000.0f, acc[2] / 1000.0f).normalized();
	
	// 导航坐标系下的标准重力向量
	Eigen::Vector3f gravity = Eigen::Vector3f(0.0f, 0.0f, 1.0f);
	
	// 测量的加速度从载体系转换到导航系
	acceleration = (quaternion.toRotationMatrix() * acceleration).normalized();
	
	// 计算旋转轴
	Eigen::Vector3f rotate_vector = acceleration.cross(gravity);
	
	// 计算旋转角度
	float theta = acos(acceleration.dot(gravity));
	
	// 得到旋转对应的四元数
	Eigen::Vector3f v = rotate_vector.normalized() * std::sin(theta / 2.0f);
	Eigen::Quaternionf q = Eigen::Quaternionf(std::cos(theta / 2.0f), v.x(), v.y(), v.z()).normalized();
	
	// 应用旋转
	quaternion = (q * quaternion).normalized();
}