61 lines
2.2 KiB
Markdown
61 lines
2.2 KiB
Markdown
|
# **Mahony姿态解算**
|
|||
|
|
|
|||
|
|
**2022-09-27 ：詹力**
|
|||
|
|
|
|||
|
|
## 一、从IMU到Mahony的坐标系
|
|||
|
|
|
|||
|
|
实际上，Mahony是没有明确的坐标系的概念的，不管是什么传感器的坐标轴输入到Mahony，解算出来的四元数的qx,qy,qz就和原始输入Mahony的IMU的轴是对应的。但是，这样做的会造成四元数到欧拉角的计算出现混乱麻烦。为了代码方便，我们最好是先 **将IMU的坐标系映射到笛卡尔3D坐标系** 。以MPU9250为例子，其坐标轴定义如下图所示(加速度计和陀螺仪，MPU9250内置磁力计坐标系不同)。
|
|||
|
|
|
|||
|
|
<div align=center><img src="./Image/MPU9250_Cord.png" alt="MPU9250_Cord" style="zoom:50%;" /></div>
|
|||
|
|
|
|||
|
|
笛卡尔3D坐标系如下图所示：
|
|||
|
|
|
|||
|
|
<img src="./Image/CartesianCoordinates.png" alt="CartesianCoordinates" style="zoom: 33%;" />
|
|||
|
|
|
|||
|
|
将Mahony算法确定以笛卡尔3D坐标系方式描述，那么对应的转换代码如下所示：
|
|||
|
|
|
|||
|
|
```C++
|
|||
|
|
Mahony.x = -Imu.y;
|
|||
|
|
Mahony.y = Imu.x;
|
|||
|
|
Mahony.z = Imu.z;
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
## 二、从四元数到欧拉角的转换
|
|||
|
|
|
|||
|
|
四元数到欧拉角的转换是和坐标系的定义存在关系的，在不同的坐标系下需要推导四元数到欧拉角的转换公式。这样非常的麻烦，除非在必要的情况下，一般用标准的笛卡尔3D坐标计算会比较方便。
|
|||
|
|
|
|||
|
|
笛卡尔3D坐标系下的四元数转换到欧拉角代码，如下所示：
|
|||
|
|
|
|||
|
|
```c++
|
|||
|
|
Mahony.Pitch = asin(-2.0f*(z*x-w*y))* (180.0f /PI);
|
|||
|
|
Mahony.Yaw = atan2(y*x + w*z,0.5f - y*y - z*z)* (180.0f /PI);
|
|||
|
|
Mahony.Roll = atan2(y*z + w*x,0.5f - y*y - x*x)* (180.0f /PI);
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
## 三、Mahony算法中载体系的重力投影到地理系
|
|||
|
|
|
|||
|
|
常见代码的写法如下图所示：
|
|||
|
|
|
|||
|
|
```C
|
|||
|
|
// 写法1
|
|||
|
|
float halfvx = q1 * q3 - q0 * q2;
|
|||
|
|
float halfvy = q0 * q1 + q2 * q3;
|
|||
|
|
float halfvz = q0 * q0 - 0.5f + q3 * q3;
|
|||
|
|
// 写法2
|
|||
|
|
float vx = 2.0f*(q1*q3 - q0*q2);
|
|||
|
|
float vy = 2.0f*(q0*q1 + q2*q3);
|
|||
|
|
float vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
比较有差异的主要在于vz的写法，这两种的写法其实是一样的，推导如下所示
|
|||
|
|
|
|||
|
|
$$
|
|||
|
|
\begin{align*}
|
|||
|
|
vz &= q_0^2-0.5+q_3^2 \\
|
|||
|
|
2vz &= 2q_0^2-1+2q_3^2 \\
|
|||
|
|
&= 2q_0^2+2q_3^2-q_0^2-q_1^2-q_2^2-q_3^2 \\
|
|||
|
|
&= q_0^2-q_1^2-q_2^2+q_3^2 \\
|
|||
|
|
\end{align*}
|
|||
|
|
$$
|
|||
|
|
|