转载的

最新推荐文章于 2024-04-16 20:02:17 发布

转载最新推荐文章于 2024-04-16 20:02:17 发布 · 581 阅读

坐标转换有很多种方法，不同的领域有不同的使用习惯。
上两篇文章我们讲了旋转矩阵和欧拉角，可知欧拉角是可以由旋转矩阵转化而来。
那么怎么从欧拉角转化为旋转矩阵呢？

欧拉角（Euler angles）与旋转矩阵（Rotation Matrix）

假设坐标系1的欧拉角yaw（Azimuth）、pitch、roll的角度为α,β,γα,β,γ,可以由公式：

R(α,β,γ)=Rz(α)Ry(β)Rx(γ)=⎡⎣⎢cosαcosβsinαcosβ−sinβcosαsinβsinγ−sinαcosγsinαsinβsinγ+cosαcosγcosβsinγcosαsinβcosγ+sinαsinγsinαsinβcosγ−cosαsinγcosβcosγ⎤⎦⎥R(α,β,γ)=Rz(α)Ry(β)Rx(γ)=[cosαcosβcosαsinβsinγ−sinαcosγcosαsinβcosγ+sinαsinγsinαcosβsinαsinβsinγ+cosαcosγsinαsinβcosγ−cosαsinγ−sinβcosβsinγcosβcosγ]

旋转矩阵是一个正交矩阵，其中，

Rz(α)=⎡⎣⎢cosαsinα0−sinαcosα0001⎤⎦⎥Rz(α)=[cosα−sinα0sinαcosα0001]

Ry(β)=⎡⎣⎢cosβ0−sinβ010sinβ0cosβ⎤⎦⎥Ry(β)=[cosβ0sinβ010−sinβ0cosβ]

Rx(γ)=⎡⎣⎢1000cosγsinγ0−sinγcosγ⎤⎦⎥Rx(γ)=[1000cosγ−sinγ0sinγcosγ]

那么假设有坐标系1中的点坐标为 P1P1 ,经过欧拉角的旋转变换变成了坐标系2的坐标 P2P2，那么有

P1=R⋅P2P1=R·P2

注意相乘的顺序,顺序不同旋转矩阵也不同。

旋转矩阵与方向余弦矩阵(Direction Cosine Matrix)

方向余弦就是各个坐标轴之间的夹角的余弦。上面提到的欧拉角就是按照某坐标轴顺序依次进行旋转，每相乘一次即乘以一个矩阵，而这个矩阵其实就是方向余弦矩阵。
比如欧拉角yaw（Azimuth），是围绕z轴旋转，那么就是乘以Rz(α)Rz(α)。这里的Rz(α)Rz(α)就是一个方向余弦矩阵。
其实可以看出，旋转矩阵就是三个方向余弦矩阵的相乘，本质上都是一样的。所以在下面的讨论中不再做区分，统称旋转矩阵。

在matlab中，可以用函数angle2dcm和dcm2angle来完成旋转矩阵与欧拉角之间的相互转换。

轴角表示（Axis-Angle）

旋转的轴角表示用两个值参数化了旋转: 一个直线（轴），和描述绕这个轴旋转的一个角。也叫做旋转的指数坐标。

比如我们可以使用<Axis,Angle>=<(x,y,z),θ>=(x,y,z,θ)<Axis,Angle>=<(x,y,z),θ>=(x,y,z,θ)来表示坐标的旋转。其中Axis可以是任意一条直线，大小也不一。

旋转向量（Rotation vector）

轴角表示很直观得说明了坐标系的旋转情况，但还是不够简洁。旋转向量本质上还是轴角表示，但是只是用了一个向量来代替。
还以上一个例子展开说明，若一个旋转情况的轴角表示为(x,y,z,θ)(x,y,z,θ),那么其旋转向量为

(θ⋅x′,θ⋅y′,θ⋅z′)(θ·x′,θ·y′,θ·z′)

其中，(x′,y′,z′)(x′,y′,z′)为(x,y,z)(x,y,z)的单位向量。
旋转向量在安卓中可以用TYPE_ROTATION_VECTOR直接拿到旋转向量，但要注意的是，安卓中的旋转向量是

(x′⋅sin(θ2),y′⋅sin(θ2),z′⋅sin(θ2))(x′·sin(θ2),y′·sin(θ2),z′·sin(θ2))

matlab中，可以使用angle2rod函数来从欧拉角转化为旋转向量。

四元数（Quaternion）

四元数是旋转向量的变种，在旋转向量前(或后)加了一项，变成了：

(cos(θ2),x′sin(θ2),y′sin(θ2),z′sin(θ2))(cos(θ2),x′sin(θ2),y′sin(θ2),z′sin(θ2))

设四元数

q=⎡⎣⎢⎢⎢⎢qiqjqkqr⎤⎦⎥⎥⎥⎥=qii+qjj+qkk+qrq=[qiqjqkqr]=qii+qjj+qkk+qr,(这里把角度放在了后面)

则需满足：

q2i+q2j+q2k+q2r=1qi2+qj2+qk2+qr2=1

