无人机中的坐标系、旋转矩阵与相机姿态计算

这篇具有很好参考价值的文章主要介绍了无人机中的坐标系、旋转矩阵与相机姿态计算。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

球坐标系

球坐标系是三维坐标系中的一种,在无人机中一般使用球坐标系来表示相机姿态,相机姿态的坐标是相对于无人机的,而无人机的飞行姿态则是相对于大地坐标系的。这里我们使用的相机是2自由度的相机,即可以水平 ϕ \phi ϕ 和垂直 θ \theta θ 两个方向转动,其中 ϕ ∈ [ 0 , 2 π ] \phi \in [0,2\pi] ϕ[0,2π] [ − π , π ] [-\pi,\pi] [π,π] θ ∈ [ 0 , π ] \theta \in [0,\pi] θ[0,π],这里 ρ \rho ρ 为球半径(默认我们使用右手坐标系
无人机影像姿态,Programming,无人机,坐标系,相机姿态

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

在后面的计算中因为球坐标不方便作旋转变换,我们需要用到球坐标与笛卡尔坐标的坐标转换
无人机影像姿态,Programming,无人机,坐标系,相机姿态

球坐标到笛卡尔坐标

   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轴指向地面)。
无人机影像姿态,Programming,无人机,坐标系,相机姿态

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

无人机姿态一般包括3个角度即:偏航角(Yaw)、俯仰角(Pitch)、翻滚角(Roll)。偏行角一般指无人机相对北极的顺时针角度,也即整个坐标系沿 z z z 轴顺时针旋转的角度,俯仰角即飞机的攻角或者飞机机头的俯仰角,在上面的坐标系中是沿 y y y 轴旋转的角度。翻滚角则是飞机左右倾斜的角度,在上面的坐标系中是沿 x x x 轴旋转的角度。
无人机影像姿态,Programming,无人机,坐标系,相机姿态
以上需要注意的是:偏航角与其他角不同,这里偏航角的旋转的角度是: Y a w − 90 Yaw-90 Yaw90,如上图,是3维坐标系的俯视图,蓝色坐标系即是无人机载体坐标系NED,黑色坐标系即是大地坐标系。所以这里无人机载体的坐标系是相对于大地坐标系顺时针旋转了 Y a w − 90 Yaw-90 Yaw90度。另一个需要注意的是:最后得到的相机水平 ϕ \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(α)0sin(α)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(β)0sin(β)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(γ)0sin(γ)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(β)010sin(β)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(α),同理沿其他轴也是一样的,所以两种矩阵是形式等价的,只用一种就行了,注意旋转方向的正负。

代码实现

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

  1. Geometric transformations in 3D and coordinate frames

