当前位置：首页 > news >正文

无人机中的坐标系与相机姿态计算

news 来源：原创 2024/9/29 21:27:38

球坐标系

球坐标系是三维坐标系中的一种，在无人机中一般使用球坐标系来表示无人机与相机姿态，相机姿态的坐标是相对于无人机的，而无人机的飞行姿态则是相对于大地坐标系的。这里我们使用的相机是2自由度的相机，即可以水平 $\phi$ 和垂直 $\theta$ 两个方向转动，其中 $\phi \in [0,2\pi]$ 或 $[-\pi,\pi]$ ， $\theta \in [0,\pi]$ ，这里 $\rho$ 为球半径（默认我们使用右手坐标系）
在这里插入图片描述

球坐标与笛卡尔坐标的转换

在后面的计算中我们需要用到球坐标与笛卡尔坐标的坐标转换
在这里插入图片描述

球坐标到笛卡尔坐标

$=\rho*sin(\theta)*cos(\phi)$

$y=\rho*sin(\theta)*sin(\phi)$

$z=\rho*cos(\theta)$

笛卡尔坐标到球坐标

$\rho = \sqrt{x^2+y^2+z^2}$

$\theta = acos(z \div \rho)$

$\phi=atan2(y,x)$

相机相对大地坐标系的姿态

为了计算方便，我们一般旋转坐标系将 $z$ 轴向下，这个时候无人机机头方向即是 $x$ 轴方向， $\phi$ 即是相对于无人机机头的相机水平旋转角度， $\theta$ 在垂直于无人机方向上为0度
在这里插入图片描述

在无人机飞行中，无人机平台由于飞行运动及气流运动等因素，会影响无人机的飞行姿态，这时搭载的相机姿态相对大地坐标系会发生变化，需要加入无人机姿态去计算修正，以便于更准确计算相机的观测位置

无人机姿态一般包括3个角度即：偏航角、俯仰角、翻滚角。偏行角一般指无人机相对北极的顺时针角度，也即整个坐标系沿 $z$ 轴旋转的角度，我们一般要将这个角度转换为直角坐标系中的角度以便后续的计算，转换公式为 $\tilde{a} = - a + \pi/2$ 。俯仰角即飞机的攻角或者飞机机头的俯仰角，在上面的坐标系中是沿 $y$ 轴旋转的角度。翻滚角则是飞机左右倾斜的角度，在上面的坐标系中是沿 $x$ 轴旋转的角度。以上需要注意的是偏航角与其他角不同，偏航角的旋转是整个坐标系，即将无人机坐标系转换为大地坐标系，这里没有使用位移量，需要时加上经纬度的偏移

三维空间的旋转矩阵

沿 $x$ 轴的旋转

$R_x(\alpha)=\begin{bmatrix} 1&0 & 0 \\ 0&cos(\alpha)&-sin(\alpha) \\ 0&sin(\alpha)&cos(\alpha) \end{bmatrix}$

沿 $y$ 轴的旋转

$R_y(\beta)=\begin{bmatrix} cos(\beta)&0&sin(\beta) \\ 0&1 & 0 \\ -sin(\beta)&0&cos(\beta) \end{bmatrix}$

沿 $z$ 轴的旋转

$R_z(\gamma)=\begin{bmatrix} cos(\gamma)&-sin(\gamma) &0 \\ sin(\gamma)&cos(\gamma) &0 \\ 0&0 & 1 \end{bmatrix}$

合并旋转后的三维度旋转矩阵

$R_{3d} = R_x(\alpha)R_y(\beta)R_z(\gamma)$

代码实现

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
    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