由磁场数据和加速度数据计算初始姿态角_ros2 角速度和加速度计算姿态角-程序员宅基地

技术标签：算法 python java 组合导航

前言

一般惯导姿态角的初始值是由加速度数据和磁场数据计算得到的，加速度数据计算得到滚转角和俯仰角，由滚转角，俯仰角以及磁场数据经过计算再得到初始的偏航角。下面是具体的计算和推导过程。

计算过程:

1.计算滚转角和俯仰角

这里我们的导航坐标系为东北天坐标系（ENU），载体坐标系为右、前，上（x,y,z）,姿态角的旋转顺序为：z,x,y(偏航、俯仰，滚转)。

对于加速度计，有以下公式：

表示导航系到载体系的转换矩阵。设偏航，滚转、俯仰的初始值分别为z0 ，y0 , x0 。

并且满足：

经过计算得到:

所以：

在推导过程中纯手推容易出错，可以借助matlab计算矩阵，这样不易出错，我在推导这部分时也使用matlab来计算。代码如下：

syms x0  
syms y0  
syms z0
syms g   
 
x_r_matrix=[1,0,0;0,cos(x0),-sin(x0);0,sin(x0),cos(x0)];
y_r_matrix=[cos(y0),0,sin(y0);0,1,0;-sin(y0),0,cos(y0)];
z_r_matrix=[cos(z0),-sin(z0),0;sin(z0),cos(z0),0;0,0,1];
Cn_b = z_r_matrix*x_r_matrix*y_r_matrix; 
Cb_n = Cn_b'; 
g_n=[0;0;g];
g_b=Cb_n*g_n;

2.计算偏航角

这块可以想象一下，假设一开始平稳放置磁力计，则偏航角的计算公式如下：

该平稳状态设定为a状态，即俯仰角和滚转都为0,对于磁力计放置的任意状态，有以下公式：

根据上式求出和之后，再根据公式就可求出偏航角。

Matlab推导代码如下：

syms x0  
syms y0   
syms z0    
syms mgbx  
syms mgby   
syms mgbz    
 
x_r_matrix=[1,0,0;0,cos(x0),-sin(x0);0,sin(x0),cos(x0)];
y_r_matrix=[cos(y0),0,sin(y0);0,1,0;-sin(y0),0,cos(y0)];
z_r_matrix=[1,0,0;0,1,0;0,0,1];
Cn_b = z_r_matrix*x_r_matrix*y_r_matrix; m_b=[mgbx;mgby;mgbz]; 
m_n = Cn_b*m_b;

实际验证

我这里验证使用的IMU型号为ADI公司的ADIS16470，其载体坐标系的定义如下图所示：

和上文提到的载体坐标系是一致的，右，前，上代表载体坐标系的x,y,z轴，软件界面如下图所示：

具体的验证过程很简单，这里不再详叙，不同IMU的载体坐标系定义是不一样的，这点在使用时要注意。当逆时针绕y轴旋转，x轴数据为负， z轴向数据为正；当顺时针绕y轴旋转，x轴向数据为正，z轴数据为正；当逆时针绕x轴旋转，y轴数据为正，z轴数据为正；当顺时针绕x轴旋转，y轴数据为负，z轴数据为正。和前面公式计算得到的角度的正负号验证是一致的。

参考文献

Euler angles, quaternions, and transformation matrices for space shuttle analysis
A Double Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9D IMU

本文链接：https://blog.csdn.net/waihekor/article/details/103551408

原作者删帖不实内容删帖广告或垃圾文章投诉

智能推荐

C语言学习计算平均分_c语言求三门课程的平均成绩-程序员宅基地

文章浏览阅读866次。已知某位学生的数学、英语和计算机课程的成绩分别是87分、72分和93分，求该生3门课程的平均成绩（结果按整型输出）。提示：该整数与题目无关，为了保证系统稳定性需要有输入语句的代码。_c语言求三门课程的平均成绩

openCV学习之错误记录_opencv_ffmpeg2413.dll-程序员宅基地

文章浏览阅读2.2k次。本片blog用来记录平时学习过程中遇到的错误，以及解决办法；错误1错误提示：在using namespace cv;前缺少一个“;” 原因，是在引用的头文件中存在一个类等结束后未加“;”.错误2cv::Mat_::const_iterator it= binary.begin();程序运行到此处，出现中断。原因：在使用迭代器时，如果图像channels为3，定义应为cv_opencv_ffmpeg2413.dll

PyTorch——VGG实现（附完整代码）_vgg pytorch-程序员宅基地

文章浏览阅读1w次，点赞13次，收藏31次。参考链接https://tangshusen.me/Dive-into-DL-PyTorch/#/chapter05_CNN/5.7_vggVGGVGG的名字来源于论文作者所在的实验室Visual Geometry Group。VGG提出了可以通过重复使用简单的基础块来构建深度模型的思路。VGG块VGG块的组成规律是：连续使用数个相同的填充为1、窗口形状为3×3的卷积层后接上一个步幅为2、窗口形状为2×2的最大池化层。卷积层保持输入的高和宽不变，而池化层则对其减半。我们使用vgg_block函_vgg pytorch

jqprint js修改横向打印，纵向打印-程序员宅基地

文章浏览阅读1.8k次。(function($) { var opt; $.fn.jqprint = function (options) { opt = $.extend({}, $.fn.jqprint.defaults, options); var $element = (this instanceof jQuery) ? this : $(this); ..._前端jqprint打印功能怎么做到同时有两种打印方向

PyQt导入UI文件运行时报错“AttributeError: ‘PySide2.QtWidgets.QMainWindow‘ object has no attribute ‘button‘_ui_mainwindow' object has no attribute 'button_ima-程序员宅基地

文章浏览阅读3.5k次。学习PyQt时，导入UI文件运行时报错“AttributeError: ‘PySide2.QtWidgets.QMainWindow’ object has no attribute ‘button’”，检查UI文件中确实有控件名称为‘button’问题原因问题原因不清楚处理方法将‘button’改成‘Button’就可以正常运行了..._ui_mainwindow' object has no attribute 'button_image_open

ProjectServer2010+SharepointServer2010 错误_sharepoint 2010 projectserver-程序员宅基地

文章浏览阅读1.4k次。ProjectServer2010+SharepointServer2010 错误环境为：win2008R2+sqlserver2008R2+project server2010+sharepoint server2010按照文档操作，当创建PWA时日志报如下错误，如果一切正常，会看到状态为“已提供”设置“PWA”: 未能设置数据库。出现异常: Failed to grant log_sharepoint 2010 projectserver