球坐标系
球坐标系是三维坐标系中的一种,在无人机中一般使用球坐标系来表示相机姿态,相机姿态的坐标是相对于无人机的,而无人机的飞行姿态则是相对于大地坐标系的。这里我们使用的相机是2自由度的相机,即可以水平
ϕ
\phi
ϕ 和垂直
θ
\theta
θ 两个方向转动,其中
ϕ
∈
[
0
,
2
π
]
\phi \in [0,2\pi]
ϕ∈[0,2π] 或
[
−
π
,
π
]
[-\pi,\pi]
[−π,π],
θ
∈
[
0
,
π
]
\theta \in [0,\pi]
θ∈[0,π],这里
ρ
\rho
ρ 为球半径(默认我们使用右手坐标系)
球坐标与笛卡尔坐标的转换
在后面的计算中因为球坐标不方便作旋转变换,我们需要用到球坐标与笛卡尔坐标的坐标转换
球坐标到笛卡尔坐标
x = ρ ∗ s i n ( θ ) ∗ c o s ( ϕ ) x =\rho*sin(\theta)*cos(\phi) x=ρ∗sin(θ)∗cos(ϕ)
y = ρ ∗ s i n ( θ ) ∗ s i n ( ϕ ) y=\rho*sin(\theta)*sin(\phi) y=ρ∗sin(θ)∗sin(ϕ)
z = ρ ∗ c o s ( θ ) z=\rho*cos(\theta) z=ρ∗cos(θ)
笛卡尔坐标到球坐标
ρ = x 2 + y 2 + z 2 \rho = \sqrt{x^2+y^2+z^2} ρ=x2+y2+z2
θ = a r c c o s ( z ÷ ρ ) \theta = arccos(z \div \rho) θ=arccos(z÷ρ)
ϕ
=
a
r
c
t
a
n
(
y
,
x
)
\phi=arctan(y,x)
ϕ=arctan(y,x)
相机相对大地坐标系的姿态
为了计算方便,我们一般旋转坐标系将
z
z
z轴向下,这个时候无人机机头方向即是
x
x
x 轴方向,
ϕ
\phi
ϕ即是相对于无人机机头的相机水平旋转角度,
θ
\theta
θ在垂直于无人机方向上为0度;这时的无人机载体的坐标系就是我们常用的北东地坐标系(NED,即
x
x
x轴指向北,
y
y
y轴指向东,
z
z
z轴指向地面)。
在无人机飞行中,无人机平台由于飞行运动及气流运动等因素,会影响无人机的飞行姿态,这时搭载的相机姿态相对大地坐标系会发生变化,需要加入无人机姿态去计算修正,以便于更准确计算相机的观测位置
无人机姿态一般包括3个角度即:偏航角(Yaw)、俯仰角(Pitch)、翻滚角(Roll)。偏行角一般指无人机相对北极的顺时针角度,也即整个坐标系沿
z
z
z 轴顺时针旋转的角度,俯仰角即飞机的攻角或者飞机机头的俯仰角,在上面的坐标系中是沿
y
y
y 轴旋转的角度。翻滚角则是飞机左右倾斜的角度,在上面的坐标系中是沿
x
x
x 轴旋转的角度。
以上需要注意的是:偏航角与其他角不同,这里偏航角的旋转的角度是:
Y
a
w
−
90
Yaw-90
Yaw−90,如上图,是3维坐标系的俯视图,蓝色坐标系即是无人机载体坐标系NED,黑色坐标系即是大地坐标系。所以这里无人机载体的坐标系是相对于大地坐标系顺时针旋转了
Y
a
w
−
90
Yaw-90
Yaw−90度。另一个需要注意的是:最后得到的相机水平
ϕ
\phi
ϕ 角度需要取反,即
−
ϕ
-\phi
−ϕ;因为NED坐标系和大地坐标系中
y
y
y轴是对称的,如:上图可将蓝色
x
x
x轴转到
X
X
X轴上,这时俩坐标系
y
y
y轴是对称的,所以角度是相反的。
三维空间的旋转矩阵
沿 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 z ( γ ) R y ( β ) R x ( α ) R_{3d} = R_z(\gamma)R_y(\beta)R_x(\alpha) R3d=Rz(γ)Ry(β)Rx(α)
这里需要注意的是:1.旋转矩阵相乘的顺序,矩阵乘法不满足交换律,所以不同顺序一般最后得到的相机旋转角度不同(比较常用的顺序为 X Y Z XYZ XYZ 和 Z Y X ZYX ZYX,这个依赖于惯性测量单元(IMU)的姿态解算约定的旋转顺序 )。 2. 旋转方向,这里默认写的都是正向旋转。下面贴一下逆向的旋转矩阵作为参考:
沿 x x x 轴的逆向旋转
R x c ( α ) = [ 1 0 0 0 c o s ( α ) s i n ( α ) 0 − s i n ( α ) c o s ( α ) ] R_x^c(\alpha)=\begin{bmatrix} 1&0 & 0 \\ 0&cos(\alpha)&sin(\alpha) \\ 0&-sin(\alpha)&cos(\alpha) \end{bmatrix} Rxc(α)= 1000cos(α)−sin(α)0sin(α)cos(α)
沿 y y y 轴的逆向旋转
R y c ( β ) = [ c o s ( β ) 0 − s i n ( β ) 0 1 0 s i n ( β ) 0 c o s ( β ) ] R_y^c(\beta)=\begin{bmatrix} cos(\beta)&0&-sin(\beta) \\ 0&1 & 0 \\ sin(\beta)&0&cos(\beta) \end{bmatrix} Ryc(β)= cos(β)0sin(β)010−sin(β)0cos(β)
沿 z z z 轴的逆向旋转
R z c ( γ ) = [ c o s ( γ ) s i n ( γ ) 0 − s i n ( γ ) c o s ( γ ) 0 0 0 1 ] R_z^c(\gamma)=\begin{bmatrix} cos(\gamma)&sin(\gamma) &0 \\ -sin(\gamma)&cos(\gamma) &0 \\ 0&0 & 1 \end{bmatrix} Rzc(γ)= cos(γ)−sin(γ)0sin(γ)cos(γ)0001
很显然 R x c ( α ) = R x ( − α ) R_x^c(\alpha) = R_x(-\alpha) Rxc(α)=Rx(−α),同理沿其他轴也是一样的,所以两种矩阵是形式等价的,只用一种就行了,注意旋转方向的正负。文章来源:https://www.toymoban.com/news/detail-519643.html
代码实现
from math import *
import numpy as np
def rad(x):
return radians(x)
def deg(x):
return degrees(x)
def coord_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
def coord_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)
def uav_rot(x,y,z,RX,RY,RZ):
R = rad(RX) #Roll-X
P = rad(RY) #Pitch-Y
Y = rad(RZ) #Yaw-Z
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 = Rz @ Ry @ Rx
xyz = np.array([x,y,z])
return R3d @ xyz
def camera_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
def rot_mat_to_symbol():
from sympy import Symbol,Matrix,sin,cos
R = Symbol("R") #Roll-X
P = Symbol("P") #Pitch-Y
Y = Symbol("Y") #Yaw-Z
x = Symbol("x")
y = Symbol("y")
z = Symbol("z")
xyz = Matrix([[x],[y],[z]])
Rx = Matrix([[1,0,0],
[0,cos(R),-sin(R)],
[0,sin(R),cos(R)]
])
Ry = Matrix([[cos(P),0,sin(P)],
[0,1,0],
[-sin(P),0,cos(P)],
])
Rz = Matrix([[cos(Y),-sin(Y),0],
[sin(Y),cos(Y),0],
[0,0,1]
])
res = Rz*Ry*Rx*xyz
for a,m in zip(xyz,res):
print(a,"=",m)
参考:文章来源地址https://www.toymoban.com/news/detail-519643.html
- Geometric transformations in 3D and coordinate frames
到了这里,关于无人机中的坐标系、旋转矩阵与相机姿态计算的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!