yaw公式_坐标系和yaw, pitch, roll等基础概念

坐标系

右手坐标系：把右手拇指食指中指伸直并正交，拇指X，食指Y，中指Z。一般无特殊说明，都是右手坐标系。

右手坐标系的旋转正方向：从轴的正方向看向原点，逆时针方向即是旋转正向。或者，伸出右手，拇指指向旋转轴正向，四指弯曲，四指指向的旋转方向就是正向。

世界坐标系之NED坐标：X轴向北，Y轴向东，Z轴向下

世界坐标系之ENU坐标：X轴向东，Y轴向北，Z轴向上

世界坐标系之NWU坐标：X轴向北，Y轴向西，Z轴向上(这个坐标系最符合人类想象)

自身坐标系之FRD坐标：X轴向前，Y轴向右，Z轴向下。

自身坐标系之FLU坐标：X轴向前，Y轴向左，Z轴向上。(这个坐标系最符合人类想象)

FRD和NED配合：当自身面北平放的时候，FRD坐标系和NED坐标系重合。

FLU和ENU配合：当自身面东平放的时候，FLU坐标系和ENU坐标系重合。

FLU和NWU配合：当自身面北平放的时候，FLU坐标系和NWU坐标系重合。(最佳配合)

建议：当做一个系统的时候，要先统一系统内部的坐标系，优先统一到FLU+NWU。输入的坐标，在输入的时候，就要转为内部坐标系；输出的坐标，在输出的时候，根据用户的要求转为相应的外部坐标系。

yaw, pitch, roll

yaw：偏航角。是沿世界坐标系的Z轴旋转的角度。0表示面向世界坐标系的X轴正向。

在NED坐标下，0度是面向正北。在ENU做坐标下，0度是面向正东。

面向正北，在NED下，是0度，在ENU下，则是PI/2弧度。

面向正北偏东1度(0.01745弧度)，在NED坐标系下，yaw=0.01745，在ENU坐标系中，yaw=PI/2-0.01745

所以，可以推算出来，yaw从END坐标系转到ENU坐标系的转换公式为：yaw_ENU = PI/2 - yaw_END

pitch：俯仰角。就是抬头或低头，和大地水平面的夹角。换句话说，是沿自身坐标系(X轴向前的坐标系)的Y轴旋转的角度。

抬头1度，在FRD坐标系下，pitch=0.01745，在FLU坐标系中，pitch=-0.01745。

所以，可以推算出来，pitch从FRD坐标系转到FLU坐标系的转换公式为：pitch_FLU = - pitch_FRD

roll：翻滚角。就是左倾或右倾，和大地水平面的夹角。换句话说，是沿自身坐标系(X轴向前的坐标系)的X轴旋转的角度。

右倾1度，在FRD坐标系下，roll=0.01745，在FLU坐标系中，roll=0.01745。

所以，可以推算出来，roll从FRD坐标系转到FLU坐标系的转换公式为：roll_FLU = - roll_FRD

注意，不同的惯导设备，采用的坐标系是不同的，有的用FRD，有的用FLU。例如ROS就缺省使用FLU，如果需要将某个FRD设备的数据发布到ROS，就需要做一个转换。

欧拉角

用3次连续转角来描述2个坐标系之间的旋转关系。

设xyz 为全局坐标，保持不动

设XYZ 为局部坐标，随着物体一起运动

那么zXZ的欧拉角为：

1) 物体绕全局的 z 轴旋转 alpha 角

2) 继续绕自己的 X 轴旋转 beta 角

3) 最后绕自己的 Z 轴旋转 gamma 角

Z-Y-X欧拉角

参考 Introduction to Robotics Machanics and Control.pdf P43

ZYX欧拉角是yaw-pitch-roll次序。

1) 先沿着Z轴旋转一个yaw角

2) 再沿着自己的Y轴旋转一个pitch角

3) 再沿着自己的X轴旋转一个roll角

设R为3*3的旋转矩阵，则R = R_yaw * R_pitch * R_roll

R_yaw是旋转后z不变的阵，R_pitch是旋转后y不变的阵，R_roll是旋转后x不变的阵。

求一个点在另一个坐标系中的坐标

参考 Introduction to Robotics Machanics and Control.pdf P27 Mappings involving general frames 公式2.19

设有2个坐标系 A 和 B，现在有一个点P，它在B坐标系中的坐标是P_b，那么求它在A坐标系中的坐标P_a。

已知B的原点在A坐标系中的坐标是O_ba，已知B相对于A的旋转矩阵是R_ab。

那么，我们可以先保持B的原点不动，把B旋转为B1，和A同向。此时该点P的坐标在B1里面是P_b1。而P_a = P_b1 + O_ba。

所以有 P_a = R_ab P_b + O_ba(2.17)

以上的公式，可以写成更酷的一个公式：

[P_a, 1]T = T_ab [P_b, 1] (2.19)

其中，T_ab为1个4*4的矩阵，左上角是R_ab，右上角是O_ba，左下角是 0 0 0，右下角是1。

四元数用于旋转

四元数有x,y,z,w这4个分量。错误的理解是：x,y,z表示旋转轴，w表示旋转角度。但实际上，如果ax,ay,az是旋转轴向量，theta是旋转角度的话，四元数实际是：

w = cos(theta/2)

x = ax * sin(theta/2)

y = ay * sin(theta/2)

z = az * sin(theta/2)

这样定义很不直观，但好处是可以可以插值。

给定两个四元数p和q，分别代表旋转P和Q，则乘积pq表示两个旋转的合成(即旋转了Q之后再旋转P)

举个例子，我们要把一个enu的航向角yaw，转成nwu的yaw，就可以直接用一个表示旋转的四元数乘它既可：

tf::Quanterion enu2nwu;

enu2nwu.setRPY(0, 0, -M_PI_2); // 以目标坐标nwu为基准，看源坐标enu在目标坐标中，是沿z轴正方向旋转了-PI/2。

yaw_nwu = enu2nwu * yaw_enu; // yaw本身表示一个旋转，左边乘以enu2nwu，表示先旋转坐标系得到新的坐标系，然后再旋转本身。