由加速度计解算得到姿态角

先说结论，再解释原理
假设世界坐标系下重力向量为g=(001)Tg=(001)Tg=( \begin{matrix} 0 & 0 & 1 \end{matrix} )^T
机体坐标系下，将加速度计测量得到的数据归一化后得到a=(axayaz)Ta=(axayaz)T a = ( \begin{matrix} ax & ay & az \end{matrix} )^T
横滚角、俯仰角、偏航角可简单理解为加速度计绕X、Y、Z轴旋转的角度
若使用mpu6050，则将芯片上的X轴对准前方，Y轴向左，Z轴向上(mpu6050芯片上显示的坐标轴为陀螺仪的坐标轴，mpu6050中加速度计和陀螺仪坐标轴正方向相反)即将加速度计X轴对准后方，Y轴向右，Z轴向下(这样表示的好处是如果加速度方向为前，则加速度计所测得的加速度为正)
使用右手定则，大拇指分别指向陀螺仪X、Y、Z轴正方向，四指所指方向角度为正
俯仰角θ=asin(−ax)θ=asin(−ax)\theta=asin(-ax)
横滚角ϕ=atan(ay/az)ϕ=atan(ay/az)\phi=atan(ay/az)
在只有加速度计的情况下，无法测得偏航角ψψ\psi

以下为原理
假设加速度计绑定在机体上，并且加速度X、Y、Z轴与机体X、Y、Z轴重合，在机体不剧烈运动的情况下，可认为加速度计测出的加速度表示重力加速度，可根据这一特性，解算得出姿态。

在初始未旋转的状态，即机体坐标系与世界坐标系重合时，将加速度计测得的数据归一化后得到a=g=(001)Ta=g=(001)Ta=g=( \begin{matrix} 0 & 0 & 1 \end{matrix} )^T
假设经过旋转RnRnR_n，得到加速度计归一化后的数据a=(axayaz)Ta=(axayaz)T a = ( \begin{matrix} ax & ay & az \end{matrix} )^T
则a=Rn∗ga=Rn∗ga =R_n*g
其中
旋转矩阵R=R=R=

⎡⎣⎢⎢cosθcosψcosψsinθsinϕ−sinψcosϕcosψsinθcosϕ+sinψsinϕcosθsinψsinψsinθsinϕ+cosψcosϕsinψsinθcosϕ−cosψsinϕ−sinθsinϕcosθcosϕcosθ⎤⎦⎥⎥[cosθcosψcosθsinψ−sinθcosψsinθsinϕ−sinψcosϕsinψsinθsinϕ+cosψcosϕsinϕcosθcosψsinθcosϕ+sinψsinϕsinψsinθcosϕ−cosψsinϕcosϕcosθ]

\left[ \begin{matrix}cos\theta cos\psi & cos\theta sin\psi & -sin\theta\\cos\psi sin\theta sin\phi - sin\psi cos\phi & sin\psi sin\theta sin\phi+cos\psi cos\phi & sin\phi cos\theta\\cos\psi sin\theta cos\phi + sin\psi sin\phi & sin\psi sin\theta cos\phi-cos\psi sin\phi & cos\phi cos\theta\end{matrix} \right]
Rn=R(−θ,−ψ,−ϕ)Rn=R(−θ,−ψ,−ϕ)R_n=R(-\theta, -\psi, -\phi)
其中 θθ\theta 为俯仰角， ψψ\psi为偏航角， ϕϕ\phi为横滚角
由此，得到 ax=sinθax=sinθax=sin\theta， ay=−sinϕcosθay=−sinϕcosθay=-sin\phi cos\theta， az=cosϕcosθaz=cosϕcosθaz=cos\phi cos\theta
因此得到
θ=asin(ax)θ=asin(ax)\theta=asin(ax)
ϕ=atan(−ay/az)ϕ=atan(−ay/az)\phi=atan(-ay/az)
使用mpu6050加速度计和陀螺仪数据时，分别用这两种数据解算得到的姿态角是相反的，一般将加速度计的旋转矩阵取反，即以芯片标明的XYZ轴方向为正方向
因此本文开头部分计算姿态角公式时对旋转矩阵取反
θ=asin(−ax)θ=asin(−ax)\theta=asin(-ax)
ϕ=atan(ay/az)ϕ=atan(ay/az)\phi=atan(ay/az)

向量的旋转与坐标系的旋转

向量旋转
旋转矩阵RRR表示向量n" role="presentation">nnn按照Z−Y−XZ−Y−XZ-Y-X的顺序，绕这三个轴分别旋转ψ、ϕ、θψ、ϕ、θ\psi、\phi、\theta
得到向量旋转后的坐标为n′=R∗nn′=R∗nn'=R*n
注意上面的旋转矩阵表示的是向量的旋转，而非坐标系的旋转
坐标系旋转
如果向量nnn不旋转，而向量n所在坐标系按照Z−Y−X" role="presentation">Z−Y−XZ−Y−XZ-Y-X的顺序旋转
则可以得到坐标系旋转后n向量的坐标为n″=Rn∗n=RT∗nn″=Rn∗n=RT∗nn''=R_n*n=R^T*n