设有坐标系1里的点P1P1,需要通过四元数转换到坐标系2点P2P2，则有：

P2=qP1q−1P2=qP1q−1

其中，q−1q−1是qq的共轭，

q−1=⎡⎣⎢⎢⎢⎢−qi−qj−qkqr⎤⎦⎥⎥⎥⎥q−1=[−qi−qj−qkqr]

四元数的好处就是计算方便快捷，安卓中可以使用getQuaternionFromVector方法由旋转矩阵得到四元数。

四元数与欧拉角的转换

注意，不同的旋转顺序，坐标表示，转换的方式都不尽相同，请在转换前留意。

若绕z-y-x轴的顺序旋转，欧拉角为yaw (ψ), pitch (θ) and roll (φ) 。
可以求出四元数q=(qi,qj,qk,qr)q=(qi,qj,qk,qr),这里qrqr为角度，即角度放在了后面：

qiqjqkqr=sinϕ2cosθ2cosψ2−cosϕ2sinθ2sinψ2=cosϕ2sinθ2cosψ2+sinϕ2cosθ2sinψ2=cosϕ2cosθ2sinψ2−sinϕ2sinθ2cosψ2=cosϕ2cosθ2cosψ2+sinϕ2sinθ2sinψ2qi=sin⁡ϕ2cos⁡θ2cos⁡ψ2−cos⁡ϕ2sin⁡θ2sin⁡ψ2qj=cos⁡ϕ2sin⁡θ2cos⁡ψ2+sin⁡ϕ2cos⁡θ2sin⁡ψ2qk=cos⁡ϕ2cos⁡θ2sin⁡ψ2−sin⁡ϕ2sin⁡θ2cos⁡ψ2qr=cos⁡ϕ2cos⁡θ2cos⁡ψ2+sin⁡ϕ2sin⁡θ2sin⁡ψ2

若已知四元数求欧拉角:

rollpitchyaw=atan2(2(qrqi+qjqk),1−2(q2i+q2j))=arcsin(2(qrqj−qkqi))=atan2(2(qrqk+qiqj),1−2(q2j+q2k))roll=atan2⁡(2(qrqi+qjqk),1−2(qi2+qj2))pitch=arcsin⁡(2(qrqj−qkqi))yaw=atan2⁡(2(qrqk+qiqj),1−2(qj2+qk2))

如果把角度放在前面，即q=(q0,q1,q2,q3)q=(q0,q1,q2,q3),有：

q=⎡⎣⎢⎢⎢⎢cos(ψ/2)00sin(ψ/2)⎤⎦⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢cos(θ/2)0sin(θ/2)0⎤⎦⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢cos(ϕ/2)sin(ϕ/2)00⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢cos(ϕ/2)cos(θ/2)cos(ψ/2)+sin(ϕ/2)sin(θ/2)sin(ψ/2)sin(ϕ/2)cos(θ/2)cos(ψ/2)−cos(ϕ/2)sin(θ/2)sin(ψ/2)cos(ϕ/2)sin(θ/2)cos(ψ/2)+sin(ϕ/2)cos(θ/2)sin(ψ/2)cos(ϕ/2)cos(θ/2)sin(ψ/2)−sin(ϕ/2)sin(θ/2)cos(ψ/2)⎤⎦⎥⎥⎥⎥q=[cos⁡(ψ/2)00sin⁡(ψ/2)][cos⁡(θ/2)0sin⁡(θ/2)0][cos⁡(ϕ/2)sin⁡(ϕ/2)00]=[cos⁡(ϕ/2)cos⁡(θ/2)cos⁡(ψ/2)+sin⁡(ϕ/2)sin⁡(θ/2)sin⁡(ψ/2)sin⁡(ϕ/2)cos⁡(θ/2)cos⁡(ψ/2)−cos⁡(ϕ/2)sin⁡(θ/2)sin⁡(ψ/2)cos⁡(ϕ/2)sin⁡(θ/2)cos⁡(ψ/2)+sin⁡(ϕ/2)cos⁡(θ/2)sin⁡(ψ/2)cos⁡(ϕ/2)cos⁡(θ/2)sin⁡(ψ/2)−sin⁡(ϕ/2)sin⁡(θ/2)cos⁡(ψ/2)]

由欧拉角求四元数：

⎡⎣⎢ϕθψ⎤⎦⎥=⎡⎣⎢atan2(2(q0q1+q2q3),1−2(q21+q22))asin(2(q0q2−q3q1))atan2(2(q0q3+q1q2),1−2(q22+q23))⎤⎦⎥[ϕθψ]=[atan2(2(q0q1+q2q3),1−2(q12+q22))asin(2(q0q2−q3q1))atan2(2(q0q3+q1q2),1−2(q22+q32))]

matlab中(Aerospace Toolbox)，
quatmultiply，quatinv，quatconj分别对应四元数的乘法、逆、和共轭。
angle2quat和quat2angle进行欧拉角与四元数的互转。
dcm2quat和quat2dcm进行旋转矩阵与四元数的互转。
rod2quat和quat2rod进行旋转向量与四元数的互转。

Reference: