四元数和欧拉角的个人理解

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

https://blog.youkuaiyun.com/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.youkuaiyun.com/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