至于为什么Rn=R(−θ,−ψ,−ϕ)Rn=R(−θ,−ψ,−ϕ)R_n=R(-\theta, -\psi, -\phi)，可类比二维向量的旋转
二维旋转矩阵

r(θ)=[cosθsinθ−sinθcosθ]r(θ)=[cosθ−sinθsinθcosθ]

r(\theta)=\left[ \begin{matrix}cos\theta & -sin\theta \\ sin\theta &cos\theta \end{matrix}\right]
向量 b=(xy)Tb=(xy)Tb=( \begin{matrix} x & y \end{matrix} )^T 绕 ZZZ轴（沿纸面向外）逆时针旋转θ" role="presentation">θθ\theta 可表示为
b′=r(θ)∗bb′=r(θ)∗bb'=r(\theta)*b
若向量 bbb不旋转，而Y−X" role="presentation">Y−XY−XY-X坐标系绕 ZZZ轴逆时针旋转θ" role="presentation">θθ\theta后， bbb的坐标变为
b″=r(−θ)∗b" role="presentation">b″=r(−θ)∗bb″=r(−θ)∗bb''=r(-\theta)*b
三维的旋转矩阵与二维的旋转矩阵有相似的规律

由加速度计解算得到姿态角相关推荐

python 数据处理姿态角数据解算
对于姿态角解算,目前我所知道的只有两种,一种是DMP库解算出姿态角.一种是算法进行姿态解算: 本次说的是姿态解算,采集的是六轴的原始数据,用python进行姿态解算:废话不多说,看程序,有注解的.. ...
imu初始对准的姿态角解算注意事项
众所周知,在b系下的一个向量要投影到n系,需要其在b系下的坐标,乘上方向余弦矩阵Cnb.根据此原理,通过分别找到两个坐标系下对应的的三个不平行矢量,即可求解出该矩阵.在武大牛小骥的ppt中介绍了姿态阵 ...
单片机开发教程3——串口发送MPU6050姿态角
文章目录 1. 简介 1.1 模块原理图 1.2 引脚说明 1.3 接线方式 2. IIC通信 2.1 IIC介绍 2.2 例程讲解 3. 姿态解算 3.1 欧拉角 3.2 解算方法 3.3 一阶互补 ...
树莓派pico mpu6050 一阶互补滤波四元数法解算姿态角
micro-python:一阶互补滤波&四元数法代码 2.系统方案 2.1.组成本系统由供电部分, 主控部分, 姿态传感器与通信部份组成 2.2.供电部分电池为一节14500锂电池, 容 ...
STM32实现四驱小车（三）传感任务——姿态角解算
目录一. 绪论二. 惯性传感器测量原理 1. 三轴加速度计 2. 三轴陀螺仪 3. 三轴磁力计三. 状态估计 1. 姿态估计 (1)线性互补滤波器 (2)非线性互补滤波器 (3)卡尔曼滤波器 2 ...
四元数解算姿态角解析
本文来自:链接一.概述无人机求解姿态角有多种算法,但由于各种算法的自身限制及计算机计算速度的限制,所以我们需要选择一个较佳的求解算法,下面我们先来看看几种求解姿态角的算法: 1. 欧拉角法: 欧拉 ...
关于无人机四元数解算姿态角解析你知道吗？
原文链接http://www.elecfans.com/d/705815.html 一.概述无人机求解姿态角有多种算法,但由于各种算法的自身限制及计算机计算速度的限制,所以我们需要选择一个较佳的求解 ...
MPU6050卡尔曼滤波解算姿态角
前言自己在课上吹的牛,课程作业再麻烦也得干.模了好几天鱼,终于在DDL前一天弄完了惯导模块的简单demo,卡尔曼滤波算是我弄的最久的了(大概2-3天),虽然没有彻底弄懂原理(概率论没学,隐马尔可夫链 ...
PX4飞控中利用EKF估计姿态角代码详解
PX4飞控中利用EKF估计姿态角代码详解 PX4飞控中主要用EKF算法来估计飞行器三轴姿态角,具体c文件在px4\Firmware\src\modules\attitude_estimator_ekf ...
matlab求逆矩阵_MPU6050姿态解算2-欧拉角amp;旋转矩阵
1 IMU姿态解算 IMU,即惯性测量单元,一般包含三轴陀螺仪与三轴加速度计.之前的文章码农爱学习:MPU6050姿态解算方式1-DMPzhuanlan.zhihu.com 已将对MPU6050这 ...

由加速度计解算得到姿态角

向量的旋转与坐标系的旋转

由加速度计解算得到姿态角相关推荐

最新文章

热门文章