四元数和欧拉角的个人理解_orientation x y z w求在水平面的角度-程序员宅基地

技术标签: matlab  开发语言  

欧垃角可以表示两个坐标之间的转换,但是存在一些弊端,比如万象锁,四元数也可以表述
问题分析:
比如,一个坐标系绕y轴旋转pi/2,那么我们就可以用欧拉角roll,pitch,yaw和四元数x,y,z,w进行表述,

https://blog.csdn.net/shenshen211/article/details/78492055
w = cos(theta/2)
x = ax * sin(theta/2)
y = ay * sin(theta/2)
z = az * sin(theta/2)
其中w,x,y,z是四元数的值,通过四元数的值可以计算上述式子ax,ay,az,theta四个参数值,(ax,ay,az)表示旋转轴,theta表示旋转角,这一点我们可以想象出怎么旋转的以及旋转多少度

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
void oritention_to_eluer(geometry_msgs::PoseStamped &pose_stamped)
{
  //geometry_msgs::PoseStamped pose_stamped = move_group.getCurrentPose();
  double x = pose_stamped.pose.orientation.x;
  double y = pose_stamped.pose.orientation.y;
  double z = pose_stamped.pose.orientation.z;
  double w = pose_stamped.pose.orientation.w;
  tf::Quaternion q; //或者使用(w,x,y,z);
  tf::quaternionMsgToTF(pose_stamped.pose.orientation, q);
  double roll, pitch, yaw;
  tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
  std::cout<<"roll="<<roll<<"pitch="<<pitch<<"yaw="<<yaw<<std::endl;
}
void eluer_to_oritention( double roll, double pitch,double yaw)
{
  geometry_msgs::PoseStamped pose_stamped_new;
  pose_stamped_new.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
  std::cout<<"x="<<pose_stamped_new.pose.orientation.x<<"y="<<pose_stamped_new.pose.orientation.y<<"z="
          <<pose_stamped_new.pose.orientation.z<<"w="<<pose_stamped_new.pose.orientation.w<<std::endl;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "my_tf_listener");
  ros::NodeHandle node;

  //Eluer to Oritenion
  eluer_to_oritention(0,3.14/2,0);
  //Oritenion to Eluer
  geometry_msgs::PoseStamped pose_stamped1;//defined
  pose_stamped1.pose.orientation.x=0;
  pose_stamped1.pose.orientation.y=0.706825;
  pose_stamped1.pose.orientation.z=0;
  pose_stamped1.pose.orientation.w=0.707388;
  oritention_to_eluer(pose_stamped1);
  //----------------example of Quaternion to Eluer--------
  }
return 0;
}

结果展示:

^Cwang@wang:~/open_cv_ws$ rosrun robot_setup_tf tf_tutorials_yd
x=0  y=0.706825  z=0  w=0.707388
roll=0  pitch=1.57  yaw=0

x=0,y=0.706825,z=0,w=0.707388
反解得到(ax,ay,az)=(0,1,0)旋转轴,theta=90°,这就是四元数表达的影藏含义在这里插入图片描述
在这里插入图片描述

参考文献
【1】https://blog.csdn.net/shenshen211/article/details/78492055
【2】别人代码,找不到链接网址了,抱歉

附加matlab的.m文件

clc;
clear;
pi=3.141592653;
degree_To_hudu=pi/180;%hudu/meidu

yaw=0.7854 ;%绕z轴转yaw弧度
pitch=0.1;%绕y轴转pitch弧度
roll=0;%绕x轴转roll弧度
syms w;syms x;syms y;syms z;

[w,x,y,z]=Eular_to_quaternion(yaw,pitch,roll)%"ZYX"欧拉角转四元素
[roll,pitch,yaw]=quaternion_to_Eular(w,x,y,z)%"ZYX"四元素转欧拉角

%------------函数1----------------------------------------
function [ w,x,y,z ] = Eular_to_quaternion(yaw,pitch,roll)
%Eular to quaternion
ps=yaw;
st=pitch;
fi=roll;
c_fi=cos(0.5*fi);
c_st=cos(0.5*st);
c_ps=cos(0.5*ps);

s_fi=sin(0.5*fi);
s_st=sin(0.5*st);
s_ps=sin(0.5*ps);

w=c_fi*c_st*c_ps+s_fi*s_st*s_ps;
x=s_fi*c_st*c_ps-c_fi*s_st*s_ps;
y=c_fi*s_st*c_ps+s_fi*c_st*s_ps;
z=c_fi*c_st*s_ps-s_fi*s_st*c_ps;

end
%------------函数2----------------------------------------
function [ roll,pitch,yaw] = quaternion_to_Eular(w,x,y,z)

m1=(2*(w*x+y*z))/(1-2*(x^2+y^2));
m2=2*(w*y-z*x);
m3=(2*(w*z+x*y))/(1-2*(y^2+z^2));
roll=atan(m1);
pitch=atan(m2);
yaw=atan(m3);
end
版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/yyd__/article/details/99679718

智能推荐

安装centos7之后要做的几件事_centos libtoolize-程序员宅基地

文章浏览阅读5.5k次,点赞3次,收藏6次。本文主要记录安装Centos7之后要做的几件事,作为备忘一 安装第三方源:(此处以aliyun为例)ps:本文所配置环境均为root权限下,如需在普通环境下配置,命令之前请加上sudoEPEL源: yum install epel-release执行完该命令后,在 /etc/yum.repos.d 目录下会多一个 epel.repo 文件ELRepo 源ELRepo 包含..._centos libtoolize

echarts 显示下载按钮,echarts 自定义按钮,echarts 添加按钮-程序员宅基地

文章浏览阅读2.5k次。echarts 显示下载按钮,echarts 自定义按钮,echarts 添加按钮 &gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&gt;&_echarts 图标上的下载按钮不能用

el-table表头添加图标_el table 表头加图片-程序员宅基地

文章浏览阅读149次。el-table的表头添加特殊内容。_el table 表头加图片

如何防止网页的背景图片重复 no-repeat_网页重复背景-程序员宅基地

文章浏览阅读1.2w次。(1)现在一般都用css设定了,比如在一个div下设背景图片切不让其重复,则可设一个样式,比如:.nobgrepeat{background:url(背景图片地址) left top no-repeat;}(2)也可以设置背景图片的高宽,要与背景图片的大小一样注意:但是有时候加了 no-repeate之后,图片就会不见了,这种情况与设置的 left _网页重复背景

计算机网络的性能指标以及相关计算_计算机网络数据率计算公式-程序员宅基地

文章浏览阅读1.5k次,点赞2次,收藏17次。用来表示网络的通信线路所能传送数据的能力,因此网络带宽表示在单位时间内从网络中的某一点到另一点所能通过的"最高数据率"单位和速率一样:b/s(kb/s,Mb/s,Gb/s);RTT计算公式:因为是从发送方发送完数据才开始计算的所以公式中并没有结点A的发送时延,但是有结点B的发送时延;结论:若主机在一段链路上连续不断的发送分组,则总时延为所有分组的发送时延+信号在这一段链路上的传播时延;吞吐量受网络的带宽或额定速率的限制;分组丢失率,是指在一定的时间范围内,传输过程中的丢失的分组数量与总分组数量的比率;_计算机网络数据率计算公式

012信息打点-Web应用&企业产权&指纹识别&域名资产&网络空间&威胁情报_资产指纹和应用-程序员宅基地

文章浏览阅读188次。针对各种应用的信息收集(Web网站、APP应用、PC应用、小程序应用、微信公众号等企业资产分五类:Web应用、APP应用、PC端应用、小程序应用、微信公众号、其他产品信息收集目标:某某企业、某某学校、网址、ip地址_资产指纹和应用

随便推点

《QT从基础到进阶·三十五》QT插件实现侧边工具栏tabBar_qt侧面工具栏-程序员宅基地

文章浏览阅读655次,点赞3次,收藏4次。内部先拿到TabBar的dll转为QObject,再把QObject强转为TabBarPlugin对象,这是插件的使用方法,也可以直接new一个TabBarPlugin对象。四个小工具也是由插件实现,先拿到他们的对象,通过对象指针获取他们的界面,再把每个界面addWidget添加到标签栏中。(2)调换tabBar标签栏中所有界面的循序和删除小工具界面。(3)把tabBar标签栏中的小工具界面拖出来在放回去。3、现在可以对标签栏上的所有界面进行交互。(1)可以收缩和展开tabBar。_qt侧面工具栏

Microsoft Office LTSC 2021企业办公新标杆,稳定高效助力业务发展-程序员宅基地

文章浏览阅读267次,点赞4次,收藏10次。总的来说,Office套件的功能特色在于其强大的文档处理、数据分析、演示制作和邮件管理等功能,以及各应用程序之间的协同工作,能够极大地提高用户的工作效率。除了上述三个主要应用程序外,Office套件还包括其他应用程序,如Outlook、OneNote、Publisher和Access等,每个应用程序都有其独特的功能特色,满足用户在不同场景下的办公需求。Office是一款功能强大的办公软件套件,包含了多个应用程序,每个应用程序都有其独特的功能特色。

Elasticsearch--解决JVM 堆内存使用率很高的问题_elasticsearch8占用大量内存-程序员宅基地

文章浏览阅读2k次,点赞22次,收藏16次。本文介绍如何解决ES的JVM 堆内存使用率飙升的问题。_elasticsearch8占用大量内存

《Selenium 2 自动化测试实战》读书笔记_selenium2自动化测试实战-程序员宅基地

文章浏览阅读136次。最近在弄 appium,然后顺便发现了 Selenium 框架和这本书,恰好这本书也介绍了一些软件测试 & 自动化测试的理论知识,遂拿过来学习学习。所以本文几乎没有实践内容,大多都是概念和工具的 mark,后续若有实践,我会来补充的。_selenium2自动化测试实战

树莓派4B 与 stm32 通信_树莓派4b 与 stm32 采用usb-ttl通信-程序员宅基地

文章浏览阅读1.7w次,点赞33次,收藏486次。本文用来记录智能控制课设中的通信环节的实现硬件便是树莓派4Bstm32(我用的是正点原子的STM32F103ZET6的核心板)从头开始做的,带着你去从烧录树莓派,安装STM32的编译环境开始,直到二者成功通信。如何使用串口助手进行调试,以及如何通过代码进行通信。1.准备环节(树莓派已经烧录好镜像可以正常使用,stm32也已经装好keil5的同学可以略过)1.1 树莓派新买的树莓派4B一般TF卡没有烧入镜像(也就是树莓派的操作系统)官方推荐系统是以下三种32位Lite版(无桌面)32位桌面版_树莓派4b 与 stm32 采用usb-ttl通信

Ubuntu系统下JDK环境切换_ubuntu切换jdk-程序员宅基地

文章浏览阅读251次。请注意,如果系统上没有安装Java,你将看到一个类似"command not found"的错误信息,这意味着你需要先安装Java。如果你需要切换到一个新的Java版本,首先需要安装该版本。这将列出系统中已安装的Java版本,并要求你选择默认的Java版本。完成上述步骤后,你已经成功安装并配置了新的Java版本。如果需要切换回先前的Java版本或者安装其他版本,重复步骤2和步骤3即可。首先,我们需要检查系统上已安装的Java版本。接下来,需要配置系统使用新安装的Java版本。_ubuntu切换jdk