在静止状态下根据IMU(加速度计)计算姿态角

一、姿态角定义：

1. 滚转角的定义:机体Z轴与过机体x轴的垂面的夹角。向右滚转为正，范围【-180deg-180deg】。

2.俯仰角的定义:机体x轴与水平面的夹角。向上为正，范围【-90deg-90deg】。

二、根据重力加速度计算姿态角的原理:

1.滚转角计算，首先通过机体x轴与重力加速度的外积，计算出“过机体x轴的垂面”的法向量。然后通过计算内积的方法，求该法向量与机体z轴的夹角，这个夹角与滚转角互余。

2.俯仰角计算:通过计算内积的方法，求机体x轴与重力加速度的夹角，这个夹角与俯仰角互余。

三、代码实现：

本人亲自测试，计算结果合格。使用方法是调用makekam_gacc_to_att函数，第一个参数是三轴重力加速度，第二和第三个参数是计算出的滚转俯仰角。

extern void makekam_gacc_to_att(float gravity_acc[3]/*输入静止状态下的机体系加速度*/,float *ret_roll/*计算结果*/,float *ret_pitch)；/*计算向量的长度，用于判定pitch为90deg时的特殊情况*/
float get_vector_mod(const float vector[3])
{return sqrtf(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
}/*通过内积法，求两个向量的夹角*/
float makekam_vec_ang(const float vec_a[3],const float vec_b[3])/*return 0~180deg*/
{float angle = 0.0f;float mod_of_vec_a = 0.0f;float mod_of_vec_b = 0.0f;mod_of_vec_a = get_vector_mod(vec_a);mod_of_vec_b = get_vector_mod(vec_b);if((0.001f >= mod_of_vec_a) || (0.001f >= mod_of_vec_b)){angle = 0.0f;/*无法计算某向量与零向量的夹角，返回0*/}else{float cos_value = (vec_a[0]*vec_b[0] + vec_a[1]*vec_b[1] + vec_a[2]*vec_b[2]) / (mod_of_vec_a * mod_of_vec_b);if(cos_value > 1.0f){cos_value=1.0f;}if(cos_value < -1.0f){cos_value=-1.0f;}angle = acos(cos_value);angle = angle * 180.0f / 3.1415f;}return angle;
}/*通过外积法，求平面两个非同向向量所在平面的法向量*/
inline void makekam_outer_product(const float vec1[3],const float vec2[3],float vecDst[3])
{vecDst[0] = vec1[1]*vec2[2] - vec2[1]*vec1[2];/*y1z2-y2z1*/vecDst[1] = vec2[0]*vec1[2] - vec1[0]*vec2[2];/*x2z1-x1z2*/vecDst[2] = vec1[0]*vec2[1] - vec2[0]*vec1[1];/*x1y2-x2y1*/
}/*根据重力加速度，计算姿态角*/
void makekam_gacc_to_att(float gravity_acc[3],float *ret_roll,float *ret_pitch)
{float roll = 0.0f;float pitch = 0.0f;float flight_z_acc[3] = {0.0f,0.0f,10.0f};float flight_x_acc[3] = {10.0f,0.0f,0.0f};float plane_xog_normal_vec[3] ={0};pitch = 90.0f - makekam_vec_ang(flight_x_acc,gravity_acc);/*pitch处于-90°到+90°之间*/if((90.0f - fabsf(pitch)) > 0.1f){/*pitch与±90度相差大于0.1°*/makekam_outer_product(flight_x_acc,gravity_acc,plane_xog_normal_vec);roll = makekam_vec_ang(flight_z_acc,plane_xog_normal_vec) - 90.0f;if(gravity_acc[2] > 0.0f){if(roll < 0.0f){roll = -180.0f - roll;}else{roll = 180 - roll;}}}else{roll = 0.0f;/*判定x轴与重力方向处于一条直线，此为特殊情况，滚转角设定为0°*/}*ret_roll = roll;*ret_pitch = pitch;
}

在静止状态下根据IMU(加速度计)计算姿态角相关推荐

IMU静态初始粗对准计算姿态角
初始对准的目的是确定惯性导航系统各坐标轴相对于参考坐标系指向过程.在捷联导航系统中,姿态信息可以以方向余弦矩阵形式表示,角度对准的目的就是去欸的那个方向余弦矩阵,方向余弦矩阵定义了惯性导航敏感轴与参考 ...
imu初始对准matlab,IMU静态初始粗对准计算姿态角
初始对准的目的是确定惯性导航系统各坐标轴相对于参考坐标系指向过程.在捷联导航系统中,姿态信息可以以方向余弦矩阵形式表示,角度对准的目的就是去欸的那个方向余弦矩阵 ,方向余弦矩阵定义了惯性导航敏感轴与参 ...
imu初始对准的姿态角解算注意事项
众所周知,在b系下的一个向量要投影到n系,需要其在b系下的坐标,乘上方向余弦矩阵Cnb.根据此原理,通过分别找到两个坐标系下对应的的三个不平行矢量,即可求解出该矩阵.在武大牛小骥的ppt中介绍了姿态阵 ...
磁力计如何用来计算姿态（2）
上一篇磁力计如何用来计算姿态(1)介绍了磁强计算姿态角的原理. 本篇介绍, 在无人飞行器上常用的加速度计+磁强计的定姿方法. 静止状态抑或悬停状态:利用加速度计算横滚角(roll)和俯仰 ...
四元数解算姿态角解析
本文来自:链接一.概述无人机求解姿态角有多种算法,但由于各种算法的自身限制及计算机计算速度的限制,所以我们需要选择一个较佳的求解算法,下面我们先来看看几种求解姿态角的算法: 1. 欧拉角法: 欧拉 ...
关于无人机四元数解算姿态角解析你知道吗？
原文链接http://www.elecfans.com/d/705815.html 一.概述无人机求解姿态角有多种算法,但由于各种算法的自身限制及计算机计算速度的限制,所以我们需要选择一个较佳的求解 ...
2. IMU原理及姿态融合算法详解
文章目录 2. IMU原理及姿态融合算法详解一.组合二. 原理 a) 陀螺仪 b) 加速度计 c) 磁力计三. 旋转的表达 a) 欧拉角 b) 旋转矩阵 c) 四元数 d) 李群 SO(3)\t ...
【传感器】IMU (加速度计 + 陀螺仪)PI数据融合以及结算四元数并求解欧拉角
参考文章: 四元数完全解析及资料汇总 mpu6050姿态解算与卡尔曼滤波(1)数学写在开头, 首先我不太想做一个搬运工, 这样没有一点意思, 我会从我的视角(小白)来尝试理解以下问题: 我们从IMU ...
使用 MWC V2.5 中的 MPU6050中的DMP进行计算姿态（转载）
玩四轴的都知道, MWC V2.5 飞控主板,板载陀螺仪传感器就是MPU6050.(不信你可以拿放大镜看). 而默认的MWC开源程序是自己读取MPU6050的原始数据,经过,自己的一套算法.算出来四 ...
树莓派IIC通讯获取BMI08x IMU数据进行姿态解算，并通过UART/TCP在rviz上显示
截至2021年,树莓派出的最新款应该是Raspberry Pi 400,设计得跟键盘一样,很难想象到这是个树莓派,尤其是它的标语写的很好"你的下一个电脑,何必是电脑",不言而喻.反 ...

在静止状态下根据IMU(加速度计)计算姿态角

在静止状态下根据IMU(加速度计)计算姿态角相关推荐

最新文章

热门文章