ROS中的旋转坐标与四元数

参考：
https://blog.csdn.net/tchenjiant/article/details/51485745
http://blog.sina.com.cn/s/blog_ab603ca00101m9mc.html
https://blog.csdn.net/harrycomeon/article/details/96272274
https://blog.csdn.net/kingov/article/details/77888289

好用的工具

https://quaternions.online/

欧拉角那里记得选 ZYX
旋转顺序是有影响的，这里应该是先后绕Z、Y、X轴旋转

四元数与旋转轴是一一对应的关系。除了相差一个标量倍数外（标量倍数，是指这么一个意思：对于任何数或者向量a,a与k*a(k是一个实数)本质上是相等的。如a= [1 2 3]，b= [2 4 6]或者b=[0.1 0.2 0.3]）都是相等的。

四元数的定义：q = [w,x,y,z]其中w是实部，当然也有资料会把四元数写成q = [x,y,z,w]其中w是实部。这都是正确的。注意：q^2 = 1.

四元数的基本数学方程为 : q = cos (a/2) + i(x * sin(a/2)) + j (y * sin(a/2)) + k(z * sin(a/2)) 其中a表示旋转角度，(x,y,z)表示旋转轴。

说明：三维空间坐标系下，物体的旋转通常都会涉及到两个坐标系，一个是世界坐标系，一个是物体坐标系。

常识告诉我们，对于一个物体点A，物体A点在世界坐标系下和物体坐标系下，有两个坐标，X_local=[x_local,y_local,z_local],X_body=[x_body,y_body,z_body],物体点A在物体坐标系下的坐标是不会发生改变的，但在世界坐标系下的坐标会随着整个物体坐标系发生改变而改变。举个不贴切的例子，假定以自己做为物体坐标系，地球做为世界坐标系，其中眼睛做为物体点A，物体点A在物体坐标系下的坐标是不会发生改变的。但是物体点A在世界坐标系下是不会发生改变的（我们经常去不同的地方嘛）。不知道你懂没有，反正我的表述能力差不多就这样了。

下面是如何把具体的四元数与旋转轴和旋转角度对应起来。

1.指出旋转轴和旋转角度，如何转化为四元素。

假定旋转轴是：RAxis = Z轴，换算成三维空间单位向量就是RAxis = [0 0 1],旋转60度
那么转化成四元数就是

q.w=cos((60/2)*pi/180) = 0.866
q.x=RAix.x*sin((60/2)*pi/180) = 0*0.5=0
q.y=RAix.y*sin((60/2)*pi/180) = 0*0.5=0
q.z=RAix.z*sin((60/2)*pi/180) = 1*0.5=0.5

例子验证：从三维空间中看，假定物体点A=[0 1 0],绕 RAxis = Z轴，旋转30度(假定顺时针为正，因为matlab就是顺时针为正，而下面的quat2dcm函数是matlab自带的)

那么物体点A旋转后在世界坐标系下的坐标将是B=[0.866 0.5 0],
如何用四元数计算出呢？思路是这样的：任何一个四元数对应着一个旋转3*3矩阵。
M=quat2dcm(q)*A’=[0.866;0.5;0],关于quat2dcm在软件matlab里面有。

2.指出四元数，怎么知道旋转轴和旋转角度呢。

  假定q=[0.866,0,0,0.5](其实这个是上面的反例子而已)

q.w=cos((a/2)*pi/180) = 0.866
q.x=RAix.x*sin((a/2)*pi/180) = 0
q.y=RAix.y*sin((a/2)*pi/180) = 0
q.z=RAix.z*sin((a/2)*pi/180) = 0.5

从上面可以得到:RAix.x=RAix.y=0
由cos((a/2)pi/180) = 0.866，得到
a = 60或120
由RAix.zsin((a/2)*pi/180) = 0.5，得到
a = 60或150

因此a = 60度(四元数的旋转角度一般是在0-360之间，之后就是多一圈的问题。
于是可得RAix.z = 1,因此其他q=[0.866,0,0,0.5]意味着旋转轴是RAxis =[0 0 1]，旋转角度是60度，其他的类似可以计算