无人机姿态一般包括3个角度即:偏航角(Yaw)、俯仰角(Pitch)、翻滚角(Roll)。偏行角一般指无人机相对北极的顺时针角度,也即整个坐标系沿
z
z
z 轴旋转的角度,我们一般要将这个角度转换为直角坐标系中的角度以便后续的计算,转换公式为
Y
a
w
~
=
−
(
Y
a
w
+
ϕ
~
)
+
π
/
2
\tilde{Yaw} = - (Yaw+\tilde{\phi} )+ \pi/2
Yaw~=−(Yaw+ϕ~)+π/2,其中
ϕ
~
\tilde{\phi}
ϕ~为计算修正后的相机水平旋转角(计算修正时设置Yaw角度为0)。俯仰角即飞机的攻角或者飞机机头的俯仰角,在上面的坐标系中是沿
y
y
y 轴旋转的角度。翻滚角则是飞机左右倾斜的角度,在上面的坐标系中是沿
x
x
x 轴旋转的角度。以上需要注意的是偏航角与其他角不同,偏航角的旋转是整个坐标系,即将无人机坐标系转换为大地坐标系,这里没有使用位移量,需要时加上经纬度的偏移
三维空间的旋转矩阵
沿
x
x
x 轴的旋转
R
x
(
α
)
=
[
1
0
0
0
c
o
s
(
α
)
−
s
i
n
(
α
)
0
s
i
n
(
α
)
c
o
s
(
α
)
]
R_x(\alpha)=\begin{bmatrix} 1&0 & 0 \\ 0&cos(\alpha)&-sin(\alpha) \\ 0&sin(\alpha)&cos(\alpha) \end{bmatrix}
Rx(α)=⎣⎡1000cos(α)sin(α)0−sin(α)cos(α)⎦⎤
沿
y
y
y 轴的旋转
R
y
(
β
)
=
[
c
o
s
(
β
)
0
s
i
n
(
β
)
0
1
0
−
s
i
n
(
β
)
0
c
o
s
(
β
)
]
R_y(\beta)=\begin{bmatrix} cos(\beta)&0&sin(\beta) \\ 0&1 & 0 \\ -sin(\beta)&0&cos(\beta) \end{bmatrix}
Ry(β)=⎣⎡cos(β)0−sin(β)010sin(β)0cos(β)⎦⎤
沿
z
z
z 轴的旋转
R
z
(
γ
)
=
[
c
o
s
(
γ
)
−
s
i
n
(
γ
)
0
s
i
n
(
γ
)
c
o
s
(
γ
)
0
0
0
1
]
R_z(\gamma)=\begin{bmatrix} cos(\gamma)&-sin(\gamma) &0 \\ sin(\gamma)&cos(\gamma) &0 \\ 0&0 & 1 \end{bmatrix}
Rz(γ)=⎣⎡cos(γ)sin(γ)0−sin(γ)cos(γ)0001⎦⎤
合并旋转后的三维旋转矩阵
R
3
d
=
R
x
(
α
)
R
y
(
β
)
R
z
(
γ
)
R_{3d} = R_x(\alpha)R_y(\beta)R_z(\gamma)
R3d=Rx(α)Ry(β)Rz(γ)
代码实现
from math import*import numpy as np
defrad(x):return radians(x)defdeg(x):return degrees(x)defcoord_sphere2cart(theta,phi, rho =1):
x = rho*sin(rad(theta))*cos(rad(phi))
y = rho*sin(rad(theta))*sin(rad(phi))
z = rho*cos(rad(theta))return x,y,z
defcoord_cart2sphere(x,y,z):
rho = sqrt(x**2+y**2+z**2)
theta = acos(z/rho)
phi = atan2(y,x)return rho,deg(theta),deg(phi)defuav_rot(x,y,z,RX,RY,RZ=0):
R = rad(RX)#Roll
P = rad(RY)#Pitch
Y = rad(RZ)#Yaw, set defalut to 0
Rx =[[1,0,0],[0,cos(R),-sin(R)],[0,sin(R),cos(R)]]
Ry =[[cos(P),0,sin(P)],[0,1,0],[-sin(P),0,cos(P)]]
Rz =[[cos(Y),-sin(Y),0],[sin(Y),cos(Y),0],[0,0,1]]
Rx = np.array(Rx)
Ry = np.array(Ry)
Rz = np.array(Rz)
R3d = np.dot(np.dot(Rx,Ry),Rz)
xyz = np.array([x,y,z])return np.dot(R3d,xyz)defcamera_rectify(pan,tilt,yaw,pitch,roll):
x,y,z = coord_sphere2cart(theta=tilt,phi=pan,rho=1)
x_,y_,z_ = uav_rot(x,y,z,RX=roll,RY=pitch,RZ=yaw)
rho,theta,phi = coord_cart2sphere(x_,y_,z_)
pan = phi
tilt = theta
return pan,tilt