球坐标系
球坐标系是无人三维坐标系中的一种,在无人机中一般使用球坐标系来表示无人机与相机姿态,机中计算相机姿态的标系坐标是相对于无人机的,而无人机的相机飞行姿态则是相对于大地坐标系的。这里我们使用的姿态相机是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 c o s ( z ÷ ρ ) \theta = acos(z \div \rho) θ=acos(z÷ρ)
ϕ = a t a n 2 ( y , x ) \phi=atan2(y,x) ϕ=atan2(y,x)
相机相对大地坐标系的姿态
为了计算方便,我们一般旋转坐标系将 z z z轴向下,姿态这个时候无人机机头方向即是无人 x x x 轴方向, ϕ \phi ϕ即是机中计算相对于无人机机头的相机水平旋转角度, θ \theta θ在垂直于无人机方向上为0度
在无人机飞行中,标系无人机平台由于飞行运动及气流运动等因素,相机会影响无人机的姿态飞行姿态,这时搭载的相机姿态相对大地坐标系会发生变化,需要加入无人机姿态去计算修正,以便于更准确计算相机的观测位置
无人机姿态一般包括3个角度即:偏航角、俯仰角、翻滚角。偏行角一般指无人机相对北极的顺时针角度,也即整个坐标系沿 z z z 轴旋转的角度,我们一般要将这个角度转换为直角坐标系中的角度以便后续的计算,转换公式为 a ~ = − a + π / 2 \tilde{ a} = - a + \pi/2 a~=−a+π/2。俯仰角即飞机的攻角或者飞机机头的俯仰角,在上面的坐标系中是沿 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 npdef 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,zdef 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 P = rad(RY) #Pitch Y = rad(RZ) #Yaw 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]) xyz_ = np.dot(R3d,xyz) return 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) r,theta,phi = coord_cart2sphere(x_,y_,z_) pan = phi tilt = theta return pan,tilt
未完待续。。。