无人机中的坐标系与相机姿态计算
球坐标系
球坐标系是三维坐标系中的一种,在无人机中一般使用球坐标系来表示相机姿态,相机姿态的坐标是相对于无人机的,而无人机的飞行姿态则是相对于大地坐标系的。这里我们使用的相机是2自由度的相机,即可以水平 ϕ\phiϕ 和垂直 θ\thetaθ 两个方向转动,其中 ϕ∈[0,2π]\phi \in [0,2\pi]ϕ∈[0,2π] 或 [−π,π][-\pi,\pi][−π,π],θ∈[0,π]\theta \in [0,\pi]θ∈[0,π],这里 ρ\rhoρ 为球半径(默认我们使用右手坐标系)
球坐标与笛卡尔坐标的转换
在后面的计算中因为球坐标不方便作旋转变换,我们需要用到球坐标与笛卡尔坐标的坐标转换
球坐标到笛卡尔坐标
x=ρ∗sin(θ)∗cos(ϕ)x =\rho*sin(\theta)*cos(\phi)x=ρ∗sin(θ)∗cos(ϕ)
y=ρ∗sin(θ)∗sin(ϕ)y=\rho*sin(\theta)*sin(\phi)y=ρ∗sin(θ)∗sin(ϕ)
z=ρ∗cos(θ)z=\rho*cos(\theta)z=ρ∗cos(θ)
笛卡尔坐标到球坐标
ρ=x2+y2+z2\rho = \sqrt{x^2+y^2+z^2}ρ=x2+y2+z2
θ=arccos(z÷ρ)\theta = arccos(z \div \rho)θ=arccos(z÷ρ)
ϕ=arctan(y,x)\phi=arctan(y,x)ϕ=arctan(y,x)
相机相对大地坐标系的姿态
为了计算方便,我们一般旋转坐标系将zzz轴向下,这个时候无人机机头方向即是 xxx 轴方向,ϕ\phiϕ即是相对于无人机机头的相机水平旋转角度,θ\thetaθ在垂直于无人机方向上为0度
在无人机飞行中,无人机平台由于飞行运动及气流运动等因素,会影响无人机的飞行姿态,这时搭载的相机姿态相对大地坐标系会发生变化,需要加入无人机姿态去计算修正,以便于更准确计算相机的观测位置
无人机姿态一般包括3个角度即:偏航角(Yaw)、俯仰角(Pitch)、翻滚角(Roll)。偏行角一般指无人机相对北极的顺时针角度,也即整个坐标系沿 zzz 轴旋转的角度,我们一般要将这个角度转换为直角坐标系中的角度以便后续的计算,转换公式为 Yaw~=−(Yaw+ϕ~)+π/2\tilde{Yaw} = - (Yaw+\tilde{\phi} )+ \pi/2Yaw~=−(Yaw+ϕ~)+π/2,其中ϕ~\tilde{\phi}ϕ~为计算修正后的相机水平旋转角(计算修正时设置Yaw角度为0)。俯仰角即飞机的攻角或者飞机机头的俯仰角,在上面的坐标系中是沿 yyy 轴旋转的角度。翻滚角则是飞机左右倾斜的角度,在上面的坐标系中是沿 xxx 轴旋转的角度。以上需要注意的是偏航角与其他角不同,偏航角的旋转是整个坐标系,即将无人机坐标系转换为大地坐标系,这里没有使用位移量,需要时加上经纬度的偏移
三维空间的旋转矩阵
沿 xxx 轴的旋转
Rx(α)=[1000cos(α)−sin(α)0sin(α)cos(α)]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(α)⎦⎤
沿 yyy 轴的旋转
Ry(β)=[cos(β)0sin(β)010−sin(β)0cos(β)]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(β)⎦⎤
沿 zzz 轴的旋转
Rz(γ)=[cos(γ)−sin(γ)0sin(γ)cos(γ)0001]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⎦⎤
合并旋转后的三维旋转矩阵
R3d=Rx(α)Ry(β)Rz(γ)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=0):R = rad(RX) #RollP = rad(RY) #PitchY = rad(RZ) #Yaw, set defalut to 0Rx = [[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)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 = phitilt = thetareturn pan,tilt
无人机中的坐标系与相机姿态计算相关推荐
- ROS中ENU坐标系与无人机中NED坐标系的转换关系理解
ROS中ENU坐标系与无人机中NED坐标系的转换关系理解 项目地址 无人机中NED坐标理解 ENU与NED转换 无人机中NED坐标理解 机体坐标系:机体坐标系固连飞机,其原点 取在多旋翼的重心位置上. ...
- 投射式AR/AR眼镜的标定之相机姿态、Unity投影矩阵的计算
首先感谢这位大佬的系列文章 https://zzlzz.blog.csdn.net/article/details/53215105 https://blog.csdn.net/zzlyw/artic ...
- 无人机运动学控制中的坐标系,及惯性坐标系与机体坐标系之间的矩阵转换 欧拉角
一.无人机控制中的坐标系 无人机运动学中,有三种需要了解的坐标系 1.1.地球中心坐标系(ECEF) 地球中心坐标系,即坐标系原点位于地心.X轴通过格林尼治线和赤道线的交点,正方向为原点指向交点方向. ...
- 机器视觉基础回顾:相机标定中的坐标系
前一段尝试了相机标定,虽然做到一半放弃了,但感觉机器视觉很多东西仍然有相当大的必要去深入了解下.就像程序员多少也要懂点计算机硬件的东西,才能从底层思维的角度去优化代码. 所以留一些痕迹. 前置基础知识 ...
- 关于OpenCV的那些事——相机姿态更新
上一节我们使用张正友相机标定法获得了相机内参,这一节我们使用 PnP (Perspective-n-Point)算法估计相机初始姿态并更新之.推荐3篇我学习的博客:[姿态估计]Pose estimat ...
- 基于单目视觉的平面目标定位和坐标测量 (下) - 相机姿态估计和目标测量
上回说到,我们回顾了机器视觉的基础知识,然后一顿操作之后得到了相机内参A和畸变系数dist. 现在我们开始定位目标的坐标. 这个过程包括几个子部分: 1. 标记检测 2. 指定世界坐标系, ...
- 双目立体视觉中的坐标系与转换关系 [留意~摄影测量学与计算机视觉学科中的差异]
文章目录 前言 影像坐标系 相机坐标系 世界坐标系 影像坐标系与相机坐标系之间的相互转换 相机坐标系与世界坐标系之间的相互转换 前言 通过模拟人眼立体视觉,两个摄像机拍摄同一场景可构成双目成像模型 ...
- 全空间视频融合---学习之路(一)--直线提取检测--投影纹理--相机姿态标定
在增强现实的实际应用环境中,人们会为全空间大规模场景构造三维模型.为了增强虚拟环境的逼真性和动态性,可以向环境中融合真实世界的视频.透过视频可以获知模型该区域发生的事件.这就有了 摄像机的视频信息和三 ...
- P3P相机姿态估计数学推导,求解及自定义实现
P3P相机姿态估计数学推导,求解及自定义实现 微信公众号:幼儿园的学霸 目录 文章目录 P3P相机姿态估计数学推导,求解及自定义实现 目录 前言 相机到空间点距离求解 角度θ的计算 相机坐标系下的坐标 ...
- OA-SLAM:在视觉SLAM中利用对象进行相机重新定位
作者 | 鱼肖浓 编辑 | 汽车人 原文链接:https://zhuanlan.zhihu.com/p/587010197 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷 ...
最新文章
- 值得推荐的C/C++框架和库 【强烈推荐】
- python 语言教程(3)变量之字典
- .net数据源控件绑定mysql_理解asp.net中DropDownList编辑数据源,绑定数据库数据。...
- 《如何更改其他程序ListView控件中某个Item的内容》
- oracle 删除已备份归档,通过RMAN联机全库备份,包括控制文件,归档日志文件,备份成功后,删除已备份的归档日志。...
- opencv显示中文
- 学习java 第三天 数据类型以及存储大小取值范围 (one 大白 (●—●))
- 1990-2000年事务处理流程图和数据流图试题分析
- C语言齿轮参数计算程序,C语言程序实现齿轮基本参数几何尺寸计算.pdf
- 撤销工作表保护密码忘记了怎么办?
- node js 生成视频链接(视频流)
- web3js开发技术
- 参观微软亚洲研究院有感
- 单阶段和两阶段目标检测
- 淘宝旺旺号转userid 或 uid 接口与方法
- 技术人的充电时刻,24个精彩研发案例在等你
- Ngrok的外网映射
- 计算机视觉的顶会顶刊
- HTTPSS证书制作笔记
- Python统计模型探索性数据分析(EDA)系统(单变量-双变量-相关性-缺失值)