到了这里,关于无人机中的坐标系、旋转矩阵与相机姿态计算的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • matplotlib 齐次坐标系 绘制旋转 3D 立体

    齐次坐标系描述了刚体的坐标系、位置,而且还提供了一套相对旋转、相对移动、绝对旋转、绝对移动的方法,用来绘制旋转的 3D 立体是再好不过的选择 将笛卡尔坐标系的三个轴记为 ,将任意的齐次坐标系记为 我们使用这样一个矩阵来描述 坐标系与 坐标系之间的关系:

    2024年02月09日
    浏览(40)
  • 【学习笔记】空间坐标系旋转与四元数

      最近在学惯性器件,想着先把理论知识脉络打通,于是便开始学习空间坐标系旋转和四元数,正好结合刚刚结课的课程《机器人控制技术》,记录一下学习心得。 旋转矩阵和齐次变换矩阵部分主要参考自教材 《机器人学导论》 中的第2章 【有需要的可以去z-library上免费

    2024年02月01日
    浏览(42)
  • 2D坐标系下的点的转换矩阵(平移、缩放、旋转、错切)

    1. 平移 (Translation) 在2D空间中,我们经常需要将一个点平移到另一个位置。假设空间中的一点 P ( x , y ) P(x,y) P ( x , y ) ;将其向 x , y x, y x , y 方向分别平移 t x t_x t x ​ , t y t_y t y ​ , 假设平移后点的坐标为 ( x ′ , y ′ ) (x\\\',y\\\') ( x ′ , y ′ ) ,则上述点的平移操作可以归纳为

    2024年02月15日
    浏览(39)
  • 3Dslicer医学图像三维坐标系(xyz,RAS,IJK)差异,转换,旋转,平面角

    目录 World coordinate system世界坐标系xyz Anatomical coordinate system解剖学坐标系(LPS/RAS/RAI) Image coordinate system图像坐标系ijk Image transformation图像转换 三维坐标变换 A.旋转矩阵和旋转向量 B.欧拉角 C.四元数​编辑 计算平面角Angle Planes插件 参考链接 处理医学图像和应用程序时的问题之一

    2024年01月17日
    浏览(156)
  • 自动驾驶中的坐标系

    自动驾驶中的坐标系 总结于网络。重点在世界坐标系理解。 图自 图自 针孔模型: 图自 Velodyne生产的64线激光雷达HDL64: 64线激光雷达进行环境扫描过程:单帧扫描数据 64线激光雷达进行环境扫描过程:全周扫描数据 激光雷达坐标系统 此节来自 车体坐标系用来描述车辆周围

    2024年01月17日
    浏览(40)
  • 通过USRP B200软件无线电SDR方式解码无人机坐标飞手坐标,实现无人机探测侦测监控,继而实现反制压制

    环境 详情 电脑 联想 系统 Windows gnuradio gnuradio-3.7.5 uhd UHD-Mirror-release_003_009_002 USRP 型号 USRP-B200-mini          无人机的应用领域越来越广泛,但是其目前大范围广泛使用给安全、安保和隐私带来了许多风险。例如,攻击方可能会使用无人机进行监视、运输非法物品,或通过侵

    2024年02月09日
    浏览(79)
  • 输出无重复的3位数和计算无人机飞行坐标

    题目描述 从{1,2,3,4,5,6,7,8,9}中随机挑选不重复的5个数字作为输入数组‘selectedDigits’,能组成多少个互不相同且无重复数字的3位数?请编写程》序,从小到大顺序,以数组形式输出这些3位数 输入描述: 输出描述 核心代码 题目描述 编写一个程序,模拟无人机的飞行路径。给定

    2024年02月12日
    浏览(38)
  • Cesium for UE4中的坐标系及其转换(再续)

    A lightweight structure to encapsulate coordinate transforms. 轻量级的坐标转换类,可以在ECEF、经纬度和UE4坐标之间进行转换。 Earth-Centered, Earth-Fixed (ECEF) coordinates Georeferenced coordinates (Latitude/Longitude/Height) Unreal coordinates (relative to the unreal world origin) 地心坐标系 Cesium for UE4 wiki

    2024年02月11日
    浏览(49)
  • 简单理解opencv中的坐标系,x,y,h,w

    在查阅opencv有关的技术文章时,经常能够看到如下风格的函数头: 那么这里的x究竟是指图像从左往右数,还是从上往下数呢? 这里直接给出结论: 在图片img上画矩形,坐标原点是图片左上角,向右为x轴正方向,向下为y轴正方向: 上面介绍的是x,y的概念。那么height,wid

    2024年02月11日
    浏览(48)
  • 【Python&GIS】无人机影像的像素坐标计算图片某点的地理/投影坐标

            又是掉头发的一天,今天的任务是通过图片中心点的地理坐标以及图片中某点的像素坐标(即这个点位于图片中的x,y坐标)计算该点的地理/投影坐标。经过一整天的搜索,发现网上并没有这方面的教程。然后就是想啊想,头发一抓一大把,终于在网上零零散散的

    2024年02月16日
    浏览(43)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包